npm - @bloomengine/engine - Versions diffs - 0.3.1 - Mend

@bloomengine/engine 0.3.1

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.

Files changed (562) hide show

package/native/third_party/JoltPhysics/Jolt/Physics/Body/BodyManager.h ADDED Viewed

@@ -0,0 +1,403 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+#pragma once
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Core/Mutex.h>
+#include <Jolt/Core/MutexArray.h>
+JPH_NAMESPACE_BEGIN
+// Classes
+class BodyCreationSettings;
+class SoftBodyCreationSettings;
+class BodyActivationListener;
+class StateRecorderFilter;
+struct PhysicsSettings;
+#ifdef JPH_DEBUG_RENDERER
+class DebugRenderer;
+class BodyDrawFilter;
+#endif // JPH_DEBUG_RENDERER
+#ifdef JPH_DEBUG_RENDERER
+/// Defines how to color soft body constraints
+enum class ESoftBodyConstraintColor
+{
+	ConstraintType,				/// Draw different types of constraints in different colors
+	ConstraintGroup,			/// Draw constraints in the same group in the same color, non-parallel group will be red
+	ConstraintOrder,			/// Draw constraints in the same group in the same color, non-parallel group will be red, and order within each group will be indicated with gradient
+};
+#endif // JPH_DEBUG_RENDERER
+/// Array of bodies
+using BodyVector = Array<Body *>;
+/// Array of body ID's
+using BodyIDVector = Array<BodyID>;
+/// Class that contains all bodies
+///
+/// WARNING: This class is an internal part of PhysicsSystem, it has no functions that can be called by users of the library.
+/// Its functionality is exposed through PhysicsSystem, BodyInterface, BodyLockRead and BodyLockWrite.
+class JPH_EXPORT BodyManager : public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+	/// Destructor
+									~BodyManager();
+	/// Initialize the manager
+	void							Init(uint inMaxBodies, uint inNumBodyMutexes, const BroadPhaseLayerInterface &inLayerInterface);
+	/// Gets the current amount of bodies that are in the body manager
+	uint							GetNumBodies() const;
+	/// Gets the max bodies that we can support
+	uint							GetMaxBodies() const						{ return uint(mBodies.capacity()); }
+	/// Helper struct that counts the number of bodies of each type
+	struct BodyStats
+	{
+		uint						mNumBodies					= 0;			///< Total number of bodies in the body manager
+		uint						mMaxBodies					= 0;			///< Max allowed number of bodies in the body manager (as configured in Init(...))
+		uint						mNumBodiesStatic			= 0;			///< Number of static bodies
+		uint						mNumBodiesDynamic			= 0;			///< Number of dynamic bodies
+		uint						mNumActiveBodiesDynamic		= 0;			///< Number of dynamic bodies that are currently active
+		uint						mNumBodiesKinematic			= 0;			///< Number of kinematic bodies
+		uint						mNumActiveBodiesKinematic	= 0;			///< Number of kinematic bodies that are currently active
+		uint						mNumSoftBodies				= 0;			///< Number of soft bodies
+		uint						mNumActiveSoftBodies		= 0;			///< Number of soft bodies that are currently active
+	};
+	/// Get stats about the bodies in the body manager (slow, iterates through all bodies)
+	BodyStats						GetBodyStats() const;
+	/// Create a body using creation settings. The returned body will not be part of the body manager yet.
+	Body *							AllocateBody(const BodyCreationSettings &inBodyCreationSettings) const;
+	/// Create a soft body using creation settings. The returned body will not be part of the body manager yet.
+	Body *							AllocateSoftBody(const SoftBodyCreationSettings &inSoftBodyCreationSettings) const;
+	/// Free a body that has not been added to the body manager yet (if it has, use DestroyBodies).
+	void							FreeBody(Body *inBody) const;
+	/// Add a body to the body manager, assigning it the next available ID. Returns false if no more IDs are available.
+	bool							AddBody(Body *ioBody);
+	/// Add a body to the body manager, assigning it a custom ID. Returns false if the ID is not valid.
+	bool							AddBodyWithCustomID(Body *ioBody, const BodyID &inBodyID);
+	/// Remove a list of bodies from the body manager
+	void							RemoveBodies(const BodyID *inBodyIDs, int inNumber, Body **outBodies);
+	/// Remove a set of bodies from the body manager and frees them.
+	void							DestroyBodies(const BodyID *inBodyIDs, int inNumber);
+	/// Activate a list of bodies.
+	/// This function should only be called when an exclusive lock for the bodies are held.
+	void							ActivateBodies(const BodyID *inBodyIDs, int inNumber);
+	/// Deactivate a list of bodies.
+	/// This function should only be called when an exclusive lock for the bodies are held.
+	void							DeactivateBodies(const BodyID *inBodyIDs, int inNumber);
+	/// Update the motion quality for a body
+	void							SetMotionQuality(Body &ioBody, EMotionQuality inMotionQuality);
+	/// Get copy of the list of active bodies under protection of a lock.
+	void							GetActiveBodies(EBodyType inType, BodyIDVector &outBodyIDs) const;
+	/// Get the list of active bodies. Note: Not thread safe. The active bodies list can change at any moment.
+	const BodyID *					GetActiveBodiesUnsafe(EBodyType inType) const { return mActiveBodies[int(inType)]; }
+	/// Get the number of active bodies.
+	uint32							GetNumActiveBodies(EBodyType inType) const	{ return mNumActiveBodies[int(inType)].load(memory_order_acquire); }
+	/// Get the number of active bodies that are using continuous collision detection
+	uint32							GetNumActiveCCDBodies() const				{ return mNumActiveCCDBodies; }
+	/// Listener that is notified whenever a body is activated/deactivated
+	void							SetBodyActivationListener(BodyActivationListener *inListener);
+	BodyActivationListener *		GetBodyActivationListener() const			{ return mActivationListener; }
+	/// Check if this is a valid body pointer. When a body is freed the memory that the pointer occupies is reused to store a freelist.
+	static inline bool				sIsValidBodyPointer(const Body *inBody)		{ return (uintptr_t(inBody) & cIsFreedBody) == 0; }
+	/// Get all bodies. Note that this can contain invalid body pointers, call sIsValidBodyPointer to check.
+	const BodyVector &				GetBodies() const							{ return mBodies; }
+	/// Get all bodies. Note that this can contain invalid body pointers, call sIsValidBodyPointer to check.
+	BodyVector &					GetBodies()									{ return mBodies; }
+	/// Get all body IDs under the protection of a lock
+	void							GetBodyIDs(BodyIDVector &outBodies) const;
+	/// Access a body (not protected by lock)
+	const Body &					GetBody(const BodyID &inID) const			{ return *mBodies[inID.GetIndex()]; }
+	/// Access a body (not protected by lock)
+	Body &							GetBody(const BodyID &inID)					{ return *mBodies[inID.GetIndex()]; }
+	/// Access a body, will return a nullptr if the body ID is no longer valid (not protected by lock)
+	const Body *					TryGetBody(const BodyID &inID) const
+	{
+		uint32 idx = inID.GetIndex();
+		if (idx >= mBodies.size())
+			return nullptr;
+		const Body *body = mBodies[idx];
+		if (sIsValidBodyPointer(body) && body->GetID() == inID)
+			return body;
+		return nullptr;
+	}
+	/// Access a body, will return a nullptr if the body ID is no longer valid (not protected by lock)
+	Body *							TryGetBody(const BodyID &inID)
+	{
+		uint32 idx = inID.GetIndex();
+		if (idx >= mBodies.size())
+			return nullptr;
+		Body *body = mBodies[idx];
+		if (sIsValidBodyPointer(body) && body->GetID() == inID)
+			return body;
+		return nullptr;
+	}
+	/// Access the mutex for a single body
+	SharedMutex &					GetMutexForBody(const BodyID &inID) const	{ return mBodyMutexes.GetMutexByObjectIndex(inID.GetIndex()); }
+	/// Bodies are protected using an array of mutexes (so a fixed number, not 1 per body). Each bit in this mask indicates a locked mutex.
+	using MutexMask = uint64;
+	///@name Batch body mutex access (do not use directly)
+	///@{
+	MutexMask						GetAllBodiesMutexMask() const				{ return mBodyMutexes.GetNumMutexes() == sizeof(MutexMask) * 8? ~MutexMask(0) : (MutexMask(1) << mBodyMutexes.GetNumMutexes()) - 1; }
+	MutexMask						GetMutexMask(const BodyID *inBodies, int inNumber) const;
+	void							LockRead(MutexMask inMutexMask) const;
+	void							UnlockRead(MutexMask inMutexMask) const;
+	void							LockWrite(MutexMask inMutexMask) const;
+	void							UnlockWrite(MutexMask inMutexMask) const;
+	///@}
+	/// Lock all bodies. This should only be done during PhysicsSystem::Update().
+	void							LockAllBodies() const;
+	/// Unlock all bodies. This should only be done during PhysicsSystem::Update().
+	void							UnlockAllBodies() const;
+	/// Function to update body's layer (should only be called by the BodyInterface since it also requires updating the broadphase)
+	inline void						SetBodyObjectLayerInternal(Body &ioBody, ObjectLayer inLayer) const { ioBody.mObjectLayer = inLayer; ioBody.mBroadPhaseLayer = mBroadPhaseLayerInterface->GetBroadPhaseLayer(inLayer); }
+	/// Set the Body::EFlags::InvalidateContactCache flag for the specified body. This means that the collision cache is invalid for any body pair involving that body until the next physics step.
+	void							InvalidateContactCacheForBody(Body &ioBody);
+	/// Reset the Body::EFlags::InvalidateContactCache flag for all bodies. All contact pairs in the contact cache will now by valid again.
+	void							ValidateContactCacheForAllBodies();
+	/// Saving state for replay
+	void							SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const;
+	/// Restoring state for replay. Returns false if failed.
+	bool							RestoreState(StateRecorder &inStream);
+	/// Save the state of a single body for replay
+	void							SaveBodyState(const Body &inBody, StateRecorder &inStream) const;
+	/// Save the state of a single body for replay
+	void							RestoreBodyState(Body &inBody, StateRecorder &inStream);
+#ifdef JPH_DEBUG_RENDERER
+	enum class EShapeColor
+	{
+		InstanceColor,				///< Random color per instance
+		ShapeTypeColor,				///< Convex = green, scaled = yellow, compound = orange, mesh = red
+		MotionTypeColor,			///< Static = grey, keyframed = green, dynamic = random color per instance
+		SleepColor,					///< Static = grey, keyframed = green, dynamic = yellow, sleeping = red
+		IslandColor,				///< Static = grey, active = random color per island, sleeping = light grey
+		MaterialColor,				///< Color as defined by the PhysicsMaterial of the shape
+	};
+	/// Draw settings
+	///
+	/// Note that there are several debug drawing features that are not exposed through this interface since they use information
+	/// that is only available deep inside the simulation update and are mostly there to facilitate debugging Jolt. These options
+	/// use DebugRenderer::sInstance to draw.
+	///
+	/// E.g.:
+	/// * To draw contact information, use ContactConstraintManager::sDrawContactManifolds.
+	/// * To draw when continuous collision detection is used, use PhysicsSystem::sDrawMotionQualityLinearCast.
+	/// * To draw what's going on in a CharacterVirtual update, use CharacterVirtual::sDrawConstraints, CharacterVirtual::sDrawWalkStairs and CharacterVirtual::sDrawStickToFloor.
+	/// * To draw the volume of water that interacts with a shape, use Shape::sDrawSubmergedVolumes.
+	struct DrawSettings
+	{
+		bool						mDrawGetSupportFunction = false;				///< Draw the GetSupport() function, used for convex collision detection
+		bool						mDrawSupportDirection = false;					///< When drawing the support function, also draw which direction mapped to a specific support point
+		bool						mDrawGetSupportingFace = false;					///< Draw the faces that were found colliding during collision detection
+		bool						mDrawShape = true;								///< Draw the shapes of all bodies
+		bool						mDrawShapeWireframe = false;					///< When mDrawShape is true and this is true, the shapes will be drawn in wireframe instead of solid.
+		EShapeColor					mDrawShapeColor = EShapeColor::MotionTypeColor; ///< Coloring scheme to use for shapes
+		bool						mDrawBoundingBox = false;						///< Draw a bounding box per body
+		bool						mDrawCenterOfMassTransform = false;				///< Draw the center of mass for each body
+		bool						mDrawWorldTransform = false;					///< Draw the world transform (which can be different than the center of mass) for each body
+		bool						mDrawVelocity = false;							///< Draw the velocity vector for each body
+		bool						mDrawMassAndInertia = false;					///< Draw the mass and inertia (as the box equivalent) for each body
+		bool						mDrawSleepStats = false;						///< Draw stats regarding the sleeping algorithm of each body
+		bool						mDrawSoftBodyVertices = false;					///< Draw the vertices of soft bodies
+		bool						mDrawSoftBodyVertexVelocities = false;			///< Draw the velocities of the vertices of soft bodies
+		bool						mDrawSoftBodyEdgeConstraints = false;			///< Draw the edge constraints of soft bodies
+		bool						mDrawSoftBodyBendConstraints = false;			///< Draw the bend constraints of soft bodies
+		bool						mDrawSoftBodyVolumeConstraints = false;			///< Draw the volume constraints of soft bodies
+		bool						mDrawSoftBodySkinConstraints = false;			///< Draw the skin constraints of soft bodies
+		bool						mDrawSoftBodyLRAConstraints = false;			///< Draw the LRA constraints of soft bodies
+		bool						mDrawSoftBodyRods = false;						///< Draw the rods of soft bodies
+		bool						mDrawSoftBodyRodStates = false;					///< Draw the rod states (orientation and angular velocity) of soft bodies
+		bool						mDrawSoftBodyRodBendTwistConstraints = false;	///< Draw the rod bend twist constraints of soft bodies
+		bool						mDrawSoftBodyPredictedBounds = false;			///< Draw the predicted bounds of soft bodies
+		ESoftBodyConstraintColor	mDrawSoftBodyConstraintColor = ESoftBodyConstraintColor::ConstraintType; ///< Coloring scheme to use for soft body constraints
+	};
+	/// Draw the state of the bodies (debugging purposes)
+	void							Draw(const DrawSettings &inSettings, const PhysicsSettings &inPhysicsSettings, DebugRenderer *inRenderer, const BodyDrawFilter *inBodyFilter = nullptr);
+#endif // JPH_DEBUG_RENDERER
+#ifdef JPH_ENABLE_ASSERTS
+	/// Lock the active body list, asserts when Activate/DeactivateBody is called.
+	void							SetActiveBodiesLocked(bool inLocked)		{ mActiveBodiesLocked = inLocked; }
+	/// Per thread override of the locked state, to be used by the PhysicsSystem only!
+	class GrantActiveBodiesAccess
+	{
+	public:
+		inline GrantActiveBodiesAccess(bool inAllowActivation, bool inAllowDeactivation)
+		{
+			JPH_ASSERT(!sGetOverrideAllowActivation());
+			sSetOverrideAllowActivation(inAllowActivation);
+			JPH_ASSERT(!sGetOverrideAllowDeactivation());
+			sSetOverrideAllowDeactivation(inAllowDeactivation);
+		}
+		inline ~GrantActiveBodiesAccess()
+		{
+			sSetOverrideAllowActivation(false);
+			sSetOverrideAllowDeactivation(false);
+		}
+	};
+#endif
+#ifdef JPH_DEBUG
+	/// Validate if the cached bounding boxes are correct for all active bodies
+	void							ValidateActiveBodyBounds();
+#endif // JPH_DEBUG
+#ifdef JPH_TRACK_SIMULATION_STATS
+	/// Resets the per body simulation stats
+	void							ResetSimulationStats();
+#ifdef JPH_PROFILE_ENABLED
+	/// Dump the per body simulation stats to the TTY
+	void							ReportSimulationStats();
+#endif
+#endif
+private:
+	/// Increment and get the sequence number of the body
+#ifdef JPH_COMPILER_CLANG
+	__attribute__((no_sanitize("implicit-conversion"))) // We intentionally overflow the uint8 sequence number
+#endif
+	inline uint8					GetNextSequenceNumber(int inBodyIndex)		{ return ++mBodySequenceNumbers[inBodyIndex]; }
+	/// Add a single body to mActiveBodies, note doesn't lock the active body mutex!
+	inline void						AddBodyToActiveBodies(Body &ioBody);
+	/// Remove a single body from mActiveBodies, note doesn't lock the active body mutex!
+	inline void						RemoveBodyFromActiveBodies(Body &ioBody);
+	/// Helper function to remove a body from the manager
+	JPH_INLINE Body *				RemoveBodyInternal(const BodyID &inBodyID);
+	/// Helper function to delete a body (which could actually be a BodyWithMotionProperties)
+	inline static void				sDeleteBody(Body *inBody);
+#if defined(JPH_DEBUG) && defined(JPH_ENABLE_ASSERTS)
+	/// Function to check that the free list is not corrupted
+	void							ValidateFreeList() const;
+#endif // defined(JPH_DEBUG) && _defined(JPH_ENABLE_ASSERTS)
+	/// List of pointers to all bodies. Contains invalid pointers for deleted bodies, check with sIsValidBodyPointer. Note that this array is reserved to the max bodies that is passed in the Init function so that adding bodies will not reallocate the array.
+	BodyVector						mBodies;
+	/// Current number of allocated bodies
+	uint							mNumBodies = 0;
+	/// Indicates that there are no more freed body IDs
+	static constexpr uintptr_t		cBodyIDFreeListEnd = ~uintptr_t(0);
+	/// Bit that indicates a pointer in mBodies is actually the index of the next freed body. We use the lowest bit because we know that Bodies need to be 16 byte aligned so addresses can never end in a 1 bit.
+	static constexpr uintptr_t		cIsFreedBody = uintptr_t(1);
+	/// Amount of bits to shift to get an index to the next freed body
+	static constexpr uint			cFreedBodyIndexShift = 1;
+	/// Index of first entry in mBodies that is unused
+	uintptr_t						mBodyIDFreeListStart = cBodyIDFreeListEnd;
+	/// Protects mBodies array (but not the bodies it points to), mNumBodies and mBodyIDFreeListStart
+	mutable Mutex					mBodiesMutex;
+	/// An array of mutexes protecting the bodies in the mBodies array
+	using BodyMutexes = MutexArray<SharedMutex>;
+	mutable BodyMutexes				mBodyMutexes;
+	/// List of next sequence number for a body ID
+	Array<uint8>					mBodySequenceNumbers;
+	/// Mutex that protects the mActiveBodies array
+	mutable Mutex					mActiveBodiesMutex;
+	/// List of all active dynamic bodies (size is equal to max amount of bodies)
+	BodyID *						mActiveBodies[cBodyTypeCount] = { };
+	/// How many bodies there are in the list of active bodies
+	atomic<uint32>					mNumActiveBodies[cBodyTypeCount] = { };
+	/// How many of the active bodies have continuous collision detection enabled
+	uint32							mNumActiveCCDBodies = 0;
+	/// Mutex that protects the mBodiesCacheInvalid array
+	mutable Mutex					mBodiesCacheInvalidMutex;
+	/// List of all bodies that should have their cache invalidated
+	BodyIDVector					mBodiesCacheInvalid;
+	/// Listener that is notified whenever a body is activated/deactivated
+	BodyActivationListener *		mActivationListener = nullptr;
+	/// Cached broadphase layer interface
+	const BroadPhaseLayerInterface *mBroadPhaseLayerInterface = nullptr;
+#ifdef JPH_ENABLE_ASSERTS
+	static bool						sGetOverrideAllowActivation();
+	static void						sSetOverrideAllowActivation(bool inValue);
+	static bool						sGetOverrideAllowDeactivation();
+	static void						sSetOverrideAllowDeactivation(bool inValue);
+	/// Debug system that tries to limit changes to active bodies during the PhysicsSystem::Update()
+	bool							mActiveBodiesLocked = false;
+#endif
+};
+JPH_NAMESPACE_END

package/native/third_party/JoltPhysics/Jolt/Physics/Body/BodyPair.h ADDED Viewed

@@ -0,0 +1,36 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+#pragma once
+#include <Jolt/Physics/Body/BodyID.h>
+#include <Jolt/Core/HashCombine.h>
+JPH_NAMESPACE_BEGIN
+/// Structure that holds a body pair
+struct alignas(uint64) BodyPair
+{
+	JPH_OVERRIDE_NEW_DELETE
+	/// Constructor
+							BodyPair() = default;
+							BodyPair(BodyID inA, BodyID inB)							: mBodyA(inA), mBodyB(inB) { }
+	/// Equals operator
+	bool					operator == (const BodyPair &inRHS) const					{ return *reinterpret_cast<const uint64 *>(this) == *reinterpret_cast<const uint64 *>(&inRHS); }
+	/// Smaller than operator, used for consistently ordering body pairs
+	bool					operator < (const BodyPair &inRHS) const					{ return *reinterpret_cast<const uint64 *>(this) < *reinterpret_cast<const uint64 *>(&inRHS); }
+	/// Get the hash value of this object
+	uint64					GetHash() const												{ return Hash64(*reinterpret_cast<const uint64 *>(this)); }
+	BodyID					mBodyA;
+	BodyID					mBodyB;
+};
+static_assert(sizeof(BodyPair) == sizeof(uint64), "Mismatch in class size");
+JPH_NAMESPACE_END

package/native/third_party/JoltPhysics/Jolt/Physics/Body/BodyType.h ADDED Viewed

@@ -0,0 +1,19 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+#pragma once
+JPH_NAMESPACE_BEGIN
+/// Type of body
+enum class EBodyType : uint8
+{
+	RigidBody,				///< Rigid body consisting of a rigid shape
+	SoftBody,				///< Soft body consisting of a deformable shape
+};
+/// How many types of bodies there are
+static constexpr uint cBodyTypeCount = 2;
+JPH_NAMESPACE_END

package/native/third_party/JoltPhysics/Jolt/Physics/Body/MassProperties.cpp ADDED Viewed

@@ -0,0 +1,185 @@
+// 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/MassProperties.h>
+#include <Jolt/Math/Matrix.h>
+#include <Jolt/Math/Vector.h>
+#include <Jolt/Math/EigenValueSymmetric.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/Core/InsertionSort.h>
+JPH_NAMESPACE_BEGIN
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(MassProperties)
+{
+	JPH_ADD_ATTRIBUTE(MassProperties, mMass)
+	JPH_ADD_ATTRIBUTE(MassProperties, mInertia)
+}
+bool MassProperties::DecomposePrincipalMomentsOfInertia(Mat44 &outRotation, Vec3 &outDiagonal) const
+{
+	// Using eigendecomposition to get the principal components of the inertia tensor
+	// See: https://en.wikipedia.org/wiki/Eigendecomposition_of_a_matrix
+	Matrix<3, 3> inertia;
+	inertia.CopyPart(mInertia, 0, 0, 3, 3, 0, 0);
+	Matrix<3, 3> eigen_vec = Matrix<3, 3>::sIdentity();
+	Vector<3> eigen_val;
+	if (!EigenValueSymmetric(inertia, eigen_vec, eigen_val))
+		return false;
+	// Sort so that the biggest value goes first
+	int indices[] = { 0, 1, 2 };
+	InsertionSort(indices, indices + 3, [&eigen_val](int inLeft, int inRight) { return eigen_val[inLeft] > eigen_val[inRight]; });
+	// Convert to a regular Mat44 and Vec3
+	outRotation = Mat44::sIdentity();
+	for (int i = 0; i < 3; ++i)
+	{
+		outRotation.SetColumn3(i, Vec3(reinterpret_cast<Float3 &>(eigen_vec.GetColumn(indices[i]))));
+		outDiagonal.SetComponent(i, eigen_val[indices[i]]);
+	}
+	// Make sure that the rotation matrix is a right handed matrix
+	if (outRotation.GetAxisX().Cross(outRotation.GetAxisY()).Dot(outRotation.GetAxisZ()) < 0.0f)
+		outRotation.SetAxisZ(-outRotation.GetAxisZ());
+#ifdef JPH_ENABLE_ASSERTS
+	// Validate that the solution is correct, for each axis we want to make sure that the difference in inertia is
+	// smaller than some fraction of the inertia itself in that axis
+	Mat44 new_inertia = outRotation * Mat44::sScale(outDiagonal) * outRotation.Inversed();
+	for (int i = 0; i < 3; ++i)
+		JPH_ASSERT(new_inertia.GetColumn3(i).IsClose(mInertia.GetColumn3(i), mInertia.GetColumn3(i).LengthSq() * 1.0e-10f));
+#endif
+	return true;
+}
+void MassProperties::SetMassAndInertiaOfSolidBox(Vec3Arg inBoxSize, float inDensity)
+{
+	// Calculate mass
+	mMass = inBoxSize.GetX() * inBoxSize.GetY() * inBoxSize.GetZ() * inDensity;
+	// Calculate inertia
+	Vec3 size_sq = inBoxSize * inBoxSize;
+	Vec3 scale = (size_sq.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_X>() + size_sq.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Y>()) * (mMass / 12.0f);
+	mInertia = Mat44::sScale(scale);
+}
+void MassProperties::ScaleToMass(float inMass)
+{
+	if (mMass > 0.0f)
+	{
+		// Calculate how much we have to scale the inertia tensor
+		float mass_scale = inMass / mMass;
+		// Update mass
+		mMass = inMass;
+		// Update inertia tensor
+		for (int i = 0; i < 3; ++i)
+			mInertia.SetColumn4(i, mInertia.GetColumn4(i) * mass_scale);
+	}
+	else
+	{
+		// Just set the mass
+		mMass = inMass;
+	}
+}
+Vec3 MassProperties::sGetEquivalentSolidBoxSize(float inMass, Vec3Arg inInertiaDiagonal)
+{
+	// Moment of inertia of a solid box has diagonal:
+	// mass / 12 * [size_y^2 + size_z^2, size_x^2 + size_z^2, size_x^2 + size_y^2]
+	// Solving for size_x, size_y and size_y (diagonal and mass are known):
+	Vec3 diagonal = inInertiaDiagonal * (12.0f / inMass);
+	return Vec3(sqrt(0.5f * (-diagonal[0] + diagonal[1] + diagonal[2])), sqrt(0.5f * (diagonal[0] - diagonal[1] + diagonal[2])), sqrt(0.5f * (diagonal[0] + diagonal[1] - diagonal[2])));
+}
+void MassProperties::Scale(Vec3Arg inScale)
+{
+	// See: https://en.wikipedia.org/wiki/Moment_of_inertia#Inertia_tensor
+	// The diagonal of the inertia tensor can be calculated like this:
+	// Ixx = sum_{k = 1 to n}(m_k * (y_k^2 + z_k^2))
+	// Iyy = sum_{k = 1 to n}(m_k * (x_k^2 + z_k^2))
+	// Izz = sum_{k = 1 to n}(m_k * (x_k^2 + y_k^2))
+	//
+	// We want to isolate the terms x_k, y_k and z_k:
+	// d = [0.5, 0.5, 0.5].[Ixx, Iyy, Izz]
+	// [sum_{k = 1 to n}(m_k * x_k^2), sum_{k = 1 to n}(m_k * y_k^2), sum_{k = 1 to n}(m_k * z_k^2)] = [d, d, d] - [Ixx, Iyy, Izz]
+	Vec3 diagonal = mInertia.GetDiagonal3();
+	Vec3 xyz_sq = Vec3::sReplicate(Vec3::sReplicate(0.5f).Dot(diagonal)) - diagonal;
+	// When scaling a shape these terms change like this:
+	// sum_{k = 1 to n}(m_k * (scale_x * x_k)^2) = scale_x^2 * sum_{k = 1 to n}(m_k * x_k^2)
+	// Same for y_k and z_k
+	// Using these terms we can calculate the new diagonal of the inertia tensor:
+	Vec3 xyz_scaled_sq = inScale * inScale * xyz_sq;
+	float i_xx = xyz_scaled_sq.GetY() + xyz_scaled_sq.GetZ();
+	float i_yy = xyz_scaled_sq.GetX() + xyz_scaled_sq.GetZ();
+	float i_zz = xyz_scaled_sq.GetX() + xyz_scaled_sq.GetY();
+	// The off diagonal elements are calculated like:
+	// Ixy = -sum_{k = 1 to n}(x_k y_k)
+	// Ixz = -sum_{k = 1 to n}(x_k z_k)
+	// Iyz = -sum_{k = 1 to n}(y_k z_k)
+	// Scaling these is simple:
+	float i_xy = inScale.GetX() * inScale.GetY() * mInertia(0, 1);
+	float i_xz = inScale.GetX() * inScale.GetZ() * mInertia(0, 2);
+	float i_yz = inScale.GetY() * inScale.GetZ() * mInertia(1, 2);
+	// Update inertia tensor
+	mInertia(0, 0) = i_xx;
+	mInertia(0, 1) = i_xy;
+	mInertia(1, 0) = i_xy;
+	mInertia(1, 1) = i_yy;
+	mInertia(0, 2) = i_xz;
+	mInertia(2, 0) = i_xz;
+	mInertia(1, 2) = i_yz;
+	mInertia(2, 1) = i_yz;
+	mInertia(2, 2) = i_zz;
+	// Mass scales linear with volume (note that the scaling can be negative and we don't want the mass to become negative)
+	float mass_scale = abs(inScale.GetX() * inScale.GetY() * inScale.GetZ());
+	mMass *= mass_scale;
+	// Inertia scales linear with mass. This updates the m_k terms above.
+	mInertia *= mass_scale;
+	// Ensure that the bottom right element is a 1 again
+	mInertia(3, 3) = 1.0f;
+}
+void MassProperties::Rotate(Mat44Arg inRotation)
+{
+	mInertia = inRotation.Multiply3x3(mInertia).Multiply3x3RightTransposed(inRotation);
+}
+void MassProperties::Translate(Vec3Arg inTranslation)
+{
+	// Transform the inertia using the parallel axis theorem: I' = I + m * (translation^2 E - translation translation^T)
+	// Where I is the original body's inertia and E the identity matrix
+	// See: https://en.wikipedia.org/wiki/Parallel_axis_theorem
+	mInertia += mMass * (Mat44::sScale(inTranslation.Dot(inTranslation)) - Mat44::sOuterProduct(inTranslation, inTranslation));
+	// Ensure that inertia is a 3x3 matrix, adding inertias causes the bottom right element to change
+	mInertia.SetColumn4(3, Vec4(0, 0, 0, 1));
+}
+void MassProperties::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mMass);
+	inStream.Write(mInertia);
+}
+void MassProperties::RestoreBinaryState(StreamIn &inStream)
+{
+	inStream.Read(mMass);
+	inStream.Read(mInertia);
+}
+JPH_NAMESPACE_END