Fix issues and add conversion from Euler angles to Quaternion

examples/collisionshapes/Scene.h

@@ -40,13 +40,13 @@
 #include "../common/Viewer.h"
 
 // Constants
-const int NB_BOXES = 4;
+const int NB_BOXES = 2;
-const int NB_SPHERES = 2;
+const int NB_SPHERES = 3;
-const int NB_CONES = 3;
+const int NB_CONES = 1;
-const int NB_CYLINDERS = 1;
+const int NB_CYLINDERS = 2;
 const int NB_CAPSULES = 1;
-const int NB_MESHES = 2;
+const int NB_MESHES = 3;
-const int NB_COMPOUND_SHAPES = 3;
+const int NB_COMPOUND_SHAPES = 1;
 const openglframework::Vector3 BOX_SIZE(2, 2, 2);
 const float SPHERE_RADIUS = 1.5f;
 const float CONE_RADIUS = 2.0f;

examples/common/Dumbbell.cpp

@@ -61,7 +61,8 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position,
 
     // Initial position and orientation of the rigid body
     rp3d::Vector3 initPosition(position.x, position.y, position.z);
-    rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
+    rp3d::decimal angleAroundX = rp3d::PI / 4;
+    rp3d::Quaternion initOrientation(angleAroundX, 0, 0);
     rp3d::Transform transformBody(initPosition, initOrientation);
 
     // Initial transform of the first sphere collision shape of the dumbbell (in local-space)
@@ -77,8 +78,8 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position,
     mRigidBody = dynamicsWorld->createRigidBody(transformBody);
 
     // Add the three collision shapes to the body and specify the mass and transform of the shapes
-    mRigidBody->addCollisionShape(sphereCollisionShape, massSphere, transformSphereShape1);
+    mRigidBody->addCollisionShape(sphereCollisionShape, 1, transformSphereShape1);
-    mRigidBody->addCollisionShape(sphereCollisionShape, massSphere, transformSphereShape2);
+    mRigidBody->addCollisionShape(sphereCollisionShape, 4, transformSphereShape2);
     mRigidBody->addCollisionShape(cylinderCollisionShape, massCylinder, transformCylinderShape);
 }
 

src/body/CollisionBody.cpp

@@ -158,7 +158,7 @@ void CollisionBody::removeAllCollisionShapes() {
 }
 
 // Reset the contact manifold lists
-void CollisionBody::resetContactManifoldsList(MemoryAllocator& memoryAllocator) {
+void CollisionBody::resetContactManifoldsList() {
 
     // Delete the linked list of contact manifolds of that body
     ContactManifoldListElement* currentElement = mContactManifoldsList;
@@ -167,13 +167,11 @@ void CollisionBody::resetContactManifoldsList(MemoryAllocator& memoryAllocator)
 
         // Delete the current element
         currentElement->ContactManifoldListElement::~ContactManifoldListElement();
-        memoryAllocator.release(currentElement, sizeof(ContactManifoldListElement));
+        mWorld.mMemoryAllocator.release(currentElement, sizeof(ContactManifoldListElement));
 
         currentElement = nextElement;
     }
     mContactManifoldsList = NULL;
-
-    assert(mContactManifoldsList == NULL);
 }
 
 // Update the broad-phase state for this body (because it has moved for instance)
@@ -190,3 +188,14 @@ void CollisionBody::updateBroadPhaseState() const {
         mWorld.mCollisionDetection.updateProxyCollisionShape(shape, aabb);
     }
 }
+
+// Ask the broad-phase to test again the collision shapes of the body for collision
+// (as if the body has moved).
+void CollisionBody::askForBroadPhaseCollisionCheck() const {
+
+    // For all the proxy collision shapes of the body
+    for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
+
+        mWorld.mCollisionDetection.askForBroadPhaseCollisionCheck(shape);
+    }
+}

src/body/CollisionBody.h

@@ -102,7 +102,7 @@ class CollisionBody : public Body {
         CollisionBody& operator=(const CollisionBody& body);
 
         /// Reset the contact manifold lists
-        void resetContactManifoldsList(MemoryAllocator& memoryAllocator);
+        void resetContactManifoldsList();
 
         /// Remove all the collision shapes
         void removeAllCollisionShapes();
@@ -113,6 +113,10 @@ class CollisionBody : public Body {
         /// Update the broad-phase state for this body (because it has moved for instance)
         void updateBroadPhaseState() const;
 
+        /// Ask the broad-phase to test again the collision shapes of the body for collision
+        /// (as if the body has moved).
+        void askForBroadPhaseCollisionCheck() const;
+
     public :
 
         // -------------------- Methods -------------------- //

src/body/RigidBody.cpp

@@ -82,6 +82,17 @@ void RigidBody::setType(BodyType type) {
 
     // Awake the body
     setIsSleeping(false);
+
+    // Remove all the contacts with this body
+    resetContactManifoldsList();
+
+    // Ask the broad-phase to test again the collision shapes of the body for collision
+    // detection (as if the body has moved)
+    askForBroadPhaseCollisionCheck();
+
+    // Reset the force and torque on the body
+    mExternalForce.setToZero();
+    mExternalTorque.setToZero();
 }
 
 // Set the local inertia tensor of the body (in body coordinates)

src/collision/CollisionDetection.h

@@ -135,6 +135,9 @@ class CollisionDetection {
         /// Remove a pair of bodies that cannot collide with each other
         void removeNoCollisionPair(CollisionBody* body1, CollisionBody* body2);
 
+        /// Ask for a collision shape to be tested again during broad-phase.
+        void askForBroadPhaseCollisionCheck(ProxyShape* shape);
+
         /// Compute the collision detection
         void computeCollisionDetection();
 
@@ -183,6 +186,13 @@ inline void CollisionDetection::removeNoCollisionPair(CollisionBody* body1,
     mNoCollisionPairs.erase(OverlappingPair::computeBodiesIndexPair(body1, body2));
 }
 
+// Ask for a collision shape to be tested again during broad-phase.
+/// We simply put the shape in the list of collision shape that have moved in the
+/// previous frame so that it is tested for collision again in the broad-phase.
+inline void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape) {
+    mBroadPhaseAlgorithm.addMovedCollisionShape(shape->mBroadPhaseID);
+}
+
 // Update a proxy collision shape (that has moved for instance)
 inline void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb) {
     mBroadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb);

src/engine/DynamicsWorld.cpp

@@ -36,12 +36,6 @@ using namespace std;
 
 // TODO : Check if we really need to store the contact manifolds also in mContactManifolds.
 
-// TODO : Check how to compute the initial inertia tensor now (especially when a body has multiple
-//        collision shapes.
-
-// TODO : Check the Body::setType() function in Box2D to be sure we are not missing any update
-// of the collision shapes and broad-phase modification.
-
 // Constructor
 DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP)
               : CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityEnabled(true),
@@ -194,7 +188,7 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
             }
 
             // Get current position and orientation of the body
-            const Vector3& currentPosition = bodies[b]->getTransform().getPosition();
+            const Vector3& currentPosition = bodies[b]->mCenterOfMassWorld;
             const Quaternion& currentOrientation = bodies[b]->getTransform().getOrientation();
 
             // Update the new constrained position and orientation of the body
@@ -202,6 +196,10 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
             mConstrainedOrientations[indexArray] = currentOrientation +
                                                    Quaternion(0, newAngVelocity) *
                                                    currentOrientation * decimal(0.5) * dt;
+
+            // TODO : DELETE THIS
+            Vector3 newPos = mConstrainedPositions[indexArray];
+            Quaternion newOrientation = mConstrainedOrientations[indexArray];
         }
     }
 }
@@ -221,6 +219,10 @@ void DynamicsWorld::updateBodiesState() {
 
             uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
 
+            // TODO : Delete this
+            Vector3 linVel = mConstrainedLinearVelocities[index];
+            Vector3 angVel = mConstrainedAngularVelocities[index];
+
             // Update the linear and angular velocity of the body
             bodies[b]->mLinearVelocity = mConstrainedLinearVelocities[index];
             bodies[b]->mAngularVelocity = mConstrainedAngularVelocities[index];
@@ -228,6 +230,9 @@ void DynamicsWorld::updateBodiesState() {
             // Update the position of the center of mass of the body
             bodies[b]->mCenterOfMassWorld = mConstrainedPositions[index];
 
+            // TODO : DELETE THIS
+            Quaternion newOrient = mConstrainedOrientations[index];
+
             // Update the orientation of the body
             bodies[b]->mTransform.setOrientation(mConstrainedOrientations[index].getUnit());
 
@@ -450,23 +455,9 @@ void DynamicsWorld::solvePositionCorrection() {
     // Do not continue if there is no constraints
     if (mJoints.empty()) return;
 
-    // ---------- Get the position/orientation of the rigid bodies ---------- //
-
     // For each island of the world
     for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
 
-        // For each body of the island
-        RigidBody** bodies = mIslands[islandIndex]->getBodies();
-        for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
-
-            uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
-
-            // Get the position/orientation of the rigid body
-            const Transform& transform = bodies[b]->getTransform();
-            mConstrainedPositions[index] = bodies[b]->mCenterOfMassWorld;
-            mConstrainedOrientations[index]= transform.getOrientation();
-        }
-
         // ---------- Solve the position error correction for the constraints ---------- //
 
         // For each iteration of the position (error correction) solver
@@ -516,7 +507,7 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
     }
 
     // Reset the contact manifold list of the body
-    rigidBody->resetContactManifoldsList(mMemoryAllocator);
+    rigidBody->resetContactManifoldsList();
 
     // Call the destructor of the rigid body
     rigidBody->RigidBody::~RigidBody();
@@ -655,7 +646,7 @@ void DynamicsWorld::resetContactManifoldListsOfBodies() {
     for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
 
         // Reset the contact manifold list of the body
-        (*it)->resetContactManifoldsList(mMemoryAllocator);
+        (*it)->resetContactManifoldsList();
     }
 }
 

src/mathematics/Quaternion.cpp

@@ -47,6 +47,16 @@ Quaternion::Quaternion(decimal newW, const Vector3& v) : x(v.x), y(v.y), z(v.z),
 
 }
 
+// Constructor which convert Euler angles (in radians) to a quaternion
+Quaternion::Quaternion(decimal angleX, decimal angleY, decimal angleZ) {
+    initWithEulerAngles(angleX, angleY, angleZ);
+}
+
+// Constructor which convert Euler angles (in radians) to a quaternion
+Quaternion::Quaternion(const Vector3& eulerAngles) {
+    initWithEulerAngles(eulerAngles.x, eulerAngles.y, eulerAngles.z);
+}
+
 // Copy-constructor
 Quaternion::Quaternion(const Quaternion& quaternion)
            :x(quaternion.x), y(quaternion.y), z(quaternion.z), w(quaternion.w) {
@@ -219,3 +229,32 @@ Quaternion Quaternion::slerp(const Quaternion& quaternion1,
     // Compute and return the interpolated quaternion
     return quaternion1 * coeff1 + quaternion2 * coeff2;
 }
+
+// Initialize the quaternion using Euler angles
+void Quaternion::initWithEulerAngles(decimal angleX, decimal angleY, decimal angleZ) {
+
+    decimal angle = angleX * decimal(0.5);
+    const decimal sinX = std::sin(angle);
+    const decimal cosX = std::cos(angle);
+
+    angle = angleY * decimal(0.5);
+    const decimal sinY = std::sin(angle);
+    const decimal cosY = std::cos(angle);
+
+    angle = angleZ * decimal(0.5);
+    const decimal sinZ = std::sin(angle);
+    const decimal cosZ = std::cos(angle);
+
+    const decimal cosYcosZ = cosY * cosZ;
+    const decimal sinYcosZ = sinY * cosZ;
+    const decimal cosYsinZ = cosY * sinZ;
+    const decimal sinYsinZ = sinY * sinZ;
+
+    x = sinX * cosYcosZ - cosX * sinYsinZ;
+    y = cosX * sinYcosZ + sinX * cosYsinZ;
+    z = cosX * cosYsinZ - sinX * sinYcosZ;
+    w = cosX * cosYcosZ + sinX * sinYsinZ;
+
+    // Normalize the quaternion
+    normalize();
+}

src/mathematics/Quaternion.h

@@ -69,6 +69,12 @@ struct Quaternion {
         /// Constructor with the component w and the vector v=(x y z)
         Quaternion(decimal newW, const Vector3& v);
 
+        /// Constructor which convert Euler angles (in radians) to a quaternion
+        Quaternion(decimal angleX, decimal angleY, decimal angleZ);
+
+        /// Constructor which convert Euler angles (in radians) to a quaternion
+        Quaternion(const Vector3& eulerAngles);
+
         /// Copy-constructor
         Quaternion(const Quaternion& quaternion);
 
@@ -153,6 +159,11 @@ struct Quaternion {
 
         /// Overloaded operator for equality condition
         bool operator==(const Quaternion& quaternion) const;
+
+    private:
+
+        /// Initialize the quaternion using Euler angles
+        void initWithEulerAngles(decimal angleX, decimal angleY, decimal angleZ);
 };
 
 /// Set all the values

test/Test.h

@@ -90,7 +90,7 @@ class Test {
         Test(std::ostream* stream = &std::cout);
 
         /// Destructor
-        ~Test();
+        virtual ~Test();
 
         /// Return the number of passed tests
         long getNbPassedTests() const;

test/tests/mathematics/TestQuaternion.h

@@ -77,7 +77,7 @@ class TestQuaternion : public Test {
         void testConstructors() {
 
             Quaternion quaternion1(mQuaternion1);
-            test(mQuaternion1== quaternion1);
+            test(mQuaternion1 == quaternion1);
 
             Quaternion quaternion2(4, 5, 6, 7);
             test(quaternion2 == Quaternion(4, 5, 6, 7));
@@ -90,6 +90,34 @@ class TestQuaternion : public Test {
             test(approxEqual(quaternion4.y, mQuaternion1.y));
             test(approxEqual(quaternion4.z, mQuaternion1.z));
             test(approxEqual(quaternion4.w, mQuaternion1.w));
+
+            // Test conversion from Euler angles to quaternion
+
+            const decimal PI_OVER_2 = PI * decimal(0.5);
+            const decimal PI_OVER_4 = PI_OVER_2 * decimal(0.5);
+            Quaternion quaternion5(PI_OVER_2, 0, 0);
+            Quaternion quaternionTest5(std::sin(PI_OVER_4), 0, 0, std::cos(PI_OVER_4));
+            quaternionTest5.normalize();
+            test(approxEqual(quaternion5.x, quaternionTest5.x));
+            test(approxEqual(quaternion5.y, quaternionTest5.y));
+            test(approxEqual(quaternion5.z, quaternionTest5.z));
+            test(approxEqual(quaternion5.w, quaternionTest5.w));
+
+            Quaternion quaternion6(0, PI_OVER_2, 0);
+            Quaternion quaternionTest6(0, std::sin(PI_OVER_4), 0, std::cos(PI_OVER_4));
+            quaternionTest6.normalize();
+            test(approxEqual(quaternion6.x, quaternionTest6.x));
+            test(approxEqual(quaternion6.y, quaternionTest6.y));
+            test(approxEqual(quaternion6.z, quaternionTest6.z));
+            test(approxEqual(quaternion6.w, quaternionTest6.w));
+
+            Quaternion quaternion7(Vector3(0, 0, PI_OVER_2));
+            Quaternion quaternionTest7(0, 0, std::sin(PI_OVER_4), std::cos(PI_OVER_4));
+            quaternionTest7.normalize();
+            test(approxEqual(quaternion7.x, quaternionTest7.x));
+            test(approxEqual(quaternion7.y, quaternionTest7.y));
+            test(approxEqual(quaternion7.z, quaternionTest7.z));
+            test(approxEqual(quaternion7.w, quaternionTest7.w));
         }
 
         /// Test unit, length, normalize methods