From 4ef690018cc81689dfd21bdfcc213243ac0d7341 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 31 Jan 2019 07:23:43 +0100
Subject: [PATCH] Add new asserts

---
 src/body/CollisionBody.cpp            | 2 ++
 src/constraint/BallAndSocketJoint.cpp | 2 ++
 src/constraint/FixedJoint.cpp         | 3 +++
 src/constraint/HingeJoint.cpp         | 3 +++
 src/constraint/SliderJoint.cpp        | 3 +++
 src/engine/DynamicsWorld.cpp          | 1 +
 6 files changed, 14 insertions(+)

diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp
index 855869d5f..b94ce3519 100644
--- a/src/body/CollisionBody.cpp
+++ b/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();
 
diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp
index 0da681856..bddcaeaed 100644
--- a/src/constraint/BallAndSocketJoint.cpp
+++ b/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));
 }
 
diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp
index 99fca6c27..79f5c816c 100644
--- a/src/constraint/FixedJoint.cpp
+++ b/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));
 }
 
diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp
index 50106d13d..d249f68b9 100644
--- a/src/constraint/HingeJoint.cpp
+++ b/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));
 }
 
 
diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp
index b01fb4105..3c7070cf2 100644
--- a/src/constraint/SliderJoint.cpp
+++ b/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
diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp
index 4ed879de3..b721a095a 100644
--- a/src/engine/DynamicsWorld.cpp
+++ b/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));
         }
     }
 }