@bloomengine/engine 0.3.3 → 0.4.1

package/native/third_party/JoltPhysics/Jolt/Physics/Body/MassProperties.h

@@ -1,58 +0,0 @@
-// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
-// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
-// SPDX-License-Identifier: MIT
-
-#pragma once
-
-#include <Jolt/ObjectStream/SerializableObject.h>
-
-JPH_NAMESPACE_BEGIN
-
-class StreamIn;
-class StreamOut;
-
-/// Describes the mass and inertia properties of a body. Used during body construction only.
-class JPH_EXPORT MassProperties
-{
-	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, MassProperties)
-
-public:
-	/// Using eigendecomposition, decompose the inertia tensor into a diagonal matrix D and a right-handed rotation matrix R so that the inertia tensor is \f$R \: D \: R^{-1}\f$.
-	/// @see https://en.wikipedia.org/wiki/Moment_of_inertia section 'Principal axes'
-	/// @param outRotation The rotation matrix R
-	/// @param outDiagonal The diagonal of the diagonal matrix D
-	/// @return True if successful, false if failed
-	bool					DecomposePrincipalMomentsOfInertia(Mat44 &outRotation, Vec3 &outDiagonal) const;
-
-	/// Set the mass and inertia of a box with edge size inBoxSize and density inDensity
-	void					SetMassAndInertiaOfSolidBox(Vec3Arg inBoxSize, float inDensity);
-
-	/// Set the mass and scale the inertia tensor to match the mass
-	void					ScaleToMass(float inMass);
-
-	/// Calculates the size of the solid box that has an inertia tensor diagonal inInertiaDiagonal
-	static Vec3				sGetEquivalentSolidBoxSize(float inMass, Vec3Arg inInertiaDiagonal);
-
-	/// Rotate the inertia by 3x3 matrix inRotation
-	void					Rotate(Mat44Arg inRotation);
-
-	/// Translate the inertia by a vector inTranslation
-	void					Translate(Vec3Arg inTranslation);
-
-	/// Scale the mass and inertia by inScale, note that elements can be < 0 to flip the shape
-	void					Scale(Vec3Arg inScale);
-
-	/// Saves the state of this object in binary form to inStream.
-	void					SaveBinaryState(StreamOut &inStream) const;
-
-	/// Restore the state of this object from inStream.
-	void					RestoreBinaryState(StreamIn &inStream);
-
-	/// Mass of the shape (kg)
-	float					mMass = 0.0f;
-
-	/// Inertia tensor of the shape (kg m^2)
-	Mat44					mInertia = Mat44::sZero();
-};
-
-JPH_NAMESPACE_END

package/native/third_party/JoltPhysics/Jolt/Physics/Body/MotionProperties.cpp

@@ -1,92 +0,0 @@
-// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
-// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
-// SPDX-License-Identifier: MIT
-
-#include <Jolt/Jolt.h>
-
-#include <Jolt/Physics/Body/MotionProperties.h>
-#include <Jolt/Physics/StateRecorder.h>
-
-JPH_NAMESPACE_BEGIN
-
-void MotionProperties::SetMassProperties(EAllowedDOFs inAllowedDOFs, const MassProperties &inMassProperties)
-{
-	// Store allowed DOFs
-	mAllowedDOFs = inAllowedDOFs;
-
-	// Decompose DOFs
-	uint allowed_translation_axis = uint(inAllowedDOFs) & 0b111;
-	uint allowed_rotation_axis = (uint(inAllowedDOFs) >> 3) & 0b111;
-
-	// Set inverse mass
-	if (allowed_translation_axis == 0)
-	{
-		// No translation possible
-		mInvMass = 0.0f;
-	}
-	else
-	{
-		JPH_ASSERT(inMassProperties.mMass > 0.0f, "Invalid mass. "
-			"Some shapes like MeshShape or TriangleShape cannot calculate mass automatically, "
-			"in this case you need to provide it by setting BodyCreationSettings::mOverrideMassProperties and mMassPropertiesOverride.");
-		mInvMass = 1.0f / inMassProperties.mMass;
-	}
-
-	if (allowed_rotation_axis == 0)
-	{
-		// No rotation possible
-		mInvInertiaDiagonal = Vec3::sZero();
-		mInertiaRotation = Quat::sIdentity();
-	}
-	else
-	{
-		// Set inverse inertia
-		Mat44 rotation;
-		Vec3 diagonal;
-		if (inMassProperties.DecomposePrincipalMomentsOfInertia(rotation, diagonal)
-			&& !diagonal.IsNearZero())
-		{
-			mInvInertiaDiagonal = diagonal.Reciprocal();
-			mInertiaRotation = rotation.GetQuaternion();
-		}
-		else
-		{
-			// Failed! Fall back to inertia tensor of sphere with radius 1.
-			mInvInertiaDiagonal = Vec3::sReplicate(2.5f * mInvMass);
-			mInertiaRotation = Quat::sIdentity();
-		}
-	}
-
-	JPH_ASSERT(mInvMass != 0.0f || mInvInertiaDiagonal != Vec3::sZero(), "Can't lock all axes, use a static body for this. This will crash with a division by zero later!");
-}
-
-void MotionProperties::SaveState(StateRecorder &inStream) const
-{
-	// Only write properties that can change at runtime
-	inStream.Write(mLinearVelocity);
-	inStream.Write(mAngularVelocity);
-	inStream.Write(mForce);
-	inStream.Write(mTorque);
-#ifdef JPH_DOUBLE_PRECISION
-	inStream.Write(mSleepTestOffset);
-#endif // JPH_DOUBLE_PRECISION
-	inStream.Write(mSleepTestSpheres);
-	inStream.Write(mSleepTestTimer);
-	inStream.Write(mAllowSleeping);
-}
-
-void MotionProperties::RestoreState(StateRecorder &inStream)
-{
-	inStream.Read(mLinearVelocity);
-	inStream.Read(mAngularVelocity);
-	inStream.Read(mForce);
-	inStream.Read(mTorque);
-#ifdef JPH_DOUBLE_PRECISION
-	inStream.Read(mSleepTestOffset);
-#endif // JPH_DOUBLE_PRECISION
-	inStream.Read(mSleepTestSpheres);
-	inStream.Read(mSleepTestTimer);
-	inStream.Read(mAllowSleeping);
-}
-
-JPH_NAMESPACE_END

package/native/third_party/JoltPhysics/Jolt/Physics/Body/MotionProperties.h

@@ -1,308 +0,0 @@
-// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
-// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
-// SPDX-License-Identifier: MIT
-
-#pragma once
-
-#include <Jolt/Geometry/Sphere.h>
-#include <Jolt/Physics/Body/AllowedDOFs.h>
-#include <Jolt/Physics/Body/MotionQuality.h>
-#include <Jolt/Physics/Body/BodyAccess.h>
-#include <Jolt/Physics/Body/MotionType.h>
-#include <Jolt/Physics/Body/BodyType.h>
-#include <Jolt/Physics/Body/MassProperties.h>
-#include <Jolt/Physics/DeterminismLog.h>
-
-JPH_NAMESPACE_BEGIN
-
-class StateRecorder;
-
-/// Enum that determines if an object can go to sleep
-enum class ECanSleep
-{
-	CannotSleep = 0,																		///< Object cannot go to sleep
-	CanSleep = 1,																			///< Object can go to sleep
-};
-
-/// The Body class only keeps track of state for static bodies, the MotionProperties class keeps the additional state needed for a moving Body. It has a 1-on-1 relationship with the body.
-class JPH_EXPORT MotionProperties
-{
-public:
-	JPH_OVERRIDE_NEW_DELETE
-
-	/// Motion quality, or how well it detects collisions when it has a high velocity
-	EMotionQuality			GetMotionQuality() const										{ return mMotionQuality; }
-
-	/// Get the allowed degrees of freedom that this body has (this can be changed by calling SetMassProperties)
-	inline EAllowedDOFs		GetAllowedDOFs() const											{ return mAllowedDOFs; }
-
-	/// If this body can go to sleep.
-	inline bool				GetAllowSleeping() const										{ return mAllowSleeping; }
-
-	/// Get world space linear velocity of the center of mass
-	inline Vec3				GetLinearVelocity() const										{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::Read)); return mLinearVelocity; }
-
-	/// Set world space linear velocity of the center of mass
-	void					SetLinearVelocity(Vec3Arg inLinearVelocity)						{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); JPH_ASSERT(inLinearVelocity.Length() <= mMaxLinearVelocity); mLinearVelocity = LockTranslation(inLinearVelocity); }
-
-	/// Set world space linear velocity of the center of mass, will make sure the value is clamped against the maximum linear velocity
-	void					SetLinearVelocityClamped(Vec3Arg inLinearVelocity)				{ mLinearVelocity = LockTranslation(inLinearVelocity); ClampLinearVelocity(); }
-
-	/// Get world space angular velocity of the center of mass
-	inline Vec3				GetAngularVelocity() const										{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::Read)); return mAngularVelocity; }
-
-	/// Set world space angular velocity of the center of mass
-	void					SetAngularVelocity(Vec3Arg inAngularVelocity)					{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); JPH_ASSERT(inAngularVelocity.Length() <= mMaxAngularVelocity); mAngularVelocity = LockAngular(inAngularVelocity); }
-
-	/// Set world space angular velocity of the center of mass, will make sure the value is clamped against the maximum angular velocity
-	void					SetAngularVelocityClamped(Vec3Arg inAngularVelocity)			{ mAngularVelocity = LockAngular(inAngularVelocity); ClampAngularVelocity(); }
-
-	/// Set velocity of body such that it will be rotate/translate by inDeltaPosition/Rotation in inDeltaTime seconds.
-	inline void				MoveKinematic(Vec3Arg inDeltaPosition, QuatArg inDeltaRotation, float inDeltaTime);
-
-	///@name Velocity limits
-	///@{
-
-	/// Maximum linear velocity that a body can achieve. Used to prevent the system from exploding.
-	inline float			GetMaxLinearVelocity() const									{ return mMaxLinearVelocity; }
-	inline void				SetMaxLinearVelocity(float inLinearVelocity)					{ JPH_ASSERT(inLinearVelocity >= 0.0f); mMaxLinearVelocity = inLinearVelocity; }
-
-	/// Maximum angular velocity that a body can achieve. Used to prevent the system from exploding.
-	inline float			GetMaxAngularVelocity() const									{ return mMaxAngularVelocity; }
-	inline void				SetMaxAngularVelocity(float inAngularVelocity)					{ JPH_ASSERT(inAngularVelocity >= 0.0f); mMaxAngularVelocity = inAngularVelocity; }
-	///@}
-
-	/// Clamp velocity according to limit
-	inline void				ClampLinearVelocity();
-	inline void				ClampAngularVelocity();
-
-	/// Get linear damping: dv/dt = -c * v. c. Value should be zero or positive and is usually close to 0.
-	inline float			GetLinearDamping() const										{ return mLinearDamping; }
-	void					SetLinearDamping(float inLinearDamping)							{ JPH_ASSERT(inLinearDamping >= 0.0f); mLinearDamping = inLinearDamping; }
-
-	/// Get angular damping: dw/dt = -c * w. c. Value should be zero or positive and is usually close to 0.
-	inline float			GetAngularDamping() const										{ return mAngularDamping; }
-	void					SetAngularDamping(float inAngularDamping)						{ JPH_ASSERT(inAngularDamping >= 0.0f); mAngularDamping = inAngularDamping; }
-
-	/// Get gravity factor (1 = normal gravity, 0 = no gravity)
-	inline float			GetGravityFactor() const										{ return mGravityFactor; }
-	void					SetGravityFactor(float inGravityFactor)							{ mGravityFactor = inGravityFactor; }
-
-	/// Set the mass and inertia tensor
-	void					SetMassProperties(EAllowedDOFs inAllowedDOFs, const MassProperties &inMassProperties);
-
-	/// Get inverse mass (1 / mass). Should only be called on a dynamic object (static or kinematic bodies have infinite mass so should be treated as 1 / mass = 0)
-	inline float			GetInverseMass() const											{ JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); return mInvMass; }
-	inline float			GetInverseMassUnchecked() const									{ return mInvMass; }
-
-	/// Set the inverse mass (1 / mass).
-	/// Note that mass and inertia are linearly related (e.g. inertia of a sphere with mass m and radius r is \f$2/5 \: m \: r^2\f$).
-	/// If you change mass, inertia should probably change as well. You can use ScaleToMass to update mass and inertia at the same time.
-	/// If all your translation degrees of freedom are restricted, make sure this is zero (see EAllowedDOFs).
-	void					SetInverseMass(float inInverseMass)								{ mInvMass = inInverseMass; }
-
-	/// Diagonal of inverse inertia matrix: D. Should only be called on a dynamic object (static or kinematic bodies have infinite mass so should be treated as D = 0)
-	inline Vec3				GetInverseInertiaDiagonal() const								{ JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); return mInvInertiaDiagonal; }
-
-	/// Rotation (R) that takes inverse inertia diagonal to local space: \f$I_{body}^{-1} = R \: D \: R^{-1}\f$
-	inline Quat				GetInertiaRotation() const										{ return mInertiaRotation; }
-
-	/// Set the inverse inertia tensor in local space by setting the diagonal and the rotation: \f$I_{body}^{-1} = R \: D \: R^{-1}\f$.
-	/// Note that mass and inertia are linearly related (e.g. inertia of a sphere with mass m and radius r is \f$2/5 \: m \: r^2\f$).
-	/// If you change inertia, mass should probably change as well. You can use ScaleToMass to update mass and inertia at the same time.
-	/// If all your rotation degrees of freedom are restricted, make sure this is zero (see EAllowedDOFs).
-	void					SetInverseInertia(Vec3Arg inDiagonal, QuatArg inRot)			{ mInvInertiaDiagonal = inDiagonal; mInertiaRotation = inRot; }
-
-	/// Sets the mass to inMass and scale the inertia tensor based on the ratio between the old and new mass.
-	/// Note that this only works when the current mass is finite (i.e. the body is dynamic and translational degrees of freedom are not restricted).
-	void					ScaleToMass(float inMass);
-
-	/// Get inverse inertia matrix (\f$I_{body}^{-1}\f$). Will be a matrix of zeros for a static or kinematic object.
-	inline Mat44			GetLocalSpaceInverseInertia() const;
-
-	/// Same as GetLocalSpaceInverseInertia() but doesn't check if the body is dynamic
-	inline Mat44			GetLocalSpaceInverseInertiaUnchecked() const;
-
-	/// Get inverse inertia matrix (\f$I^{-1}\f$) for a given object rotation (translation will be ignored). Zero if object is static or kinematic.
-	inline Mat44			GetInverseInertiaForRotation(Mat44Arg inRotation) const;
-
-	/// Multiply a vector with the inverse world space inertia tensor (\f$I_{world}^{-1}\f$). Zero if object is static or kinematic.
-	JPH_INLINE Vec3			MultiplyWorldSpaceInverseInertiaByVector(QuatArg inBodyRotation, Vec3Arg inV) const;
-
-	/// Velocity of point inPoint (in center of mass space, e.g. on the surface of the body) of the body (unit: m/s)
-	JPH_INLINE Vec3			GetPointVelocityCOM(Vec3Arg inPointRelativeToCOM) const			{ return mLinearVelocity + mAngularVelocity.Cross(inPointRelativeToCOM); }
-
-	// Get the total amount of force applied to the center of mass this time step (through Body::AddForce calls). Note that it will reset to zero after PhysicsSystem::Update.
-	JPH_INLINE Vec3			GetAccumulatedForce() const										{ return Vec3::sLoadFloat3Unsafe(mForce); }
-
-	// Get the total amount of torque applied to the center of mass this time step (through Body::AddForce/Body::AddTorque calls). Note that it will reset to zero after PhysicsSystem::Update.
-	JPH_INLINE Vec3			GetAccumulatedTorque() const									{ return Vec3::sLoadFloat3Unsafe(mTorque); }
-
-	// Reset the total accumulated force, note that this will be done automatically after every time step.
-	JPH_INLINE void			ResetForce()													{ mForce = Float3(0, 0, 0); }
-
-	// Reset the total accumulated torque, note that this will be done automatically after every time step.
-	JPH_INLINE void			ResetTorque()													{ mTorque = Float3(0, 0, 0); }
-
-	// Reset the current velocity and accumulated force and torque.
-	JPH_INLINE void			ResetMotion()
-	{
-		JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
-		mLinearVelocity = mAngularVelocity = Vec3::sZero();
-		mForce = mTorque = Float3(0, 0, 0);
-	}
-
-	/// Returns a vector where the linear components that are not allowed by mAllowedDOFs are set to 0 and the rest to 0xffffffff
-	JPH_INLINE UVec4		GetLinearDOFsMask() const
-	{
-		UVec4 mask(uint32(EAllowedDOFs::TranslationX), uint32(EAllowedDOFs::TranslationY), uint32(EAllowedDOFs::TranslationZ), 0);
-		return UVec4::sEquals(UVec4::sAnd(UVec4::sReplicate(uint32(mAllowedDOFs)), mask), mask);
-	}
-
-	/// Takes a translation vector inV and returns a vector where the components that are not allowed by mAllowedDOFs are set to 0
-	JPH_INLINE Vec3			LockTranslation(Vec3Arg inV) const
-	{
-		return Vec3::sAnd(inV, Vec3(GetLinearDOFsMask().ReinterpretAsFloat()));
-	}
-
-	/// Returns a vector where the angular components that are not allowed by mAllowedDOFs are set to 0 and the rest to 0xffffffff
-	JPH_INLINE UVec4		GetAngularDOFsMask() const
-	{
-		UVec4 mask(uint32(EAllowedDOFs::RotationX), uint32(EAllowedDOFs::RotationY), uint32(EAllowedDOFs::RotationZ), 0);
-		return UVec4::sEquals(UVec4::sAnd(UVec4::sReplicate(uint32(mAllowedDOFs)), mask), mask);
-	}
-
-	/// Takes an angular velocity / torque vector inV and returns a vector where the components that are not allowed by mAllowedDOFs are set to 0
-	JPH_INLINE Vec3			LockAngular(Vec3Arg inV) const
-	{
-		return Vec3::sAnd(inV, Vec3(GetAngularDOFsMask().ReinterpretAsFloat()));
-	}
-
-	/// Used only when this body is dynamic and colliding. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island.
-	void					SetNumVelocityStepsOverride(uint inN)							{ JPH_ASSERT(inN < 256); mNumVelocityStepsOverride = uint8(inN); }
-	uint					GetNumVelocityStepsOverride() const								{ return mNumVelocityStepsOverride; }
-
-	/// Used only when this body is dynamic and colliding. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island.
-	void					SetNumPositionStepsOverride(uint inN)							{ JPH_ASSERT(inN < 256); mNumPositionStepsOverride = uint8(inN); }
-	uint					GetNumPositionStepsOverride() const								{ return mNumPositionStepsOverride; }
-
-#ifdef JPH_TRACK_SIMULATION_STATS
-	/// Stats for this body. These are average for the simulation island the body was part of.
-	struct SimulationStats
-	{
-		void				Reset()															{ mBroadPhaseTicks = 0; mNarrowPhaseTicks.store(0, memory_order_relaxed); mVelocityConstraintTicks = 0; mPositionConstraintTicks = 0; mUpdateBoundsTicks = 0; mCCDTicks.store(0, memory_order_relaxed); mNumContactConstraints.store(0, memory_order_relaxed); mNumVelocitySteps = 0; mNumPositionSteps = 0; mIsLargeIsland = false; }
-
-		uint64				mBroadPhaseTicks = 0;											///< Number of processor ticks spent doing broad phase collision detection
-		atomic<uint64>		mNarrowPhaseTicks = 0;											///< Number of ticks spent doing narrow phase collision detection
-		uint64				mVelocityConstraintTicks = 0;									///< Number of ticks spent solving velocity constraints
-		uint64				mPositionConstraintTicks = 0;									///< Number of ticks spent solving position constraints
-		uint64				mUpdateBoundsTicks = 0;											///< Number of ticks spent updating the broadphase and checking if the body should go to sleep
-		atomic<uint64>		mCCDTicks = 0;													///< Number of ticks spent doing CCD
-		atomic<uint32>		mNumContactConstraints = 0;										///< Number of contact constraints created for this body
-		uint8				mNumVelocitySteps = 0;											///< Number of velocity iterations performed
-		uint8				mNumPositionSteps = 0;											///< Number of position iterations performed
-		bool				mIsLargeIsland = false;											///< If this body was part of a large island
-	};
-
-	const SimulationStats &	GetSimulationStats() const										{ return mSimulationStats; }
-	SimulationStats &		GetSimulationStats()											{ return mSimulationStats; }
-#endif // JPH_TRACK_SIMULATION_STATS
-
-	////////////////////////////////////////////////////////////
-	// FUNCTIONS BELOW THIS LINE ARE FOR INTERNAL USE ONLY
-	////////////////////////////////////////////////////////////
-
-	///@name Update linear and angular velocity (used during constraint solving)
-	///@{
-	inline void				AddLinearVelocityStep(Vec3Arg inLinearVelocityChange)			{ JPH_DET_LOG("AddLinearVelocityStep: " << inLinearVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); mLinearVelocity = LockTranslation(mLinearVelocity + inLinearVelocityChange); JPH_ASSERT(!mLinearVelocity.IsNaN()); }
-	inline void				SubLinearVelocityStep(Vec3Arg inLinearVelocityChange)			{ JPH_DET_LOG("SubLinearVelocityStep: " << inLinearVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); mLinearVelocity = LockTranslation(mLinearVelocity - inLinearVelocityChange); JPH_ASSERT(!mLinearVelocity.IsNaN()); }
-	inline void				AddAngularVelocityStep(Vec3Arg inAngularVelocityChange)			{ JPH_DET_LOG("AddAngularVelocityStep: " << inAngularVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); mAngularVelocity += inAngularVelocityChange; JPH_ASSERT(!mAngularVelocity.IsNaN()); }
-	inline void				SubAngularVelocityStep(Vec3Arg inAngularVelocityChange)			{ JPH_DET_LOG("SubAngularVelocityStep: " << inAngularVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); mAngularVelocity -= inAngularVelocityChange; JPH_ASSERT(!mAngularVelocity.IsNaN()); }
-	///@}
-
-	/// Apply the gyroscopic force (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem)
-	inline void				ApplyGyroscopicForceInternal(QuatArg inBodyRotation, float inDeltaTime);
-
-	/// Apply all accumulated forces, torques and drag (should only be called by the PhysicsSystem)
-	inline void				ApplyForceTorqueAndDragInternal(QuatArg inBodyRotation, Vec3Arg inGravity, float inDeltaTime);
-
-	/// Access to the island index
-	uint32					GetIslandIndexInternal() const									{ return mIslandIndex; }
-	void					SetIslandIndexInternal(uint32 inIndex)							{ mIslandIndex = inIndex; }
-
-	/// Access to the index in the active bodies array
-	uint32					GetIndexInActiveBodiesInternal() const							{ return mIndexInActiveBodies; }
-
-#ifdef JPH_DOUBLE_PRECISION
-	inline DVec3			GetSleepTestOffset() const										{ return DVec3::sLoadDouble3Unsafe(mSleepTestOffset); }
-#endif // JPH_DOUBLE_PRECISION
-
-	/// Reset spheres to center around inPoints with radius 0
-	inline void				ResetSleepTestSpheres(const RVec3 *inPoints);
-
-	/// Reset the sleep test timer without resetting the sleep test spheres
-	inline void				ResetSleepTestTimer()											{ mSleepTestTimer = 0.0f; }
-
-	/// Accumulate sleep time and return if a body can go to sleep
-	inline ECanSleep		AccumulateSleepTime(float inDeltaTime, float inTimeBeforeSleep);
-
-	/// Saving state for replay
-	void					SaveState(StateRecorder &inStream) const;
-
-	/// Restoring state for replay
-	void					RestoreState(StateRecorder &inStream);
-
-	static constexpr uint32	cInactiveIndex = uint32(-1);									///< Constant indicating that body is not active
-
-private:
-	friend class BodyManager;
-	friend class Body;
-
-	// 1st cache line
-	// 16 byte aligned
-	Vec3					mLinearVelocity { Vec3::sZero() };								///< World space linear velocity of the center of mass (m/s)
-	Vec3					mAngularVelocity { Vec3::sZero() };								///< World space angular velocity (rad/s)
-	Vec3					mInvInertiaDiagonal;											///< Diagonal of inverse inertia matrix: D
-	Quat					mInertiaRotation;												///< Rotation (R) that takes inverse inertia diagonal to local space: Ibody^-1 = R * D * R^-1
-
-	// 2nd cache line
-	// 4 byte aligned
-	Float3					mForce { 0, 0, 0 };												///< Accumulated world space force (N). Note loaded through intrinsics so ensure that the 4 bytes after this are readable!
-	Float3					mTorque { 0, 0, 0 };											///< Accumulated world space torque (N m). Note loaded through intrinsics so ensure that the 4 bytes after this are readable!
-	float					mInvMass;														///< Inverse mass of the object (1/kg)
-	float					mLinearDamping;													///< Linear damping: dv/dt = -c * v. Value should be zero or positive and is usually close to 0.
-	float					mAngularDamping;												///< Angular damping: dw/dt = -c * w. Value should be zero or positive and is usually close to 0.
-	float					mMaxLinearVelocity;												///< Maximum linear velocity that this body can reach (m/s)
-	float					mMaxAngularVelocity;											///< Maximum angular velocity that this body can reach (rad/s)
-	float					mGravityFactor;													///< Factor to multiply gravity with
-	uint32					mIndexInActiveBodies = cInactiveIndex;							///< If the body is active, this is the index in the active body list or cInactiveIndex if it is not active (note that there are 2 lists, one for rigid and one for soft bodies)
-	uint32					mIslandIndex = cInactiveIndex;									///< Index of the island that this body is part of, when the body has not yet been updated or is not active this is cInactiveIndex
-
-	// 1 byte aligned
-	EMotionQuality			mMotionQuality;													///< Motion quality, or how well it detects collisions when it has a high velocity
-	bool					mAllowSleeping;													///< If this body can go to sleep
-	EAllowedDOFs			mAllowedDOFs = EAllowedDOFs::All;								///< Allowed degrees of freedom for this body
-	uint8					mNumVelocityStepsOverride = 0;									///< Used only when this body is dynamic and colliding. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island.
-	uint8					mNumPositionStepsOverride = 0;									///< Used only when this body is dynamic and colliding. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island.
-
-	// 3rd cache line (least frequently used)
-	// 4 byte aligned (or 8 byte if running in double precision)
-#ifdef JPH_DOUBLE_PRECISION
-	Double3					mSleepTestOffset;												///< mSleepTestSpheres are relative to this offset to prevent floating point inaccuracies. Warning: Loaded using sLoadDouble3Unsafe which will read 8 extra bytes.
-#endif // JPH_DOUBLE_PRECISION
-	Sphere					mSleepTestSpheres[3];											///< Measure motion for 3 points on the body to see if it is resting: COM, COM + largest bounding box axis, COM + second largest bounding box axis
-	float					mSleepTestTimer;												///< How long this body has been within the movement tolerance
-
-#ifdef JPH_ENABLE_ASSERTS
-	EBodyType				mCachedBodyType;												///< Copied from Body::mBodyType and cached for asserting purposes
-	EMotionType				mCachedMotionType;												///< Copied from Body::mMotionType and cached for asserting purposes
-#endif
-
-#ifdef JPH_TRACK_SIMULATION_STATS
-	SimulationStats			mSimulationStats;
-#endif // JPH_TRACK_SIMULATION_STATS
-};
-
-JPH_NAMESPACE_END
-
-#include "MotionProperties.inl"

package/native/third_party/JoltPhysics/Jolt/Physics/Body/MotionProperties.inl

@@ -1,178 +0,0 @@
-// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
-// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
-// SPDX-License-Identifier: MIT
-
-#pragma once
-
-JPH_NAMESPACE_BEGIN
-
-void MotionProperties::MoveKinematic(Vec3Arg inDeltaPosition, QuatArg inDeltaRotation, float inDeltaTime)
-{
-	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
-	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read));
-	JPH_ASSERT(mCachedBodyType == EBodyType::RigidBody);
-	JPH_ASSERT(mCachedMotionType != EMotionType::Static);
-
-	// Calculate required linear velocity
-	mLinearVelocity = LockTranslation(inDeltaPosition / inDeltaTime);
-
-	// Calculate required angular velocity
-	Vec3 axis;
-	float angle;
-	inDeltaRotation.GetAxisAngle(axis, angle);
-	mAngularVelocity = LockAngular(axis * (angle / inDeltaTime));
-}
-
-void MotionProperties::ClampLinearVelocity()
-{
-	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
-
-	float len_sq = mLinearVelocity.LengthSq();
-	JPH_ASSERT(isfinite(len_sq));
-	if (len_sq > Square(mMaxLinearVelocity))
-		mLinearVelocity *= mMaxLinearVelocity / sqrt(len_sq);
-}
-
-void MotionProperties::ClampAngularVelocity()
-{
-	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
-
-	float len_sq = mAngularVelocity.LengthSq();
-	JPH_ASSERT(isfinite(len_sq));
-	if (len_sq > Square(mMaxAngularVelocity))
-		mAngularVelocity *= mMaxAngularVelocity / sqrt(len_sq);
-}
-
-inline Mat44 MotionProperties::GetLocalSpaceInverseInertiaUnchecked() const
-{
-	Mat44 rotation = Mat44::sRotation(mInertiaRotation);
-	Mat44 rotation_mul_scale_transposed(mInvInertiaDiagonal.SplatX() * rotation.GetColumn4(0), mInvInertiaDiagonal.SplatY() * rotation.GetColumn4(1), mInvInertiaDiagonal.SplatZ() * rotation.GetColumn4(2), Vec4(0, 0, 0, 1));
-	return rotation.Multiply3x3RightTransposed(rotation_mul_scale_transposed);
-}
-
-inline void MotionProperties::ScaleToMass(float inMass)
-{
-	JPH_ASSERT(mInvMass > 0.0f, "Body must have finite mass");
-	JPH_ASSERT(inMass > 0.0f, "New mass cannot be zero");
-
-	float new_inv_mass = 1.0f / inMass;
-	mInvInertiaDiagonal *= new_inv_mass / mInvMass;
-	mInvMass = new_inv_mass;
-}
-
-inline Mat44 MotionProperties::GetLocalSpaceInverseInertia() const
-{
-	JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
-	return GetLocalSpaceInverseInertiaUnchecked();
-}
-
-Mat44 MotionProperties::GetInverseInertiaForRotation(Mat44Arg inRotation) const
-{
-	JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
-
-	Mat44 rotation = inRotation.Multiply3x3(Mat44::sRotation(mInertiaRotation));
-	Mat44 rotation_mul_scale_transposed(mInvInertiaDiagonal.SplatX() * rotation.GetColumn4(0), mInvInertiaDiagonal.SplatY() * rotation.GetColumn4(1), mInvInertiaDiagonal.SplatZ() * rotation.GetColumn4(2), Vec4(0, 0, 0, 1));
-	Mat44 inverse_inertia = rotation.Multiply3x3RightTransposed(rotation_mul_scale_transposed);
-
-	// We need to mask out both the rows and columns of DOFs that are not allowed
-	Vec4 angular_dofs_mask = GetAngularDOFsMask().ReinterpretAsFloat();
-	inverse_inertia.SetColumn4(0, Vec4::sAnd(inverse_inertia.GetColumn4(0), Vec4::sAnd(angular_dofs_mask, angular_dofs_mask.SplatX())));
-	inverse_inertia.SetColumn4(1, Vec4::sAnd(inverse_inertia.GetColumn4(1), Vec4::sAnd(angular_dofs_mask, angular_dofs_mask.SplatY())));
-	inverse_inertia.SetColumn4(2, Vec4::sAnd(inverse_inertia.GetColumn4(2), Vec4::sAnd(angular_dofs_mask, angular_dofs_mask.SplatZ())));
-
-	return inverse_inertia;
-}
-
-Vec3 MotionProperties::MultiplyWorldSpaceInverseInertiaByVector(QuatArg inBodyRotation, Vec3Arg inV) const
-{
-	JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
-
-	// Mask out columns of DOFs that are not allowed
-	Vec3 angular_dofs_mask = Vec3(GetAngularDOFsMask().ReinterpretAsFloat());
-	Vec3 v = Vec3::sAnd(inV, angular_dofs_mask);
-
-	// Multiply vector by inverse inertia
-	Mat44 rotation = Mat44::sRotation(inBodyRotation * mInertiaRotation);
-	Vec3 result = rotation.Multiply3x3(mInvInertiaDiagonal * rotation.Multiply3x3Transposed(v));
-
-	// Mask out rows of DOFs that are not allowed
-	return Vec3::sAnd(result, angular_dofs_mask);
-}
-
-void MotionProperties::ApplyGyroscopicForceInternal(QuatArg inBodyRotation, float inDeltaTime)
-{
-	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
-	JPH_ASSERT(mCachedBodyType == EBodyType::RigidBody);
-	JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
-
-	// Calculate local space inertia tensor (a diagonal in local space)
-	UVec4 is_zero = Vec3::sEquals(mInvInertiaDiagonal, Vec3::sZero());
-	Vec3 denominator = Vec3::sSelect(mInvInertiaDiagonal, Vec3::sOne(), is_zero);
-	Vec3 nominator = Vec3::sSelect(Vec3::sOne(), Vec3::sZero(), is_zero);
-	Vec3 local_inertia = nominator / denominator; // Avoid dividing by zero, inertia in this axis will be zero
-
-	// Calculate local space angular momentum
-	Quat inertia_space_to_world_space = inBodyRotation * mInertiaRotation;
-	Vec3 local_angular_velocity = inertia_space_to_world_space.InverseRotate(mAngularVelocity);
-	Vec3 local_momentum = local_inertia * local_angular_velocity;
-
-	// The gyroscopic force applies a torque: T = -w x I w where w is angular velocity and I the inertia tensor
-	// Calculate the new angular momentum by applying the gyroscopic force and make sure the new magnitude is the same as the old one
-	// to avoid introducing energy into the system due to the Euler step
-	Vec3 new_local_momentum = local_momentum - inDeltaTime * local_angular_velocity.Cross(local_momentum);
-	float new_local_momentum_len_sq = new_local_momentum.LengthSq();
-	new_local_momentum = new_local_momentum_len_sq > 0.0f? new_local_momentum * sqrt(local_momentum.LengthSq() / new_local_momentum_len_sq) : Vec3::sZero();
-
-	// Convert back to world space angular velocity
-	mAngularVelocity = inertia_space_to_world_space * (mInvInertiaDiagonal * new_local_momentum);
-}
-
-void MotionProperties::ApplyForceTorqueAndDragInternal(QuatArg inBodyRotation, Vec3Arg inGravity, float inDeltaTime)
-{
-	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
-	JPH_ASSERT(mCachedBodyType == EBodyType::RigidBody);
-	JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
-
-	// Update linear velocity
-	mLinearVelocity = LockTranslation(mLinearVelocity + inDeltaTime * (mGravityFactor * inGravity + mInvMass * GetAccumulatedForce()));
-
-	// Update angular velocity
-	mAngularVelocity += inDeltaTime * MultiplyWorldSpaceInverseInertiaByVector(inBodyRotation, GetAccumulatedTorque());
-
-	// Linear damping: dv/dt = -c * v
-	// Solution: v(t) = v(0) * e^(-c * t) or v2 = v1 * e^(-c * dt)
-	// Taylor expansion of e^(-c * dt) = 1 - c * dt + ...
-	// Since dt is usually in the order of 1/60 and c is a low number too this approximation is good enough
-	mLinearVelocity *= max(0.0f, 1.0f - mLinearDamping * inDeltaTime);
-	mAngularVelocity *= max(0.0f, 1.0f - mAngularDamping * inDeltaTime);
-
-	// Clamp velocities
-	ClampLinearVelocity();
-	ClampAngularVelocity();
-}
-
-void MotionProperties::ResetSleepTestSpheres(const RVec3 *inPoints)
-{
-#ifdef JPH_DOUBLE_PRECISION
-	// Make spheres relative to the first point and initialize them to zero radius
-	DVec3 offset = inPoints[0];
-	offset.StoreDouble3(&mSleepTestOffset);
-	mSleepTestSpheres[0] = Sphere(Vec3::sZero(), 0.0f);
-	for (int i = 1; i < 3; ++i)
-		mSleepTestSpheres[i] = Sphere(Vec3(inPoints[i] - offset), 0.0f);
-#else
-	// Initialize the spheres to zero radius around the supplied points
-	for (int i = 0; i < 3; ++i)
-		mSleepTestSpheres[i] = Sphere(inPoints[i], 0.0f);
-#endif
-
-	mSleepTestTimer = 0.0f;
-}
-
-ECanSleep MotionProperties::AccumulateSleepTime(float inDeltaTime, float inTimeBeforeSleep)
-{
-	mSleepTestTimer += inDeltaTime;
-	return mSleepTestTimer >= inTimeBeforeSleep? ECanSleep::CanSleep : ECanSleep::CannotSleep;
-}
-
-JPH_NAMESPACE_END