ConvexVsMesh test runs 5% faster by reducing the amount of branches in the inner loop of the contact solver

Jolt/Physics/Body/Body.h

@@ -172,10 +172,12 @@ public:
 	inline const AABox &	GetWorldSpaceBounds() const										{ return mBounds; }
 
 	/// Access to the motion properties
-	MotionProperties *		GetMotionProperties() const										{ JPH_ASSERT(!IsStatic()); return mMotionProperties; }
+	const MotionProperties *GetMotionProperties() const										{ JPH_ASSERT(!IsStatic()); return mMotionProperties; }
+	MotionProperties *		GetMotionProperties()											{ JPH_ASSERT(!IsStatic()); return mMotionProperties; }
 
 	/// Access to the motion properties (version that does not check if the object is kinematic or dynamic)
-	MotionProperties *		GetMotionPropertiesUnchecked() const							{ return mMotionProperties; }
+	const MotionProperties *GetMotionPropertiesUnchecked() const							{ return mMotionProperties; }
+	MotionProperties *		GetMotionPropertiesUnchecked()									{ return mMotionProperties; }
 
 	/// Access to the user data pointer
 	void *					GetUserData() const												{ return mUserData; }

Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h

@@ -40,7 +40,8 @@ namespace JPH {
 class AxisConstraintPart
 {
 	/// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated
-	JPH_INLINE bool				ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inLambda)
+	template <EMotionType Type1, EMotionType Type2>
+	JPH_INLINE bool				ApplyVelocityStep(MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, Vec3Arg inWorldSpaceAxis, float inLambda) const
 	{
 		// Apply impulse if delta is not zero
 		if (inLambda != 0.0f)
@@ -52,17 +53,15 @@ class AxisConstraintPart
 			//
 			// Euler velocity integration: 
 			// v' = v + M^-1 P
-			if (ioBody1.IsDynamic())
+			if constexpr (Type1 == EMotionType::Dynamic)
 			{
-				MotionProperties *mp1 = ioBody1.GetMotionProperties();
-				mp1->SubLinearVelocityStep((inLambda * mp1->GetInverseMass()) * inWorldSpaceAxis);
-				mp1->SubAngularVelocityStep(inLambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis));
+				ioMotionProperties1->SubLinearVelocityStep((inLambda * ioMotionProperties1->GetInverseMass()) * inWorldSpaceAxis);
+				ioMotionProperties1->SubAngularVelocityStep(inLambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis));
 			}
-			if (ioBody2.IsDynamic())
+			if constexpr (Type2 == EMotionType::Dynamic)
 			{
-				MotionProperties *mp2 = ioBody2.GetMotionProperties();
-				mp2->AddLinearVelocityStep((inLambda * mp2->GetInverseMass()) * inWorldSpaceAxis);
-				mp2->AddAngularVelocityStep(inLambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis));
+				ioMotionProperties2->AddLinearVelocityStep((inLambda * ioMotionProperties2->GetInverseMass()) * inWorldSpaceAxis);
+				ioMotionProperties2->AddAngularVelocityStep(inLambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis));
 			}
 			return true;
 		}
@@ -137,6 +136,15 @@ public:
 		return mEffectiveMass != 0.0f;
 	}
 
+	/// Templated form of WarmStart with the motion types baked in
+	template <EMotionType Type1, EMotionType Type2>
+	inline void					TemplatedWarmStart(MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, Vec3Arg inWorldSpaceAxis, float inWarmStartImpulseRatio)
+	{
+		mTotalLambda *= inWarmStartImpulseRatio;
+
+		ApplyVelocityStep<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, inWorldSpaceAxis, mTotalLambda);
+	}
+
 	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
 	/// @param ioBody1 The first body that this constraint is attached to
 	/// @param ioBody2 The second body that this constraint is attached to
@@ -144,8 +152,57 @@ public:
 	/// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame
 	inline void					WarmStart(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inWarmStartImpulseRatio)
 	{
-		mTotalLambda *= inWarmStartImpulseRatio;
-		ApplyVelocityStep(ioBody1, ioBody2, inWorldSpaceAxis, mTotalLambda);
+		EMotionType motion_type1 = ioBody1.GetMotionType();
+		MotionProperties *motion_properties1 = ioBody1.GetMotionPropertiesUnchecked();
+
+		EMotionType motion_type2 = ioBody2.GetMotionType();
+		MotionProperties *motion_properties2 = ioBody2.GetMotionPropertiesUnchecked();
+
+		// To reduce the amount of ifs we do a high level switch and then go to specialized code paths based on which configuration we hit
+		if (motion_type1 == EMotionType::Dynamic)
+		{
+			if (motion_type2 == EMotionType::Dynamic)
+				TemplatedWarmStart<EMotionType::Dynamic, EMotionType::Dynamic>(motion_properties1, motion_properties2, inWorldSpaceAxis, inWarmStartImpulseRatio);
+			else
+				TemplatedWarmStart<EMotionType::Dynamic, EMotionType::Static>(motion_properties1, motion_properties2, inWorldSpaceAxis, inWarmStartImpulseRatio);
+		}
+		else
+		{
+			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
+			TemplatedWarmStart<EMotionType::Static, EMotionType::Dynamic>(motion_properties1, motion_properties2, inWorldSpaceAxis, inWarmStartImpulseRatio);
+		}
+	}
+
+	/// Templated form of SolveVelocityConstraint with the motion types baked in
+	template <EMotionType Type1, EMotionType Type2>
+	inline bool					TemplatedSolveVelocityConstraint(MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda)
+	{
+		// Calculate jacobian multiplied by linear velocity
+		float jv;
+		if constexpr (Type1 != EMotionType::Static && Type2 != EMotionType::Static)
+			jv = inWorldSpaceAxis.Dot(ioMotionProperties1->GetLinearVelocity() - ioMotionProperties2->GetLinearVelocity());
+		else if constexpr (Type1 != EMotionType::Static)
+			jv = inWorldSpaceAxis.Dot(ioMotionProperties1->GetLinearVelocity());
+		else if constexpr (Type2 != EMotionType::Static)
+			jv = inWorldSpaceAxis.Dot(-ioMotionProperties2->GetLinearVelocity());
+		else
+			JPH_ASSERT(false); // Static vs static is nonsensical!
+
+		// Calculate jacobian multiplied by angular velocity
+		if constexpr (Type1 != EMotionType::Static)
+			jv += Vec3::sLoadFloat3Unsafe(mR1PlusUxAxis).Dot(ioMotionProperties1->GetAngularVelocity());
+		if constexpr (Type2 != EMotionType::Static)
+			jv -= Vec3::sLoadFloat3Unsafe(mR2xAxis).Dot(ioMotionProperties2->GetAngularVelocity());
+
+		// Lagrange multiplier is:
+		//
+		// lambda = -K^-1 (J v + b)
+		float lambda = mEffectiveMass * (jv - mSpringPart.GetBias(mTotalLambda));
+		float new_lambda = Clamp(mTotalLambda + lambda, inMinLambda, inMaxLambda); // Clamp impulse
+		lambda = new_lambda - mTotalLambda; // Lambda potentially got clamped, calculate the new impulse to apply
+		mTotalLambda = new_lambda; // Store accumulated impulse
+
+		return ApplyVelocityStep<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, inWorldSpaceAxis, lambda);
 	}
 
 	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
@@ -156,15 +213,47 @@ public:
 	/// @param inMaxLambda Maximum value of constraint impulse to apply (N s)
 	inline bool					SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda)
 	{
-		// Lagrange multiplier is:
-		//
-		// lambda = -K^-1 (J v + b)
-		float lambda = mEffectiveMass * (inWorldSpaceAxis.Dot(ioBody1.GetLinearVelocity() - ioBody2.GetLinearVelocity()) + Vec3::sLoadFloat3Unsafe(mR1PlusUxAxis).Dot(ioBody1.GetAngularVelocity()) - Vec3::sLoadFloat3Unsafe(mR2xAxis).Dot(ioBody2.GetAngularVelocity()) - mSpringPart.GetBias(mTotalLambda));
-		float new_lambda = Clamp(mTotalLambda + lambda, inMinLambda, inMaxLambda); // Clamp impulse
-		lambda = new_lambda - mTotalLambda; // Lambda potentially got clamped, calculate the new impulse to apply
-		mTotalLambda = new_lambda; // Store accumulated impulse
+		EMotionType motion_type1 = ioBody1.GetMotionType();
+		MotionProperties *motion_properties1 = ioBody1.GetMotionPropertiesUnchecked();
+
+		EMotionType motion_type2 = ioBody2.GetMotionType();
+		MotionProperties *motion_properties2 = ioBody2.GetMotionPropertiesUnchecked();
 
-		return ApplyVelocityStep(ioBody1, ioBody2, inWorldSpaceAxis, lambda);
+		// To reduce the amount of ifs we do a high level switch and then go to specialized code paths based on which configuration we hit
+		switch (motion_type1)
+		{
+		case EMotionType::Dynamic:
+			switch (motion_type2)
+			{
+			case EMotionType::Dynamic:
+				return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Dynamic>(motion_properties1, motion_properties2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+			case EMotionType::Kinematic:
+				return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Kinematic>(motion_properties1, motion_properties2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+			case EMotionType::Static:
+				return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Static>(motion_properties1, motion_properties2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+			default:
+				JPH_ASSERT(false);
+				break;
+			}
+			break;
+
+		case EMotionType::Kinematic:
+			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
+			return TemplatedSolveVelocityConstraint<EMotionType::Kinematic, EMotionType::Dynamic>(motion_properties1, motion_properties2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+		case EMotionType::Static:
+			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
+			return TemplatedSolveVelocityConstraint<EMotionType::Static, EMotionType::Dynamic>(motion_properties1, motion_properties2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+		default:
+			JPH_ASSERT(false);
+			break;
+		}
+
+		return false;
 	}
 
 	/// Iteratively update the position constraint. Makes sure C(...) = 0.
@@ -202,12 +291,12 @@ public:
 			// integrate + a position integrate and then discard the velocity change.
 			if (ioBody1.IsDynamic())
 			{
-				ioBody1.SubPositionStep((lambda * ioBody1.GetMotionProperties()->GetInverseMass()) * inWorldSpaceAxis);
+				ioBody1.SubPositionStep((lambda * ioBody1.GetMotionPropertiesUnchecked()->GetInverseMass()) * inWorldSpaceAxis);
 				ioBody1.SubRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis));
 			}
 			if (ioBody2.IsDynamic())
 			{
-				ioBody2.AddPositionStep((lambda * ioBody2.GetMotionProperties()->GetInverseMass()) * inWorldSpaceAxis);
+				ioBody2.AddPositionStep((lambda * ioBody2.GetMotionPropertiesUnchecked()->GetInverseMass()) * inWorldSpaceAxis);
 				ioBody2.AddRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis));
 			}
 			return true;

Jolt/Physics/Constraints/ContactConstraintManager.cpp

@@ -1182,6 +1182,22 @@ void ContactConstraintManager::SetupVelocityConstraints(uint32 *inConstraintIdxB
 	}
 }
 
+template <EMotionType Type1, EMotionType Type2>
+JPH_INLINE void ContactConstraintManager::sWarmStartConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, Vec3Arg inTangent1, Vec3Arg inTangent2, float inWarmStartImpulseRatio)
+{
+	for (WorldContactPoint &wcp : ioConstraint.mContactPoints)
+	{
+		// Warm starting: Apply impulse from last frame
+		if (wcp.mFrictionConstraint1.IsActive())
+		{
+			JPH_ASSERT(wcp.mFrictionConstraint2.IsActive());
+			wcp.mFrictionConstraint1.TemplatedWarmStart<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, inTangent1, inWarmStartImpulseRatio);
+			wcp.mFrictionConstraint2.TemplatedWarmStart<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, inTangent2, inWarmStartImpulseRatio);
+		}
+		wcp.mNonPenetrationConstraint.TemplatedWarmStart<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, ioConstraint.mWorldSpaceNormal, inWarmStartImpulseRatio);
+	}
+}
+
 void ContactConstraintManager::WarmStartVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio)
 {
 	JPH_PROFILE_FUNCTION();
@@ -1192,24 +1208,69 @@ void ContactConstraintManager::WarmStartVelocityConstraints(const uint32 *inCons
 
 		// Fetch bodies
 		Body &body1 = *constraint.mBody1;
+		EMotionType motion_type1 = body1.GetMotionType();
+		MotionProperties *motion_properties1 = body1.GetMotionPropertiesUnchecked();
+
 		Body &body2 = *constraint.mBody2;
+		EMotionType motion_type2 = body2.GetMotionType();
+		MotionProperties *motion_properties2 = body2.GetMotionPropertiesUnchecked();
 				
 		// Calculate tangents
 		Vec3 t1, t2;
 		constraint.GetTangents(t1, t2);
 		
-		for (WorldContactPoint &wcp : constraint.mContactPoints)
+		// To reduce the amount of ifs we do a high level switch and then go to specialized code paths based on which configuration we hit
+		if (motion_type1 == EMotionType::Dynamic)
 		{
-			// Warm starting: Apply impulse from last frame
-			if (wcp.mFrictionConstraint1.IsActive())
-			{
-				JPH_ASSERT(wcp.mFrictionConstraint2.IsActive());
-				wcp.mFrictionConstraint1.WarmStart(body1, body2, t1, inWarmStartImpulseRatio);
-				wcp.mFrictionConstraint2.WarmStart(body1, body2, t2, inWarmStartImpulseRatio);
-			}
-			wcp.mNonPenetrationConstraint.WarmStart(body1, body2, constraint.mWorldSpaceNormal, inWarmStartImpulseRatio);
+			if (motion_type2 == EMotionType::Dynamic)
+				sWarmStartConstraint<EMotionType::Dynamic, EMotionType::Dynamic>(constraint, motion_properties1, motion_properties2, t1, t2, inWarmStartImpulseRatio);
+			else
+				sWarmStartConstraint<EMotionType::Dynamic, EMotionType::Static>(constraint, motion_properties1, motion_properties2, t1, t2, inWarmStartImpulseRatio);
+		}
+		else
+		{
+			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
+			sWarmStartConstraint<EMotionType::Static, EMotionType::Dynamic>(constraint, motion_properties1, motion_properties2, t1, t2, inWarmStartImpulseRatio);
+		}
+	}
+}
+
+template <EMotionType Type1, EMotionType Type2>
+JPH_INLINE bool ContactConstraintManager::sSolveVelocityConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, Vec3Arg inTangent1, Vec3Arg inTangent2)
+{
+	bool any_impulse_applied = false;
+
+	// First apply all friction constraints (non-penetration is more important than friction)
+	for (WorldContactPoint &wcp : ioConstraint.mContactPoints)
+	{
+		// Check if friction is enabled
+		if (wcp.mFrictionConstraint1.IsActive())
+		{
+			JPH_ASSERT(wcp.mFrictionConstraint2.IsActive());
+
+			// Calculate max impulse that can be applied. Note that we're using the non-penetration impulse from the previous iteration here.
+			// We do this because non-penetration is more important so is solved last (the last things that are solved in an iterative solver
+			// contribute the most).
+			float max_lambda_f = ioConstraint.mSettings.mCombinedFriction * wcp.mNonPenetrationConstraint.GetTotalLambda();
+
+			// Solve friction velocities
+			// Note that what we're doing is not fully correct since the max force we can apply is 2 * max_lambda_f instead of max_lambda_f since we're solving axis independently
+			if (wcp.mFrictionConstraint1.TemplatedSolveVelocityConstraint<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, inTangent1, -max_lambda_f, max_lambda_f))
+				any_impulse_applied = true;
+			if (wcp.mFrictionConstraint2.TemplatedSolveVelocityConstraint<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, inTangent2, -max_lambda_f, max_lambda_f))
+				any_impulse_applied = true;
 		}
 	}
+
+	// Then apply all non-penetration constraints
+	for (WorldContactPoint &wcp : ioConstraint.mContactPoints)
+	{
+		// Solve non penetration velocities
+		if (wcp.mNonPenetrationConstraint.TemplatedSolveVelocityConstraint<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, ioConstraint.mWorldSpaceNormal, 0.0f, FLT_MAX))
+			any_impulse_applied = true;
+	}
+
+	return any_impulse_applied;
 }
 
 bool ContactConstraintManager::SolveVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd)
@@ -1224,40 +1285,54 @@ bool ContactConstraintManager::SolveVelocityConstraints(const uint32 *inConstrai
 
 		// Fetch bodies
 		Body &body1 = *constraint.mBody1;
+		EMotionType motion_type1 = body1.GetMotionType();
+		MotionProperties *motion_properties1 = body1.GetMotionPropertiesUnchecked();
+
 		Body &body2 = *constraint.mBody2;
+		EMotionType motion_type2 = body2.GetMotionType();
+		MotionProperties *motion_properties2 = body2.GetMotionPropertiesUnchecked();
 
 		// Calculate tangents
 		Vec3 t1, t2;
 		constraint.GetTangents(t1, t2);
 
-		// First apply all friction constraints (non-penetration is more important than friction)
-		for (WorldContactPoint &wcp : constraint.mContactPoints)
+		// To reduce the amount of ifs we do a high level switch and then go to specialized code paths based on which configuration we hit
+		switch (motion_type1)
 		{
-			// Check if friction is enabled
-			if (wcp.mFrictionConstraint1.IsActive())
+		case EMotionType::Dynamic:
+			switch (motion_type2)
 			{
-				JPH_ASSERT(wcp.mFrictionConstraint2.IsActive());
+			case EMotionType::Dynamic:
+				any_impulse_applied |= sSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Dynamic>(constraint, motion_properties1, motion_properties2, t1, t2);
+				break;
 
-				// Calculate max impulse that can be applied. Note that we're using the non-penetration impulse from the previous iteration here.
-				// We do this because non-penetration is more important so is solved last (the last things that are solved in an iterative solver
-				// contribute the most).
-				float max_lambda_f = constraint.mSettings.mCombinedFriction * wcp.mNonPenetrationConstraint.GetTotalLambda();
+			case EMotionType::Kinematic:
+				any_impulse_applied |= sSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Kinematic>(constraint, motion_properties1, motion_properties2, t1, t2);
+				break;
 
-				// Solve friction velocities
-				// Note that what we're doing is not fully correct since the max force we can apply is 2 * max_lambda_f instead of max_lambda_f since we're solving axis independently
-				if (wcp.mFrictionConstraint1.SolveVelocityConstraint(body1, body2, t1, -max_lambda_f, max_lambda_f))
-					any_impulse_applied = true;
-				if (wcp.mFrictionConstraint2.SolveVelocityConstraint(body1, body2, t2, -max_lambda_f, max_lambda_f))
-					any_impulse_applied = true;
+			case EMotionType::Static:
+				any_impulse_applied |= sSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Static>(constraint, motion_properties1, motion_properties2, t1, t2);
+				break;
+
+			default:
+				JPH_ASSERT(false);
+				break;
 			}
-		}
+			break;
 
-		// Then apply all non-penetration constraints
-		for (WorldContactPoint &wcp : constraint.mContactPoints)
-		{
-			// Solve non penetration velocities
-			if (wcp.mNonPenetrationConstraint.SolveVelocityConstraint(body1, body2, constraint.mWorldSpaceNormal, 0.0f, FLT_MAX))
-				any_impulse_applied = true;
+		case EMotionType::Kinematic:
+			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
+			any_impulse_applied |= sSolveVelocityConstraint<EMotionType::Kinematic, EMotionType::Dynamic>(constraint, motion_properties1, motion_properties2, t1, t2);
+			break;
+
+		case EMotionType::Static:
+			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
+			any_impulse_applied |= sSolveVelocityConstraint<EMotionType::Static, EMotionType::Dynamic>(constraint, motion_properties1, motion_properties2, t1, t2);
+			break;
+
+		default:
+			JPH_ASSERT(false);
+			break;
 		}
 	}
 

Jolt/Physics/Constraints/ContactConstraintManager.h

@@ -435,6 +435,14 @@ private:
 		WorldContactPoints		mContactPoints;
 	};
 
+	/// Internal helper function to warm start contact constraint. Templated to the motion type to reduce the amount of branches.
+	template <EMotionType Type1, EMotionType Type2>
+	JPH_INLINE static void		sWarmStartConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, Vec3Arg inTangent1, Vec3Arg inTangent2, float inWarmStartImpulseRatio);
+
+	/// Internal helper function to solve a single contact constraint. Templated to the motion type to reduce the amount of branches.
+	template <EMotionType Type1, EMotionType Type2>
+	JPH_INLINE static bool		sSolveVelocityConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, Vec3Arg inTangent1, Vec3Arg inTangent2);
+
 	/// The main physics settings instance
 	const PhysicsSettings &		mPhysicsSettings;
 

Jolt/Physics/PhysicsSystem.cpp

@@ -698,7 +698,7 @@ void PhysicsSystem::JobApplyGravity(const PhysicsUpdateContext *ioContext, Physi
 		// Process the batch
 		while (active_body_idx < active_body_idx_end)
 		{
-			const Body &body = mBodyManager.GetBody(active_bodies[active_body_idx]);
+			Body &body = mBodyManager.GetBody(active_bodies[active_body_idx]);
 			if (body.IsDynamic())
 				body.GetMotionProperties()->ApplyForceTorqueAndDragInternal(body.GetRotation(), mGravity, delta_time);
 			active_body_idx++;