Add unit tests for the RigidBody class and refactor methods to apply forces and torques to rigid bodies

CHANGELOG.md

@@ -15,9 +15,12 @@ do not hesitate to take a look at the user manual.
  - Method Joint::getReactionTorque() has been added to retrieve the current reaction torque of a joint
  - Method RigidBody::setLinearLockAxisFactor() to lock the translational movement of a body along the world-space x, y and z axes
  - Method RigidBody::setAngularLockAxisFactor() to lock the rotational movement of a body around the world-space x, y and z axes
- - Method RigidBody::applyLocalForceAtWorldPosition()
+ - Method RigidBody::applyLocalForceAtWorldPosition() to manually apply a force to a rigid body
- - Method RigidBody::applyLocalForceAtLocalPosition()
+ - Method RigidBody::applyLocalForceAtLocalPosition() to manually apply a force to a rigid body
- - Method RigidBody::applyLocalForceToCenterOfMass()
+ - Method RigidBody::applyLocalForceToCenterOfMass() to manually apply a force to a rigid body
+ - Method RigidBody::applyLocalTorque() to apply a local-space torque to a rigid body
+ - Method RigidBody::getForce() to get the total manually applied force on a rigid body
+ - Method RigidBody::getTorque() to get the total manually applied torque on a rigid body
  - A cone limit can now be set to the ball-and-socket joint (this is useful for ragdolls)
  - Bridge scene has been added to the testbed application
  - Ragdoll scene has been added to the testbed application
@@ -32,6 +35,7 @@ do not hesitate to take a look at the user manual.
  - Rename method RigidBody::applyForceAtWorldPosition() into RigidBody::applyWorldForceAtWorldPosition()
  - Rename method RigidBody::applyForceAtLocalPosition() into RigidBody::applyWorldForceAtLocalPosition()
  - Rename method RigidBody::applyForceToCenterOfMass() into RigidBody::applyWorldForceAtCenterOfMass()
+ - Rename method RigidBody::applyTorque() into RigidBody::applyWorldTorque()
  - The raycasting broad-phase performance has been improved
  - The raycasting performance against HeighFieldShape has been improved (better middle-phase algorithm)
  - Robustness of polyhedron vs polyhedron collision detection has been improved in SAT algorithm (face contacts are favored over edge-edge contacts for better stability)

include/reactphysics3d/body/RigidBody.h

@@ -165,33 +165,42 @@ class RigidBody : public CollisionBody {
         /// Set the lock rotation factor
         void setAngularLockAxisFactor(const Vector3& angularLockAxisFactor) const;
 
-        /// Apply an external force (in local-space) to the body at its center of mass.
+        /// Manually apply an external force (in local-space) to the body at its center of mass.
         void applyLocalForceAtCenterOfMass(const Vector3& force);
 
-        /// Apply an external force (in world-space) to the body at its center of mass.
+        /// Manually apply an external force (in world-space) to the body at its center of mass.
         void applyWorldForceAtCenterOfMass(const Vector3& force);
 
-        /// Apply an external force (in local-space) to the body at a given point (in local-space).
+        /// Manually apply an external force (in local-space) to the body at a given point (in local-space).
         void applyLocalForceAtLocalPosition(const Vector3& force, const Vector3& point);
 
-        /// Apply an external force (in world-space) to the body at a given point (in local-space).
+        /// Manually apply an external force (in world-space) to the body at a given point (in local-space).
         void applyWorldForceAtLocalPosition(const Vector3& force, const Vector3& point);
 
-        /// Apply an external force (in local-space) to the body at a given point (in world-space).
+        /// Manually apply an external force (in local-space) to the body at a given point (in world-space).
         void applyLocalForceAtWorldPosition(const Vector3& force, const Vector3& point);
 
-        /// Apply an external force (in world-space) to the body at a given point (in world-space).
+        /// Manually apply an external force (in world-space) to the body at a given point (in world-space).
         void applyWorldForceAtWorldPosition(const Vector3& force, const Vector3& point);
 
-        /// Apply an external torque to the body.
+        /// Manually apply an external torque to the body (in world-space).
-        void applyTorque(const Vector3& torque);
+        void applyWorldTorque(const Vector3& torque);
 
-        /// Reset the accumulated force to zero
+        /// Manually apply an external torque to the body (in local-space).
+        void applyLocalTorque(const Vector3& torque);
+
+        /// Reset the manually applied force to zero
         void resetForce();
 
-        /// Reset the accumulated torque to zero
+        /// Reset the manually applied torque to zero
         void resetTorque();
 
+        /// Return the total manually applied force on the body (in world-space)
+        const Vector3& getForce() const;
+
+        /// Return the total manually applied torque on the body (in world-space)
+        const Vector3& getTorque() const;
+
         /// Return whether or not the body is allowed to sleep
         bool isAllowedToSleep() const;
 

include/reactphysics3d/mathematics/Vector2.h

@@ -148,9 +148,6 @@ struct Vector2 {
         /// Return the zero vector
         static Vector2 zero();
 
-        /// Function to test if two vectors are (almost) equal
-        static bool approxEqual(const Vector2& vec1, const Vector2& vec2, decimal epsilon = MACHINE_EPSILON);
-
         // -------------------- Friends -------------------- //
 
         friend Vector2 operator+(const Vector2& vector1, const Vector2& vector2);
@@ -375,7 +372,7 @@ RP3D_FORCE_INLINE Vector2 Vector2::zero() {
 }
 
 // Function to test if two vectors are (almost) equal
-RP3D_FORCE_INLINE bool approxEqual(const Vector2& vec1, const Vector2& vec2, decimal epsilon) {
+RP3D_FORCE_INLINE bool approxEqual(const Vector2& vec1, const Vector2& vec2, decimal epsilon = MACHINE_EPSILON) {
     return approxEqual(vec1.x, vec2.x, epsilon) && approxEqual(vec1.y, vec2.y, epsilon);
 }
 

include/reactphysics3d/mathematics/Vector3.h

@@ -163,9 +163,6 @@ struct Vector3 {
         /// Return the zero vector
         static Vector3 zero();
 
-        /// Function to test if two vectors are (almost) equal
-        static bool approxEqual(const Vector3& vec1, const Vector3& vec2, decimal epsilon = MACHINE_EPSILON);
-
         // -------------------- Friends -------------------- //
 
         friend Vector3 operator+(const Vector3& vector1, const Vector3& vector2);
@@ -417,7 +414,7 @@ RP3D_FORCE_INLINE Vector3 Vector3::zero() {
 }
 
 // Function to test if two vectors are (almost) equal
-RP3D_FORCE_INLINE bool approxEqual(const Vector3& vec1, const Vector3& vec2, decimal epsilon) {
+RP3D_FORCE_INLINE bool approxEqual(const Vector3& vec1, const Vector3& vec2, decimal epsilon = MACHINE_EPSILON) {
     return approxEqual(vec1.x, vec2.x, epsilon) && approxEqual(vec1.y, vec2.y, epsilon) &&
            approxEqual(vec1.z, vec2.z, epsilon);
 }

src/body/RigidBody.cpp

@@ -127,7 +127,7 @@ decimal RigidBody::getMass() const {
     return mWorld.mRigidBodyComponents.getMass(mEntity);
 }
 
-// Apply an external force (in local-space) to the body at a given point (in local-space).
+// Manually apply an external force (in local-space) to the body at a given point (in local-space).
 /// If the point is not at the center of mass of the body, it will also
 /// generate some torque and therefore, change the angular velocity of the body.
 /// If the body is sleeping, calling this method will wake it up. Note that the
@@ -146,7 +146,7 @@ void RigidBody::applyLocalForceAtLocalPosition(const Vector3& force, const Vecto
     applyWorldForceAtLocalPosition(worldForce, point);
 }
 
-// Apply an external force (in world-space) to the body at a given point (in local-space).
+// Manually apply an external force (in world-space) to the body at a given point (in local-space).
 /// If the point is not at the center of mass of the body, it will also
 /// generate some torque and therefore, change the angular velocity of the body.
 /// If the body is sleeping, calling this method will wake it up. Note that the
@@ -178,7 +178,7 @@ void RigidBody::applyWorldForceAtLocalPosition(const Vector3& force, const Vecto
     mWorld.mRigidBodyComponents.setExternalTorque(mEntity, externalTorque + (worldPoint - centerOfMassWorld).cross(force));
 }
 
-// Apply an external force (in local-space) to the body at a given point (in world-space).
+// Manually apply an external force (in local-space) to the body at a given point (in world-space).
 /// If the point is not at the center of mass of the body, it will also
 /// generate some torque and therefore, change the angular velocity of the body.
 /// If the body is sleeping, calling this method will wake it up. Note that the
@@ -197,7 +197,7 @@ void RigidBody::applyLocalForceAtWorldPosition(const Vector3& force, const Vecto
     applyWorldForceAtWorldPosition(worldForce, point);
 }
 
-// Apply an external force (in world-space) to the body at a given point (in world-space).
+// Manually apply an external force (in world-space) to the body at a given point (in world-space).
 /// If the point is not at the center of mass of the body, it will also
 /// generate some torque and therefore, change the angular velocity of the body.
 /// If the body is sleeping, calling this method will wake it up. Note that the
@@ -261,7 +261,7 @@ void RigidBody::setLocalInertiaTensor(const Vector3& inertiaTensorLocal) {
              "Body " + std::to_string(mEntity.id) + ": Set inertiaTensorLocal=" + inertiaTensorLocal.to_string(),  __FILE__, __LINE__);
 }
 
-// Apply an external force (in local-space) to the body at its center of mass.
+// Manually apply an external force (in local-space) to the body at its center of mass.
 /// If the body is sleeping, calling this method will wake it up. Note that the
 /// force will we added to the sum of the applied forces and that this sum will be
 /// reset to zero at the end of each call of the PhyscisWorld::update() method.
@@ -277,7 +277,7 @@ void RigidBody::applyLocalForceAtCenterOfMass(const Vector3& force) {
     applyWorldForceAtCenterOfMass(worldForce);
 }
 
-// Apply an external force (in world-space) to the body at its center of mass.
+// Manually apply an external force (in world-space) to the body at its center of mass.
 /// If the body is sleeping, calling this method will wake it up. Note that the
 /// force will we added to the sum of the applied forces and that this sum will be
 /// reset to zero at the end of each call of the PhyscisWorld::update() method.
@@ -885,15 +885,15 @@ void RigidBody::setAngularLockAxisFactor(const Vector3& angularLockAxisFactor) c
     mWorld.mRigidBodyComponents.setAngularLockAxisFactor(mEntity, angularLockAxisFactor);
 }
 
-// Apply an external torque to the body.
+// Manually apply an external torque to the body (in world-space).
 /// If the body is sleeping, calling this method will wake it up. Note that the
 /// force will we added to the sum of the applied torques and that this sum will be
 /// reset to zero at the end of each call of the PhyscisWorld::update() method.
 /// You can only apply a force to a dynamic body otherwise, this method will do nothing.
 /**
- * @param torque The external torque to apply on the body
+ * @param torque The external torque to apply on the body (in world-space)
  */
-void RigidBody::applyTorque(const Vector3& torque) {
+void RigidBody::applyWorldTorque(const Vector3& torque) {
 
     // If it is not a dynamic body, we do nothing
     if (mWorld.mRigidBodyComponents.getBodyType(mEntity) != BodyType::DYNAMIC) return;
@@ -908,6 +908,22 @@ void RigidBody::applyTorque(const Vector3& torque) {
     mWorld.mRigidBodyComponents.setExternalTorque(mEntity, externalTorque + torque);
 }
 
+// Manually apply an external torque to the body (in local-space).
+/// If the body is sleeping, calling this method will wake it up. Note that the
+/// force will we added to the sum of the applied torques and that this sum will be
+/// reset to zero at the end of each call of the PhyscisWorld::update() method.
+/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
+/**
+ * @param torque The external torque to apply on the body (in local-space)
+ */
+void RigidBody::applyLocalTorque(const Vector3& torque) {
+
+    // Convert the local-space torque to world-space
+    const Vector3 worldTorque = mWorld.mTransformComponents.getTransform(mEntity).getOrientation() * torque;
+
+    applyWorldTorque(worldTorque);
+}
+
 // Reset the accumulated force to zero
 void RigidBody::resetForce() {
 
@@ -928,6 +944,16 @@ void RigidBody::resetTorque() {
     mWorld.mRigidBodyComponents.setExternalTorque(mEntity, Vector3(0, 0, 0));
 }
 
+// Return the total manually applied force on the body (in world-space)
+const Vector3& RigidBody::getForce() const {
+    return mWorld.mRigidBodyComponents.getExternalForce(mEntity);
+}
+
+// Return the total manually applied torque on the body (in world-space)
+const Vector3& RigidBody::getTorque() const {
+    return mWorld.mRigidBodyComponents.getExternalTorque(mEntity);
+}
+
 // Set the variable to know whether or not the body is sleeping
 void RigidBody::setIsSleeping(bool isSleeping) {
 

test/CMakeLists.txt

@@ -27,6 +27,7 @@ set (RP3D_TESTS_HEADERS
     "tests/mathematics/TestTransform.h"
     "tests/mathematics/TestVector2.h"
     "tests/mathematics/TestVector3.h"
+    "tests/engine/TestRigidBody.h"
 )
 
 # Source files

test/main.cpp

@@ -44,6 +44,7 @@
 #include "tests/containers/TestSet.h"
 #include "tests/containers/TestDeque.h"
 #include "tests/containers/TestStack.h"
+#include "tests/engine/TestRigidBody.h"
 
 using namespace reactphysics3d;
 
@@ -79,6 +80,11 @@ int main() {
     testSuite.addTest(new TestDynamicAABBTree("DynamicAABBTree"));
     testSuite.addTest(new TestHalfEdgeStructure("HalfEdgeStructure"));
 
+
+    // ---------- Engine tests ---------- //
+
+    testSuite.addTest(new TestRigidBody("RigidBody"));
+
     // Run the tests
     testSuite.run();
 

test/tests/collision/TestCollisionWorld.h

@@ -2432,7 +2432,7 @@ class TestCollisionWorld : public Test {
             Transform transform1(Vector3(10, 22, 50), Quaternion::identity());
             Transform transform2(Vector3(10, 20, 50), Quaternion::identity());
 
-            // Move spheres to collide with each other
+            // Move bodies to collide with each other
             mBoxBody1->setTransform(transform1);
             mConcaveMeshBody->setTransform(transform2);
 
@@ -2454,7 +2454,10 @@ class TestCollisionWorld : public Test {
             // Get collision data
             const CollisionData* collisionData = mCollisionCallback.getCollisionData(mBoxCollider1, mConcaveMeshCollider);
             rp3d_test(collisionData != nullptr);
+            auto test = collisionData->getNbContactPairs();
             rp3d_test(collisionData->getNbContactPairs() == 1);
+            auto test1 = collisionData->getTotalNbContactPoints();
+
             rp3d_test(collisionData->getTotalNbContactPoints() == 4);
 
             for (size_t i=0; i<collisionData->contactPairs[0].contactPoints.size(); i++) {

test/tests/engine/TestRigidBody.h

@@ -0,0 +1,316 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef TEST_RIGIDBODY_H
+#define TEST_RIGIDBODY_H
+
+// Libraries
+#include <reactphysics3d/reactphysics3d.h>
+
+/// Reactphysics3D namespace
+namespace reactphysics3d {
+
+// Class TestRigidBody
+/**
+ * Unit test for the RigidBody class.
+ */
+class TestRigidBody : public Test {
+
+    private :
+
+        // ---------- Atributes ---------- //
+
+        PhysicsCommon mPhysicsCommon;
+        PhysicsWorld* mWorld;
+
+        RigidBody* mRigidBody1;
+        RigidBody* mRigidBody2Box;
+        RigidBody* mRigidBody2Sphere;
+        RigidBody* mRigidBody2Convex;
+        RigidBody* mRigidBody3;
+
+        Collider* mBoxCollider;
+        Collider* mSphereCollider;
+        Collider* mConvexMeshCollider;
+
+        PolygonVertexArray* mConvexMeshPolygonVertexArray;
+        PolyhedronMesh* mConvexMeshPolyhedronMesh;
+        PolygonVertexArray::PolygonFace* mConvexMeshPolygonFaces;
+        TriangleVertexArray* mConcaveMeshTriangleVertexArray;
+        Vector3 mConvexMeshCubeVertices[8];
+        int mConvexMeshCubeIndices[24];
+
+    public :
+
+        // ---------- Methods ---------- //
+
+        /// Constructor
+        TestRigidBody(const std::string& name) : Test(name) {
+
+            mWorld = mPhysicsCommon.createPhysicsWorld();
+
+            const Transform transform1(Vector3(1, 2, 3), Quaternion::identity());
+            mRigidBody1 = mWorld->createRigidBody(transform1);
+
+            //  Box
+
+            const Transform transform2(Vector3(0, 0, 0), Quaternion::identity());
+            mRigidBody2Box = mWorld->createRigidBody(transform2);
+            BoxShape* boxShape = mPhysicsCommon.createBoxShape(Vector3(2, 2, 2));
+            mBoxCollider = mRigidBody2Box->addCollider(boxShape, Transform::identity());
+
+            //  Sphere
+
+            mRigidBody2Sphere = mWorld->createRigidBody(transform2);
+            SphereShape* sphereShape = mPhysicsCommon.createSphereShape(4);
+            mSphereCollider = mRigidBody2Sphere->addCollider(sphereShape, Transform::identity());
+
+            //  Convex Meshes (in the shape of a box)
+
+            mConvexMeshCubeVertices[0] = Vector3(-3, -3, 3);
+            mConvexMeshCubeVertices[1] = Vector3(3, -3, 3);
+            mConvexMeshCubeVertices[2] = Vector3(3, -3, -3);
+            mConvexMeshCubeVertices[3] = Vector3(-3, -3, -3);
+            mConvexMeshCubeVertices[4] = Vector3(-3, 3, 3);
+            mConvexMeshCubeVertices[5] = Vector3(3, 3, 3);
+            mConvexMeshCubeVertices[6] = Vector3(3, 3, -3);
+            mConvexMeshCubeVertices[7] = Vector3(-3, 3, -3);
+
+            mConvexMeshCubeIndices[0] = 0; mConvexMeshCubeIndices[1] = 3; mConvexMeshCubeIndices[2] = 2; mConvexMeshCubeIndices[3] = 1;
+            mConvexMeshCubeIndices[4] = 4; mConvexMeshCubeIndices[5] = 5; mConvexMeshCubeIndices[6] = 6; mConvexMeshCubeIndices[7] = 7;
+            mConvexMeshCubeIndices[8] = 0; mConvexMeshCubeIndices[9] = 1; mConvexMeshCubeIndices[10] = 5; mConvexMeshCubeIndices[11] = 4;
+            mConvexMeshCubeIndices[12] = 1; mConvexMeshCubeIndices[13] = 2; mConvexMeshCubeIndices[14] = 6; mConvexMeshCubeIndices[15] = 5;
+            mConvexMeshCubeIndices[16] = 2; mConvexMeshCubeIndices[17] = 3; mConvexMeshCubeIndices[18] = 7; mConvexMeshCubeIndices[19] = 6;
+            mConvexMeshCubeIndices[20] = 0; mConvexMeshCubeIndices[21] = 4; mConvexMeshCubeIndices[22] = 7; mConvexMeshCubeIndices[23] = 3;
+
+            mConvexMeshPolygonFaces = new rp3d::PolygonVertexArray::PolygonFace[6];
+            rp3d::PolygonVertexArray::PolygonFace* face = mConvexMeshPolygonFaces;
+            for (int f = 0; f < 6; f++) {
+                face->indexBase = f * 4;
+                face->nbVertices = 4;
+                face++;
+            }
+            mConvexMeshPolygonVertexArray = new rp3d::PolygonVertexArray(8, &(mConvexMeshCubeVertices[0]), sizeof(Vector3),
+                    &(mConvexMeshCubeIndices[0]), sizeof(int), 6, mConvexMeshPolygonFaces,
+                    rp3d::PolygonVertexArray::VertexDataType::VERTEX_FLOAT_TYPE,
+                    rp3d::PolygonVertexArray::IndexDataType::INDEX_INTEGER_TYPE);
+            mConvexMeshPolyhedronMesh = mPhysicsCommon.createPolyhedronMesh(mConvexMeshPolygonVertexArray);
+            ConvexMeshShape* convexMeshShape = mPhysicsCommon.createConvexMeshShape(mConvexMeshPolyhedronMesh);
+            Transform transform3(Vector3(10, 0, 0), Quaternion::identity());
+            mRigidBody2Convex = mWorld->createRigidBody(transform3);
+            mConvexMeshCollider = mRigidBody2Convex->addCollider(convexMeshShape, Transform::identity());
+
+            //  Rigidbody 3
+
+            const decimal angleRad = 20 * PI_RP3D / 180.0;
+            const Transform transform4(Vector3(1, 2, 3), Quaternion::fromEulerAngles(angleRad, angleRad, angleRad));
+            mRigidBody3 = mWorld->createRigidBody(transform4);
+            BoxShape* boxShape3 = mPhysicsCommon.createBoxShape(Vector3(2, 2, 2));
+            mRigidBody3->addCollider(boxShape3, Transform::identity());
+        }
+
+        /// Destructor
+        virtual ~TestRigidBody() {
+
+            mWorld->destroyRigidBody(mRigidBody1);
+            mWorld->destroyRigidBody(mRigidBody2Box);
+            mWorld->destroyRigidBody(mRigidBody2Sphere);
+        }
+
+        /// Run the tests
+        void run() {
+            testGettersSetters();
+            testMassPropertiesMethods();
+            testApplyForcesAndTorques();
+        }
+
+        void testGettersSetters() {
+
+           mRigidBody1->setMass(34);
+           rp3d_test(mRigidBody1->getMass() == 34);
+
+           mRigidBody1->setLinearDamping(0.6);
+           rp3d_test(approxEqual(mRigidBody1->getLinearDamping(), 0.6));
+
+           mRigidBody1->setAngularDamping(0.6);
+           rp3d_test(approxEqual(mRigidBody1->getAngularDamping(), 0.6));
+
+           mRigidBody1->setLinearLockAxisFactor(Vector3(0.2, 0.3, 0.4));
+           rp3d_test(approxEqual(mRigidBody1->getLinearLockAxisFactor(), Vector3(0.2, 0.3, 0.4)));
+
+           mRigidBody1->setAngularLockAxisFactor(Vector3(0.2, 0.3, 0.4));
+           rp3d_test(approxEqual(mRigidBody1->getAngularLockAxisFactor(), Vector3(0.2, 0.3, 0.4)));
+
+           mRigidBody1->setLinearVelocity(Vector3(2, 3, 4));
+           rp3d_test(approxEqual(mRigidBody1->getLinearVelocity(), Vector3(2, 3, 4)));
+
+           mRigidBody1->setAngularVelocity(Vector3(2, 3, 4));
+           rp3d_test(approxEqual(mRigidBody1->getAngularVelocity(), Vector3(2, 3, 4)));
+
+           mRigidBody1->setTransform(Transform(Vector3(5, 4, 3), Quaternion::fromEulerAngles(1.7, 1.8, 1.9)));
+           rp3d_test(approxEqual(mRigidBody1->getTransform().getPosition(), Vector3(5, 4, 3)));
+           rp3d_test(approxEqual(mRigidBody1->getTransform().getOrientation().x, Quaternion::fromEulerAngles(1.7, 1.8, 1.9).x));
+           rp3d_test(approxEqual(mRigidBody1->getTransform().getOrientation().y, Quaternion::fromEulerAngles(1.7, 1.8, 1.9).y));
+           rp3d_test(approxEqual(mRigidBody1->getTransform().getOrientation().z, Quaternion::fromEulerAngles(1.7, 1.8, 1.9).z));
+           rp3d_test(approxEqual(mRigidBody1->getTransform().getOrientation().w, Quaternion::fromEulerAngles(1.7, 1.8, 1.9).w));
+
+           mRigidBody1->setLocalCenterOfMass(Vector3(10, 20, 30));
+           rp3d_test(approxEqual(mRigidBody1->getLocalCenterOfMass(), Vector3(10, 20, 30)));
+
+           mRigidBody1->setType(BodyType::KINEMATIC);
+           rp3d_test(mRigidBody1->getType() == BodyType::KINEMATIC);
+
+           mRigidBody1->setLocalInertiaTensor(Vector3(2, 4, 6));
+           rp3d_test(approxEqual(mRigidBody1->getLocalInertiaTensor(), Vector3(2, 4, 6)));
+        }
+
+        void testMassPropertiesMethods() {
+
+            // Box collider
+            mBoxCollider->getMaterial().setMassDensity(3);
+            mRigidBody2Box->updateMassFromColliders();
+            rp3d_test(approxEqual(mRigidBody2Box->getMass(), 64 * 3));
+
+            mRigidBody2Box->setLocalCenterOfMass(Vector3(1, 2, 3));
+            mRigidBody2Box->setMass(1);
+            mRigidBody2Box->updateMassPropertiesFromColliders();
+            rp3d_test(approxEqual(mRigidBody2Box->getMass(), 64 * 3));
+            rp3d_test(approxEqual(mRigidBody2Box->getLocalCenterOfMass(), Vector3::zero()));
+
+            mRigidBody2Box->setLocalCenterOfMass(Vector3(1, 2, 3));
+            mRigidBody2Box->updateLocalCenterOfMassFromColliders();
+            rp3d_test(approxEqual(mRigidBody2Box->getLocalCenterOfMass(), Vector3::zero()));
+
+            mRigidBody2Box->setLocalInertiaTensor(Vector3(1, 2, 3));
+            mRigidBody2Box->updateLocalInertiaTensorFromColliders();
+            decimal tensorBox = 1.0 / 6.0 * 64 * 3 * 4 * 4;
+            rp3d_test(approxEqual(mRigidBody2Box->getLocalInertiaTensor(), Vector3(tensorBox, tensorBox, tensorBox)));
+
+            // Sphere collider
+            mSphereCollider->getMaterial().setMassDensity(3);
+            mRigidBody2Sphere->updateMassFromColliders();
+            const decimal sphereMass = 4.0 / 3.0 * PI_RP3D * 64 * 3;
+            rp3d_test(approxEqual(mRigidBody2Sphere->getMass(), sphereMass));
+
+            mRigidBody2Sphere->setLocalCenterOfMass(Vector3(1, 2, 3));
+            mRigidBody2Sphere->setMass(1);
+            mRigidBody2Sphere->updateMassPropertiesFromColliders();
+            rp3d_test(approxEqual(mRigidBody2Sphere->getMass(), sphereMass));
+            rp3d_test(approxEqual(mRigidBody2Sphere->getLocalCenterOfMass(), Vector3::zero()));
+
+            mRigidBody2Sphere->setLocalCenterOfMass(Vector3(1, 2, 3));
+            mRigidBody2Sphere->updateLocalCenterOfMassFromColliders();
+            rp3d_test(approxEqual(mRigidBody2Sphere->getLocalCenterOfMass(), Vector3::zero()));
+
+            mRigidBody2Sphere->setLocalInertiaTensor(Vector3(1, 2, 3));
+            mRigidBody2Sphere->updateLocalInertiaTensorFromColliders();
+            const decimal tensorSphere = 2.0 / 5.0 * sphereMass * 4 * 4;
+            rp3d_test(approxEqual(mRigidBody2Sphere->getLocalInertiaTensor(), Vector3(tensorSphere, tensorSphere, tensorSphere)));
+
+            // Convex mesh collider
+            mConvexMeshCollider->getMaterial().setMassDensity(3);
+            mRigidBody2Convex->updateMassFromColliders();
+            rp3d_test(approxEqual(mRigidBody2Convex->getMass(), 648));
+
+            mRigidBody2Convex->setLocalCenterOfMass(Vector3(1, 2, 3));
+            mRigidBody2Convex->setMass(1);
+            mConvexMeshCollider->getMaterial().setMassDensity(2);
+            mRigidBody2Convex->updateMassPropertiesFromColliders();
+            rp3d_test(approxEqual(mRigidBody2Convex->getMass(), 432));
+            rp3d_test(approxEqual(mRigidBody2Convex->getLocalCenterOfMass(), Vector3::zero()));
+
+            mRigidBody2Convex->setLocalCenterOfMass(Vector3(1, 2, 3));
+            mRigidBody2Convex->updateLocalCenterOfMassFromColliders();
+            rp3d_test(approxEqual(mRigidBody2Convex->getLocalCenterOfMass(), Vector3::zero()));
+
+            mRigidBody2Convex->setLocalInertiaTensor(Vector3(1, 2, 3));
+            mRigidBody2Convex->updateLocalInertiaTensorFromColliders();
+            tensorBox = 1.0 / 6.0 * 432 * 6 * 6;
+            rp3d_test(approxEqual(mRigidBody2Convex->getLocalInertiaTensor(), Vector3(tensorBox, tensorBox, tensorBox)));
+        }
+
+        void testApplyForcesAndTorques() {
+
+            const Transform& worldTransform = mRigidBody3->getTransform();
+            const Quaternion orientation = worldTransform.getOrientation();
+
+            mRigidBody3->applyLocalForceAtCenterOfMass(Vector3(4, 5, 6));
+            rp3d_test(approxEqual(mRigidBody3->getForce(), orientation * Vector3(4, 5, 6)));
+            rp3d_test(approxEqual(mRigidBody3->getTorque(), orientation * Vector3::zero()));
+
+            mRigidBody3->resetForce();
+            mRigidBody3->resetTorque();
+
+            mRigidBody3->applyWorldForceAtCenterOfMass(Vector3(4, 5, 6));
+            rp3d_test(approxEqual(mRigidBody3->getForce(), Vector3(4, 5, 6)));
+            rp3d_test(approxEqual(mRigidBody3->getTorque(), Vector3::zero()));
+            mRigidBody3->resetForce();
+            mRigidBody3->resetTorque();
+
+            mRigidBody3->applyLocalForceAtLocalPosition(Vector3(0, 0, 3), Vector3(2, 0, 0));
+            rp3d_test(approxEqual(mRigidBody3->getForce(), orientation * Vector3(0, 0, 3)));
+
+            rp3d_test(approxEqual(mRigidBody3->getTorque(), orientation * Vector3(0, -3 * 2, 0), decimal(0.0001)));
+            mRigidBody3->resetForce();
+            mRigidBody3->resetTorque();
+
+            rp3d_test(approxEqual(mRigidBody3->getForce(), Vector3::zero()));
+            rp3d_test(approxEqual(mRigidBody3->getTorque(), Vector3::zero()));
+
+            mRigidBody3->applyLocalForceAtWorldPosition(Vector3(0, 0, 3), worldTransform * Vector3(2, 0, 0));
+            rp3d_test(approxEqual(mRigidBody3->getForce(), orientation * Vector3(0, 0, 3)));
+            rp3d_test(approxEqual(mRigidBody3->getTorque(), orientation * Vector3(0, -3 * 2, 0), decimal(0.0001)));
+            mRigidBody3->resetForce();
+            mRigidBody3->resetTorque();
+
+            mRigidBody3->applyWorldForceAtLocalPosition(orientation * Vector3(0, 0, 3), Vector3(2, 0, 0));
+            rp3d_test(approxEqual(mRigidBody3->getForce(), orientation * Vector3(0, 0, 3)));
+            rp3d_test(approxEqual(mRigidBody3->getTorque(), orientation * Vector3(0, -3 * 2, 0), decimal(0.0001)));
+            mRigidBody3->resetForce();
+            mRigidBody3->resetTorque();
+
+            mRigidBody3->applyWorldForceAtWorldPosition(orientation * Vector3(0, 0, 3), worldTransform * Vector3(2, 0, 0));
+            rp3d_test(approxEqual(mRigidBody3->getForce(), orientation * Vector3(0, 0, 3)));
+            rp3d_test(approxEqual(mRigidBody3->getTorque(), orientation * Vector3(0, -3 * 2, 0), decimal(0.0001)));
+            mRigidBody3->resetForce();
+            mRigidBody3->resetTorque();
+
+            mRigidBody3->applyWorldTorque(Vector3(0, 4, 0));
+            rp3d_test(approxEqual(mRigidBody3->getForce(), Vector3::zero()));
+            rp3d_test(approxEqual(mRigidBody3->getTorque(), Vector3(0, 4, 0), decimal(0.0001)));
+            mRigidBody3->resetForce();
+            mRigidBody3->resetTorque();
+
+            mRigidBody3->applyLocalTorque(Vector3(0, 4, 0));
+            rp3d_test(approxEqual(mRigidBody3->getForce(), Vector3::zero()));
+            rp3d_test(approxEqual(mRigidBody3->getTorque(), orientation * Vector3(0, 4, 0), decimal(0.0001)));
+            mRigidBody3->resetForce();
+            mRigidBody3->resetTorque();
+        }
+ };
+
+}
+
+#endif