Merge branch 'develop' of https://code.google.com/p/reactphysics3d into develop

CMakeLists.txt

@@ -9,6 +9,7 @@ SET(LIBRARY_OUTPUT_PATH lib/)
 
 # Options
 OPTION(COMPILE_EXAMPLES "Select this if you want to build the examples" OFF)
+OPTION(COMPILE_TESTS "Select this if you want to build the tests" OFF)
 
 # Headers
 INCLUDE_DIRECTORIES(src)
@@ -31,3 +32,8 @@ ADD_LIBRARY (
 IF (COMPILE_EXAMPLES)
    add_subdirectory(examples/fallingcubes)
 ENDIF (COMPILE_EXAMPLES)
+
+# If we need to compile the tests
+IF (COMPILE_TESTS)
+   add_subdirectory(test/)
+ENDIF (COMPILE_TESTS)

src/body/CollisionBody.cpp

@@ -44,13 +44,11 @@ CollisionBody::CollisionBody(const Transform& transform, CollisionShape *collisi
     // Initialize the old transform
     mOldTransform = transform;
 
-    // Create the AABB for broad-phase collision detection
+    // Initialize the AABB for broad-phase collision detection
-    mAabb = new AABB(transform, collisionShape->getLocalExtents(OBJECT_MARGIN));
+    mCollisionShape->updateAABB(mAabb, transform);
 }
 
 // Destructor
 CollisionBody::~CollisionBody() {
 
-    // Delete the AABB
-    delete mAabb;
 }

src/body/CollisionBody.h

@@ -71,7 +71,7 @@ class CollisionBody : public Body {
         bool mIsCollisionEnabled;
 
         /// AABB for Broad-Phase collision detection
-        AABB* mAabb;
+        AABB mAabb;
 
         /// True if the body has moved during the last frame
         bool mHasMoved;
@@ -119,7 +119,7 @@ class CollisionBody : public Body {
         void setTransform(const Transform& transform);
 
         /// Return the AAABB of the body
-        const AABB* getAABB() const;
+        const AABB& getAABB() const;
 
         /// Return the interpolated transform for rendering
         Transform getInterpolatedTransform() const;
@@ -216,7 +216,7 @@ inline void CollisionBody::setTransform(const Transform& transform) {
 }
 
 // Return the AAABB of the body
-inline const AABB* CollisionBody::getAABB() const {
+inline const AABB& CollisionBody::getAABB() const {
     return mAabb;
 }
 
@@ -242,7 +242,7 @@ inline void CollisionBody::updateAABB() {
     // TODO : An AABB should not be updated every frame but only if the body has moved
 
     // Update the AABB
-    mAabb->update(mTransform, mCollisionShape->getLocalExtents(OBJECT_MARGIN));
+    mCollisionShape->updateAABB(mAabb, mTransform);
 }
 
 }

src/body/RigidBody.cpp

@@ -39,11 +39,7 @@ using namespace reactphysics3d;
 
     mRestitution = decimal(1.0);
 
-    // Set the body pointer of the AABB and the collision shape
-    mAabb->setBodyPointer(this);
-
     assert(collisionShape);
-    assert(mAabb);
 }
 
 // Destructor

src/body/RigidBody.h

@@ -153,7 +153,7 @@ class RigidBody : public CollisionBody {
         decimal getRestitution() const;
 
         /// Set the restitution coefficient
-        void setRestitution(decimal restitution) throw(std::invalid_argument);
+        void setRestitution(decimal restitution);
 
         /// Get the friction coefficient
         decimal getFrictionCoefficient() const;
@@ -273,15 +273,9 @@ inline decimal RigidBody::getRestitution() const {
 }
 
 // Set the restitution coefficient
-inline void RigidBody::setRestitution(decimal restitution) throw(std::invalid_argument) {
+inline void RigidBody::setRestitution(decimal restitution) {
-
+    assert(restitution >= 0.0 && restitution <= 1.0);
-    // Check if the restitution coefficient is between 0 and 1
+    mRestitution = restitution;
-    if (restitution >= 0.0 && restitution <= 1.0) {
-        mRestitution = restitution;
-    }
-    else {
-        throw std::invalid_argument("Error : the restitution coefficent must be between 0 and 1");
-    }
 }
 
 // Get the friction coefficient

src/collision/CollisionDetection.cpp

@@ -37,8 +37,6 @@
 #include <set>
 #include <utility>
 #include <utility>
-#include <sys/time.h>       // TODO : Delete this
-#include <iostream>         // TODO : Delete this
 
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;
@@ -62,48 +60,33 @@ CollisionDetection::~CollisionDetection() {
 }
 
 // Compute the collision detection
-bool CollisionDetection::computeCollisionDetection() {
+void CollisionDetection::computeCollisionDetection() {
-	    
-    // TODO : Remove this code
-    timeval timeValueStart;
-    timeval timeValueStop;
-    gettimeofday(&timeValueStart, NULL);
 	    
     // Compute the broad-phase collision detection
     computeBroadPhase();
     
-    // TODO : Remove this code
-    gettimeofday(&timeValueStop, NULL);
-    double startTime = timeValueStart.tv_sec * 1000000.0 + (timeValueStart.tv_usec);
-    double stopTime = timeValueStop.tv_sec * 1000000.0 + (timeValueStop.tv_usec);
-    double deltaTime = stopTime - startTime;
-    //printf("Broadphase time : %f micro sec \n", deltaTime);
-    
     // Compute the narrow-phase collision detection
-    bool collisionExists = computeNarrowPhase();
+    computeNarrowPhase();
-	
-    // Return true if at least one contact has been found
-    return collisionExists;
 }
 
 // Compute the broad-phase collision detection
 void CollisionDetection::computeBroadPhase() {
 
     // Notify the broad-phase algorithm about the bodies that have moved since last frame
-    for (set<CollisionBody*>::iterator it = mWorld->getBodiesBeginIterator(); it != mWorld->getBodiesEndIterator(); it++) {
+    for (set<CollisionBody*>::iterator it = mWorld->getBodiesBeginIterator();
+         it != mWorld->getBodiesEndIterator(); it++) {
 
         // If the body has moved
         if ((*it)->getHasMoved()) {
 
             // Notify the broad-phase that the body has moved
-            mBroadPhaseAlgorithm->updateObject(*it, *((*it)->getAABB()));
+            mBroadPhaseAlgorithm->updateObject(*it, (*it)->getAABB());
         }
     }  
 }
 
 // Compute the narrow-phase collision detection
-bool CollisionDetection::computeNarrowPhase() {
+void CollisionDetection::computeNarrowPhase() {
-    bool collisionExists = false;
     map<bodyindexpair, BroadPhasePair*>::iterator it;
     
     // For each possible collision pair of bodies
@@ -120,16 +103,19 @@ bool CollisionDetection::computeNarrowPhase() {
         mWorld->updateOverlappingPair(pair);
         
         // Select the narrow phase algorithm to use according to the two collision shapes
-        NarrowPhaseAlgorithm& narrowPhaseAlgorithm = SelectNarrowPhaseAlgorithm(body1->getCollisionShape(), body2->getCollisionShape());
+        NarrowPhaseAlgorithm& narrowPhaseAlgorithm = SelectNarrowPhaseAlgorithm(
+                                                        body1->getCollisionShape(),
+                                                        body2->getCollisionShape());
         
         // Notify the narrow-phase algorithm about the overlapping pair we are going to test
         narrowPhaseAlgorithm.setCurrentOverlappingPair(pair);
         
-        // Use the narrow-phase collision detection algorithm to check if there really is a collision
+        // Use the narrow-phase collision detection algorithm to check
+        // if there really is a collision
         if (narrowPhaseAlgorithm.testCollision(body1->getCollisionShape(), body1->getTransform(),
-                                               body2->getCollisionShape(), body2->getTransform(), contactInfo)) {
+                                               body2->getCollisionShape(), body2->getTransform(),
+                                               contactInfo)) {
             assert(contactInfo != NULL);
-            collisionExists = true;
 
             // Notify the world about the new narrow-phase contact
             mWorld->notifyNewContact(pair, contactInfo);
@@ -139,8 +125,6 @@ bool CollisionDetection::computeNarrowPhase() {
             mMemoryPoolContactInfos.freeObject(contactInfo);
         }
     }
-    
-    return collisionExists;
 }
 
 // Allow the broadphase to notify the collision detection about an overlapping pair.
@@ -151,11 +135,14 @@ void CollisionDetection::broadPhaseNotifyAddedOverlappingPair(BodyPair* addedPai
     bodyindexpair indexPair = addedPair->getBodiesIndexPair();
 
     // Create the corresponding broad-phase pair object
-    BroadPhasePair* broadPhasePair = new (mMemoryPoolBroadPhasePairs.allocateObject()) BroadPhasePair(addedPair->body1, addedPair->body2);
+    BroadPhasePair* broadPhasePair = new (mMemoryPoolBroadPhasePairs.allocateObject())
+                                             BroadPhasePair(addedPair->body1, addedPair->body2);
     assert(broadPhasePair != NULL);
 
     // Add the pair into the set of overlapping pairs (if not there yet)
-    pair<map<bodyindexpair, BroadPhasePair*>::iterator, bool> check = mOverlappingPairs.insert(make_pair(indexPair, broadPhasePair));
+    pair<map<bodyindexpair, BroadPhasePair*>::iterator, bool> check = mOverlappingPairs.insert(
+                                                                            make_pair(indexPair,
+                                                                            broadPhasePair));
     assert(check.second);
 
     // Notify the world about the new broad-phase overlapping pair

src/collision/CollisionDetection.h

@@ -94,7 +94,7 @@ class CollisionDetection {
         void computeBroadPhase();
 
         /// Compute the narrow-phase collision detection
-        bool computeNarrowPhase();
+        void computeNarrowPhase();
 
         /// Select the narrow phase algorithm to use given two collision shapes
         NarrowPhaseAlgorithm& SelectNarrowPhaseAlgorithm(CollisionShape* collisionShape1,
@@ -117,7 +117,7 @@ class CollisionDetection {
         void removeBody(CollisionBody* body);
 
         /// Compute the collision detection
-        bool computeCollisionDetection();
+        void computeCollisionDetection();
 
         /// Allow the broadphase to notify the collision detection about a new overlapping pair.
         void broadPhaseNotifyAddedOverlappingPair(BodyPair* pair);
@@ -143,7 +143,7 @@ inline NarrowPhaseAlgorithm& CollisionDetection::SelectNarrowPhaseAlgorithm(
 inline void CollisionDetection::addBody(CollisionBody* body) {
     
     // Add the body to the broad-phase
-    mBroadPhaseAlgorithm->addObject(body, *(body->getAABB()));
+    mBroadPhaseAlgorithm->addObject(body, body->getAABB());
 }  
 
 // Remove a body from the collision detection

src/collision/broadphase/SweepAndPruneAlgorithm.cpp

@@ -101,8 +101,8 @@ void SweepAndPruneAlgorithm::addObject(CollisionBody* body, const AABB& aabb) {
     // Create a new box
     BoxAABB* box = &mBoxes[boxIndex];
     box->body = body;
-    const uint minEndPointValue = encodeFloatIntoInteger(DECIMAL_LARGEST - 2.0);
+    const uint minEndPointValue = encodeFloatIntoInteger(FLT_MAX - 2.0f);
-    const uint maxEndPointValue = encodeFloatIntoInteger(DECIMAL_LARGEST - 1.0);
+    const uint maxEndPointValue = encodeFloatIntoInteger(FLT_MAX - 1.0f);
     for (uint axis=0; axis<3; axis++) {
         box->min[axis] = indexLimitEndPoint;
         box->max[axis] = indexLimitEndPoint + 1;
@@ -131,7 +131,7 @@ void SweepAndPruneAlgorithm::removeObject(CollisionBody* body) {
     // in order to remove all overlapping pairs from the pair manager
     const decimal max = DECIMAL_LARGEST;
     const Vector3 maxVector(max, max, max);
-    const AABB aabb(maxVector, maxVector, body);
+    const AABB aabb(maxVector, maxVector);
     updateObject(body, aabb);
 
     // Get the corresponding box

src/collision/narrowphase/EPA/TriangleEPA.cpp

@@ -72,7 +72,7 @@ bool TriangleEPA::computeClosestPoint(const Vector3* vertices) {
     // If the determinant is positive
     if (mDet > 0.0) {
         // Compute the closest point v
-        mClosestPoint = p0 + 1.0 / mDet * (mLambda1 * v1 + mLambda2 * v2);
+        mClosestPoint = p0 + decimal(1.0) / mDet * (mLambda1 * v1 + mLambda2 * v2);
 
         // Compute the square distance of closest point to the origin
         mDistSquare = mClosestPoint.dot(mClosestPoint);

src/collision/narrowphase/EPA/TriangleEPA.h

@@ -184,7 +184,7 @@ inline bool TriangleEPA::isVisibleFromVertex(const Vector3* vertices, uint index
 // Compute the point of an object closest to the origin
 inline Vector3 TriangleEPA::computeClosestPointOfObject(const Vector3* supportPointsOfObject) const{
     const Vector3& p0 = supportPointsOfObject[mIndicesVertices[0]];
-    return p0 + 1.0/mDet * (mLambda1 * (supportPointsOfObject[mIndicesVertices[1]] - p0) +
+    return p0 + decimal(1.0)/mDet * (mLambda1 * (supportPointsOfObject[mIndicesVertices[1]] - p0) +
                            mLambda2 * (supportPointsOfObject[mIndicesVertices[2]] - p0));
 }
 

src/collision/narrowphase/GJK/Simplex.cpp

@@ -309,7 +309,7 @@ void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const {
     }
 
     assert(deltaX > 0.0);
-    decimal factor = 1.0 / deltaX;
+    decimal factor = decimal(1.0) / deltaX;
     pA *= factor;
     pB *= factor;
 }
@@ -390,5 +390,5 @@ Vector3 Simplex::computeClosestPointForSubset(Bits subset) {
     assert(deltaX > 0.0);
 
     // Return the closet point "v" in the convex hull for the given subset
-    return (1.0 / deltaX) * v;
+    return (decimal(1.0) / deltaX) * v;
 }

src/collision/shapes/AABB.cpp

@@ -45,21 +45,16 @@ using namespace reactphysics3d;
 using namespace std;
 
 // Constructor
-AABB::AABB() : mBodyPointer(NULL) {
+AABB::AABB() {
     
 }
 
 // Constructor
-AABB::AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates, Body* modyPointer)
+AABB::AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates)
-     :mMinCoordinates(minCoordinates), mMaxCoordinates(maxCoordinates), mBodyPointer(modyPointer) {
+     :mMinCoordinates(minCoordinates), mMaxCoordinates(maxCoordinates) {
 
 }
 
-// Constructor
-AABB::AABB(const Transform& transform, const Vector3& extents) : mBodyPointer(NULL) {
-    update(transform, extents);
-}
-
 // Destructor
 AABB::~AABB() {
 

src/collision/shapes/AABB.h

@@ -54,10 +54,6 @@ class AABB {
         /// Maximum world coordinates of the AABB on the x,y and z axis
         Vector3 mMaxCoordinates;
 
-        /// Pointer to the owner body (not the abstract class Body
-        /// but its derivative which is instanciable)
-        Body* mBodyPointer;
-
         // -------------------- Methods -------------------- //
 
         /// Private copy-constructor
@@ -66,6 +62,9 @@ class AABB {
         /// Private assignment operator
         AABB& operator=(const AABB& aabb);
 
+        /// Constructor
+        AABB(const Transform& transform, const Vector3& extents);
+
     public :
 
         // -------------------- Methods -------------------- //
@@ -74,10 +73,9 @@ class AABB {
         AABB();
 
         /// Constructor
-        AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates, Body* modyPointer);
+        AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates);
+
 
-        /// Constructor
-        AABB(const Transform& transform, const Vector3& extents);
 
         /// Destructor
         virtual ~AABB();
@@ -88,22 +86,18 @@ class AABB {
         /// Return the minimum coordinates of the AABB
         const Vector3& getMin() const;
 
+        /// Set the minimum coordinates of the AABB
+        void setMin(const Vector3& min);
+
         /// Return the maximum coordinates of the AABB
         const Vector3& getMax() const;
 
-        /// Return a pointer to the owner body
+        /// Set the maximum coordinates of the AABB
-        Body* getBodyPointer() const;
+        void setMax(const Vector3& max);
-
-        /// Set the body pointer
-        void setBodyPointer(Body* bodyPointer);
 
         /// Return true if the current AABB is overlapping with the AABB in argument
         bool testCollision(const AABB& aabb) const;
 
-        /// Update the oriented bounding box orientation
-        /// according to a new orientation of the rigid body
-        virtual void update(const Transform& newTransform, const Vector3& extents);
-
 #ifdef VISUAL_DEBUG
        /// Draw the AABB (only for testing purpose)
        virtual void draw() const;
@@ -120,19 +114,19 @@ inline const Vector3& AABB::getMin() const {
     return mMinCoordinates;
 }
 
+// Set the minimum coordinates of the AABB
+inline void AABB::setMin(const Vector3& min) {
+    mMinCoordinates = min;
+}
+
 // Return the maximum coordinates of the AABB
 inline const Vector3& AABB::getMax() const {
     return mMaxCoordinates;
 }
 
-// Return a pointer to the owner body
+/// Set the maximum coordinates of the AABB
-inline Body* AABB::getBodyPointer() const {
+inline void AABB::setMax(const Vector3& max) {
-    return mBodyPointer;
+    mMaxCoordinates = max;
-}
-
-// Set the body pointer
-inline void AABB::setBodyPointer(Body* bodyPointer) {
-    mBodyPointer = bodyPointer;
 }
 
 // Return true if the current AABB is overlapping with the AABB in argument.
@@ -147,16 +141,6 @@ inline bool AABB::testCollision(const AABB& aabb) const {
     return true;
 }
 
-// Update the world minimum and maximum coordinates of the AABB on the three x,y and z axis
-inline void AABB::update(const Transform& newTransform, const Vector3& extents) {
-    Matrix3x3 worldAxis = newTransform.getOrientation().getMatrix().getAbsoluteMatrix();
-    Vector3 worldExtents = Vector3(worldAxis.getColumn(0).dot(extents),
-                                     worldAxis.getColumn(1).dot(extents),
-                                     worldAxis.getColumn(2).dot(extents));
-    mMinCoordinates = newTransform.getPosition() - worldExtents;
-    mMaxCoordinates = newTransform.getPosition() + worldExtents;
-}
-
 }
 
 #endif

src/collision/shapes/CollisionShape.cpp

@@ -38,3 +38,24 @@ CollisionShape::CollisionShape(CollisionShapeType type) : mType(type) {
 CollisionShape::~CollisionShape() {
 
 }
+
+// Update the AABB of a body using its collision shape
+inline void CollisionShape::updateAABB(AABB& aabb, const Transform& transform) {
+
+    // Get the local extents in x,y and z direction
+    Vector3 extents = getLocalExtents(OBJECT_MARGIN);
+
+    // Rotate the local extents according to the orientation of the body
+    Matrix3x3 worldAxis = transform.getOrientation().getMatrix().getAbsoluteMatrix();
+    Vector3 worldExtents = Vector3(worldAxis.getColumn(0).dot(extents),
+                                   worldAxis.getColumn(1).dot(extents),
+                                   worldAxis.getColumn(2).dot(extents));
+
+    // Compute the minimum and maximum coordinates of the rotated extents
+    Vector3 minCoordinates = transform.getPosition() - worldExtents;
+    Vector3 maxCoordinates = transform.getPosition() + worldExtents;
+
+    // Update the AABB with the new minimum and maximum coordinates
+    aabb.setMin(minCoordinates);
+    aabb.setMax(maxCoordinates);
+}

src/collision/shapes/CollisionShape.h

@@ -30,6 +30,7 @@
 #include <cassert>
 #include "../../mathematics/Vector3.h"
 #include "../../mathematics/Matrix3x3.h"
+#include "AABB.h"
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -89,6 +90,9 @@ class CollisionShape {
 
         /// Return the local inertia tensor of the collision shapes
         virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0;
+
+        /// Update the AABB of a body using its collision shape
+        virtual void updateAABB(AABB& aabb, const Transform& transform);
 };
 
 // Return the type of the collision shape

src/collision/shapes/CylinderShape.cpp

@@ -44,7 +44,7 @@ using namespace reactphysics3d;
 
 // Constructor
 CylinderShape::CylinderShape(decimal radius, decimal height)
-                 : CollisionShape(CYLINDER), mRadius(radius), mHalfHeight(height/2.0) {
+                 : CollisionShape(CYLINDER), mRadius(radius), mHalfHeight(height/decimal(2.0)) {
 
 }
 

src/collision/shapes/CylinderShape.h

@@ -116,7 +116,7 @@ inline void CylinderShape::setRadius(decimal radius) {
 
 // Return the height
 inline decimal CylinderShape::getHeight() const {
-    return mHalfHeight * 2.0;
+    return mHalfHeight * decimal(2.0);
 }
 
 // Set the height

src/collision/shapes/SphereShape.h

@@ -86,6 +86,9 @@ class SphereShape : public CollisionShape {
         /// Return the margin distance around the shape
         virtual decimal getMargin() const;
 
+        /// Update the AABB of a body using its collision shape
+        virtual void updateAABB(AABB& aabb, const Transform& transform);
+
 #ifdef VISUAL_DEBUG
         /// Draw the sphere (only for testing purpose)
         virtual void draw() const;
@@ -145,6 +148,21 @@ inline decimal SphereShape::getMargin() const {
     return mRadius + OBJECT_MARGIN;
 }
 
+// Update the AABB of a body using its collision shape
+inline void SphereShape::updateAABB(AABB& aabb, const Transform& transform) {
+
+    // Get the local extents in x,y and z direction
+    Vector3 extents = getLocalExtents(OBJECT_MARGIN);
+
+    // Compute the minimum and maximum coordinates of the rotated extents
+    Vector3 minCoordinates = transform.getPosition() - extents;
+    Vector3 maxCoordinates = transform.getPosition() + extents;
+
+    // Update the AABB with the new minimum and maximum coordinates
+    aabb.setMin(minCoordinates);
+    aabb.setMax(maxCoordinates);
+}
+
 }
 
 #endif

src/configuration.h

@@ -48,7 +48,7 @@ namespace reactphysics3d {
 
 typedef unsigned int uint;
 typedef long unsigned int luint;
-typedef short unsigned int bodyindex;
+typedef luint bodyindex;
 typedef std::pair<bodyindex, bodyindex> bodyindexpair;
 
 // ------------------- Constants ------------------- //

src/constraint/Constraint.cpp

@@ -35,7 +35,7 @@ Constraint::Constraint(RigidBody* const body1, RigidBody* const body2,
             mNbConstraints(nbConstraints), mType(type) {
     
     // Initialize the cached lambda values
-    for (int i=0; i<nbConstraints; i++) {
+    for (uint i=0; i<nbConstraints; i++) {
         mCachedLambdas.push_back(0.0);
     }
 }

src/constraint/Constraint.h

@@ -103,10 +103,10 @@ class Constraint {
         unsigned int getNbConstraints() const;
 
         /// Get one cached lambda value
-        decimal getCachedLambda(int index) const;
+        decimal getCachedLambda(uint index) const;
 
         /// Set on cached lambda value
-        void setCachedLambda(int index, decimal lambda);
+        void setCachedLambda(uint index, decimal lambda);
 };
 
 // Return the reference to the body 1
@@ -136,14 +136,14 @@ inline uint Constraint::getNbConstraints() const {
 }
 
 // Get one previous lambda value
-inline decimal Constraint::getCachedLambda(int index) const {
+inline decimal Constraint::getCachedLambda(uint index) const {
-    assert(index >= 0 && index < mNbConstraints);
+    assert(index < mNbConstraints);
     return mCachedLambdas[index];
 } 
 
 // Set on cached lambda value  
-inline void Constraint::setCachedLambda(int index, decimal lambda) {
+inline void Constraint::setCachedLambda(uint index, decimal lambda) {
-    assert(index >= 0 && index < mNbConstraints);
+    assert(index < mNbConstraints);
     mCachedLambdas[index] = lambda;
 }                                                   
 

src/engine/ContactManifold.cpp

@@ -24,6 +24,7 @@
 ********************************************************************************/
 
 // Libraries
+#include <iostream>
 #include "ContactManifold.h"
 
 using namespace reactphysics3d;
@@ -77,8 +78,8 @@ void ContactManifold::addContactPoint(ContactPoint* contact) {
 }
 
 // Remove a contact point from the manifold
-void ContactManifold::removeContactPoint(int index) {
+void ContactManifold::removeContactPoint(uint index) {
-    assert(index >= 0 && index < mNbContactPoints);
+    assert(index < mNbContactPoints);
     assert(mNbContactPoints > 0);
 	
 	// Call the destructor explicitly and tell the memory pool that
@@ -101,10 +102,11 @@ void ContactManifold::removeContactPoint(int index) {
 /// the contacts with a too large distance between the contact points in the plane orthogonal to the
 /// contact normal.
 void ContactManifold::update(const Transform& transform1, const Transform& transform2) {
+
     if (mNbContactPoints == 0) return;
 
     // Update the world coordinates and penetration depth of the contact points in the manifold
-    for (int i=0; i<mNbContactPoints; i++) {
+    for (uint i=0; i<mNbContactPoints; i++) {
         mContactPoints[i]->setWorldPointOnBody1(transform1 * mContactPoints[i]->getLocalPointOnBody1());
         mContactPoints[i]->setWorldPointOnBody2(transform2 * mContactPoints[i]->getLocalPointOnBody2());
         mContactPoints[i]->setPenetrationDepth((mContactPoints[i]->getWorldPointOnBody1() - mContactPoints[i]->getWorldPointOnBody2()).dot(mContactPoints[i]->getNormal()));
@@ -114,8 +116,8 @@ void ContactManifold::update(const Transform& transform1, const Transform& trans
                                                      PERSISTENT_CONTACT_DIST_THRESHOLD;
 
     // Remove the contact points that don't represent very well the contact manifold
-    for (int i=mNbContactPoints-1; i>=0; i--) {
+    for (int i=static_cast<int>(mNbContactPoints)-1; i>=0; i--) {
-        assert(i>= 0 && i < mNbContactPoints);
+        assert(i < static_cast<int>(mNbContactPoints));
 
         // Compute the distance between contact points in the normal direction
         decimal distanceNormal = -mContactPoints[i]->getPenetrationDepth();

src/engine/ContactManifold.h

@@ -106,7 +106,7 @@ class ContactManifold {
         int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const;
 
         /// Remove a contact point from the manifold
-        void removeContactPoint(int index);
+        void removeContactPoint(uint index);
 
         /// Return true if two vectors are approximatively equal
         bool isApproxEqual(const Vector3& vector1, const Vector3& vector2) const;

src/engine/ContactSolver.cpp

@@ -27,6 +27,7 @@
 #include "ContactSolver.h"
 #include "DynamicsWorld.h"
 #include "../body/RigidBody.h"
+#include <limits>
 
 using namespace reactphysics3d;
 using namespace std;

src/engine/DynamicsWorld.cpp

@@ -81,7 +81,7 @@ void DynamicsWorld::update() {
         if (!mContactManifolds.empty()) {
 
             // Solve the contacts
-            mContactSolver.solve(mTimer.getTimeStep());
+            mContactSolver.solve(static_cast<decimal>(mTimer.getTimeStep()));
         }
 
         // Update the timer
@@ -106,7 +106,7 @@ void DynamicsWorld::update() {
 
 // Update the position and orientation of the rigid bodies
 void DynamicsWorld::updateRigidBodiesPositionAndOrientation() {
-    decimal dt = mTimer.getTimeStep();
+    decimal dt = static_cast<decimal>(mTimer.getTimeStep());
     
     // For each rigid body of the world
     set<RigidBody*>::iterator it;
@@ -289,7 +289,7 @@ void DynamicsWorld::removeAllConstraints() {
 void DynamicsWorld::notifyAddedOverlappingPair(const BroadPhasePair* addedPair) {
 
     // Get the pair of body index
-    std::pair<bodyindex, bodyindex> indexPair = addedPair->getBodiesIndexPair();
+    bodyindexpair indexPair = addedPair->getBodiesIndexPair();
 
     // Add the pair into the set of overlapping pairs (if not there yet)
     OverlappingPair* newPair = new (mMemoryPoolOverlappingPairs.allocateObject()) OverlappingPair(

src/engine/Timer.h

@@ -34,6 +34,7 @@
 #include "../configuration.h"
 
 #if defined(WINDOWS_OS)   // For Windows platform
+   #define NOMINMAX       // This is used to avoid definition of max() and min() macros
    #include <windows.h>
 #else                                   // For Mac OS or Linux platform
    #include <sys/time.h>
@@ -119,7 +120,7 @@ class Timer {
         void nextStep();
 
         /// Compute the interpolation factor
-        double computeInterpolationFactor();
+        decimal computeInterpolationFactor();
 };
 
 // Return the timestep of the physics engine
@@ -188,8 +189,8 @@ inline void Timer::nextStep() {
 }
 
 // Compute the interpolation factor
-inline double Timer::computeInterpolationFactor() {
+inline decimal Timer::computeInterpolationFactor() {
-    return (mAccumulator / mTimeStep);
+    return (decimal(mAccumulator / mTimeStep));
 }
 
 // Compute the time since the last update() call and add it to the accumulator

src/mathematics/Matrix3x3.cpp

@@ -56,9 +56,9 @@ Matrix3x3::~Matrix3x3() {
 
 // Copy-constructor
 Matrix3x3::Matrix3x3(const Matrix3x3& matrix) {
-    setAllValues(matrix.mArray[0][0], matrix.mArray[0][1], matrix.mArray[0][2],
+    setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], matrix.mRows[0][2],
-                 matrix.mArray[1][0], matrix.mArray[1][1], matrix.mArray[1][2],
+                 matrix.mRows[1][0], matrix.mRows[1][1], matrix.mRows[1][2],
-                 matrix.mArray[2][0], matrix.mArray[2][1], matrix.mArray[2][2]);
+                 matrix.mRows[2][0], matrix.mRows[2][1], matrix.mRows[2][2]);
 }
 
 // Assignment operator
@@ -66,9 +66,9 @@ Matrix3x3& Matrix3x3::operator=(const Matrix3x3& matrix) {
 
     // Check for self-assignment
     if (&matrix != this) {
-        setAllValues(matrix.mArray[0][0], matrix.mArray[0][1], matrix.mArray[0][2],
+        setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], matrix.mRows[0][2],
-                     matrix.mArray[1][0], matrix.mArray[1][1], matrix.mArray[1][2],
+                     matrix.mRows[1][0], matrix.mRows[1][1], matrix.mRows[1][2],
-                     matrix.mArray[2][0], matrix.mArray[2][1], matrix.mArray[2][2]);
+                     matrix.mRows[2][0], matrix.mRows[2][1], matrix.mRows[2][2]);
     }
     return *this;
 }
@@ -84,15 +84,15 @@ Matrix3x3 Matrix3x3::getInverse() const {
 
     decimal invDeterminant = decimal(1.0) / determinant;
 
-    Matrix3x3 tempMatrix((mArray[1][1]*mArray[2][2]-mArray[2][1]*mArray[1][2]),
+    Matrix3x3 tempMatrix((mRows[1][1]*mRows[2][2]-mRows[2][1]*mRows[1][2]),
-                         -(mArray[0][1]*mArray[2][2]-mArray[2][1]*mArray[0][2]),
+                         -(mRows[0][1]*mRows[2][2]-mRows[2][1]*mRows[0][2]),
-                         (mArray[0][1]*mArray[1][2]-mArray[0][2]*mArray[1][1]),
+                         (mRows[0][1]*mRows[1][2]-mRows[0][2]*mRows[1][1]),
-                            -(mArray[1][0]*mArray[2][2]-mArray[2][0]*mArray[1][2]),
+                            -(mRows[1][0]*mRows[2][2]-mRows[2][0]*mRows[1][2]),
-                         (mArray[0][0]*mArray[2][2]-mArray[2][0]*mArray[0][2]),
+                         (mRows[0][0]*mRows[2][2]-mRows[2][0]*mRows[0][2]),
-                         -(mArray[0][0]*mArray[1][2]-mArray[1][0]*mArray[0][2]),
+                         -(mRows[0][0]*mRows[1][2]-mRows[1][0]*mRows[0][2]),
-                            (mArray[1][0]*mArray[2][1]-mArray[2][0]*mArray[1][1]),
+                            (mRows[1][0]*mRows[2][1]-mRows[2][0]*mRows[1][1]),
-                         -(mArray[0][0]*mArray[2][1]-mArray[2][0]*mArray[0][1]),
+                         -(mRows[0][0]*mRows[2][1]-mRows[2][0]*mRows[0][1]),
-                         (mArray[0][0]*mArray[1][1]-mArray[0][1]*mArray[1][0]));
+                         (mRows[0][0]*mRows[1][1]-mRows[0][1]*mRows[1][0]));
 
     // Return the inverse matrix
     return (invDeterminant * tempMatrix);

src/mathematics/Matrix3x3.h

@@ -45,8 +45,8 @@ class Matrix3x3 {
 
         // -------------------- Attributes -------------------- //
 
-        /// Array with the values of the matrix
+        /// Rows of the matrix;
-        decimal mArray[3][3];
+        Vector3 mRows[3];
 
     public :
 
@@ -71,19 +71,19 @@ class Matrix3x3 {
         /// Assignment operator
         Matrix3x3& operator=(const Matrix3x3& matrix);
 
-        /// Get a value in the matrix
-        decimal getValue(int i, int j) const;
-
-        /// Set a value in the matrix
-        void setValue(int i, int j, decimal value);
-
         /// Set all the values in the matrix
         void setAllValues(decimal a1, decimal a2, decimal a3, decimal b1, decimal b2, decimal b3,
                   decimal c1, decimal c2, decimal c3);
 
+        /// Set the matrix to zero
+        void setToZero();
+
         /// Return a column
         Vector3 getColumn(int i) const;
 
+        /// Return a row
+        Vector3 getRow(int i) const;
+
         /// Return the transpose matrix
         Matrix3x3 getTranspose() const;
 
@@ -140,110 +140,120 @@ class Matrix3x3 {
 
         /// Overloaded operator for multiplication with a number with assignment
         Matrix3x3& operator*=(decimal nb);
+
+        /// Overloaded operator to read element of the matrix.
+        const Vector3& operator[](int row) const;
+
+        /// Overloaded operator to read/write element of the matrix.
+        Vector3& operator[](int row);
 };
 
-
-// Method to get a value in the matrix (inline)
-inline decimal Matrix3x3::getValue(int i, int j) const {
-    assert(i>=0 && i<3 && j>=0 && j<3);
-    return mArray[i][j];
-}
-
-// Method to set a value in the matrix (inline)
-inline void Matrix3x3::setValue(int i, int j, decimal value) {
-    assert(i>=0 && i<3 && j>=0 && j<3);
-    mArray[i][j] = value;
-}
-
 // Method to set all the values in the matrix
 inline void Matrix3x3::setAllValues(decimal a1, decimal a2, decimal a3,
                                     decimal b1, decimal b2, decimal b3,
                                     decimal c1, decimal c2, decimal c3) {
-    mArray[0][0] = a1; mArray[0][1] = a2; mArray[0][2] = a3;
+    mRows[0][0] = a1; mRows[0][1] = a2; mRows[0][2] = a3;
-    mArray[1][0] = b1; mArray[1][1] = b2; mArray[1][2] = b3;
+    mRows[1][0] = b1; mRows[1][1] = b2; mRows[1][2] = b3;
-    mArray[2][0] = c1; mArray[2][1] = c2; mArray[2][2] = c3;
+    mRows[2][0] = c1; mRows[2][1] = c2; mRows[2][2] = c3;
+}
+
+// Set the matrix to zero
+inline void Matrix3x3::setToZero() {
+    mRows[0].setToZero();
+    mRows[1].setToZero();
+    mRows[2].setToZero();
 }
 
 // Return a column
 inline Vector3 Matrix3x3::getColumn(int i) const {
     assert(i>= 0 && i<3);
-    return Vector3(mArray[0][i], mArray[1][i], mArray[2][i]);
+    return Vector3(mRows[0][i], mRows[1][i], mRows[2][i]);
+}
+
+// Return a row
+inline Vector3 Matrix3x3::getRow(int i) const {
+    assert(i>= 0 && i<3);
+    return mRows[i];
 }
 
 // Return the transpose matrix
 inline Matrix3x3 Matrix3x3::getTranspose() const {
+
     // Return the transpose matrix
-    return Matrix3x3(mArray[0][0], mArray[1][0], mArray[2][0],
+    return Matrix3x3(mRows[0][0], mRows[1][0], mRows[2][0],
-                     mArray[0][1], mArray[1][1], mArray[2][1],
+                     mRows[0][1], mRows[1][1], mRows[2][1],
-                     mArray[0][2], mArray[1][2], mArray[2][2]);
+                     mRows[0][2], mRows[1][2], mRows[2][2]);
 }
 
 // Return the determinant of the matrix
 inline decimal Matrix3x3::getDeterminant() const {
+
     // Compute and return the determinant of the matrix
-    return (mArray[0][0]*(mArray[1][1]*mArray[2][2]-mArray[2][1]*mArray[1][2]) -
+    return (mRows[0][0]*(mRows[1][1]*mRows[2][2]-mRows[2][1]*mRows[1][2]) -
-            mArray[0][1]*(mArray[1][0]*mArray[2][2]-mArray[2][0]*mArray[1][2]) +
+            mRows[0][1]*(mRows[1][0]*mRows[2][2]-mRows[2][0]*mRows[1][2]) +
-            mArray[0][2]*(mArray[1][0]*mArray[2][1]-mArray[2][0]*mArray[1][1]));
+            mRows[0][2]*(mRows[1][0]*mRows[2][1]-mRows[2][0]*mRows[1][1]));
 }
 
 // Return the trace of the matrix
 inline decimal Matrix3x3::getTrace() const {
+
     // Compute and return the trace
-    return (mArray[0][0] + mArray[1][1] + mArray[2][2]);
+    return (mRows[0][0] + mRows[1][1] + mRows[2][2]);
 }
 
 // Set the matrix to the identity matrix
 inline void Matrix3x3::setToIdentity() {
-    mArray[0][0] = 1.0; mArray[0][1] = 0.0; mArray[0][2] = 0.0;
+    mRows[0][0] = 1.0; mRows[0][1] = 0.0; mRows[0][2] = 0.0;
-    mArray[1][0] = 0.0; mArray[1][1] = 1.0; mArray[1][2] = 0.0;
+    mRows[1][0] = 0.0; mRows[1][1] = 1.0; mRows[1][2] = 0.0;
-    mArray[2][0] = 0.0; mArray[2][1] = 0.0; mArray[2][2] = 1.0;
+    mRows[2][0] = 0.0; mRows[2][1] = 0.0; mRows[2][2] = 1.0;
 }
 
 // Return the 3x3 identity matrix
 inline Matrix3x3 Matrix3x3::identity() {
+
     // Return the isdentity matrix
     return Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
 }
 
 // Return the matrix with absolute values
 inline Matrix3x3 Matrix3x3::getAbsoluteMatrix() const {
-    return Matrix3x3(fabs(mArray[0][0]), fabs(mArray[0][1]), fabs(mArray[0][2]),
+    return Matrix3x3(fabs(mRows[0][0]), fabs(mRows[0][1]), fabs(mRows[0][2]),
-                     fabs(mArray[1][0]), fabs(mArray[1][1]), fabs(mArray[1][2]),
+                     fabs(mRows[1][0]), fabs(mRows[1][1]), fabs(mRows[1][2]),
-                     fabs(mArray[2][0]), fabs(mArray[2][1]), fabs(mArray[2][2]));
+                     fabs(mRows[2][0]), fabs(mRows[2][1]), fabs(mRows[2][2]));
 }
 
 // Overloaded operator for addition
 inline Matrix3x3 operator+(const Matrix3x3& matrix1, const Matrix3x3& matrix2) {
-    return Matrix3x3(matrix1.mArray[0][0] + matrix2.mArray[0][0], matrix1.mArray[0][1] +
+    return Matrix3x3(matrix1.mRows[0][0] + matrix2.mRows[0][0], matrix1.mRows[0][1] +
-                     matrix2.mArray[0][1], matrix1.mArray[0][2] + matrix2.mArray[0][2],
+                     matrix2.mRows[0][1], matrix1.mRows[0][2] + matrix2.mRows[0][2],
-                     matrix1.mArray[1][0] + matrix2.mArray[1][0], matrix1.mArray[1][1] +
+                     matrix1.mRows[1][0] + matrix2.mRows[1][0], matrix1.mRows[1][1] +
-                     matrix2.mArray[1][1], matrix1.mArray[1][2] + matrix2.mArray[1][2],
+                     matrix2.mRows[1][1], matrix1.mRows[1][2] + matrix2.mRows[1][2],
-                     matrix1.mArray[2][0] + matrix2.mArray[2][0], matrix1.mArray[2][1] +
+                     matrix1.mRows[2][0] + matrix2.mRows[2][0], matrix1.mRows[2][1] +
-                     matrix2.mArray[2][1], matrix1.mArray[2][2] + matrix2.mArray[2][2]);
+                     matrix2.mRows[2][1], matrix1.mRows[2][2] + matrix2.mRows[2][2]);
 }
 
 // Overloaded operator for substraction
 inline Matrix3x3 operator-(const Matrix3x3& matrix1, const Matrix3x3& matrix2) {
-    return Matrix3x3(matrix1.mArray[0][0] - matrix2.mArray[0][0], matrix1.mArray[0][1] -
+    return Matrix3x3(matrix1.mRows[0][0] - matrix2.mRows[0][0], matrix1.mRows[0][1] -
-                     matrix2.mArray[0][1], matrix1.mArray[0][2] - matrix2.mArray[0][2],
+                     matrix2.mRows[0][1], matrix1.mRows[0][2] - matrix2.mRows[0][2],
-                     matrix1.mArray[1][0] - matrix2.mArray[1][0], matrix1.mArray[1][1] -
+                     matrix1.mRows[1][0] - matrix2.mRows[1][0], matrix1.mRows[1][1] -
-                     matrix2.mArray[1][1], matrix1.mArray[1][2] - matrix2.mArray[1][2],
+                     matrix2.mRows[1][1], matrix1.mRows[1][2] - matrix2.mRows[1][2],
-                     matrix1.mArray[2][0] - matrix2.mArray[2][0], matrix1.mArray[2][1] -
+                     matrix1.mRows[2][0] - matrix2.mRows[2][0], matrix1.mRows[2][1] -
-                     matrix2.mArray[2][1], matrix1.mArray[2][2] - matrix2.mArray[2][2]);
+                     matrix2.mRows[2][1], matrix1.mRows[2][2] - matrix2.mRows[2][2]);
 }
 
 // Overloaded operator for the negative of the matrix
 inline Matrix3x3 operator-(const Matrix3x3& matrix) {
-    return Matrix3x3(-matrix.mArray[0][0], -matrix.mArray[0][1], -matrix.mArray[0][2],
+    return Matrix3x3(-matrix.mRows[0][0], -matrix.mRows[0][1], -matrix.mRows[0][2],
-                     -matrix.mArray[1][0], -matrix.mArray[1][1], -matrix.mArray[1][2],
+                     -matrix.mRows[1][0], -matrix.mRows[1][1], -matrix.mRows[1][2],
-                     -matrix.mArray[2][0], -matrix.mArray[2][1], -matrix.mArray[2][2]);
+                     -matrix.mRows[2][0], -matrix.mRows[2][1], -matrix.mRows[2][2]);
 }
 
 // Overloaded operator for multiplication with a number
 inline Matrix3x3 operator*(decimal nb, const Matrix3x3& matrix) {
-    return Matrix3x3(matrix.mArray[0][0] * nb, matrix.mArray[0][1] * nb, matrix.mArray[0][2] * nb,
+    return Matrix3x3(matrix.mRows[0][0] * nb, matrix.mRows[0][1] * nb, matrix.mRows[0][2] * nb,
-                     matrix.mArray[1][0] * nb, matrix.mArray[1][1] * nb, matrix.mArray[1][2] * nb,
+                     matrix.mRows[1][0] * nb, matrix.mRows[1][1] * nb, matrix.mRows[1][2] * nb,
-                     matrix.mArray[2][0] * nb, matrix.mArray[2][1] * nb, matrix.mArray[2][2] * nb);
+                     matrix.mRows[2][0] * nb, matrix.mRows[2][1] * nb, matrix.mRows[2][2] * nb);
 }
 
 // Overloaded operator for multiplication with a matrix
@@ -253,44 +263,44 @@ inline Matrix3x3 operator*(const Matrix3x3& matrix, decimal nb) {
 
 // Overloaded operator for matrix multiplication
 inline Matrix3x3 operator*(const Matrix3x3& matrix1, const Matrix3x3& matrix2) {
-    return Matrix3x3(matrix1.mArray[0][0]*matrix2.mArray[0][0] + matrix1.mArray[0][1] *
+    return Matrix3x3(matrix1.mRows[0][0]*matrix2.mRows[0][0] + matrix1.mRows[0][1] *
-                     matrix2.mArray[1][0] + matrix1.mArray[0][2]*matrix2.mArray[2][0],
+                     matrix2.mRows[1][0] + matrix1.mRows[0][2]*matrix2.mRows[2][0],
-                     matrix1.mArray[0][0]*matrix2.mArray[0][1] + matrix1.mArray[0][1] *
+                     matrix1.mRows[0][0]*matrix2.mRows[0][1] + matrix1.mRows[0][1] *
-                     matrix2.mArray[1][1] + matrix1.mArray[0][2]*matrix2.mArray[2][1],
+                     matrix2.mRows[1][1] + matrix1.mRows[0][2]*matrix2.mRows[2][1],
-                     matrix1.mArray[0][0]*matrix2.mArray[0][2] + matrix1.mArray[0][1] *
+                     matrix1.mRows[0][0]*matrix2.mRows[0][2] + matrix1.mRows[0][1] *
-                     matrix2.mArray[1][2] + matrix1.mArray[0][2]*matrix2.mArray[2][2],
+                     matrix2.mRows[1][2] + matrix1.mRows[0][2]*matrix2.mRows[2][2],
-                     matrix1.mArray[1][0]*matrix2.mArray[0][0] + matrix1.mArray[1][1] *
+                     matrix1.mRows[1][0]*matrix2.mRows[0][0] + matrix1.mRows[1][1] *
-                     matrix2.mArray[1][0] + matrix1.mArray[1][2]*matrix2.mArray[2][0],
+                     matrix2.mRows[1][0] + matrix1.mRows[1][2]*matrix2.mRows[2][0],
-                     matrix1.mArray[1][0]*matrix2.mArray[0][1] + matrix1.mArray[1][1] *
+                     matrix1.mRows[1][0]*matrix2.mRows[0][1] + matrix1.mRows[1][1] *
-                     matrix2.mArray[1][1] + matrix1.mArray[1][2]*matrix2.mArray[2][1],
+                     matrix2.mRows[1][1] + matrix1.mRows[1][2]*matrix2.mRows[2][1],
-                     matrix1.mArray[1][0]*matrix2.mArray[0][2] + matrix1.mArray[1][1] *
+                     matrix1.mRows[1][0]*matrix2.mRows[0][2] + matrix1.mRows[1][1] *
-                     matrix2.mArray[1][2] + matrix1.mArray[1][2]*matrix2.mArray[2][2],
+                     matrix2.mRows[1][2] + matrix1.mRows[1][2]*matrix2.mRows[2][2],
-                     matrix1.mArray[2][0]*matrix2.mArray[0][0] + matrix1.mArray[2][1] *
+                     matrix1.mRows[2][0]*matrix2.mRows[0][0] + matrix1.mRows[2][1] *
-                     matrix2.mArray[1][0] + matrix1.mArray[2][2]*matrix2.mArray[2][0],
+                     matrix2.mRows[1][0] + matrix1.mRows[2][2]*matrix2.mRows[2][0],
-                     matrix1.mArray[2][0]*matrix2.mArray[0][1] + matrix1.mArray[2][1] *
+                     matrix1.mRows[2][0]*matrix2.mRows[0][1] + matrix1.mRows[2][1] *
-                     matrix2.mArray[1][1] + matrix1.mArray[2][2]*matrix2.mArray[2][1],
+                     matrix2.mRows[1][1] + matrix1.mRows[2][2]*matrix2.mRows[2][1],
-                     matrix1.mArray[2][0]*matrix2.mArray[0][2] + matrix1.mArray[2][1] *
+                     matrix1.mRows[2][0]*matrix2.mRows[0][2] + matrix1.mRows[2][1] *
-                     matrix2.mArray[1][2] + matrix1.mArray[2][2]*matrix2.mArray[2][2]);
+                     matrix2.mRows[1][2] + matrix1.mRows[2][2]*matrix2.mRows[2][2]);
 }
 
 // Overloaded operator for multiplication with a vector
 inline Vector3 operator*(const Matrix3x3& matrix, const Vector3& vector) {
-    return Vector3(matrix.mArray[0][0]*vector.x + matrix.mArray[0][1]*vector.y +
+    return Vector3(matrix.mRows[0][0]*vector.x + matrix.mRows[0][1]*vector.y +
-                   matrix.mArray[0][2]*vector.z,
+                   matrix.mRows[0][2]*vector.z,
-                   matrix.mArray[1][0]*vector.x + matrix.mArray[1][1]*vector.y +
+                   matrix.mRows[1][0]*vector.x + matrix.mRows[1][1]*vector.y +
-                   matrix.mArray[1][2]*vector.z,
+                   matrix.mRows[1][2]*vector.z,
-                   matrix.mArray[2][0]*vector.x + matrix.mArray[2][1]*vector.y +
+                   matrix.mRows[2][0]*vector.x + matrix.mRows[2][1]*vector.y +
-                   matrix.mArray[2][2]*vector.z);
+                   matrix.mRows[2][2]*vector.z);
 }
 
 // Overloaded operator for equality condition
 inline bool Matrix3x3::operator==(const Matrix3x3& matrix) const {
-    return (mArray[0][0] == matrix.mArray[0][0] && mArray[0][1] == matrix.mArray[0][1] &&
+    return (mRows[0][0] == matrix.mRows[0][0] && mRows[0][1] == matrix.mRows[0][1] &&
-            mArray[0][2] == matrix.mArray[0][2] &&
+            mRows[0][2] == matrix.mRows[0][2] &&
-            mArray[1][0] == matrix.mArray[1][0] && mArray[1][1] == matrix.mArray[1][1] &&
+            mRows[1][0] == matrix.mRows[1][0] && mRows[1][1] == matrix.mRows[1][1] &&
-            mArray[1][2] == matrix.mArray[1][2] &&
+            mRows[1][2] == matrix.mRows[1][2] &&
-            mArray[2][0] == matrix.mArray[2][0] && mArray[2][1] == matrix.mArray[2][1] &&
+            mRows[2][0] == matrix.mRows[2][0] && mRows[2][1] == matrix.mRows[2][1] &&
-            mArray[2][2] == matrix.mArray[2][2]);
+            mRows[2][2] == matrix.mRows[2][2]);
 }
 
 // Overloaded operator for the is different condition
@@ -300,32 +310,46 @@ inline bool Matrix3x3::operator!= (const Matrix3x3& matrix) const {
 
 // Overloaded operator for addition with assignment
 inline Matrix3x3& Matrix3x3::operator+=(const Matrix3x3& matrix) {
-   mArray[0][0] += matrix.mArray[0][0]; mArray[0][1] += matrix.mArray[0][1];
+   mRows[0][0] += matrix.mRows[0][0]; mRows[0][1] += matrix.mRows[0][1];
-   mArray[0][2] += matrix.mArray[0][2]; mArray[1][0] += matrix.mArray[1][0];
+   mRows[0][2] += matrix.mRows[0][2]; mRows[1][0] += matrix.mRows[1][0];
-   mArray[1][1] += matrix.mArray[1][1]; mArray[1][2] += matrix.mArray[1][2];
+   mRows[1][1] += matrix.mRows[1][1]; mRows[1][2] += matrix.mRows[1][2];
-   mArray[2][0] += matrix.mArray[2][0]; mArray[2][1] += matrix.mArray[2][1];
+   mRows[2][0] += matrix.mRows[2][0]; mRows[2][1] += matrix.mRows[2][1];
-   mArray[2][2] += matrix.mArray[2][2];
+   mRows[2][2] += matrix.mRows[2][2];
    return *this;
 }
 
 // Overloaded operator for substraction with assignment
 inline Matrix3x3& Matrix3x3::operator-=(const Matrix3x3& matrix) {
-   mArray[0][0] -= matrix.mArray[0][0]; mArray[0][1] -= matrix.mArray[0][1];
+   mRows[0][0] -= matrix.mRows[0][0]; mRows[0][1] -= matrix.mRows[0][1];
-   mArray[0][2] -= matrix.mArray[0][2]; mArray[1][0] -= matrix.mArray[1][0];
+   mRows[0][2] -= matrix.mRows[0][2]; mRows[1][0] -= matrix.mRows[1][0];
-   mArray[1][1] -= matrix.mArray[1][1]; mArray[1][2] -= matrix.mArray[1][2];
+   mRows[1][1] -= matrix.mRows[1][1]; mRows[1][2] -= matrix.mRows[1][2];
-   mArray[2][0] -= matrix.mArray[2][0]; mArray[2][1] -= matrix.mArray[2][1];
+   mRows[2][0] -= matrix.mRows[2][0]; mRows[2][1] -= matrix.mRows[2][1];
-   mArray[2][2] -= matrix.mArray[2][2];
+   mRows[2][2] -= matrix.mRows[2][2];
    return *this;
 }
 
 // Overloaded operator for multiplication with a number with assignment
 inline Matrix3x3& Matrix3x3::operator*=(decimal nb) {
-   mArray[0][0] *= nb; mArray[0][1] *= nb; mArray[0][2] *= nb;
+   mRows[0][0] *= nb; mRows[0][1] *= nb; mRows[0][2] *= nb;
-   mArray[1][0] *= nb; mArray[1][1] *= nb; mArray[1][2] *= nb;
+   mRows[1][0] *= nb; mRows[1][1] *= nb; mRows[1][2] *= nb;
-   mArray[2][0] *= nb; mArray[2][1] *= nb; mArray[2][2] *= nb;
+   mRows[2][0] *= nb; mRows[2][1] *= nb; mRows[2][2] *= nb;
    return *this;
 }
 
+// Overloaded operator to return a row of the matrix.
+/// This operator is also used to access a matrix value using the syntax
+/// matrix[row][col].
+inline const Vector3& Matrix3x3::operator[](int row) const {
+    return mRows[row];
+}
+
+// Overloaded operator to return a row of the matrix.
+/// This operator is also used to access a matrix value using the syntax
+/// matrix[row][col].
+inline Vector3& Matrix3x3::operator[](int row) {
+    return mRows[row];
+}
+
 }
 
 #endif

src/mathematics/Quaternion.cpp

@@ -59,69 +59,62 @@ Quaternion::Quaternion(const Matrix3x3& matrix) {
     // Get the trace of the matrix
     decimal trace = matrix.getTrace();
 
-    decimal array[3][3];
-    for (int i=0; i<3; i++) {
-        for (int j=0; j<3; j++) {
-            array[i][j] = matrix.getValue(i, j);
-        }
-    }
-
     decimal r;
     decimal s;
 
     if (trace < 0.0) {
-        if (array[1][1] > array[0][0]) {
+        if (matrix[1][1] > matrix[0][0]) {
-            if(array[2][2] > array[1][1]) {
+            if(matrix[2][2] > matrix[1][1]) {
-                r = sqrt(array[2][2] - array[0][0] - array[1][1] + 1.0);
+                r = sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + decimal(1.0));
-                s = 0.5 / r;
+                s = decimal(0.5) / r;
                 
                 // Compute the quaternion
-                x = (array[2][0] + array[0][2])*s;
+                x = (matrix[2][0] + matrix[0][2]) * s;
-                y = (array[1][2] + array[2][1])*s;
+                y = (matrix[1][2] + matrix[2][1]) * s;
-                z = 0.5*r;
+                z = decimal(0.5) * r;
-                w = (array[1][0] - array[0][1])*s;
+                w = (matrix[1][0] - matrix[0][1]) * s;
             }
             else {
-                r = sqrt(array[1][1] - array[2][2] - array[0][0] + 1.0);
+                r = sqrt(matrix[1][1] - matrix[2][2] - matrix[0][0] + decimal(1.0));
-                s = 0.5 / r;
+                s = decimal(0.5) / r;
 
                 // Compute the quaternion
-                x = (array[0][1] + array[1][0])*s;
+                x = (matrix[0][1] + matrix[1][0]) * s;
-                y = 0.5 * r;
+                y = decimal(0.5) * r;
-                z = (array[1][2] + array[2][1])*s;
+                z = (matrix[1][2] + matrix[2][1]) * s;
-                w = (array[0][2] - array[2][0])*s;
+                w = (matrix[0][2] - matrix[2][0]) * s;
             }
         }
-        else if (array[2][2] > array[0][0]) {
+        else if (matrix[2][2] > matrix[0][0]) {
-            r = sqrt(array[2][2] - array[0][0] - array[1][1] + 1.0);
+            r = sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + decimal(1.0));
-            s = 0.5 / r;
+            s = decimal(0.5) / r;
 
             // Compute the quaternion
-            x = (array[2][0] + array[0][2])*s;
+            x = (matrix[2][0] + matrix[0][2]) * s;
-            y = (array[1][2] + array[2][1])*s;
+            y = (matrix[1][2] + matrix[2][1]) * s;
-            z = 0.5 * r;
+            z = decimal(0.5) * r;
-            w = (array[1][0] - array[0][1])*s;
+            w = (matrix[1][0] - matrix[0][1]) * s;
         }
         else {
-            r = sqrt(array[0][0] - array[1][1] - array[2][2] + 1.0);
+            r = sqrt(matrix[0][0] - matrix[1][1] - matrix[2][2] + decimal(1.0));
-            s = 0.5 / r;
+            s = decimal(0.5) / r;
 
             // Compute the quaternion
-            x = 0.5 * r;
+            x = decimal(0.5) * r;
-            y = (array[0][1] + array[1][0])*s;
+            y = (matrix[0][1] + matrix[1][0]) * s;
-            z = (array[2][0] - array[0][2])*s;
+            z = (matrix[2][0] - matrix[0][2]) * s;
-            w = (array[2][1] - array[1][2])*s;
+            w = (matrix[2][1] - matrix[1][2]) * s;
         }
     }
     else {
-        r = sqrt(trace + 1.0);
+        r = sqrt(trace + decimal(1.0));
-        s = 0.5/r;
+        s = decimal(0.5) / r;
 
         // Compute the quaternion
-        x = (array[2][1]-array[1][2])*s;
+        x = (matrix[2][1] - matrix[1][2]) * s;
-        y = (array[0][2]-array[2][0])*s;
+        y = (matrix[0][2] - matrix[2][0]) * s;
-        z = (array[1][0]-array[0][1])*s;
+        z = (matrix[1][0] - matrix[0][1]) * s;
-        w = 0.5 * r;
+        w = decimal(0.5) * r;
     }
 }
 
@@ -146,7 +139,7 @@ void Quaternion::getRotationAngleAxis(decimal& angle, Vector3& axis) const {
     }
 
     // Compute the roation angle
-    angle = acos(quaternion.w) * 2.0;
+    angle = acos(quaternion.w) * decimal(2.0);
 
     // Compute the 3D rotation axis
     Vector3 rotationAxis(quaternion.x, quaternion.y, quaternion.z);
@@ -165,7 +158,7 @@ Matrix3x3 Quaternion::getMatrix() const {
     decimal s = 0.0;
 
     if (nQ > 0.0) {
-        s = 2.0/nQ;
+        s = decimal(2.0) / nQ;
     }
 
     // Computations used for optimization (less multiplications)
@@ -183,9 +176,9 @@ Matrix3x3 Quaternion::getMatrix() const {
     decimal zzs = z*zs;
 
     // Create the matrix corresponding to the quaternion
-    return Matrix3x3(1.0-yys-zzs, xys-wzs, xzs + wys,
+    return Matrix3x3(decimal(1.0) - yys - zzs, xys-wzs, xzs + wys,
-                     xys + wzs, 1.0-xxs-zzs, yzs-wxs,
+                     xys + wzs, decimal(1.0) - xxs - zzs, yzs-wxs,
-                     xzs-wys, yzs + wxs, 1.0-xxs-yys);
+                     xzs-wys, yzs + wxs, decimal(1.0) - xxs - yys);
 }
 
 // Compute the spherical linear interpolation between two quaternions.
@@ -208,9 +201,9 @@ Quaternion Quaternion::slerp(const Quaternion& quaternion1,
     // Because of precision, if cos(theta) is nearly 1,
     // therefore theta is nearly 0 and we can write
     // sin((1-t)*theta) as (1-t) and sin(t*theta) as t
-    const decimal epsilon = 0.00001;
+    const decimal epsilon = decimal(0.00001);
     if(1-cosineTheta < epsilon) {
-        return quaternion1 * (1.0-t) + quaternion2 * (t * invert);
+        return quaternion1 * (decimal(1.0)-t) + quaternion2 * (t * invert);
     }
 
     // Compute the theta angle
@@ -220,7 +213,7 @@ Quaternion Quaternion::slerp(const Quaternion& quaternion1,
     decimal sineTheta = sin(theta);
 
     // Compute the two coefficients that are in the spherical linear interpolation formula
-    decimal coeff1 = sin((1.0-t)*theta) / sineTheta;
+    decimal coeff1 = sin((decimal(1.0)-t)*theta) / sineTheta;
     decimal coeff2 = sin(t*theta) / sineTheta * invert;
 
     // Compute and return the interpolated quaternion

src/mathematics/Quaternion.h

@@ -46,8 +46,17 @@ struct Quaternion {
 
         // -------------------- Attributes -------------------- //
 
-        /// Components of the quaternion
+        /// Component x
-        decimal x, y, z, w;
+        decimal x;
+
+        /// Component y
+        decimal y;
+
+        /// Component z
+        decimal z;
+
+        /// Component w
+        decimal w;
 
         // -------------------- Methods -------------------- //
 
@@ -69,12 +78,21 @@ struct Quaternion {
         /// Destructor
         ~Quaternion();
 
+        /// Set all the values
+        void setAllValues(decimal newX, decimal newY, decimal newZ, decimal newW);
+
+        /// Set the quaternion to zero
+        void setToZero();
+
         /// Return the vector v=(x y z) of the quaternion
-        Vector3 vectorV() const;
+        Vector3 getVectorV() const;
 
         /// Return the length of the quaternion
         decimal length() const;
 
+        /// Normalize the quaternion
+        void normalize();
+
         /// Return the unit quaternion
         Quaternion getUnit() const;
 
@@ -112,6 +130,9 @@ struct Quaternion {
         /// Overloaded operator for the multiplication
         Quaternion operator*(const Quaternion& quaternion) const;
 
+        /// Overloaded operator for the multiplication with a vector
+        Vector3 operator*(const Vector3& point);
+
         /// Overloaded operator for assignment
         Quaternion& operator=(const Quaternion& quaternion);
 
@@ -119,8 +140,24 @@ struct Quaternion {
         bool operator==(const Quaternion& quaternion) const;
 };
 
+/// Set all the values
+inline void Quaternion::setAllValues(decimal newX, decimal newY, decimal newZ, decimal newW) {
+    x = newX;
+    y = newY;
+    z = newZ;
+    w = newW;
+}
+
+/// Set the quaternion to zero
+inline void Quaternion::setToZero() {
+    x = 0;
+    y = 0;
+    z = 0;
+    w = 0;
+}
+
 // Return the vector v=(x y z) of the quaternion
-inline Vector3 Quaternion::vectorV() const {
+inline Vector3 Quaternion::getVectorV() const {
 
     // Return the vector v
     return Vector3(x, y, z);
@@ -131,6 +168,20 @@ inline decimal Quaternion::length() const {
     return sqrt(x*x + y*y + z*z + w*w);
 }
 
+// Normalize the quaternion
+inline void Quaternion::normalize() {
+
+    decimal l = length();
+
+    // Check if the length is not equal to zero
+    assert (l > MACHINE_EPSILON);
+
+    x /= l;
+    y /= l;
+    z /= l;
+    w /= l;
+}
+
 // Return the unit quaternion
 inline Quaternion Quaternion::getUnit() const {
     decimal lengthQuaternion = length();
@@ -192,9 +243,16 @@ inline Quaternion Quaternion::operator*(decimal nb) const {
 
 // Overloaded operator for the multiplication of two quaternions
 inline Quaternion Quaternion::operator*(const Quaternion& quaternion) const {
-    return Quaternion(w * quaternion.w - vectorV().dot(quaternion.vectorV()),
+    return Quaternion(w * quaternion.w - getVectorV().dot(quaternion.getVectorV()),
-                      w * quaternion.vectorV() + quaternion.w * vectorV() +
+                      w * quaternion.getVectorV() + quaternion.w * getVectorV() +
-                      vectorV().cross(quaternion.vectorV()));
+                      getVectorV().cross(quaternion.getVectorV()));
+}
+
+// Overloaded operator for the multiplication with a vector.
+/// This methods rotates a point given the rotation of a quaternion.
+inline Vector3 Quaternion::operator*(const Vector3& point) {
+    Quaternion p(point.x, point.y, point.z, 0.0);
+    return (((*this) * p) * getConjugate()).getVectorV();
 }
 
 // Overloaded operator for the assignment

src/mathematics/Transform.h

@@ -99,6 +99,9 @@ class Transform {
                                                const Transform& newTransform,
                                                decimal interpolationFactor);
 
+        /// Return the identity transform
+        static Transform identity();
+
         /// Return the transformed vector
         Vector3 operator*(const Vector3& vector) const;
 
@@ -153,12 +156,12 @@ inline void Transform::setFromOpenGL(decimal* openglMatrix) {
 // Get the OpenGL matrix of the transform
 inline void Transform::getOpenGLMatrix(decimal* openglMatrix) const {
     const Matrix3x3& matrix = mOrientation.getMatrix();
-    openglMatrix[0] = matrix.getValue(0, 0); openglMatrix[1] = matrix.getValue(1, 0);
+    openglMatrix[0] = matrix[0][0]; openglMatrix[1] = matrix[1][0];
-    openglMatrix[2] = matrix.getValue(2, 0); openglMatrix[3] = 0.0;
+    openglMatrix[2] = matrix[2][0]; openglMatrix[3] = 0.0;
-    openglMatrix[4] = matrix.getValue(0, 1); openglMatrix[5] = matrix.getValue(1, 1);
+    openglMatrix[4] = matrix[0][1]; openglMatrix[5] = matrix[1][1];
-    openglMatrix[6] = matrix.getValue(2, 1); openglMatrix[7] = 0.0;
+    openglMatrix[6] = matrix[2][1]; openglMatrix[7] = 0.0;
-    openglMatrix[8] = matrix.getValue(0, 2); openglMatrix[9] = matrix.getValue(1, 2);
+    openglMatrix[8] = matrix[0][2]; openglMatrix[9] = matrix[1][2];
-    openglMatrix[10] = matrix.getValue(2, 2); openglMatrix[11] = 0.0;
+    openglMatrix[10] = matrix[2][2]; openglMatrix[11] = 0.0;
     openglMatrix[12] = mPosition.x; openglMatrix[13] = mPosition.y;
     openglMatrix[14] = mPosition.z; openglMatrix[15] = 1.0;
 }
@@ -185,6 +188,11 @@ inline Transform Transform::interpolateTransforms(const Transform& oldTransform,
     return Transform(interPosition, interOrientation);
 }
 
+// Return the identity transform
+inline Transform Transform::identity() {
+    return Transform(Vector3(0, 0, 0), Quaternion::identity());
+}
+
 // Return the transformed vector
 inline Vector3 Transform::operator*(const Vector3& vector) const {
     return (mOrientation.getMatrix() * vector) + mPosition;

src/mathematics/Vector3.cpp

@@ -58,7 +58,7 @@ Vector3 Vector3::getUnit() const {
     assert(lengthVector > MACHINE_EPSILON);
 
     // Compute and return the unit vector
-    decimal lengthInv = 1.0 / lengthVector;
+    decimal lengthInv = decimal(1.0) / lengthVector;
     return Vector3(x * lengthInv, y * lengthInv, z * lengthInv);
 }
 

src/mathematics/Vector3.h

@@ -46,8 +46,14 @@ struct Vector3 {
 
         // -------------------- Attributes -------------------- //
 
-        /// Values of the 3D vector
+        /// Component x
-        decimal x, y, z;
+        decimal x;
+
+        /// Component y
+        decimal y;
+
+        /// Component z
+        decimal z;
 
         // -------------------- Methods -------------------- //
 
@@ -66,7 +72,10 @@ struct Vector3 {
         /// Set all the values of the vector
         void setAllValues(decimal newX, decimal newY, decimal newZ);
 
-        /// Return the lenght of the vector
+        /// Set the vector to zero
+        void setToZero();
+
+        /// Return the length of the vector
         decimal length() const;
 
         /// Return the square of the length of the vector
@@ -142,6 +151,13 @@ struct Vector3 {
         friend Vector3 operator/(const Vector3& vector, decimal number);
 };
 
+// Set the vector to zero
+inline void Vector3::setToZero() {
+    x = 0;
+    y = 0;
+    z = 0;
+}
+
 // Set all the values of the vector
 inline void Vector3::setAllValues(decimal newX, decimal newY, decimal newZ) {
     x = newX;
@@ -185,13 +201,6 @@ inline Vector3 Vector3::getAbsoluteVector() const {
     return Vector3(std::abs(x), std::abs(y), std::abs(z));
 }
 
-// Return true if two vectors are parallel
-inline bool Vector3::isParallelWith(const Vector3& vector) const {
-    decimal scalarProd = this->dot(vector);
-    return approxEqual(std::abs(scalarProd), length() * vector.length());
-}    
-
-
 // Return the axis with the minimal value
 inline int Vector3::getMinAxis() const {
     return (x < y ? (x < z ? 0 : 2) : (y < z ? 1 : 2));

src/mathematics/mathematics_functions.h

@@ -35,9 +35,10 @@ namespace reactphysics3d {
 
 // ---------- Mathematics functions ---------- //
 
-/// function to test if two real numbers are (almost) equal
+/// Function to test if two real numbers are (almost) equal
 /// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON]
-inline bool approxEqual(decimal a, decimal b, decimal epsilon = 1.0e-10) {
+inline bool approxEqual(decimal a, decimal b, decimal epsilon = MACHINE_EPSILON) {
+
     decimal difference = a - b;
     return (difference < epsilon && difference > -epsilon);
 }

test/CMakeLists.txt

@@ -0,0 +1,20 @@
+# Minimum cmake version required
+cmake_minimum_required(VERSION 2.6)
+
+# Project configuration
+PROJECT(TESTS)
+
+# Headers
+INCLUDE_DIRECTORIES(${REACTPHYSICS3D_SOURCE_DIR}/test)
+
+# Sources files of tests
+file (
+  GLOB_RECURSE
+  TESTS_SOURCE_FILES
+  ${REACTPHYSICS3D_SOURCE_DIR}/test/*
+)
+
+# Create the tests executable
+ADD_EXECUTABLE(tests ${TESTS_SOURCE_FILES})
+
+TARGET_LINK_LIBRARIES(tests reactphysics3d)

test/Test.cpp

@@ -0,0 +1,86 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2013 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.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "Test.h"
+
+using namespace reactphysics3d;
+
+/// Constructor
+Test::Test(std::ostream* stream) : mOutputStream(stream), mNbPassedTests(0), mNbFailedTests(0) {
+
+}
+
+/// Destructor
+Test::~Test() {
+
+}
+
+// Called to test a boolean condition.
+// This method should not be called directly in your test but you should call test() instead (macro)
+void Test::applyTest(bool condition, const std::string& testText,
+                     const char* filename, long lineNumber) {
+
+    // If the boolean condition is true
+    if (condition) {
+
+        // The test passed, call the succeed() method
+        succeed();
+    }
+    else {  // If the boolean condition is false
+
+        // The test failed, call the applyFail() method
+        applyFail(testText, filename, lineNumber);
+    }
+}
+
+// Called when a test has failed.
+// This method should not be called directly in your test buy you should call fail() instead (macro)
+void Test::applyFail(const std::string& testText, const char* filename, long lineNumber) {
+
+    if (mOutputStream) {
+
+        // Display the failure message
+        *mOutputStream << typeid(*this).name() << "failure : (" << testText << "), " <<
+                  filename << "(line " << lineNumber << ")" << std::endl;
+    }
+
+    // Increase the number of failed tests
+    mNbFailedTests++;
+}
+
+/// Display the report of the unit test and return the number of failed tests
+long Test::report() const {
+
+    if(mOutputStream) {
+        *mOutputStream << "Test \"" <<
+         typeid(*this).name()
+               << "\":\n\tPassed: " << mNbPassedTests << "\tFailed: " <<
+                  mNbFailedTests  << std::endl;
+      }
+
+    // Return the number of failed tests
+    return mNbFailedTests;
+}

test/Test.h

@@ -0,0 +1,153 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2013 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_H
+#define TEST_H
+
+// Libraries
+#include <string>
+#include <iostream>
+#include <cassert>
+
+/// Reactphysics3D namespace
+namespace reactphysics3d {
+
+// Macros
+#define test(condition) applyTest(condition, #condition, __FILE__, __LINE__)
+#define fail(text) applyFail(text, __FILE__, __LINE__);
+
+// Class Test
+/**
+ * This abstract class represents a unit test. To create a unit test, you simply
+ * need to create a class that inherits from the Test class, override the run() method and
+ * use the test() and fail() macros.
+ */
+class Test {
+
+    private :
+
+        // ---------- Attributes ---------- //
+
+        /// Number of tests that passed
+        long mNbPassedTests;
+
+        /// Number of tests that failed
+        long mNbFailedTests;
+
+        /// Output stream
+        std::ostream* mOutputStream;
+
+        // ---------- Methods ---------- //
+
+        /// Copy constructor is private
+        Test(const Test&);
+
+        /// Assignment operator is private
+        Test& operator=(const Test& test);
+
+    protected :
+
+        // ---------- Methods ---------- //
+
+        /// Called to test a boolean condition.
+        /// This method should not be called directly in your test but you should
+        /// call test() instead (macro)
+        void applyTest(bool condition, const std::string& testText,
+                       const char* filename, long lineNumber);
+
+        /// Called when a test has failed.
+        /// This method should not be called directly in your test buy you should
+        /// call fail() instead (macro)
+        void applyFail(const std::string& testText, const char* filename, long lineNumber);
+
+    public :
+
+        // ---------- Methods ---------- //
+
+        /// Constructor
+        Test(std::ostream* stream = &std::cout);
+
+        /// Destructor
+        ~Test();
+
+        /// Return the number of passed tests
+        long getNbPassedTests() const;
+
+        /// Return the number of failed tests
+        long getNbFailedTests() const;
+
+        /// Return the output stream
+        const std::ostream* getOutputStream() const;
+
+        /// Set the output stream
+        void setOutputStream(std::ostream *stream);
+
+        /// Run the unit test
+        virtual void run() = 0;
+
+        /// Called when a test passed
+        void succeed();
+
+        /// Reset the unit test
+        virtual void reset();
+
+        /// Display the report of the unit test and return the number of failed tests
+        long report() const;
+};
+
+// Called when a test passed
+inline void Test::succeed() {
+    mNbPassedTests++;
+}
+
+// Reset the unit test
+inline void Test::reset() {
+    mNbPassedTests = 0;
+    mNbFailedTests = 0;
+}
+
+// Return the number of passed tests
+inline long Test::getNbPassedTests() const {
+    return mNbPassedTests;
+}
+
+// Return the number of failed tests
+inline long Test::getNbFailedTests() const {
+    return mNbFailedTests;
+}
+
+// Return the output stream
+inline const std::ostream* Test::getOutputStream() const {
+    return mOutputStream;
+}
+
+// Set the output stream
+inline void Test::setOutputStream(std::ostream* stream) {
+    mOutputStream = stream;
+}
+
+}
+
+#endif

test/TestSuite.cpp

@@ -0,0 +1,145 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2013 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.    *
+*                                                                               *
+********************************************************************************/
+
+// Librairies
+#include "TestSuite.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+TestSuite::TestSuite(const std::string& name, std::ostream* outputStream)
+          : mName(name), mOutputStream(outputStream) {
+
+}
+
+// Return the number of passed tests
+long TestSuite::getNbPassedTests() const {
+
+    long nbPassedTests = 0;
+
+    for (size_t i=0; i<mTests.size(); i++) {
+        assert(mTests[i]);
+        nbPassedTests += mTests[i]->getNbPassedTests();
+    }
+
+    return nbPassedTests;
+}
+
+// Return the number of failed tests
+long TestSuite::getNbFailedTests() const {
+    long nbFailedTests = 0;
+
+    for (size_t i=0; i<mTests.size(); i++) {
+        assert(mTests[i]);
+        nbFailedTests += mTests[i]->getNbFailedTests();
+    }
+
+    return nbFailedTests;
+}
+
+// Add a unit test in the test suite
+void TestSuite::addTest(Test* test) {
+    if (test == NULL) {
+        throw std::invalid_argument("Error : You cannot add a NULL test in the test suite.");
+    }
+    else if (mOutputStream != NULL && test->getOutputStream() == NULL) {
+        test->setOutputStream(mOutputStream);
+    }
+
+    // Add the test to the suite
+    mTests.push_back(test);
+
+    // Reset the added test
+    test->reset();
+}
+
+// Add a test suite to the current test suite
+void TestSuite::addTestSuite(const TestSuite& testSuite) {
+
+    // Add each test of the test suite to the current one
+    for (size_t i =0; i < testSuite.mTests.size(); i++) {
+        assert(testSuite.mTests[i] != NULL);
+        addTest(testSuite.mTests[i]);
+    }
+}
+
+// Launch the tests of the test suite
+void TestSuite::run() {
+
+    // Reset all the tests
+    reset();
+
+    // Run all the tests
+    for (size_t i=0; i < mTests.size(); i++) {
+        assert(mTests[i] != NULL);
+        mTests[i]->run();
+    }
+}
+
+// Reset the test suite
+void TestSuite::reset() {
+    for(size_t i=0; i < mTests.size(); ++i) {
+        assert(mTests[i]);
+        mTests[i]->reset();
+      }
+}
+
+// Display the tests report and return the number of failed tests
+long TestSuite::report() const {
+    if (mOutputStream != NULL) {
+        long nbFailedTests = 0;
+
+        *mOutputStream << "Test Suite \"" << mName << "\"\n=====";
+        size_t i;
+        for (i=0; i < mName.size(); i++) {
+            *mOutputStream << "=";
+        }
+        *mOutputStream << "=" << std::endl;
+        for (i=0; i < mTests.size(); i++) {
+            assert(mTests[i] != NULL);
+            nbFailedTests += mTests[i]->report();
+        }
+        *mOutputStream << "=====";
+        for (i=0; i < mName.size(); i++) {
+            *mOutputStream << "=";
+        }
+        *mOutputStream << "=" << std::endl;
+
+        // Return the number of failed tests
+        return nbFailedTests;
+    }
+    else {
+        return getNbFailedTests();
+    }
+}
+
+// Delete all the tests
+void TestSuite::clear() {
+
+    for (size_t i=0; i<mTests.size(); i++) {
+        delete mTests[i];
+        mTests[i] = NULL;
+    }
+}

test/TestSuite.h

@@ -0,0 +1,125 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2013 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_SUITE_H
+#define TEST_SUITE_H
+
+// Libraries
+#include "Test.h"
+#include <vector>
+
+/// Reactphysics3D namespace
+namespace reactphysics3d {
+
+// Class TestSuite
+/**
+ * This class represents a test suite that can
+ * contains multiple unit tests. You can also add a test suite inside
+ * another test suite (all the tests of the first test suite will be added
+ * to the second one).
+ */
+class TestSuite {
+
+    private :
+
+        // ---------- Attributes ---------- //
+
+        /// Name of the test suite
+        std::string mName;
+
+        /// Output stream
+        std::ostream* mOutputStream;
+
+        /// All the tests of the test suite
+        std::vector<Test*> mTests;
+
+        // ---------- Methods ---------- //
+
+        /// Reset the test suite
+        void reset();
+
+        /// Private copy-constructor
+        TestSuite(const TestSuite& testSuite);
+
+        /// Private assigmnent operator
+        TestSuite& operator=(const TestSuite testSuite);
+
+  public :
+
+        // ---------- Methods ---------- //
+
+        /// Constructor
+        TestSuite(const std::string& name, std::ostream* outputStream = &std::cout);
+
+        /// Return the name of the test suite
+        std::string getName() const;
+
+        /// Return the number of passed tests
+        long getNbPassedTests() const;
+
+        /// Return the number of failed tests
+        long getNbFailedTests() const;
+
+        /// Return the output stream
+        const std::ostream* getOutputStream() const;
+
+        /// Set the output stream
+        void setOutputStream(std::ostream* outputStream);
+
+        /// Add a unit test in the test suite
+        void addTest(Test* test);
+
+        /// Add a test suite to the current test suite
+        void addTestSuite(const TestSuite& testSuite);
+
+        /// Launch the tests of the test suite
+        void run();
+
+        /// Display the tests report and return the number of failed tests
+        long report() const;
+
+        // Delete all the tests
+        void clear();
+
+};
+
+// Return the name of the test suite
+inline std::string TestSuite::getName() const {
+    return mName;
+}
+
+// Return the output stream
+inline const std::ostream* TestSuite::getOutputStream() const {
+    return mOutputStream;
+}
+
+// Set the output stream
+inline void TestSuite::setOutputStream(std::ostream* outputStream) {
+    mOutputStream = outputStream;
+}
+
+}
+
+#endif

test/main.cpp

@@ -0,0 +1,58 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2013 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.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "TestSuite.h"
+#include "tests/mathematics/TestVector3.h"
+#include "tests/mathematics/TestTransform.h"
+#include "tests/mathematics/TestQuaternion.h"
+#include "tests/mathematics/TestMatrix3x3.h"
+
+using namespace reactphysics3d;
+
+int main() {
+
+    TestSuite testSuite("ReactPhysics3D Tests");
+
+    // ---------- Mathematics tests ---------- //
+
+    testSuite.addTest(new TestVector3);
+    testSuite.addTest(new TestTransform);
+    testSuite.addTest(new TestQuaternion);
+    testSuite.addTest(new TestMatrix3x3);
+
+    // ----------------------------- --------- //
+
+    // Run the tests
+    testSuite.run();
+
+    // Display the report
+    long nbFailedTests = testSuite.report();
+
+    // Clear the tests from the test suite
+    testSuite.clear();
+
+    return nbFailedTests;
+}

test/tests/mathematics/TestMatrix3x3.h

@@ -0,0 +1,280 @@
+
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2013 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_MATRIX3X3_H
+#define TEST_MATRIX3X3_H
+
+#endif
+
+// Libraries
+#include "../../Test.h"
+#include "../../../src/mathematics/Matrix3x3.h"
+
+using namespace reactphysics3d;
+
+/// Reactphysics3D namespace
+namespace reactphysics3d {
+
+// Class TestMatrix3x3
+/**
+ * Unit test for the Matrix3x3 class
+ */
+class TestMatrix3x3 : public Test {
+
+    private :
+
+        // ---------- Atributes ---------- //
+
+        /// Identity transform
+        Matrix3x3 mIdentity;
+
+        /// First example matrix
+        Matrix3x3 mMatrix1;
+
+    public :
+
+        // ---------- Methods ---------- //
+
+        /// Constructor
+        TestMatrix3x3() : mIdentity(Matrix3x3::identity()),
+                          mMatrix1(2, 24, 4, 5, -6, 234, -15, 11, 66) {
+
+
+        }
+
+        /// Run the tests
+        void run() {
+            testConstructors();
+            testGetSet();
+            testIdentity();
+            testOthersMethods();
+            testOperators();
+        }
+
+        /// Test the constructors
+        void testConstructors() {
+
+            Matrix3x3 test1(5.0);
+            Matrix3x3 test2(2, 3, 4, 5, 6, 7, 8, 9, 10);
+            Matrix3x3 test3(mMatrix1);
+            test(test1[0][0] == 5);
+            test(test1[0][1] == 5);
+            test(test1[0][2] == 5);
+            test(test1[1][0] == 5);
+            test(test1[1][1] == 5);
+            test(test1[1][2] == 5);
+            test(test1[2][0] == 5);
+            test(test1[2][1] == 5);
+            test(test1[2][2] == 5);
+
+            test(test2[0][0] == 2);
+            test(test2[0][1] == 3);
+            test(test2[0][2] == 4);
+            test(test2[1][0] == 5);
+            test(test2[1][1] == 6);
+            test(test2[1][2] == 7);
+            test(test2[2][0] == 8);
+            test(test2[2][1] == 9);
+            test(test2[2][2] == 10);
+
+            test(test3 == mMatrix1);
+        }
+
+        /// Test the getter and setter methods
+        void testGetSet() {
+
+            // Test method to set all the values
+            Matrix3x3 test2;
+            test2.setAllValues(2, 24, 4, 5, -6, 234, -15, 11, 66);
+            test(test2 == mMatrix1);
+
+            // Test method to set to zero
+            test2.setToZero();
+            test(test2 == Matrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0));
+
+            // Test method that returns a column
+            Vector3 column1 = mMatrix1.getColumn(0);
+            Vector3 column2 = mMatrix1.getColumn(1);
+            Vector3 column3 = mMatrix1.getColumn(2);
+            test(column1 == Vector3(2, 5, -15));
+            test(column2 == Vector3(24, -6, 11));
+            test(column3 == Vector3(4, 234, 66));
+
+            // Test method that returns a row
+            Vector3 row1 = mMatrix1.getRow(0);
+            Vector3 row2 = mMatrix1.getRow(1);
+            Vector3 row3 = mMatrix1.getRow(2);
+            test(row1 == Vector3(2, 24, 4));
+            test(row2 == Vector3(5, -6, 234));
+            test(row3 == Vector3(-15, 11, 66));
+        }
+
+        /// Test the identity methods
+        void testIdentity() {
+
+            Matrix3x3 identity = Matrix3x3::identity();
+            Matrix3x3 test1;
+            test1.setToIdentity();
+
+            test(identity[0][0] == 1);
+            test(identity[0][1] == 0);
+            test(identity[0][2] == 0);
+            test(identity[1][0] == 0);
+            test(identity[1][1] == 1);
+            test(identity[1][2] == 0);
+            test(identity[2][0] == 0);
+            test(identity[2][1] == 0);
+            test(identity[2][2] == 1);
+
+            test(test1 == Matrix3x3::identity());
+        }
+
+        /// Test others methods
+        void testOthersMethods() {
+
+            // Test transpose
+            Matrix3x3 transpose = mMatrix1.getTranspose();
+            test(transpose == Matrix3x3(2, 5, -15, 24, -6, 11, 4, 234, 66));
+
+            // Test trace
+            test(mMatrix1.getTrace() == 62);
+            test(Matrix3x3::identity().getTrace() == 3);
+
+            // Test determinant
+            Matrix3x3 matrix(-24, 64, 253, -35, 52, 72, 21, -35, -363);
+            test(mMatrix1.getDeterminant() == -98240);
+            test(matrix.getDeterminant() == -290159);
+            test(mIdentity.getDeterminant() == 1);
+
+            // Test inverse
+            Matrix3x3 inverseMatrix = matrix.getInverse();
+            test(approxEqual(inverseMatrix[0][0], decimal(0.056369), decimal(10e-6)));
+            test(approxEqual(inverseMatrix[0][1], decimal(-0.049549), decimal(10e-6)));
+            test(approxEqual(inverseMatrix[0][2], decimal(0.029460), decimal(10e-6)));
+            test(approxEqual(inverseMatrix[1][0], decimal(0.038575), decimal(10e-6)));
+            test(approxEqual(inverseMatrix[1][1], decimal(-0.011714), decimal(10e-6)));
+            test(approxEqual(inverseMatrix[1][2], decimal(0.024562), decimal(10e-6)));
+            test(approxEqual(inverseMatrix[2][0], decimal(-0.000458), decimal(10e-6)));
+            test(approxEqual(inverseMatrix[2][1], decimal(-0.001737), decimal(10e-6)));
+            test(approxEqual(inverseMatrix[2][2], decimal(-0.003419), decimal(10e-6)));
+            Matrix3x3 inverseMatrix1 = mMatrix1.getInverse();
+            test(approxEqual(inverseMatrix1[0][0], decimal(0.030232), decimal(10e-6)));
+            test(approxEqual(inverseMatrix1[0][1], decimal(0.015676), decimal(10e-6)));
+            test(approxEqual(inverseMatrix1[0][2], decimal(-0.057410), decimal(10e-6)));
+            test(approxEqual(inverseMatrix1[1][0], decimal(0.039088), decimal(10e-6)));
+            test(approxEqual(inverseMatrix1[1][1], decimal(-0.001954), decimal(10e-6)));
+            test(approxEqual(inverseMatrix1[1][2], decimal(0.004560), decimal(10e-6)));
+            test(approxEqual(inverseMatrix1[2][0], decimal(0.000356), decimal(10e-6)));
+            test(approxEqual(inverseMatrix1[2][1], decimal(0.003888), decimal(10e-6)));
+            test(approxEqual(inverseMatrix1[2][2], decimal(0.001344), decimal(10e-6)));
+
+            // Test absolute matrix
+            Matrix3x3 matrix2(-2, -3, -4, -5, -6, -7, -8, -9, -10);
+            test(matrix.getAbsoluteMatrix() == Matrix3x3(24, 64, 253, 35, 52, 72, 21, 35, 363));
+            Matrix3x3 absoluteMatrix = matrix2.getAbsoluteMatrix();
+            test(absoluteMatrix == Matrix3x3(2, 3, 4, 5, 6, 7, 8, 9, 10));
+        }
+
+        /// Test the operators
+        void testOperators() {
+
+            // Test addition
+            Matrix3x3 matrix1(2, 3, 4, 5, 6, 7, 8, 9, 10);
+            Matrix3x3 matrix2(-2, 3, -5, 10, 4, 7, 2, 5, 8);
+            Matrix3x3 addition1 = matrix1 + matrix2;
+            Matrix3x3 addition2(matrix1);
+            addition2 += matrix2;
+            test(addition1 == Matrix3x3(0, 6, -1, 15, 10, 14, 10, 14, 18));
+            test(addition2 == Matrix3x3(0, 6, -1, 15, 10, 14, 10, 14, 18));
+
+            // Test substraction
+            Matrix3x3 substraction1 = matrix1 - matrix2;
+            Matrix3x3 substraction2(matrix1);
+            substraction2 -= matrix2;
+            test(substraction1 == Matrix3x3(4, 0, 9, -5, 2, 0, 6, 4, 2));
+            test(substraction2 == Matrix3x3(4, 0, 9, -5, 2, 0, 6, 4, 2));
+
+            // Test negative operator
+            Matrix3x3 negative = -matrix1;
+            test(negative == Matrix3x3(-2, -3, -4, -5, -6, -7, -8, -9, -10));
+
+            // Test multiplication with a number
+            Matrix3x3 multiplication1 = 3 * matrix1;
+            Matrix3x3 multiplication2 = matrix1 * 3;
+            Matrix3x3 multiplication3(matrix1);
+            multiplication3 *= 3;
+            test(multiplication1 == Matrix3x3(6, 9, 12, 15, 18, 21, 24, 27, 30));
+            test(multiplication2 == Matrix3x3(6, 9, 12, 15, 18, 21, 24, 27, 30));
+            test(multiplication3 == Matrix3x3(6, 9, 12, 15, 18, 21, 24, 27, 30));
+
+            // Test multiplication with a matrix
+            Matrix3x3 multiplication4 = matrix1 * matrix2;
+            Matrix3x3 multiplication5 = matrix2 * matrix1;
+            test(multiplication4 == Matrix3x3(34, 38, 43, 64, 74, 73, 94, 110, 103));
+            test(multiplication5 == Matrix3x3(-29, -33, -37, 96, 117, 138, 93, 108, 123));
+
+            // Test multiplication with a vector
+            Vector3 vector1(3, -32, 59);
+            Vector3 vector2(-31, -422, 34);
+            Vector3 test1 = matrix1 * vector1;
+            Vector3 test2 = matrix2 * vector2;
+            test(test1 == Vector3(146, 236, 326));
+            test(test2 == Vector3(-1374, -1760, -1900));
+
+            // Test equality operators
+            test(Matrix3x3(34, 38, 43, 64, 74, 73, 94, 110, 103) ==
+                 Matrix3x3(34, 38, 43, 64, 74, 73, 94, 110, 103));
+            test(Matrix3x3(34, 64, 43, 7, -1, 73, 94, 110, 103) !=
+                 Matrix3x3(34, 38, 43, 64, 74, 73, 94, 110, 103));
+
+            // Test operator to read a value
+            test(mMatrix1[0][0] == 2);
+            test(mMatrix1[0][1] == 24);
+            test(mMatrix1[0][2] == 4);
+            test(mMatrix1[1][0] == 5);
+            test(mMatrix1[1][1] == -6);
+            test(mMatrix1[1][2] == 234);
+            test(mMatrix1[2][0] == -15);
+            test(mMatrix1[2][1] == 11);
+            test(mMatrix1[2][2] == 66);
+
+            // Test operator to set a value
+            Matrix3x3 test3;
+            test3[0][0] = 2;
+            test3[0][1] = 24;
+            test3[0][2] = 4;
+            test3[1][0] = 5;
+            test3[1][1] = -6;
+            test3[1][2] = 234;
+            test3[2][0] = -15;
+            test3[2][1] = 11;
+            test3[2][2] = 66;
+            test(test3 == mMatrix1);
+        }
+
+ };
+
+}

test/tests/mathematics/TestQuaternion.h

@@ -0,0 +1,231 @@
+
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2013 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_QUATERNION_H
+#define TEST_QUATERNION_H
+
+#endif
+
+// Libraries
+#include "../../Test.h"
+#include "../../../src/mathematics/Quaternion.h"
+
+using namespace reactphysics3d;
+
+/// Reactphysics3D namespace
+namespace reactphysics3d {
+
+// Class TestQuaternion
+/**
+ * Unit test for the Quaternion class
+ */
+class TestQuaternion : public Test {
+
+    private :
+
+        // ---------- Atributes ---------- //
+
+        /// Identity Quaternion
+        Quaternion mIdentity;
+
+        /// First test quaternion
+        Quaternion mQuaternion1;
+
+    public :
+
+        // ---------- Methods ---------- //
+
+        /// Constructor
+        TestQuaternion() : mIdentity(Quaternion::identity()) {
+
+            decimal sinA = sin(decimal(PI/8.0));
+            decimal cosA = cos(decimal(PI/8.0));
+            Vector3 vector(2, 3, 4);
+            vector.normalize();
+            mQuaternion1 = Quaternion(vector.x * sinA, vector.y * sinA, vector.z * sinA, cosA);
+            mQuaternion1.normalize();
+        }
+
+        /// Run the tests
+        void run() {
+            testConstructors();
+            testUnitLengthNormalize();
+            testOthersMethods();
+            testOperators();
+        }
+
+        /// Test the constructors
+        void testConstructors() {
+
+            Quaternion quaternion1(mQuaternion1);
+            test(mQuaternion1== quaternion1);
+
+            Quaternion quaternion2(4, 5, 6, 7);
+            test(quaternion2 == Quaternion(4, 5, 6, 7));
+
+            Quaternion quaternion3(8, Vector3(3, 5, 2));
+            test(quaternion3 == Quaternion(3, 5, 2, 8));
+
+            Quaternion quaternion4(mQuaternion1.getMatrix());
+            test(approxEqual(quaternion4.x, mQuaternion1.x));
+            test(approxEqual(quaternion4.y, mQuaternion1.y));
+            test(approxEqual(quaternion4.z, mQuaternion1.z));
+            test(approxEqual(quaternion4.w, mQuaternion1.w));
+        }
+
+        /// Test unit, length, normalize methods
+        void testUnitLengthNormalize() {
+
+            // Test method that returns the length
+            Quaternion quaternion(2, 3, -4, 5);
+            test(approxEqual(quaternion.length(), sqrt(decimal(54.0))));
+
+            // Test method that returns a unit quaternion
+            test(approxEqual(quaternion.getUnit().length(), 1.0));
+
+            // Test the normalization method
+            Quaternion quaternion2(4, 5, 6, 7);
+            quaternion2.normalize();
+            test(approxEqual(quaternion2.length(), 1.0));
+        }
+
+        /// Test others methods
+        void testOthersMethods() {
+
+            // Test the method to set the values
+            Quaternion quaternion;
+            quaternion.setAllValues(1, 2, 3, 4);
+            test(quaternion == Quaternion(1, 2, 3, 4));
+
+            // Test the method to set the quaternion to zero
+            quaternion.setToZero();
+            test(quaternion == Quaternion(0, 0, 0, 0));
+
+            // Test the method to get the vector (x, y, z)
+            Vector3 v = mQuaternion1.getVectorV();
+            test(v.x == mQuaternion1.x);
+            test(v.y == mQuaternion1.y);
+            test(v.z == mQuaternion1.z);
+
+            // Test the conjugate method
+            Quaternion conjugate = mQuaternion1.getConjugate();
+            test(conjugate.x == -mQuaternion1.x);
+            test(conjugate.y == -mQuaternion1.y);
+            test(conjugate.z == -mQuaternion1.z);
+            test(conjugate.w == mQuaternion1.w);
+
+            // Test the inverse method
+            Quaternion inverse = mQuaternion1.getInverse();
+            Quaternion product = mQuaternion1 * inverse;
+            test(approxEqual(product.x, mIdentity.x, decimal(10e-6)));
+            test(approxEqual(product.y, mIdentity.y, decimal(10e-6)));
+            test(approxEqual(product.z, mIdentity.z, decimal(10e-6)));
+            test(approxEqual(product.w, mIdentity.w, decimal(10e-6)));
+
+            // Test the dot product
+            Quaternion quaternion1(2, 3, 4, 5);
+            Quaternion quaternion2(6, 7, 8, 9);
+            decimal dotProduct = quaternion1.dot(quaternion2);
+            test(dotProduct == 110);
+
+            // Test the method that returns the rotation angle and axis
+            Vector3 axis;
+            decimal angle;
+            Vector3 originalAxis = Vector3(2, 3, 4).getUnit();
+            mQuaternion1.getRotationAngleAxis(angle, axis);
+            test(approxEqual(axis.x, originalAxis.x));
+            test(approxEqual(angle, decimal(PI/4.0), decimal(10e-6)));
+
+            // Test the method that returns the corresponding matrix
+            Matrix3x3 matrix = mQuaternion1.getMatrix();
+            Vector3 vector(56, -2, 82);
+            Vector3 vector1 = matrix * vector;
+            Vector3 vector2 = mQuaternion1 * vector;
+            test(approxEqual(vector1.x, vector2.x));
+            test(approxEqual(vector1.y, vector2.y));
+            test(approxEqual(vector1.z, vector2.z));
+
+            // Test slerp method
+            Quaternion quatStart = quaternion1.getUnit();
+            Quaternion quatEnd = quaternion2.getUnit();
+            Quaternion test1 = Quaternion::slerp(quatStart, quatEnd, 0.0);
+            Quaternion test2 = Quaternion::slerp(quatStart, quatEnd, 1.0);
+            test(test1 == quatStart);
+            test(test2 == quatEnd);
+            decimal sinA = sin(decimal(PI/4.0));
+            decimal cosA = cos(decimal(PI/4.0));
+            Quaternion quat(sinA, 0, 0, cosA);
+            Quaternion test3 = Quaternion::slerp(mIdentity, quat, decimal(0.5));
+            test(approxEqual(test3.x, sin(decimal(PI/8.0))));
+            test(approxEqual(test3.y, 0.0));
+            test(approxEqual(test3.z, 0.0));
+            test(approxEqual(test3.w, cos(decimal(PI/8.0)), decimal(10e-6)));
+        }
+
+        /// Test overloaded operators
+        void testOperators() {
+
+            // Test addition
+            Quaternion quat1(4, 5, 2, 10);
+            Quaternion quat2(-2, 7, 8, 3);
+            Quaternion test1 = quat1 + quat2;
+            test(test1 == Quaternion(2, 12, 10, 13));
+
+            // Test substraction
+            Quaternion test2 = quat1 - quat2;
+            test(test2 == Quaternion(6, -2, -6, 7));
+
+            // Test multiplication with a number
+            Quaternion test3 = quat1 * 3.0;
+            test(test3 == Quaternion(12, 15, 6, 30));
+
+            // Test multiplication between two quaternions
+            Quaternion test4 = quat1 * quat2;
+            Quaternion test5 = mQuaternion1 * mIdentity;
+            test(test4 == Quaternion(18, 49, 124, -13));
+            test(test5 == mQuaternion1);
+
+            // Test multiplication between a quaternion and a point
+            Vector3 point(5, -24, 563);
+            Vector3 vector1 = mIdentity * point;
+            Vector3 vector2 = mQuaternion1 * point;
+            Vector3 testVector2 = mQuaternion1.getMatrix() * point;
+            test(vector1 == point);
+            test(approxEqual(vector2.x, testVector2.x, decimal(10e-6)));
+            test(approxEqual(vector2.y, testVector2.y, decimal(10e-6)));
+            test(approxEqual(vector2.z, testVector2.z, decimal(10e-6)));
+
+            // Test assignment operator
+            Quaternion quaternion;
+            quaternion = mQuaternion1;
+            test(quaternion == mQuaternion1);
+
+            // Test equality operator
+            test(mQuaternion1 == mQuaternion1);
+        }
+ };
+
+}

test/tests/mathematics/TestTransform.h

@@ -0,0 +1,218 @@
+
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2013 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_TRANSFORM_H
+#define TEST_TRANSFORM_H
+
+#endif
+
+// Libraries
+#include "../../Test.h"
+#include "../../../src/mathematics/Transform.h"
+
+using namespace reactphysics3d;
+
+/// Reactphysics3D namespace
+namespace reactphysics3d {
+
+// Class TestTransform
+/**
+ * Unit test for the Transform class
+ */
+class TestTransform : public Test {
+
+    private :
+
+        // ---------- Atributes ---------- //
+
+        /// Identity transform
+        Transform mIdentityTransform;
+
+        /// First example transform
+        Transform mTransform1;
+
+        /// Second example transform
+        Transform mTransform2;
+
+    public :
+
+        // ---------- Methods ---------- //
+
+        /// Constructor
+        TestTransform() {
+
+            mIdentityTransform.setToIdentity();
+
+            decimal sinA = sin(PI/8.0f);
+            decimal cosA = cos(PI/8.0f);
+            mTransform1 = Transform(Vector3(4, 5, 6), Quaternion(sinA, sinA, sinA, cosA));
+
+            decimal sinB = sin(PI/3.0f);
+            decimal cosB = cos(PI/3.0f);
+            mTransform2 = Transform(Vector3(8, 45, -6), Quaternion(sinB, sinB, sinB, cosB));
+        }
+
+        /// Run the tests
+        void run() {
+            testConstructors();
+            testGetSet();
+            testInverse();
+            testGetSetOpenGLMatrix();
+            testInterpolateTransform();
+            testIdentity();
+            testOperators();
+        }
+
+        /// Test the constructors
+        void testConstructors() {
+            Transform transform1(Vector3(1, 2, 3), Quaternion(6, 7, 8, 9));
+            Transform transform2(Vector3(4, 5, 6), Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1));
+            Transform transform3(transform1);
+            test(transform1.getPosition() == Vector3(1, 2, 3));
+            test(transform1.getOrientation() == Quaternion(6, 7, 8, 9));
+            test(transform2.getPosition() == Vector3(4, 5, 6));
+            test(transform2.getOrientation() == Quaternion::identity());
+            test(transform3 == transform1);
+        }
+
+        /// Test getter and setter
+        void testGetSet() {
+            test(mIdentityTransform.getPosition() == Vector3(0, 0, 0));
+            test(mIdentityTransform.getOrientation() == Quaternion::identity());
+            Transform transform;
+            transform.setPosition(Vector3(5, 7, 8));
+            transform.setOrientation(Quaternion(1, 2, 3, 1));
+            test(transform.getPosition() == Vector3(5, 7, 8));
+            test(transform.getOrientation() == Quaternion(1, 2, 3, 1));
+            transform.setToIdentity();
+            test(transform.getPosition() == Vector3(0, 0, 0));
+            test(transform.getOrientation() == Quaternion::identity());
+        }
+
+        /// Test the inverse
+        void testInverse() {
+            Transform inverseTransform = mTransform1.inverse();
+            Vector3 vector(2, 3, 4);
+            Vector3 tempVector = mTransform1 * vector;
+            Vector3 tempVector2 = inverseTransform * tempVector;
+            test(approxEqual(tempVector2.x, vector.x, decimal(10e-6)));
+            test(approxEqual(tempVector2.y, vector.y, decimal(10e-6)));
+            test(approxEqual(tempVector2.z, vector.z, decimal(10e-6)));
+        }
+
+        /// Test methods to set and get transform matrix from and to OpenGL
+        void testGetSetOpenGLMatrix() {
+            Transform transform;
+            Vector3 position = mTransform1.getPosition();
+            Matrix3x3 orientation = mTransform1.getOrientation().getMatrix();
+            decimal openglMatrix[16] = {orientation[0][0], orientation[1][0],
+                                        orientation[2][0], 0,
+                                        orientation[0][1], orientation[1][1],
+                                        orientation[2][1], 0,
+                                        orientation[0][2], orientation[1][2],
+                                        orientation[2][2], 0,
+                                        position.x, position.y, position.z, 1};
+            transform.setFromOpenGL(openglMatrix);
+            decimal openglMatrix2[16];
+            transform.getOpenGLMatrix(openglMatrix2);
+            test(approxEqual(openglMatrix2[0], orientation[0][0]));
+            test(approxEqual(openglMatrix2[1], orientation[1][0]));
+            test(approxEqual(openglMatrix2[2], orientation[2][0]));
+            test(approxEqual(openglMatrix2[3], 0));
+            test(approxEqual(openglMatrix2[4], orientation[0][1]));
+            test(approxEqual(openglMatrix2[5], orientation[1][1]));
+            test(approxEqual(openglMatrix2[6], orientation[2][1]));
+            test(approxEqual(openglMatrix2[7], 0));
+            test(approxEqual(openglMatrix2[8], orientation[0][2]));
+            test(approxEqual(openglMatrix2[9], orientation[1][2]));
+            test(approxEqual(openglMatrix2[10], orientation[2][2]));
+            test(approxEqual(openglMatrix2[11], 0));
+            test(approxEqual(openglMatrix2[12], position.x));
+            test(approxEqual(openglMatrix2[13], position.y));
+            test(approxEqual(openglMatrix2[14], position.z));
+            test(approxEqual(openglMatrix2[15], 1));
+        }
+
+        /// Test the method to interpolate transforms
+        void testInterpolateTransform() {
+            Transform transformStart = Transform::interpolateTransforms(mTransform1, mTransform2,0);
+            Transform transformEnd = Transform::interpolateTransforms(mTransform1, mTransform2,1);
+            test(transformStart == mTransform1);
+            test(transformEnd == mTransform2);
+
+            decimal sinA = sin(PI/3.0f);
+            decimal cosA = cos(PI/3.0f);
+            decimal sinB = sin(PI/6.0f);
+            decimal cosB = cos(PI/6.0f);
+            Transform transform1(Vector3(4, 5, 6), Quaternion::identity());
+            Transform transform2(Vector3(8, 11, 16), Quaternion(sinA, sinA, sinA, cosA));
+            Transform transform = Transform::interpolateTransforms(transform1, transform2, 0.5);
+            Vector3 position = transform.getPosition();
+            Quaternion orientation = transform.getOrientation();
+            test(approxEqual(position.x, 6));
+            test(approxEqual(position.y, 8));
+            test(approxEqual(position.z, 11));
+            test(approxEqual(orientation.x, sinB));
+            test(approxEqual(orientation.y, sinB));
+            test(approxEqual(orientation.z, sinB));
+            test(approxEqual(orientation.w, cosB));
+        }
+
+        /// Test the identity methods
+        void testIdentity() {
+            Transform transform = Transform::identity();
+            test(transform.getPosition() == Vector3(0, 0, 0));
+            test(transform.getOrientation() == Quaternion::identity());
+
+            Transform transform2(Vector3(5, 6, 2), Quaternion(3, 5, 1, 6));
+            transform2.setToIdentity();
+            test(transform2.getPosition() == Vector3(0, 0, 0));
+            test(transform2.getOrientation() == Quaternion::identity());
+        }
+
+        /// Test the overloaded operators
+        void testOperators() {
+
+            // Equality, inequality operator
+            test(mTransform1 == mTransform1);
+            test(mTransform1 != mTransform2);
+
+            // Assignment operator
+            Transform transform;
+            transform = mTransform1;
+            test(transform == mTransform1);
+
+            // Multiplication
+            Vector3 vector(7, 53, 5);
+            Vector3 vector2 = mTransform2 * (mTransform1 * vector);
+            Vector3 vector3 = (mTransform2 * mTransform1) * vector;
+            test(approxEqual(vector2.x, vector3.x, decimal(10e-6)));
+            test(approxEqual(vector2.y, vector3.y, decimal(10e-6)));
+            test(approxEqual(vector2.z, vector3.z, decimal(10e-6)));
+        }
+ };
+
+}

test/tests/mathematics/TestVector3.h

@@ -0,0 +1,234 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2013 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_VECTOR3_H
+#define TEST_VECTOR3_H
+
+#endif
+
+// Libraries
+#include "../../Test.h"
+#include "../../../src/mathematics/Vector3.h"
+
+using namespace reactphysics3d;
+
+/// Reactphysics3D namespace
+namespace reactphysics3d {
+
+// Class TestVector3
+/**
+ * Unit test for the Vector3 class
+ */
+class TestVector3 : public Test {
+
+    private :
+
+        // ---------- Atributes ---------- //
+
+        /// Zero vector
+        Vector3 mVectorZero;
+
+        // Vector (3, 4, 5)
+        Vector3 mVector345;
+
+    public :
+
+        // ---------- Methods ---------- //
+
+        /// Constructor
+        TestVector3() : mVectorZero(0, 0, 0), mVector345(3, 4, 5) {}
+
+        /// Run the tests
+        void run() {
+            testConstructors();
+            testLengthMethods();
+            testDotCrossProducts();
+            testOthersMethods();
+            testOperators();
+        }
+
+        /// Test the constructors, getter and setter
+        void testConstructors() {
+
+            // Test constructor
+            test(mVectorZero.x == 0.0);
+            test(mVectorZero.y == 0.0);
+            test(mVectorZero.z == 0.0);
+            test(mVector345.x == 3.0);
+            test(mVector345.y == 4.0);
+            test(mVector345.z == 5.0);
+
+            // Test copy-constructor
+            Vector3 newVector(mVector345);
+            test(newVector.x == 3.0);
+            test(newVector.y == 4.0);
+            test(newVector.z == 5.0);
+
+            // Test method to set values
+            Vector3 newVector2;
+            newVector2.setAllValues(decimal(6.1), decimal(7.2), decimal(8.6));
+            test(approxEqual(newVector2.x, decimal(6.1)));
+            test(approxEqual(newVector2.y, decimal(7.2)));
+            test(approxEqual(newVector2.z, decimal(8.6)));
+
+            // Test method to set to zero
+            newVector2.setToZero();
+            test(newVector2 == Vector3(0, 0, 0));
+        }
+
+        /// Test the length, unit vector and normalize methods
+        void testLengthMethods() {
+
+            // Test length methods
+            test(mVectorZero.length() == 0.0);
+            test(mVectorZero.lengthSquare() == 0.0);
+            test(Vector3(1, 0, 0).length() == 1.0);
+            test(Vector3(0, 1, 0).length() == 1.0);
+            test(Vector3(0, 0, 1).length() == 1.0);
+            test(mVector345.lengthSquare() == 50.0);
+
+            // Test unit vector methods
+            test(Vector3(1, 0, 0).isUnit());
+            test(Vector3(0, 1, 0).isUnit());
+            test(Vector3(0, 0, 1).isUnit());
+            test(!mVector345.isUnit());
+            test(Vector3(5, 0, 0).getUnit() == Vector3(1, 0, 0));
+            test(Vector3(0, 5, 0).getUnit() == Vector3(0, 1, 0));
+            test(Vector3(0, 0, 5).getUnit() == Vector3(0, 0, 1));
+
+            test(!mVector345.isZero());
+            test(mVectorZero.isZero());
+
+            // Test normalization method
+            Vector3 mVector100(1, 0, 0);
+            Vector3 mVector010(0, 1, 0);
+            Vector3 mVector001(0, 0, 1);
+            Vector3 mVector500(5, 0, 0);
+            Vector3 mVector050(0, 5, 0);
+            Vector3 mVector005(0, 0, 5);
+            mVector100.normalize();
+            mVector010.normalize();
+            mVector001.normalize();
+            mVector500.normalize();
+            mVector050.normalize();
+            mVector005.normalize();
+            test(mVector100 == Vector3(1, 0, 0));
+            test(mVector010 == Vector3(0, 1, 0));
+            test(mVector001 == Vector3(0, 0, 1));
+            test(mVector500 == Vector3(1, 0, 0));
+            test(mVector050 == Vector3(0, 1, 0));
+            test(mVector005 == Vector3(0, 0, 1));
+        }
+
+        /// Test the dot and cross products
+        void testDotCrossProducts() {
+
+            // Test the dot product
+            test(Vector3(5, 0, 0).dot(Vector3(0, 8, 0)) == 0);
+            test(Vector3(5, 8, 0).dot(Vector3(0, 0, 6)) == 0);
+            test(Vector3(12, 45, 83).dot(Vector3(0, 0, 0)) == 0);
+            test(Vector3(5, 7, 8).dot(Vector3(5, 7, 8)) == 138);
+            test(Vector3(3, 6, 78).dot(Vector3(-3, -6, -78)) == -6129);
+            test(Vector3(2, 3, 5).dot(Vector3(2, 3, 5)) == 38);
+            test(Vector3(4, 3, 2).dot(Vector3(8, 9, 10)) == 79);
+
+            // Test the cross product
+            test(Vector3(0, 0, 0).cross(Vector3(0, 0, 0)) == Vector3(0, 0, 0));
+            test(Vector3(6, 7, 2).cross(Vector3(6, 7, 2)) == Vector3(0, 0, 0));
+            test(Vector3(1, 0, 0).cross(Vector3(0, 1, 0)) == Vector3(0, 0, 1));
+            test(Vector3(0, 1, 0).cross(Vector3(0, 0, 1)) == Vector3(1, 0, 0));
+            test(Vector3(0, 0, 1).cross(Vector3(0, 1, 0)) == Vector3(-1, 0, 0));
+            test(Vector3(4, 7, 24).cross(Vector3(8, 13, 11)) == Vector3(-235, 148, -4));
+            test(Vector3(-4, 42, -2).cross(Vector3(35, 7, -21)) == Vector3(-868, -154, -1498));
+        }
+
+        /// Test others methods
+        void testOthersMethods() {
+
+            // Test the method that returns the absolute vector
+            test(Vector3(4, 5, 6).getAbsoluteVector() == Vector3(4, 5, 6));
+            test(Vector3(-7, -24, -12).getAbsoluteVector() == Vector3(7, 24, 12));
+
+            // Test the method that returns the minimal element
+            test(Vector3(6, 35, 82).getMinAxis() == 0);
+            test(Vector3(564, 45, 532).getMinAxis() == 1);
+            test(Vector3(98, 23, 3).getMinAxis() == 2);
+            test(Vector3(-53, -25, -63).getMinAxis() == 2);
+
+            // Test the method that returns the maximal element
+            test(Vector3(6, 35, 82).getMaxAxis() == 2);
+            test(Vector3(7, 533, 36).getMaxAxis() == 1);
+            test(Vector3(98, 23, 3).getMaxAxis() == 0);
+            test(Vector3(-53, -25, -63).getMaxAxis() == 1);
+        }
+
+        /// Test the operators
+        void testOperators() {
+
+            // Test the [] operator
+            test(mVector345[0] == 3);
+            test(mVector345[1] == 4);
+            test(mVector345[2] == 5);
+
+            // Assignment operator
+            Vector3 newVector(6, 4, 2);
+            newVector = Vector3(7, 8, 9);
+            test(newVector == Vector3(7, 8, 9));
+
+            // Equality, inequality operators
+            test(Vector3(5, 7, 3) == Vector3(5, 7, 3));
+            test(Vector3(63, 64, 24) != Vector3(63, 64, 5));
+            test(Vector3(63, 64, 24) != Vector3(12, 64, 24));
+            test(Vector3(63, 64, 24) != Vector3(63, 8, 24));
+
+            // Addition, substraction
+            Vector3 vector1(6, 33, 62);
+            Vector3 vector2(7, 68, 35);
+            test(Vector3(63, 24, 5) + Vector3(3, 4, 2) == Vector3(66, 28, 7));
+            test(Vector3(63, 24, 5) - Vector3(3, 4, 2) == Vector3(60, 20, 3));
+            vector1 += Vector3(5, 10, 12);
+            vector2 -= Vector3(10, 21, 5);
+            test(vector1 == Vector3(11, 43, 74));
+            test(vector2 == Vector3(-3, 47, 30));
+
+            // Multiplication, division
+            Vector3 vector3(6, 33, 62);
+            Vector3 vector4(15, 60, 33);
+            test(Vector3(63, 24, 5) * 3 == Vector3(189, 72, 15));
+            test(3 * Vector3(63, 24, 5) == Vector3(189, 72, 15));
+            test(Vector3(14, 8, 50) / 2 == Vector3(7, 4, 25));
+            vector3 *= 10;
+            vector4 /= 3;
+            test(vector3 == Vector3(60, 330, 620));
+            test(vector4 == Vector3(5, 20, 11));
+
+            // Negative operator
+            Vector3 vector5(-34, 5, 422);
+            Vector3 negative = -vector5;
+            test(negative == Vector3(34, -5, -422));
+        }
+ };
+
+}