GetSleepTestPoints was doing 2x quaternion * vector, cheaper is to convert to matrix once (#32)

Jolt/Physics/Body/Body.cpp

@@ -144,35 +144,6 @@ void Body::SetShapeInternal(const Shape *inShape, bool inUpdateMassProperties)
 	CalculateWorldSpaceBoundsInternal();
 }
 
-void Body::GetSleepTestPoints(Vec3 *outPoints) const
-{
-	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); 
-
-	// Center of mass is the first position
-	outPoints[0] = mPosition;
-
-	// The second and third position are on the largest axis of the bounding box
-	Vec3 extent = mShape->GetLocalBounds().GetExtent();
-	int lowest_component = extent.GetLowestComponentIndex();
-	int component = 0;
-	for (int output = 1; output < 3; ++output)
-	{
-		if (component == lowest_component)
-			++component;
-		Vec3 axis = Vec3::sZero();
-		axis.SetComponent(component, extent[component]);
-		outPoints[output] = mPosition + mRotation * axis;
-		component++;
-	}
-}
-
-void Body::ResetSleepTestSpheres()
-{
-	Vec3 points[3];
-	GetSleepTestPoints(points);
-	mMotionProperties->ResetSleepTestSpheres(points);
-}
-
 Body::ECanSleep Body::UpdateSleepStateInternal(float inDeltaTime, float inMaxMovement, float inTimeBeforeSleep)
 {
 	// Check override
@@ -185,11 +156,13 @@ Body::ECanSleep Body::UpdateSleepStateInternal(float inDeltaTime, float inMaxMov
 
 	for (int i = 0; i < 3; ++i)
 	{
+		Sphere &sphere = mMotionProperties->mSleepTestSpheres[i];
+
 		// Encapsulate the point in a sphere
-		mMotionProperties->mSleepTestSpheres[i].EncapsulatePoint(points[i]);
+		sphere.EncapsulatePoint(points[i]);
 
 		// Test if it exceeded the max movement
-		if (mMotionProperties->mSleepTestSpheres[i].GetRadius() > inMaxMovement)
+		if (sphere.GetRadius() > inMaxMovement)
 		{
 			// Body is not sleeping, reset test
 			mMotionProperties->ResetSleepTestSpheres(points);

Jolt/Physics/Body/Body.h

@@ -262,8 +262,8 @@ private:
 
 	explicit				Body(bool);														///< Alternative constructor that initializes all members
 
-	void					GetSleepTestPoints(Vec3 *outPoints) const;						///< Determine points to test for checking if body is sleeping: COM, COM + largest bounding box axis, COM + second largest bounding box axis
-	void					ResetSleepTestSpheres();										///< Reset spheres to current position as returned by GetSleepTestPoints
+	inline void				GetSleepTestPoints(Vec3 *outPoints) const;						///< Determine points to test for checking if body is sleeping: COM, COM + largest bounding box axis, COM + second largest bounding box axis
+	inline void				ResetSleepTestSpheres();										///< Reset spheres to current position as returned by GetSleepTestPoints
 
 	enum class EFlags : uint8
 	{

Jolt/Physics/Body/Body.inl

@@ -137,4 +137,45 @@ void Body::AddAngularImpulse(Vec3Arg inAngularImpulse)
 	SetAngularVelocityClamped(mMotionProperties->GetAngularVelocity() + GetInverseInertia() * inAngularImpulse);
 }
 
+void Body::GetSleepTestPoints(Vec3 *outPoints) const
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); 
+
+	// Center of mass is the first position
+	outPoints[0] = mPosition;
+
+	// The second and third position are on the largest axis of the bounding box
+	Vec3 extent = mShape->GetLocalBounds().GetExtent();
+	int lowest_component = extent.GetLowestComponentIndex();
+	Mat44 rotation = Mat44::sRotation(mRotation);
+	switch (lowest_component)
+	{
+	case 0:
+		outPoints[1] = mPosition + extent.GetY() * rotation.GetColumn3(1);
+		outPoints[2] = mPosition + extent.GetZ() * rotation.GetColumn3(2);
+		break;
+
+	case 1:
+		outPoints[1] = mPosition + extent.GetX() * rotation.GetColumn3(0);
+		outPoints[2] = mPosition + extent.GetZ() * rotation.GetColumn3(2);
+		break;
+
+	case 2:
+		outPoints[1] = mPosition + extent.GetX() * rotation.GetColumn3(0);
+		outPoints[2] = mPosition + extent.GetY() * rotation.GetColumn3(1);
+		break;
+
+	default:
+		JPH_ASSERT(false);
+		break;
+	}
+}
+
+void Body::ResetSleepTestSpheres()
+{
+	Vec3 points[3];
+	GetSleepTestPoints(points);
+	mMotionProperties->ResetSleepTestSpheres(points);
+}
+
 } // JPH

Jolt/Physics/Body/MotionProperties.cpp

@@ -8,13 +8,6 @@
 
 namespace JPH {
 
-void MotionProperties::ResetSleepTestSpheres(const Vec3 *inPoints)
-{
-	for (int i = 0; i < 3; ++i)
-		mSleepTestSpheres[i] = Sphere(inPoints[i], 0.0f);
-	mSleepTestTimer = 0.0f;
-}
-
 void MotionProperties::SaveState(StateRecorder &inStream) const
 {
 	// Only write properties that can change at runtime

Jolt/Physics/Body/MotionProperties.h

@@ -121,7 +121,7 @@ public:
 	uint32					GetIndexInActiveBodiesInternal() const							{ return mIndexInActiveBodies; }
 
 	/// Reset spheres to center around inPoints with radius 0
-	void					ResetSleepTestSpheres(const Vec3 *inPoints);
+	inline void				ResetSleepTestSpheres(const Vec3 *inPoints);
 
 	/// Saving state for replay
 	void					SaveState(StateRecorder &inStream) const;

Jolt/Physics/Body/MotionProperties.inl

@@ -113,4 +113,11 @@ void MotionProperties::ApplyForceTorqueAndDragInternal(QuatArg inBodyRotation, V
 	ClampAngularVelocity();
 }
 
+void MotionProperties::ResetSleepTestSpheres(const Vec3 *inPoints)
+{
+	for (int i = 0; i < 3; ++i)
+		mSleepTestSpheres[i] = Sphere(inPoints[i], 0.0f);
+	mSleepTestTimer = 0.0f;
+}
+
 } // JPH