Fixed bug that contact cache was partially uninitialized when collidi…

…ng two objects with inv mass override of 0. (#949)

When the contact listener would report a non zero inv mass override the next simulation step this would mean that the simulation would read garbage and potentially crash due to NaNs.

Jolt/Physics/Constraints/ContactConstraintManager.cpp

@@ -1039,28 +1039,9 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 
 	// If one of the bodies is a sensor, don't actually create the constraint
 	JPH_ASSERT(settings.mIsSensor || !(inBody1.IsSensor() || inBody2.IsSensor()), "Sensors cannot be converted into regular bodies by a contact callback!");
-	if (settings.mIsSensor)
-	{
-		// Store the contact manifold in the cache
-		for (int i = 0; i < num_contact_points; ++i)
-		{
-			// Convert to local space to the body
-			Vec3 p1 = Vec3(inverse_transform_body1 * (inManifold.mBaseOffset + inManifold.mRelativeContactPointsOn1[i]));
-			Vec3 p2 = Vec3(inverse_transform_body2 * (inManifold.mBaseOffset + inManifold.mRelativeContactPointsOn2[i]));
-
-			// Create new contact point
-			CachedContactPoint &cp = new_manifold->mContactPoints[i];
-			p1.StoreFloat3(&cp.mPosition1);
-			p2.StoreFloat3(&cp.mPosition2);
-
-			// We don't use this, but reset them anyway for determinism check
-			cp.mNonPenetrationLambda = 0.0f;
-			cp.mFrictionLambda[0] = 0.0f;
-			cp.mFrictionLambda[1] = 0.0f;
-		}
-	}
-	else if ((inBody1.IsDynamic() && settings.mInvMassScale1 != 0.0f) // One of the bodies must have mass to be able to create a contact constraint
-			|| (inBody2.IsDynamic() && settings.mInvMassScale2 != 0.0f))
+	if (!settings.mIsSensor
+		&& ((inBody1.IsDynamic() && settings.mInvMassScale1 != 0.0f) // One of the bodies must have mass to be able to create a contact constraint
+			|| (inBody2.IsDynamic() && settings.mInvMassScale2 != 0.0f)))
 	{
 		// Add contact constraint
 		uint32 constraint_idx = mNumConstraints++;
@@ -1178,6 +1159,26 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 			constraint.Draw(DebugRenderer::sInstance, Color::sOrange);
 	#endif // JPH_DEBUG_RENDERER
 	}
+	else
+	{
+		// Store the contact manifold in the cache
+		for (int i = 0; i < num_contact_points; ++i)
+		{
+			// Convert to local space to the body
+			Vec3 p1 = Vec3(inverse_transform_body1 * (inManifold.mBaseOffset + inManifold.mRelativeContactPointsOn1[i]));
+			Vec3 p2 = Vec3(inverse_transform_body2 * (inManifold.mBaseOffset + inManifold.mRelativeContactPointsOn2[i]));
+
+			// Create new contact point
+			CachedContactPoint &cp = new_manifold->mContactPoints[i];
+			p1.StoreFloat3(&cp.mPosition1);
+			p2.StoreFloat3(&cp.mPosition2);
+
+			// Reset contact impulses, we haven't applied any
+			cp.mNonPenetrationLambda = 0.0f;
+			cp.mFrictionLambda[0] = 0.0f;
+			cp.mFrictionLambda[1] = 0.0f;
+		}
+	}
 
 	// Store cached contact point in body pair cache
 	CachedBodyPair *cbp = reinterpret_cast<CachedBodyPair *>(inBodyPairHandle);