Checking if inverse effective mass is zero to avoid producing NaNs (#686)

When combining a constraint with a body with a restricted set of DOFs (see EAllowedDOFs) it is possible that the inverse effective mass becomes zero. In this case it is impossible to enforce the constraint so we turn the constraint off for the step.

Fixes #685

Jolt/Math/Mat44.h

@@ -90,7 +90,7 @@ public:
 	/// Get float component by element index
 	JPH_INLINE float			operator () (uint inRow, uint inColumn) const			{ JPH_ASSERT(inRow < 4); JPH_ASSERT(inColumn < 4); return mCol[inColumn].mF32[inRow]; }
 	JPH_INLINE float &			operator () (uint inRow, uint inColumn)					{ JPH_ASSERT(inRow < 4); JPH_ASSERT(inColumn < 4); return mCol[inColumn].mF32[inRow]; }
-	
+
 	/// Comparsion
 	JPH_INLINE bool				operator == (Mat44Arg inM2) const;
 	JPH_INLINE bool				operator != (Mat44Arg inM2) const						{ return !(*this == inM2); }
@@ -181,6 +181,9 @@ public:
 	/// Inverse 3x3 matrix
 	JPH_INLINE Mat44			Inversed3x3() const;
 
+	/// *this = inM.Inversed3x3(), returns false if the matrix is singular in which case *this is unchanged
+	JPH_INLINE bool				SetInversed3x3(Mat44Arg inM);
+
 	/// Get rotation part only (note: retains the first 3 values from the bottom row)
 	JPH_INLINE Mat44			GetRotation() const;
 

Jolt/Math/Mat44.inl

@@ -12,18 +12,18 @@ JPH_NAMESPACE_BEGIN
 
 #define JPH_EL(r, c) mCol[c].mF32[r]
 
-Mat44::Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec4Arg inC4) : 
-	mCol { inC1, inC2, inC3, inC4 } 
-{ 
+Mat44::Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec4Arg inC4) :
+	mCol { inC1, inC2, inC3, inC4 }
+{
 }
 
-Mat44::Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec3Arg inC4) : 
-	mCol { inC1, inC2, inC3, Vec4(inC4, 1.0f) } 
-{ 
+Mat44::Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec3Arg inC4) :
+	mCol { inC1, inC2, inC3, Vec4(inC4, 1.0f) }
+{
 }
 
-Mat44::Mat44(Type inC1, Type inC2, Type inC3, Type inC4) : 
-	mCol { inC1, inC2, inC3, inC4 } 
+Mat44::Mat44(Type inC1, Type inC2, Type inC3, Type inC4) :
+	mCol { inC1, inC2, inC3, inC4 }
 {
 }
 
@@ -83,7 +83,7 @@ Mat44 Mat44::sRotationZ(float inZ)
 }
 
 Mat44 Mat44::sRotation(QuatArg inQuat)
-{	
+{
 	JPH_ASSERT(inQuat.IsNormalized());
 
 	// See: https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation section 'Quaternion-derived rotation matrix'
@@ -107,7 +107,7 @@ Mat44 Mat44::sRotation(QuatArg inQuat)
 	__m128 col1 = _mm_blend_ps(_mm_blend_ps(diagonal, minus, 0b1001), plus, 0b0100);	// (2 xy - 2 zw, 1 - 2 x^2 - 2 z^2, 2 yz + 2 xw, 0)
 	__m128 col2 = _mm_blend_ps(_mm_blend_ps(minus, plus, 0b0001), diagonal, 0b0100);	// (2 xz + 2 yw, 2 yz - 2 xw, 1 - 2 x^2 - 2 y^2, 0)
 	__m128 col3 = _mm_set_ps(1, 0, 0, 0);
-	
+
 	return Mat44(col0, col1, col2, col3);
 #else
 	float x = inQuat.GetX();
@@ -210,7 +210,7 @@ Mat44 Mat44::sLookAt(Vec3Arg inPos, Vec3Arg inTarget, Vec3Arg inUp)
 	Vec3 right = direction.Cross(inUp).NormalizedOr(Vec3::sAxisX());
 	Vec3 up = right.Cross(direction);
 
-	return Mat44(Vec4(right, 0), Vec4(up, 0), Vec4(-direction, 0), Vec4(inPos, 1)).InversedRotationTranslation();	
+	return Mat44(Vec4(right, 0), Vec4(up, 0), Vec4(-direction, 0), Vec4(inPos, 1)).InversedRotationTranslation();
 }
 
 bool Mat44::operator == (Mat44Arg inM2) const
@@ -275,8 +275,8 @@ Vec3 Mat44::operator * (Vec3Arg inV) const
 	return Vec3::sFixW(t);
 #else
 	return Vec3(
-		mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2] + mCol[3].mF32[0], 
-		mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2] + mCol[3].mF32[1], 
+		mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2] + mCol[3].mF32[0],
+		mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2] + mCol[3].mF32[1],
 		mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2] + mCol[3].mF32[2]);
 #endif
 }
@@ -297,9 +297,9 @@ Vec4 Mat44::operator * (Vec4Arg inV) const
 	return t;
 #else
 	return Vec4(
-		mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2] + mCol[3].mF32[0] * inV.mF32[3], 
-		mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2] + mCol[3].mF32[1] * inV.mF32[3], 
-		mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2] + mCol[3].mF32[2] * inV.mF32[3], 
+		mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2] + mCol[3].mF32[0] * inV.mF32[3],
+		mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2] + mCol[3].mF32[1] * inV.mF32[3],
+		mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2] + mCol[3].mF32[2] * inV.mF32[3],
 		mCol[0].mF32[3] * inV.mF32[0] + mCol[1].mF32[3] * inV.mF32[1] + mCol[2].mF32[3] * inV.mF32[2] + mCol[3].mF32[3] * inV.mF32[3]);
 #endif
 }
@@ -318,8 +318,8 @@ Vec3 Mat44::Multiply3x3(Vec3Arg inV) const
 	return Vec3::sFixW(t);
 #else
 	return Vec3(
-		mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2], 
-		mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2], 
+		mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2],
+		mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2],
 		mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2]);
 #endif
 }
@@ -600,7 +600,7 @@ Mat44 Mat44::Inversed() const
 	det = _mm_add_ss(_mm_shuffle_ps(det, det, _MM_SHUFFLE(1, 0, 3, 2)), det);
 	det = _mm_div_ss(_mm_set_ss(1.0f), det);
 	det = _mm_shuffle_ps(det, det, _MM_SHUFFLE(0, 0, 0, 0));
-	
+
 	Mat44 result;
 	result.mCol[0].mValue = _mm_mul_ps(det, minor0);
 	result.mCol[1].mValue = _mm_mul_ps(det, minor1);
@@ -674,7 +674,7 @@ Mat44 Mat44::Inversed() const
 	Type det = vmulq_f32(row0, minor0);
 	det = vdupq_n_f32(vaddvq_f32(det));
 	det = vdivq_f32(vdupq_n_f32(1.0f), det);
-	
+
 	Mat44 result;
 	result.mCol[0].mValue = vmulq_f32(det, minor0);
 	result.mCol[1].mValue = vmulq_f32(det, minor1);
@@ -686,7 +686,7 @@ Mat44 Mat44::Inversed() const
 	float m01 = JPH_EL(0, 1), m11 = JPH_EL(1, 1), m21 = JPH_EL(2, 1), m31 = JPH_EL(3, 1);
 	float m02 = JPH_EL(0, 2), m12 = JPH_EL(1, 2), m22 = JPH_EL(2, 2), m32 = JPH_EL(3, 2);
 	float m03 = JPH_EL(0, 3), m13 = JPH_EL(1, 3), m23 = JPH_EL(2, 3), m33 = JPH_EL(3, 3);
-	
+
 	float m10211120 = m10 * m21 - m11 * m20;
 	float m10221220 = m10 * m22 - m12 * m20;
 	float m10231320 = m10 * m23 - m13 * m20;
@@ -755,6 +755,22 @@ Mat44 Mat44::Inversed3x3() const
 		Vec4(0, 0, 0, 1));
 }
 
+bool Mat44::SetInversed3x3(Mat44Arg inM)
+{
+	float det = inM.GetDeterminant3x3();
+
+	// If the determinant is zero the matrix is singular and we return false
+	if (det == 0.0f)
+		return false;
+
+	// Finish calculating the inverse
+	*this = inM.Adjointed3x3();
+	mCol[0] /= det;
+	mCol[1] /= det;
+	mCol[2] /= det;
+	return true;
+}
+
 Quat Mat44::GetQuaternion() const
 {
 	JPH_ASSERT(mCol[3] == Vec4(0, 0, 0, 1));
@@ -831,32 +847,32 @@ Mat44 Mat44::sQuatRightMultiply(QuatArg inQ)
 }
 
 Mat44 Mat44::GetRotation() const
-{ 
+{
 	JPH_ASSERT(mCol[0][3] == 0.0f);
 	JPH_ASSERT(mCol[1][3] == 0.0f);
 	JPH_ASSERT(mCol[2][3] == 0.0f);
 
-	return Mat44(mCol[0], mCol[1], mCol[2], Vec4(0, 0, 0, 1)); 
+	return Mat44(mCol[0], mCol[1], mCol[2], Vec4(0, 0, 0, 1));
 }
 
 Mat44 Mat44::GetRotationSafe() const
-{ 
+{
 #if defined(JPH_USE_AVX512)
 	return Mat44(_mm_maskz_mov_ps(0b0111, mCol[0].mValue),
 				 _mm_maskz_mov_ps(0b0111, mCol[1].mValue),
 				 _mm_maskz_mov_ps(0b0111, mCol[2].mValue),
-				 Vec4(0, 0, 0, 1)); 
+				 Vec4(0, 0, 0, 1));
 #elif defined(JPH_USE_SSE4_1)
-	__m128 zero = _mm_setzero_ps(); 
+	__m128 zero = _mm_setzero_ps();
 	return Mat44(_mm_blend_ps(mCol[0].mValue, zero, 8),
 				 _mm_blend_ps(mCol[1].mValue, zero, 8),
 				 _mm_blend_ps(mCol[2].mValue, zero, 8),
-				 Vec4(0, 0, 0, 1)); 
+				 Vec4(0, 0, 0, 1));
 #elif defined(JPH_USE_NEON)
 	return Mat44(vsetq_lane_f32(0, mCol[0].mValue, 3),
 				 vsetq_lane_f32(0, mCol[1].mValue, 3),
 				 vsetq_lane_f32(0, mCol[2].mValue, 3),
-				 Vec4(0, 0, 0, 1)); 
+				 Vec4(0, 0, 0, 1));
 #else
 	return Mat44(Vec4(mCol[0].mF32[0], mCol[0].mF32[1], mCol[0].mF32[2], 0),
 				 Vec4(mCol[1].mF32[0], mCol[1].mF32[1], mCol[1].mF32[2], 0),
@@ -911,7 +927,7 @@ Mat44 Mat44::Decompose(Vec3 &outScale) const
 	// Make Z axis perpendicular to Y
 	float y_dot_y = y.LengthSq();
 	z -= (y.Dot(z) / y_dot_y) * y;
-	
+
 	// Determine the scale
 	float z_dot_z = z.LengthSq();
 	outScale = Vec3(x_dot_x, y_dot_y, z_dot_z).Sqrt();

Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h

@@ -46,7 +46,7 @@ class AngleConstraintPart
 			// Impulse:
 			// P = J^T lambda
 			//
-			// Euler velocity integration: 
+			// Euler velocity integration:
 			// v' = v + M^-1 P
 			if (ioBody1.IsDynamic())
 				ioBody1.GetMotionProperties()->SubAngularVelocityStep(inLambda * mInvI1_Axis);
@@ -66,7 +66,7 @@ class AngleConstraintPart
 		// Calculate properties used below
 		mInvI1_Axis = inBody1.IsDynamic()? inBody1.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), inWorldSpaceAxis) : Vec3::sZero();
 		mInvI2_Axis = inBody2.IsDynamic()? inBody2.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody2.GetRotation(), inWorldSpaceAxis) : Vec3::sZero();
-	
+
 		// Calculate inverse effective mass: K = J M^-1 J^T
 		return inWorldSpaceAxis.Dot(mInvI1_Axis + mInvI2_Axis);
 	}
@@ -80,9 +80,15 @@ public:
 	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
 	inline void					CalculateConstraintProperties(const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f)
 	{
-		mEffectiveMass = 1.0f / CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis);
+		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis);
 
-		mSpringPart.CalculateSpringPropertiesWithBias(inBias);
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+		{
+			mEffectiveMass = 1.0f / inv_effective_mass;
+			mSpringPart.CalculateSpringPropertiesWithBias(inBias);
+		}
 	}
 
 	/// Calculate properties used during the functions below
@@ -99,7 +105,10 @@ public:
 	{
 		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis);
 
-		mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inFrequency, inDamping, mEffectiveMass);
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+			mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inFrequency, inDamping, mEffectiveMass);
 	}
 
 	/// Calculate properties used during the functions below
@@ -116,7 +125,10 @@ public:
 	{
 		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis);
 
-		mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inStiffness, inDamping, mEffectiveMass);
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+			mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inStiffness, inDamping, mEffectiveMass);
 	}
 
 	/// Selects one of the above functions based on the spring settings
@@ -195,11 +207,11 @@ public:
 			// lambda = -K^-1 * beta / dt * C
 			//
 			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
-			float lambda = -mEffectiveMass * inBaumgarte * inC; 
+			float lambda = -mEffectiveMass * inBaumgarte * inC;
 
 			// Directly integrate velocity change for one time step
 			//
-			// Euler velocity integration: 
+			// Euler velocity integration:
 			// dv = M^-1 P
 			//
 			// Impulse:
@@ -208,9 +220,9 @@ public:
 			// Euler position integration:
 			// x' = x + dv * dt
 			//
-			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and 
-			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte 
-			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity 
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
 			// integrate + a position integrate and then discard the velocity change.
 			if (ioBody1.IsDynamic())
 				ioBody1.SubRotationStep(lambda * mInvI1_Axis);

Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h

@@ -14,7 +14,7 @@ JPH_NAMESPACE_BEGIN
 
 /// Constraint that constrains motion along 1 axis
 ///
-/// @see "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.1.1 
+/// @see "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.1.1
 /// (we're not using the approximation of eq 27 but instead add the U term as in eq 55)
 ///
 /// Constraint equation (eq 25):
@@ -53,7 +53,7 @@ class AxisConstraintPart
 			// Impulse:
 			// P = J^T lambda
 			//
-			// Euler velocity integration: 
+			// Euler velocity integration:
 			// v' = v + M^-1 P
 			if constexpr (Type1 == EMotionType::Dynamic)
 			{
@@ -99,7 +99,7 @@ class AxisConstraintPart
 			mInverseMass2 = inInvMass2;
 			r2_x_axis = inR2.Cross(inWorldSpaceAxis);
 			r2_x_axis.StoreFloat3(&mR2xAxis);
-		}		
+		}
 		else
 		{
 		#ifdef _DEBUG
@@ -203,9 +203,15 @@ public:
 	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
 	inline void					CalculateConstraintProperties(const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f)
 	{
-		mEffectiveMass = 1.0f / CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis);
+		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis);
 
-		mSpringPart.CalculateSpringPropertiesWithBias(inBias);
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+		{
+			mEffectiveMass = 1.0f / inv_effective_mass;
+			mSpringPart.CalculateSpringPropertiesWithBias(inBias);
+		}
 	}
 
 	/// Calculate properties used during the functions below
@@ -223,7 +229,10 @@ public:
 	{
 		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis);
 
-		mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inFrequency, inDamping, mEffectiveMass);
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+			mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inFrequency, inDamping, mEffectiveMass);
 	}
 
 	/// Calculate properties used during the functions below
@@ -241,7 +250,10 @@ public:
 	{
 		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis);
 
-		mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inStiffness, inDamping, mEffectiveMass);
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+			mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inStiffness, inDamping, mEffectiveMass);
 	}
 
 	/// Selects one of the above functions based on the spring settings
@@ -427,11 +439,11 @@ public:
 			// lambda = -K^-1 * beta / dt * C
 			//
 			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
-			float lambda = -mEffectiveMass * inBaumgarte * inC; 
+			float lambda = -mEffectiveMass * inBaumgarte * inC;
 
 			// Directly integrate velocity change for one time step
 			//
-			// Euler velocity integration: 
+			// Euler velocity integration:
 			// dv = M^-1 P
 			//
 			// Impulse:
@@ -440,9 +452,9 @@ public:
 			// Euler position integration:
 			// x' = x + dv * dt
 			//
-			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and 
-			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte 
-			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity 
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
 			// integrate + a position integrate and then discard the velocity change.
 			if (ioBody1.IsDynamic())
 			{

Jolt/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h

@@ -25,7 +25,7 @@ JPH_NAMESPACE_BEGIN
 /// -n_1					& -n_2					\\
 /// -(r_1 + u) \times n_1	& -(r_1 + u) \times n_2	\\
 /// n_1						& n_2					\\
-/// r_2 \times n_1			& r_2 \times n_2        
+/// r_2 \times n_1			& r_2 \times n_2
 /// \end{bmatrix}\f]
 ///
 /// Used terms (here and below, everything in world space):\n
@@ -60,7 +60,7 @@ private:
 			// Impulse:
 			// P = J^T lambda
 			//
-			// Euler velocity integration: 
+			// Euler velocity integration:
 			// v' = v + M^-1 P
 			Vec3 impulse = inN1 * inLambda[0] + inN2 * inLambda[1];
 			if (ioBody1.IsDynamic())
@@ -80,7 +80,7 @@ private:
 
 		return false;
 	}
-	
+
 	/// Internal helper function to calculate the lagrange multiplier
 	inline void					CalculateLagrangeMultiplier(const Body &inBody1, const Body &inBody2, Vec3Arg inN1, Vec3Arg inN2, Vec2 &outLambda) const
 	{
@@ -133,7 +133,7 @@ public:
 		if (inBody2.IsDynamic())
 		{
 			const MotionProperties *mp2 = inBody2.GetMotionProperties();
-			Mat44 inv_i2 = mp2->GetInverseInertiaForRotation(inRotation2);	
+			Mat44 inv_i2 = mp2->GetInverseInertiaForRotation(inRotation2);
 			mInvI2_R2xN1 = inv_i2.Multiply3x3(mR2xN1);
 			mInvI2_R2xN2 = inv_i2.Multiply3x3(mR2xN2);
 
@@ -149,10 +149,7 @@ public:
 		}
 
 		if (!mEffectiveMass.SetInversed(inv_effective_mass))
-		{
-			JPH_ASSERT(false, "Determinant is zero!");
 			Deactivate();
-		}
 	}
 
 	/// Deactivate this constraint
@@ -182,13 +179,13 @@ public:
 	{
 		Vec2 lambda;
 		CalculateLagrangeMultiplier(ioBody1, ioBody2, inN1, inN2, lambda);
-		
+
 		// Store accumulated lambda
-		mTotalLambda += lambda; 
-		
+		mTotalLambda += lambda;
+
 		return ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, lambda);
 	}
-	
+
 	/// Iteratively update the position constraint. Makes sure C(...) = 0.
 	/// All input vectors are in world space
 	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inU, Vec3Arg inN1, Vec3Arg inN2, float inBaumgarte) const
@@ -207,7 +204,7 @@ public:
 
 			// Directly integrate velocity change for one time step
 			//
-			// Euler velocity integration: 
+			// Euler velocity integration:
 			// dv = M^-1 P
 			//
 			// Impulse:
@@ -216,9 +213,9 @@ public:
 			// Euler position integration:
 			// x' = x + dv * dt
 			//
-			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and 
-			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte 
-			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity 
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
 			// integrate + a position integrate and then discard the velocity change.
 			Vec3 impulse = inN1 * lambda[0] + inN2 * lambda[1];
 			if (ioBody1.IsDynamic())

Jolt/Physics/Constraints/ConstraintPart/GearConstraintPart.h

@@ -49,7 +49,7 @@ class GearConstraintPart
 			// Impulse:
 			// P = J^T lambda
 			//
-			// Euler velocity integration: 
+			// Euler velocity integration:
 			// v' = v + M^-1 P
 			ioBody1.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI1_A);
 			ioBody2.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI2_B);
@@ -78,7 +78,11 @@ public:
 		mInvI2_B = inBody2.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody2.GetRotation(), inWorldSpaceHingeAxis2);
 
 		// K^-1 = 1 / (J M^-1 J^T) = 1 / (a^T I1^-1 a + r^2 * b^T I2^-1 b)
-		mEffectiveMass = 1.0f / (inWorldSpaceHingeAxis1.Dot(mInvI1_A) + inWorldSpaceHingeAxis2.Dot(mInvI2_B) * Square(inRatio));
+		float inv_effective_mass = (inWorldSpaceHingeAxis1.Dot(mInvI1_A) + inWorldSpaceHingeAxis2.Dot(mInvI2_B) * Square(inRatio));
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+			mEffectiveMass = 1.0f / inv_effective_mass;
 	}
 
 	/// Deactivate this constraint
@@ -142,11 +146,11 @@ public:
 			// lambda = -K^-1 * beta / dt * C
 			//
 			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
-			float lambda = -mEffectiveMass * inBaumgarte * inC; 
+			float lambda = -mEffectiveMass * inBaumgarte * inC;
 
 			// Directly integrate velocity change for one time step
 			//
-			// Euler velocity integration: 
+			// Euler velocity integration:
 			// dv = M^-1 P
 			//
 			// Impulse:
@@ -155,9 +159,9 @@ public:
 			// Euler position integration:
 			// x' = x + dv * dt
 			//
-			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and 
-			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte 
-			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity 
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
 			// integrate + a position integrate and then discard the velocity change.
 			if (ioBody1.IsDynamic())
 				ioBody1.AddRotationStep(lambda * mInvI1_A);

Jolt/Physics/Constraints/ConstraintPart/HingeRotationConstraintPart.h

@@ -56,7 +56,7 @@ private:
 			// Impulse:
 			// P = J^T lambda
 			//
-			// Euler velocity integration: 
+			// Euler velocity integration:
 			// v' = v + M^-1 P
 			Vec3 impulse = mB2xA1 * inLambda[0] + mC2xA1 * inLambda[1];
 			if (ioBody1.IsDynamic())
@@ -110,10 +110,7 @@ public:
 		inv_effective_mass(1, 0) = mC2xA1.Dot(summed_inv_inertia.Multiply3x3(mB2xA1));
 		inv_effective_mass(1, 1) = mC2xA1.Dot(summed_inv_inertia.Multiply3x3(mC2xA1));
 		if (!mEffectiveMass.SetInversed(inv_effective_mass))
-		{
-			JPH_ASSERT(false, "Determinant is zero!");
 			Deactivate();
-		}
 	}
 
 	/// Deactivate this constraint
@@ -143,8 +140,8 @@ public:
 		Vec2 lambda = mEffectiveMass * jv;
 
 		// Store accumulated lambda
-		mTotalLambda += lambda; 
-		
+		mTotalLambda += lambda;
+
 		return ApplyVelocityStep(ioBody1, ioBody2, lambda);
 	}
 
@@ -166,7 +163,7 @@ public:
 
 			// Directly integrate velocity change for one time step
 			//
-			// Euler velocity integration: 
+			// Euler velocity integration:
 			// dv = M^-1 P
 			//
 			// Impulse:
@@ -175,9 +172,9 @@ public:
 			// Euler position integration:
 			// x' = x + dv * dt
 			//
-			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and 
-			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte 
-			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity 
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
 			// integrate + a position integrate and then discard the velocity change.
 			Vec3 impulse = mB2xA1 * lambda[0] + mC2xA1 * lambda[1];
 			if (ioBody1.IsDynamic())
@@ -195,7 +192,7 @@ public:
 	{
 		return mTotalLambda;
 	}
-	
+
 	/// Save state of this constraint part
 	void						SaveState(StateRecorder &inStream) const
 	{

Jolt/Physics/Constraints/ConstraintPart/IndependentAxisConstraintPart.h

@@ -23,7 +23,7 @@ JPH_NAMESPACE_BEGIN
 ///
 /// \f[(v_1 + w_1 \times r_1) \cdot n_1 + r (v_2 + w_2 \times r_2) \cdot n_2\f]
 /// \f[= v_1 \cdot n_1 + r_1 \times n_1 \cdot w_1 + r v_2 \cdot n_2 + r r_2 \times n_2 \cdot w_2\f]
-/// 
+///
 /// Jacobian:
 ///
 /// \f[J = \begin{bmatrix}n_1 & r_1 \times n_1 & r n_2 & r r_2 \times n_2\end{bmatrix}\f]
@@ -57,7 +57,7 @@ class IndependentAxisConstraintPart
 			// Impulse:
 			// P = J^T lambda
 			//
-			// Euler velocity integration: 
+			// Euler velocity integration:
 			// v' = v + M^-1 P
 			if (ioBody1.IsDynamic())
 			{
@@ -95,10 +95,10 @@ public:
 		if (!inBody1.IsStatic())
 		{
 			const MotionProperties *mp1 = inBody1.GetMotionProperties();
-			
+
 			mR1xN1 = inR1.Cross(inN1);
 			mInvI1_R1xN1 = mp1->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), mR1xN1);
-			
+
 			inv_effective_mass += mp1->GetInverseMass() + mInvI1_R1xN1.Dot(mR1xN1);
 		}
 
@@ -113,7 +113,10 @@ public:
 		}
 
 		// Calculate inverse effective mass: K = J M^-1 J^T
-		mEffectiveMass = 1.0f / inv_effective_mass;
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+			mEffectiveMass = 1.0f / inv_effective_mass;
 	}
 
 	/// Deactivate this constraint
@@ -186,11 +189,11 @@ public:
 			// lambda = -K^-1 * beta / dt * C
 			//
 			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
-			float lambda = -mEffectiveMass * inBaumgarte * inC; 
+			float lambda = -mEffectiveMass * inBaumgarte * inC;
 
 			// Directly integrate velocity change for one time step
 			//
-			// Euler velocity integration: 
+			// Euler velocity integration:
 			// dv = M^-1 P
 			//
 			// Impulse:
@@ -199,9 +202,9 @@ public:
 			// Euler position integration:
 			// x' = x + dv * dt
 			//
-			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and 
-			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte 
-			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity 
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
 			// integrate + a position integrate and then discard the velocity change.
 			if (ioBody1.IsDynamic())
 			{

Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h

@@ -15,12 +15,12 @@ JPH_NAMESPACE_BEGIN
 ///
 /// Constraint equation (eq 45):
 ///
-/// \f[C = p_2 - p_1\f] 
+/// \f[C = p_2 - p_1\f]
 ///
 /// Jacobian (transposed) (eq 47):
 ///
-/// \f[J^T = \begin{bmatrix}-E & r1x & E & -r2x^T\end{bmatrix} 
-/// = \begin{bmatrix}-E^T \\ r1x^T \\ E^T \\ -r2x^T\end{bmatrix} 
+/// \f[J^T = \begin{bmatrix}-E & r1x & E & -r2x^T\end{bmatrix}
+/// = \begin{bmatrix}-E^T \\ r1x^T \\ E^T \\ -r2x^T\end{bmatrix}
 /// = \begin{bmatrix}-E \\ -r1x \\ E \\ r2x\end{bmatrix}\f]
 ///
 /// Used terms (here and below, everything in world space):\n
@@ -49,7 +49,7 @@ class PointConstraintPart
 			// Impulse:
 			// P = J^T lambda
 			//
-			// Euler velocity integration: 
+			// Euler velocity integration:
 			// v' = v + M^-1 P
 			if (ioBody1.IsDynamic())
 			{
@@ -121,7 +121,21 @@ public:
 		}
 
 		inv_effective_mass += Mat44::sScale(summed_inv_mass);
-		mEffectiveMass = inv_effective_mass.Inversed3x3();
+		if (!mEffectiveMass.SetInversed3x3(inv_effective_mass))
+			Deactivate();
+	}
+
+	/// Deactivate this constraint
+	inline void					Deactivate()
+	{
+		mEffectiveMass = Mat44::sZero();
+		mTotalLambda = Vec3::sZero();
+	}
+
+	/// Check if constraint is active
+	inline bool					IsActive() const
+	{
+		return mEffectiveMass(3, 3) != 0.0f;
 	}
 
 	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
@@ -161,11 +175,11 @@ public:
 			// lambda = -K^-1 * beta / dt * C
 			//
 			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
-			Vec3 lambda = mEffectiveMass * -inBaumgarte * separation; 
+			Vec3 lambda = mEffectiveMass * -inBaumgarte * separation;
 
 			// Directly integrate velocity change for one time step
 			//
-			// Euler velocity integration: 
+			// Euler velocity integration:
 			// dv = M^-1 P
 			//
 			// Impulse:
@@ -174,9 +188,9 @@ public:
 			// Euler position integration:
 			// x' = x + dv * dt
 			//
-			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and 
-			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte 
-			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity 
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
 			// integrate + a position integrate and then discard the velocity change.
 			if (ioBody1.IsDynamic())
 			{
@@ -194,7 +208,7 @@ public:
 
 		return false;
 	}
-	
+
 	/// Return lagrange multiplier
 	Vec3		 				GetTotalLambda() const
 	{

Jolt/Physics/Constraints/ConstraintPart/RackAndPinionConstraintPart.h

@@ -49,7 +49,7 @@ class RackAndPinionConstraintPart
 			// Impulse:
 			// P = J^T lambda
 			//
-			// Euler velocity integration: 
+			// Euler velocity integration:
 			// v' = v + M^-1 P
 			ioBody1.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI1_A);
 			ioBody2.GetMotionProperties()->SubLinearVelocityStep(inLambda * mRatio_InvM2_B);
@@ -79,7 +79,11 @@ public:
 		mRatio_InvM2_B = inRatio * inv_m2 * inWorldSpaceSliderAxis;
 
 		// K^-1 = 1 / (J M^-1 J^T) = 1 / (a^T I1^-1 a + 1/m2 * r^2 * b . b)
-		mEffectiveMass = 1.0f / (inWorldSpaceHingeAxis.Dot(mInvI1_A) + inv_m2 * Square(inRatio));
+		float inv_effective_mass = (inWorldSpaceHingeAxis.Dot(mInvI1_A) + inv_m2 * Square(inRatio));
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+			mEffectiveMass = 1.0f / inv_effective_mass;
 	}
 
 	/// Deactivate this constraint
@@ -143,11 +147,11 @@ public:
 			// lambda = -K^-1 * beta / dt * C
 			//
 			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
-			float lambda = -mEffectiveMass * inBaumgarte * inC; 
+			float lambda = -mEffectiveMass * inBaumgarte * inC;
 
 			// Directly integrate velocity change for one time step
 			//
-			// Euler velocity integration: 
+			// Euler velocity integration:
 			// dv = M^-1 P
 			//
 			// Impulse:
@@ -156,9 +160,9 @@ public:
 			// Euler position integration:
 			// x' = x + dv * dt
 			//
-			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and 
-			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte 
-			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity 
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
 			// integrate + a position integrate and then discard the velocity change.
 			if (ioBody1.IsDynamic())
 				ioBody1.AddRotationStep(lambda * mInvI1_A);

Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h

@@ -46,7 +46,7 @@ private:
 			// Impulse:
 			// P = J^T lambda
 			//
-			// Euler velocity integration: 
+			// Euler velocity integration:
 			// v' = v + M^-1 P
 			if (ioBody1.IsDynamic())
 				ioBody1.GetMotionProperties()->SubAngularVelocityStep(mInvI1.Multiply3x3(inLambda));
@@ -62,8 +62,8 @@ public:
 	/// Return inverse of initial rotation from body 1 to body 2 in body 1 space
 	static Quat					sGetInvInitialOrientation(const Body &inBody1, const Body &inBody2)
 	{
-		// q20 = q10 r0 
-		// <=> r0 = q10^-1 q20 
+		// q20 = q10 r0
+		// <=> r0 = q10^-1 q20
 		// <=> r0^-1 = q20^-1 q10
 		//
 		// where:
@@ -83,8 +83,8 @@ public:
 	{
 		// Store inverse of initial rotation from body 1 to body 2 in body 1 space:
 		//
-		// q20 = q10 r0 
-		// <=> r0 = q10^-1 q20 
+		// q20 = q10 r0
+		// <=> r0 = q10^-1 q20
 		// <=> r0^-1 = q20^-1 q10
 		//
 		// where:
@@ -93,14 +93,14 @@ public:
 		// r0 = initial rotation rotation from body 1 to body 2 in local space of body 1
 		//
 		// We can also write this in terms of the constraint matrices:
-		// 
+		//
 		// q20 c2 = q10 c1
 		// <=> q20 = q10 c1 c2^-1
 		// => r0 = c1 c2^-1
 		// <=> r0^-1 = c2 c1^-1
-		// 
+		//
 		// where:
-		// 
+		//
 		// c1, c2 = matrix that takes us from body 1 and 2 COM to constraint space 1 and 2
 		if (inAxisX1 == inAxisX2 && inAxisY1 == inAxisY2)
 		{
@@ -143,13 +143,14 @@ public:
 		mInvI2 = inBody2.IsDynamic()? inBody2.GetMotionProperties()->GetInverseInertiaForRotation(inRotation2) : Mat44::sZero();
 
 		// Calculate effective mass: K^-1 = (J M^-1 J^T)^-1
-		mEffectiveMass = (mInvI1 + mInvI2).Inversed3x3();
+		if (!mEffectiveMass.SetInversed3x3(mInvI1 + mInvI2))
+			Deactivate();
 	}
 
 	/// Deactivate this constraint
 	inline void					Deactivate()
 	{
-		mEffectiveMass(3, 3) = 0.0f;
+		mEffectiveMass = Mat44::sZero();
 		mTotalLambda = Vec3::sZero();
 	}
 
@@ -176,7 +177,7 @@ public:
 		mTotalLambda += lambda;
 		return ApplyVelocityStep(ioBody1, ioBody2, lambda);
 	}
-	
+
 	/// Iteratively update the position constraint. Makes sure C(...) = 0.
 	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, QuatArg inInvInitialOrientation, float inBaumgarte) const
 	{
@@ -204,7 +205,7 @@ public:
 		// Where:
 		// v = rotation vector
 		// theta = rotation angle
-		// 
+		//
 		// If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is:
 		Vec3 error = 2.0f * diff.EnsureWPositive().GetXYZ();
 		if (error != Vec3::sZero())
@@ -218,7 +219,7 @@ public:
 
 			// Directly integrate velocity change for one time step
 			//
-			// Euler velocity integration: 
+			// Euler velocity integration:
 			// dv = M^-1 P
 			//
 			// Impulse:
@@ -227,9 +228,9 @@ public:
 			// Euler position integration:
 			// x' = x + dv * dt
 			//
-			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and 
-			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte 
-			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity 
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
 			// integrate + a position integrate and then discard the velocity change.
 			if (ioBody1.IsDynamic())
 				ioBody1.SubRotationStep(mInvI1.Multiply3x3(lambda));
@@ -259,7 +260,7 @@ public:
 		inStream.Read(mTotalLambda);
 	}
 
-private:	
+private:
 	Mat44						mInvI1;
 	Mat44						mInvI2;
 	Mat44						mEffectiveMass;

Jolt/Physics/Constraints/ConstraintPart/RotationQuatConstraintPart.h

@@ -11,19 +11,19 @@ JPH_NAMESPACE_BEGIN
 
 /// Quaternion based constraint that constrains rotation around all axis so that only translation is allowed.
 ///
-/// NOTE: This constraint part is more expensive than the RotationEulerConstraintPart and slightly more correct since 
+/// NOTE: This constraint part is more expensive than the RotationEulerConstraintPart and slightly more correct since
 /// RotationEulerConstraintPart::SolvePositionConstraint contains an approximation. In practice the difference
 /// is small, so the RotationEulerConstraintPart is probably the better choice.
 ///
 /// Rotation is fixed between bodies like this:
 ///
 /// q2 = q1 r0
-/// 
+///
 /// Where:
 /// q1, q2 = world space quaternions representing rotation of body 1 and 2.
 /// r0 = initial rotation between bodies in local space of body 1, this can be calculated by:
 ///
-/// q20 = q10 r0 
+/// q20 = q10 r0
 /// <=> r0 = q10^* q20
 ///
 /// Where:
@@ -77,7 +77,7 @@ JPH_NAMESPACE_BEGIN
 ///
 /// Jacobian:
 ///
-/// J = [0, -1/2 A ML(q1^*) MR(q2 r0^*) A^T, 0, 1/2 A ML(q1^*) MR(q2 r0^*) A^T] 
+/// J = [0, -1/2 A ML(q1^*) MR(q2 r0^*) A^T, 0, 1/2 A ML(q1^*) MR(q2 r0^*) A^T]
 /// = [0, -JP, 0, JP]
 ///
 /// Suggested reading:
@@ -97,7 +97,7 @@ private:
 			// Impulse:
 			// P = J^T lambda
 			//
-			// Euler velocity integration: 
+			// Euler velocity integration:
 			// v' = v + M^-1 P
 			if (ioBody1.IsDynamic())
 				ioBody1.GetMotionProperties()->SubAngularVelocityStep(mInvI1_JPT.Multiply3x3(inLambda));
@@ -113,8 +113,8 @@ public:
 	/// Return inverse of initial rotation from body 1 to body 2 in body 1 space
 	static Quat					sGetInvInitialOrientation(const Body &inBody1, const Body &inBody2)
 	{
-		// q20 = q10 r0 
-		// <=> r0 = q10^-1 q20 
+		// q20 = q10 r0
+		// <=> r0 = q10^-1 q20
 		// <=> r0^-1 = q20^-1 q10
 		//
 		// where:
@@ -137,11 +137,27 @@ public:
 		mInvI1_JPT = invi1.Multiply3x3RightTransposed(jp);
 		mInvI2_JPT = invi2.Multiply3x3RightTransposed(jp);
 
-		// Calculate effective mass: K^-1 = (J M^-1 J^T)^-1 
+		// Calculate effective mass: K^-1 = (J M^-1 J^T)^-1
 		// = (JP * I1^-1 * JP^T + JP * I2^-1 * JP^T)^-1
 		// = (JP * (I1^-1 + I2^-1) * JP^T)^-1
-		mEffectiveMass = jp.Multiply3x3(invi1 + invi2).Multiply3x3RightTransposed(jp).Inversed3x3();
-		mEffectiveMass_JP = mEffectiveMass.Multiply3x3(jp);
+		if (!mEffectiveMass.SetInversed3x3(jp.Multiply3x3(invi1 + invi2).Multiply3x3RightTransposed(jp)))
+			Deactivate();
+		else
+			mEffectiveMass_JP = mEffectiveMass.Multiply3x3(jp);
+	}
+
+	/// Deactivate this constraint
+	inline void					Deactivate()
+	{
+		mEffectiveMass = Mat44::sZero();
+		mEffectiveMass_JP = Mat44::sZero();
+		mTotalLambda = Vec3::sZero();
+	}
+
+	/// Check if constraint is active
+	inline bool					IsActive() const
+	{
+		return mEffectiveMass(3, 3) != 0.0f;
 	}
 
 	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
@@ -178,7 +194,7 @@ public:
 
 			// Directly integrate velocity change for one time step
 			//
-			// Euler velocity integration: 
+			// Euler velocity integration:
 			// dv = M^-1 P
 			//
 			// Impulse:
@@ -187,9 +203,9 @@ public:
 			// Euler position integration:
 			// x' = x + dv * dt
 			//
-			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and 
-			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte 
-			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity 
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
 			// integrate + a position integrate and then discard the velocity change.
 			if (ioBody1.IsDynamic())
 				ioBody1.SubRotationStep(mInvI1_JPT.Multiply3x3(lambda));
@@ -219,7 +235,7 @@ public:
 		inStream.Read(mTotalLambda);
 	}
 
-private:	
+private:
 	Mat44						mInvI1_JPT;
 	Mat44						mInvI2_JPT;
 	Mat44						mEffectiveMass;

Jolt/Physics/Constraints/FixedConstraint.cpp

@@ -13,6 +13,8 @@
 
 JPH_NAMESPACE_BEGIN
 
+using namespace literals;
+
 JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(FixedConstraintSettings)
 {
 	JPH_ADD_BASE_CLASS(FixedConstraintSettings, TwoBodyConstraintSettings)
@@ -81,7 +83,11 @@ FixedConstraint::FixedConstraint(Body &inBody1, Body &inBody2, const FixedConstr
 				// Otherwise use weighted anchor point towards the lightest body
 				Real inv_m1 = Real(inBody1.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked());
 				Real inv_m2 = Real(inBody2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked());
-				anchor = (inv_m1 * inBody1.GetCenterOfMassPosition() + inv_m2 * inBody2.GetCenterOfMassPosition()) / (inv_m1 + inv_m2);
+				Real total_inv_mass = inv_m1 + inv_m2;
+				if (total_inv_mass != 0.0_r)
+					anchor = (inv_m1 * inBody1.GetCenterOfMassPosition() + inv_m2 * inBody2.GetCenterOfMassPosition()) / (inv_m1 + inv_m2);
+				else
+					anchor = inBody1.GetCenterOfMassPosition();
 			}
 
 			// Store local positions

Jolt/Physics/Constraints/SliderConstraint.cpp

@@ -15,6 +15,8 @@
 
 JPH_NAMESPACE_BEGIN
 
+using namespace literals;
+
 JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SliderConstraintSettings)
 {
 	JPH_ADD_BASE_CLASS(SliderConstraintSettings, TwoBodyConstraintSettings)
@@ -113,7 +115,11 @@ SliderConstraint::SliderConstraint(Body &inBody1, Body &inBody2, const SliderCon
 				// Otherwise use weighted anchor point towards the lightest body
 				Real inv_m1 = Real(inBody1.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked());
 				Real inv_m2 = Real(inBody2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked());
-				anchor = (inv_m1 * inBody1.GetCenterOfMassPosition() + inv_m2 * inBody2.GetCenterOfMassPosition()) / (inv_m1 + inv_m2);
+				Real total_inv_mass = inv_m1 + inv_m2;
+				if (total_inv_mass != 0.0_r)
+					anchor = (inv_m1 * inBody1.GetCenterOfMassPosition() + inv_m2 * inBody2.GetCenterOfMassPosition()) / total_inv_mass;
+				else
+					anchor = inBody1.GetCenterOfMassPosition();
 			}
 
 			// Store local positions

UnitTests/Math/Mat44Tests.cpp

@@ -218,6 +218,22 @@ TEST_SUITE("Mat44Tests")
 		CHECK(identity == Mat44::sIdentity());
 	}
 
+	TEST_CASE("TestMat44SetInversed3x3")
+	{
+		Mat44 mat(Vec4(1, 2, 0, 0), Vec4(4, 0, 8, 0), Vec4(0, 16, 0, 0), Vec4(1, 2, 3, 1));
+
+		// First test succeeding inverse
+		Mat44 inverse;
+		CHECK(inverse.SetInversed3x3(mat));
+		CHECK(inverse.GetColumn4(3) == Vec4(0, 0, 0, 1));
+		Mat44 identity = mat.Multiply3x3(inverse);
+		CHECK(identity == Mat44::sIdentity());
+
+		// Now make singular
+		mat(0, 0) = 0.0f;
+		CHECK(!inverse.SetInversed3x3(mat));
+	}
+
 	TEST_CASE("TestMat44GetDeterminant3x3")
 	{
 		Mat44 mat(Vec4(1, 2, 0, 0), Vec4(4, 0, 8, 0), Vec4(0, 16, 0, 0), Vec4(1, 2, 3, 1));