Combine Body and MotionProperties in 1 allocation (#35)

Jolt/Physics/Body/Body.h

@@ -34,7 +34,7 @@ public:
 							Body() = default;
 
 	/// Destructor							
-							~Body()															{ delete mMotionProperties; }
+							~Body()															{ JPH_ASSERT(mMotionProperties == nullptr); }
 
 #ifdef _DEBUG
 	/// Name of the body for debugging purposes

Jolt/Physics/Body/BodyManager.cpp

@@ -21,6 +21,24 @@ namespace JPH {
 	thread_local bool BodyManager::sOverrideAllowDeactivation = false;
 #endif
 
+// Helper class that combines a body and its motion properties
+class BodyWithMotionProperties : public Body
+{
+public:
+	MotionProperties		mMotionProperties;
+};
+
+inline void BodyManager::sDeleteBody(Body *inBody)
+{
+	if (inBody->mMotionProperties != nullptr)
+	{
+		JPH_IF_ENABLE_ASSERTS(inBody->mMotionProperties = nullptr;)
+		delete static_cast<BodyWithMotionProperties *>(inBody);
+	}
+	else
+		delete inBody;
+}
+
 BodyManager::~BodyManager()
 {
 	UniqueLock lock(mBodiesMutex, EPhysicsLockTypes::BodiesList);
@@ -28,7 +46,7 @@ BodyManager::~BodyManager()
 	// Destroy any bodies that are still alive
 	for (Body *b : mBodies)
 		if (sIsValidBodyPointer(b))
-			delete b;
+			sDeleteBody(b);
 
 	delete [] mActiveBodies;
 }
@@ -133,7 +151,17 @@ Body *BodyManager::CreateBody(const BodyCreationSettings &inBodyCreationSettings
 	uint8 seq_no = GetNextSequenceNumber(idx);
 
 	// Fill in basic properties
-	Body *body = new Body();
+	Body *body;
+	if (inBodyCreationSettings.HasMassProperties())
+	{
+		BodyWithMotionProperties *bmp = new BodyWithMotionProperties;
+		body = bmp;
+		body->mMotionProperties = &bmp->mMotionProperties;
+	}
+	else
+	{
+	 	body = new Body;
+	}
 	body->mID = BodyID(idx, seq_no);
 	body->mShape = inBodyCreationSettings.GetShape();
 	body->SetFriction(inBodyCreationSettings.mFriction);
@@ -148,7 +176,7 @@ Body *BodyManager::CreateBody(const BodyCreationSettings &inBodyCreationSettings
 	{
 		JPH_ASSERT(!inBodyCreationSettings.mIsSensor, "Sensors should be static and moved through BodyInterface::SetPosition/SetPositionAndRotation");
 
-		MotionProperties *mp = new MotionProperties();
+		MotionProperties *mp = body->mMotionProperties;
 		mp->SetLinearDamping(inBodyCreationSettings.mLinearDamping);
 		mp->SetAngularDamping(inBodyCreationSettings.mAngularDamping);
 		mp->SetMaxLinearVelocity(inBodyCreationSettings.mMaxLinearVelocity);
@@ -160,8 +188,6 @@ Body *BodyManager::CreateBody(const BodyCreationSettings &inBodyCreationSettings
 		mp->mIslandIndex = Body::cInactiveIndex;
 		JPH_IF_ENABLE_ASSERTS(mp->mCachedMotionType = body->mMotionType;)
 		mp->SetMassProperties(inBodyCreationSettings.GetMassProperties());
-
-		body->mMotionProperties = mp;
 	}
 
 	// Position body
@@ -201,7 +227,7 @@ void BodyManager::DestroyBodies(const BodyID *inBodyIDs, int inNumber)
 		mBodyIDFreeListStart = (uintptr_t(idx) << cFreedBodyIndexShift) | cIsFreedBody;
 
 		// Free the body
-		delete body;
+		sDeleteBody(body);
 	}
 
 #if defined(_DEBUG) && defined(JPH_ENABLE_ASSERTS)

Jolt/Physics/Body/BodyManager.h

@@ -217,6 +217,9 @@ private:
 #endif
 	inline uint8					GetNextSequenceNumber(int inBodyIndex)		{ return ++mBodySequenceNumbers[inBodyIndex]; }
 
+	/// Helper function to delete a body (which could actually be a BodyWithMotionProperties)
+	inline static void				sDeleteBody(Body *inBody);
+
 	/// 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;
 

Jolt/Physics/Body/MotionProperties.h

@@ -138,15 +138,15 @@ private:
 
 	// 1st cache line
 	// 16 byte aligned
-	Vec3					mLinearVelocity;												///< World space linear velocity of the center of mass (m/s)
-	Vec3					mAngularVelocity;												///< World space angular velocity (rad/s)
+	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;															///< Accumulated world space force (N). Note loaded through intrinsics so ensure that the 4 bytes after this are readable!
-	Float3					mTorque;														///< Accumulated world space torque (N m). Note loaded through intrinsics so ensure that the 4 bytes after this are readable!
+	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. c must be between 0 and 1 but is usually close to 0.
 	float					mAngularDamping;												///< Angular damping: dw/dt = -c * w. c must be between 0 and 1 but is usually close to 0.