Added ActivateConstraint function to BodyInterface (#73)

Jolt/Physics/Body/BodyInterface.cpp

@@ -11,6 +11,7 @@
 #include <Physics/Body/BodyLock.h>
 #include <Physics/Body/BodyLockMulti.h>
 #include <Physics/Collision/PhysicsMaterial.h>
+#include <Physics/Constraints/TwoBodyConstraint.h>
 
 namespace JPH {
 
@@ -149,6 +150,12 @@ bool BodyInterface::IsActive(const BodyID &inBodyID) const
 	return lock.Succeeded() && lock.GetBody().IsActive();
 }
 
+void BodyInterface::ActivateConstraint(const TwoBodyConstraint *inConstraint)
+{
+	BodyID bodies[] = { inConstraint->GetBody1()->GetID(), inConstraint->GetBody2()->GetID() };
+	ActivateBodies(bodies, 2);
+}
+
 RefConst<Shape> BodyInterface::GetShape(const BodyID &inBodyID) const
 {
 	RefConst<Shape> shape;

Jolt/Physics/Body/BodyInterface.h

@@ -20,6 +20,7 @@ class TransformedShape;
 class PhysicsMaterial;
 class SubShapeID;
 class Shape;
+class TwoBodyConstraint;
 
 /// Class that provides operations on bodies using a body ID. Note that if you need to do multiple operations on a single body, it is more efficient to lock the body once and combine the operations.
 /// All quantities are in world space unless otherwise specified.
@@ -74,6 +75,9 @@ public:
 	bool						IsActive(const BodyID &inBodyID) const;
 	///@}
 
+	/// Activate non-static bodies attached to a constraint
+	void						ActivateConstraint(const TwoBodyConstraint *inConstraint);
+
 	///@name Access to the shape of a body
 	///@{
 

Jolt/Physics/Body/BodyManager.cpp

@@ -258,32 +258,33 @@ void BodyManager::ActivateBodies(const BodyID *inBodyIDs, int inNumber)
 	JPH_ASSERT(!mActiveBodiesLocked || sOverrideAllowActivation);
 
 	for (const BodyID *b = inBodyIDs, *b_end = inBodyIDs + inNumber; b < b_end; b++)
-	{
-		BodyID body_id = *b;
-		Body &body = *mBodies[body_id.GetIndex()];
+		if (!b->IsInvalid())
+		{
+			BodyID body_id = *b;
+			Body &body = *mBodies[body_id.GetIndex()];
 
-		JPH_ASSERT(GetMutexForBody(body_id).is_locked(), "Assuming that body has been locked!");
-		JPH_ASSERT(body.GetID() == body_id);
-		JPH_ASSERT(body.IsInBroadPhase());
+			JPH_ASSERT(GetMutexForBody(body_id).is_locked(), "Assuming that body has been locked!");
+			JPH_ASSERT(body.GetID() == body_id);
+			JPH_ASSERT(body.IsInBroadPhase());
 
-		if (!body.IsStatic()
-			&& body.mMotionProperties->mIndexInActiveBodies == Body::cInactiveIndex)
-		{
-			body.mMotionProperties->mIndexInActiveBodies = mNumActiveBodies;
-			body.ResetSleepTestSpheres();
-			JPH_ASSERT(mNumActiveBodies < GetMaxBodies());
-			mActiveBodies[mNumActiveBodies] = body_id;
-			mNumActiveBodies++; // Increment atomic after setting the body ID so that PhysicsSystem::JobFindCollisions (which doesn't lock the mActiveBodiesMutex) will only read valid IDs
-
-			// Count CCD bodies
-			if (body.mMotionProperties->GetMotionQuality() == EMotionQuality::LinearCast)
-				mNumActiveCCDBodies++;
-
-			// Call activation listener
-			if (mActivationListener != nullptr)
-				mActivationListener->OnBodyActivated(body_id, body.GetUserData());
+			if (!body.IsStatic()
+				&& body.mMotionProperties->mIndexInActiveBodies == Body::cInactiveIndex)
+			{
+				body.mMotionProperties->mIndexInActiveBodies = mNumActiveBodies;
+				body.ResetSleepTestSpheres();
+				JPH_ASSERT(mNumActiveBodies < GetMaxBodies());
+				mActiveBodies[mNumActiveBodies] = body_id;
+				mNumActiveBodies++; // Increment atomic after setting the body ID so that PhysicsSystem::JobFindCollisions (which doesn't lock the mActiveBodiesMutex) will only read valid IDs
+
+				// Count CCD bodies
+				if (body.mMotionProperties->GetMotionQuality() == EMotionQuality::LinearCast)
+					mNumActiveCCDBodies++;
+
+				// Call activation listener
+				if (mActivationListener != nullptr)
+					mActivationListener->OnBodyActivated(body_id, body.GetUserData());
+			}
 		}
-	}
 }
 
 void BodyManager::DeactivateBodies(const BodyID *inBodyIDs, int inNumber)
@@ -297,50 +298,51 @@ void BodyManager::DeactivateBodies(const BodyID *inBodyIDs, int inNumber)
 	JPH_ASSERT(!mActiveBodiesLocked || sOverrideAllowDeactivation);
 
 	for (const BodyID *b = inBodyIDs, *b_end = inBodyIDs + inNumber; b < b_end; b++)
-	{
-		BodyID body_id = *b;
-		Body &body = *mBodies[body_id.GetIndex()];
+		if (!b->IsInvalid())
+		{
+			BodyID body_id = *b;
+			Body &body = *mBodies[body_id.GetIndex()];
 
-		JPH_ASSERT(GetMutexForBody(body_id).is_locked(), "Assuming that body has been locked!");
-		JPH_ASSERT(body.GetID() == body_id);
-		JPH_ASSERT(body.IsInBroadPhase());
+			JPH_ASSERT(GetMutexForBody(body_id).is_locked(), "Assuming that body has been locked!");
+			JPH_ASSERT(body.GetID() == body_id);
+			JPH_ASSERT(body.IsInBroadPhase());
 
-		if (body.mMotionProperties != nullptr
-			&& body.mMotionProperties->mIndexInActiveBodies != Body::cInactiveIndex)
-		{
-			uint32 last_body_index = mNumActiveBodies - 1;
-			if (body.mMotionProperties->mIndexInActiveBodies != last_body_index)
+			if (body.mMotionProperties != nullptr
+				&& body.mMotionProperties->mIndexInActiveBodies != Body::cInactiveIndex)
 			{
-				// This is not the last body, use the last body to fill the hole
-				BodyID last_body_id = mActiveBodies[last_body_index];
-				mActiveBodies[body.mMotionProperties->mIndexInActiveBodies] = last_body_id;
-
-				// Update that body's index in the active list
-				Body &last_body = *mBodies[last_body_id.GetIndex()];
-				JPH_ASSERT(last_body.mMotionProperties->mIndexInActiveBodies == last_body_index);
-				last_body.mMotionProperties->mIndexInActiveBodies = body.mMotionProperties->mIndexInActiveBodies;
-			}
+				uint32 last_body_index = mNumActiveBodies - 1;
+				if (body.mMotionProperties->mIndexInActiveBodies != last_body_index)
+				{
+					// This is not the last body, use the last body to fill the hole
+					BodyID last_body_id = mActiveBodies[last_body_index];
+					mActiveBodies[body.mMotionProperties->mIndexInActiveBodies] = last_body_id;
+
+					// Update that body's index in the active list
+					Body &last_body = *mBodies[last_body_id.GetIndex()];
+					JPH_ASSERT(last_body.mMotionProperties->mIndexInActiveBodies == last_body_index);
+					last_body.mMotionProperties->mIndexInActiveBodies = body.mMotionProperties->mIndexInActiveBodies;
+				}
 
-			// Mark this body as no longer active
-			body.mMotionProperties->mIndexInActiveBodies = Body::cInactiveIndex;
-			body.mMotionProperties->mIslandIndex = Body::cInactiveIndex;
+				// Mark this body as no longer active
+				body.mMotionProperties->mIndexInActiveBodies = Body::cInactiveIndex;
+				body.mMotionProperties->mIslandIndex = Body::cInactiveIndex;
 
-			// Reset velocity
-			body.mMotionProperties->mLinearVelocity = Vec3::sZero();
-			body.mMotionProperties->mAngularVelocity = Vec3::sZero();
+				// Reset velocity
+				body.mMotionProperties->mLinearVelocity = Vec3::sZero();
+				body.mMotionProperties->mAngularVelocity = Vec3::sZero();
 
-			// Remove unused element from active bodies list
-			--mNumActiveBodies;
+				// Remove unused element from active bodies list
+				--mNumActiveBodies;
 
-			// Count CCD bodies
-			if (body.mMotionProperties->GetMotionQuality() == EMotionQuality::LinearCast)
-				mNumActiveCCDBodies--;
+				// Count CCD bodies
+				if (body.mMotionProperties->GetMotionQuality() == EMotionQuality::LinearCast)
+					mNumActiveCCDBodies--;
 
-			// Call activation listener
-			if (mActivationListener != nullptr)
-				mActivationListener->OnBodyDeactivated(body_id, body.GetUserData());
+				// Call activation listener
+				if (mActivationListener != nullptr)
+					mActivationListener->OnBodyDeactivated(body_id, body.GetUserData());
+			}
 		}
-	}
 }
 
 void BodyManager::GetActiveBodies(BodyIDVector &outBodyIDs) const