Add new asserts

src/body/CollisionBody.cpp

@@ -403,6 +403,8 @@ void CollisionBody::setTransform(const Transform& transform) {
     // Update the transform of the body
     mTransform = transform;
 
+    assert(mTransform.getOrientation().length() > decimal(0.7));
+
     // Update the broad-phase state of the body
     updateBroadPhaseState();
 

src/constraint/BallAndSocketJoint.cpp

@@ -225,5 +225,7 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
     x2 += v2;
     q2 += Quaternion(0, w2) * q2 * decimal(0.5);
     q2.normalize();
+    assert(q1.length() > decimal(0.7));
+    assert(q2.length() > decimal(0.7));
 }
 

src/constraint/FixedJoint.cpp

@@ -348,5 +348,8 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
     // Update the body position/orientation of body 2
     q2 += Quaternion(0, w2) * q2 * decimal(0.5);
     q2.normalize();
+
+    assert(q1.length() > decimal(0.7));
+    assert(q2.length() > decimal(0.7));
 }
 

src/constraint/HingeJoint.cpp

@@ -603,6 +603,9 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
             q2.normalize();
         }
     }
+
+    assert(q1.length() > decimal(0.7));
+    assert(q2.length() > decimal(0.7));
 }
 
 

src/constraint/SliderJoint.cpp

@@ -676,6 +676,9 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
             q2.normalize();
         }
     }
+
+    assert(q1.length() > decimal(0.7));
+    assert(q2.length() > decimal(0.7));
 }
 
 // Enable/Disable the limits of the joint

src/engine/DynamicsWorld.cpp

@@ -192,6 +192,7 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
             mConstrainedOrientations[indexArray] = currentOrientation +
                                                    Quaternion(0, newAngVelocity) *
                                                    currentOrientation * decimal(0.5) * mTimeStep;
+            assert(mConstrainedOrientations[indexArray].length() > decimal(0.7));
         }
     }
 }