@bloomengine/engine 0.3.1

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

@@ -0,0 +1,58 @@
+// 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

@@ -0,0 +1,92 @@
+// 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

@@ -0,0 +1,308 @@
+// 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

@@ -0,0 +1,178 @@
+// 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