diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index 4430f791..5096cb7f 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -45,7 +45,7 @@ CollisionDetection::CollisionDetection(PhysicsWorld* world) {
     broadPhaseAlgorithm = new SAPAlgorithm();
 
     // Create the narrow-phase algorithm that will be used (Separating axis algorithm)
-    narrowPhaseAlgorithm = new GJKAlgorithm();
+    narrowPhaseAlgorithm = new GJKAlgorithm();  // TODO : Use GJK algo here
 }
 
 // Destructor
@@ -90,8 +90,13 @@ void CollisionDetection::computeNarrowPhase() {
     for (unsigned int i=0; i<possibleCollisionPairs.size(); i++) {
         ContactInfo* contactInfo = NULL;
 
-        // Use the narrow-phase collision detection algorithm to check if the really are a contact
-        if (narrowPhaseAlgorithm->testCollision(possibleCollisionPairs.at(i).first->getNarrowBoundingVolume(), possibleCollisionPairs.at(i).second->getNarrowBoundingVolume(), contactInfo)) {
+        const RigidBody* rigidBody1 = dynamic_cast<const RigidBody*>(possibleCollisionPairs.at(i).first);
+        const RigidBody* rigidBody2 = dynamic_cast<const RigidBody*>(possibleCollisionPairs.at(i).second);
+        
+        // Use the narrow-phase collision detection algorithm to check if there really are a contact
+        if (narrowPhaseAlgorithm->testCollision(rigidBody1->getNarrowBoundingVolume(), rigidBody1->getTransform(),
+                                                rigidBody2->getNarrowBoundingVolume(), rigidBody2->getTransform(),
+                                                contactInfo)) {
             assert(contactInfo);
 
             // Add the contact info the current list of collision informations
@@ -108,7 +113,7 @@ void CollisionDetection::computeAllContacts() {
         assert(contactInfo);
         
         // Compute one or several new contacts and add them into the physics world
-        computeContactGJK(contactInfo);
+        computeContactGJK(contactInfo); // TODO : Call computeContactGJK() here
     }
 }
 
@@ -128,6 +133,7 @@ void CollisionDetection::computeContactGJK(const ContactInfo* const contactInfo)
      world->addConstraint(contact);
 }
 
+/* TODO : DELETE THIS 
 // Compute a contact for the SAT algorithm (and add it to the physics world) for two colliding bodies
 // This method only works for collision between two OBB bounding volumes
 void CollisionDetection::computeContactSAT(const ContactInfo* const contactInfo) {
@@ -257,4 +263,5 @@ void CollisionDetection::computeContactSAT(const ContactInfo* const contactInfo)
         world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedFace));
     }
 }
+*/
 
diff --git a/src/collision/EPA/EPAAlgorithm.cpp b/src/collision/EPA/EPAAlgorithm.cpp
index dcbf9274..c0752a25 100644
--- a/src/collision/EPA/EPAAlgorithm.cpp
+++ b/src/collision/EPA/EPAAlgorithm.cpp
@@ -80,7 +80,9 @@ int EPAAlgorithm::isOriginInTetrahedron(const Vector3D& p1, const Vector3D& p2,
 // GJK algorithm. The EPA Algorithm will extend this simplex polytope to find
 // the correct penetration depth
 bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, const NarrowBoundingVolume* const boundingVolume1,
+                                                           const Transform& transform1,
                                                            const NarrowBoundingVolume* const boundingVolume2,
+                                                           const Transform& transform2,
                                                            Vector3D& v, ContactInfo*& contactInfo) {
     Vector3D suppPointsA[MAX_SUPPORT_POINTS];       // Support points of object A in local coordinates
     Vector3D suppPointsB[MAX_SUPPORT_POINTS];       // Support points of object B in local coordinates
@@ -88,6 +90,12 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
     TrianglesStore triangleStore;                   // Store the triangles
     TriangleEPA* triangleHeap[MAX_FACETS];          // Heap that contains the face candidate of the EPA algorithm
 
+    // Transform a point from body space of shape 2 to body space of shape 1 (the GJK algorithm is done in body space of shape 1)
+    Transform shape2ToShape1 = transform1.inverse() * transform2;
+
+    // Matrix that transform a direction from body space of shape 1 into body space of shape 2
+    Matrix3x3 rotateToShape2 = transform2.getOrientation().getTranspose() * transform1.getOrientation();
+
     // Get the simplex computed previously by the GJK algorithm
     unsigned int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
 
@@ -141,17 +149,17 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
 
             // Compute the support point in the direction of v1
             suppPointsA[2] = boundingVolume1->getSupportPoint(v1, OBJECT_MARGIN);
-            suppPointsB[2] = boundingVolume2->getSupportPoint(v1.getOpposite(), OBJECT_MARGIN);
+            suppPointsB[2] = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * v1.getOpposite(), OBJECT_MARGIN);
             points[2] = suppPointsA[2] - suppPointsB[2];
 
             // Compute the support point in the direction of v2
             suppPointsA[3] = boundingVolume1->getSupportPoint(v2, OBJECT_MARGIN);
-            suppPointsB[3] = boundingVolume2->getSupportPoint(v2.getOpposite(), OBJECT_MARGIN);
+            suppPointsB[3] = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * v2.getOpposite(), OBJECT_MARGIN);
             points[3] = suppPointsA[3] - suppPointsB[3];
 
             // Compute the support point in the direction of v3
             suppPointsA[4] = boundingVolume1->getSupportPoint(v3, OBJECT_MARGIN);
-            suppPointsB[4] = boundingVolume2->getSupportPoint(v3.getOpposite(), OBJECT_MARGIN);
+            suppPointsB[4] = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * v3.getOpposite(), OBJECT_MARGIN);
             points[4] = suppPointsA[4] - suppPointsB[4];
 
             // Now we have an hexahedron (two tetrahedron glued together). We can simply keep the
@@ -246,10 +254,10 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
 
             // Compute the two new vertices to obtain a hexahedron
             suppPointsA[3] = boundingVolume1->getSupportPoint(n, OBJECT_MARGIN);
-            suppPointsB[3] = boundingVolume2->getSupportPoint(n.getOpposite(), OBJECT_MARGIN);
+            suppPointsB[3] = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * n.getOpposite(), OBJECT_MARGIN);
             points[3] = suppPointsA[3] - suppPointsB[3];
             suppPointsA[4] = boundingVolume1->getSupportPoint(n.getOpposite(), OBJECT_MARGIN);
-            suppPointsB[4] = boundingVolume2->getSupportPoint(n, OBJECT_MARGIN);
+            suppPointsB[4] = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * n, OBJECT_MARGIN);
             points[4] = suppPointsA[4] - suppPointsB[4];
 
             // Construct the triangle faces
@@ -319,7 +327,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
 
             // Compute the support point of the Minkowski difference (A-B) in the closest point direction
             suppPointsA[nbVertices] = boundingVolume1->getSupportPoint(triangle->getClosestPoint(), OBJECT_MARGIN);
-            suppPointsB[nbVertices] = boundingVolume2->getSupportPoint(triangle->getClosestPoint().getOpposite(), OBJECT_MARGIN);
+            suppPointsB[nbVertices] = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * triangle->getClosestPoint().getOpposite(), OBJECT_MARGIN);
             points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices];
 
             int indexNewVertex = nbVertices;
@@ -361,10 +369,10 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
 
     } while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
 
-    // Compute the contact info
-    v = triangle->getClosestPoint();
-    Vector3D pA = triangle->computeClosestPointOfObject(suppPointsA);
-    Vector3D pB = triangle->computeClosestPointOfObject(suppPointsB);
+    // Compute the contact info (in world-space)
+    v = transform1.getOrientation() * triangle->getClosestPoint();
+    Vector3D pA = transform1 * triangle->computeClosestPointOfObject(suppPointsA);
+    Vector3D pB = transform1 * triangle->computeClosestPointOfObject(suppPointsB);
     Vector3D normal = v.getUnit();
     double penetrationDepth = v.length();
     assert(penetrationDepth > 0.0);
diff --git a/src/collision/EPA/EPAAlgorithm.h b/src/collision/EPA/EPAAlgorithm.h
index fb681087..b21fefa1 100644
--- a/src/collision/EPA/EPAAlgorithm.h
+++ b/src/collision/EPA/EPAAlgorithm.h
@@ -83,7 +83,9 @@ class EPAAlgorithm {
         ~EPAAlgorithm();        // Destructor
 
         bool computePenetrationDepthAndContactPoints(Simplex simplex, const NarrowBoundingVolume* const boundingVolume1,
+                                                     const Transform& transform1,
                                                      const NarrowBoundingVolume* const boundingVolume2,
+                                                     const Transform& transform2,
                                                      Vector3D& v, ContactInfo*& contactInfo);                         // Compute the penetration depth with EPA algorithm
 };
 
diff --git a/src/collision/GJK/GJKAlgorithm.cpp b/src/collision/GJK/GJKAlgorithm.cpp
index 7b0c461a..224e06ef 100644
--- a/src/collision/GJK/GJKAlgorithm.cpp
+++ b/src/collision/GJK/GJKAlgorithm.cpp
@@ -59,8 +59,8 @@ GJKAlgorithm::~GJKAlgorithm() {
 // algorithm on the enlarged object to obtain a simplex polytope that contains the
 // origin, they we give that simplex polytope to the EPA algorithm which will compute
 // the correct penetration depth and contact points between the enlarged objects.
-bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolume1,
-                                 const NarrowBoundingVolume* const boundingVolume2,
+bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolume1, const Transform& transform1,
+                                 const NarrowBoundingVolume* const boundingVolume2, const Transform& transform2,
                                  ContactInfo*& contactInfo) {
     Vector3D suppA;             // Support point of object A
     Vector3D suppB;             // Support point of object B
@@ -70,6 +70,12 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
     double vDotw;
     double prevDistSquare;
 
+    // Transform a point from body space of shape 2 to body space of shape 1 (the GJK algorithm is done in body space of shape 1)
+    Transform shape2ToShape1 = transform1.inverse() * transform2;
+
+    // Matrix that transform a direction from body space of shape 1 into body space of shape 2
+    Matrix3x3 rotateToShape2 = transform2.getOrientation().getTranspose() * transform1.getOrientation();
+
     assert(boundingVolume1 != boundingVolume2);
 
     // Initialize the margin (sum of margins of both objects)
@@ -83,7 +89,7 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
     // Get the last point V (last separating axis)
     // TODO : Implement frame coherence. For each pair of body, store
     //        the last separating axis and use it to initialize the v vector
-    Vector3D v(1.0, 1.0, 1.0);
+    Vector3D v(0.0, 1.0, 0.0);
 
     // Initialize the upper bound for the square distance
     double distSquare = DBL_MAX;
@@ -91,7 +97,7 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
     do {
         // Compute the support points for original objects (without margins) A and B
         suppA = boundingVolume1->getSupportPoint(v.getOpposite());
-        suppB = boundingVolume2->getSupportPoint(v);
+        suppB = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * v);
 
         // Compute the support point for the Minkowski difference A-B
         w = suppA - suppB;
@@ -113,11 +119,11 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
             // object with the margins
             double dist = sqrt(distSquare);
             assert(dist > 0.0);
-            pA = pA - (OBJECT_MARGIN / dist) * v;
-            pB = pB + (OBJECT_MARGIN / dist) * v;
+            pA = transform1 * (pA - (OBJECT_MARGIN / dist) * v);
+            pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
 
             // Compute the contact info
-            Vector3D normal = v.getOpposite().getUnit();
+            Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit();
             double penetrationDepth = margin - dist;
             contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
                                           normal, penetrationDepth, pA, pB);
@@ -138,11 +144,11 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
             // object with the margins
             double dist = sqrt(distSquare);
             assert(dist > 0.0);
-            pA = pA - (OBJECT_MARGIN / dist) * v;
-            pB = pB + (OBJECT_MARGIN / dist) * v;
+            pA = transform1 * (pA - (OBJECT_MARGIN / dist) * v);
+            pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
 
             // Compute the contact info
-            Vector3D normal = v.getOpposite().getUnit();
+            Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit();
             double penetrationDepth = margin - dist;
             contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
                                           normal, penetrationDepth, pA, pB);
@@ -161,11 +167,11 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
             // object with the margins
             double dist = sqrt(distSquare);
             assert(dist > 0.0);
-            pA = pA - (OBJECT_MARGIN / dist) * v;
-            pB = pB + (OBJECT_MARGIN / dist) * v;
+            pA = transform1 * (pA - (OBJECT_MARGIN / dist) * v);
+            pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
 
             // Compute the contact info
-            Vector3D normal = v.getOpposite().getUnit();
+            Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit();
             double penetrationDepth = margin - dist;
             contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
                                           normal, penetrationDepth, pA, pB);
@@ -192,11 +198,11 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
             // object with the margins
             double dist = sqrt(distSquare);
             assert(dist > 0.0);
-            pA = pA - (OBJECT_MARGIN / dist) * v;
-            pB = pB + (OBJECT_MARGIN / dist) * v;
+            pA = transform1 * (pA - (OBJECT_MARGIN / dist) * v);
+            pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
 
             // Compute the contact info
-            Vector3D normal = v.getOpposite().getUnit();
+            Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit();
             double penetrationDepth = margin - dist;
             contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
                                           normal, penetrationDepth, pA, pB);
@@ -214,7 +220,7 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
     // enlarged objects to compute a simplex polytope that contains the origin. Then, we give that simplex
     // polytope to the EPA algorithm to compute the correct penetration depth and contact points between
     // the enlarged objects.
-    return computePenetrationDepthForEnlargedObjects(boundingVolume1, boundingVolume2, contactInfo, v);
+    return computePenetrationDepthForEnlargedObjects(boundingVolume1, transform1, boundingVolume2, transform2, contactInfo, v);
 }
 
 // This method runs the GJK algorithm on the two enlarged objects (with margin)
@@ -222,8 +228,8 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
 // assumed to intersect in the original objects (without margin). Therefore such
 // a polytope must exist. Then, we give that polytope to the EPA algorithm to
 // compute the correct penetration depth and contact points of the enlarged objects.
-bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const NarrowBoundingVolume* const boundingVolume1,
-                                                             const NarrowBoundingVolume* const boundingVolume2,
+bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const NarrowBoundingVolume* const boundingVolume1, const Transform& transform1,
+                                                             const NarrowBoundingVolume* const boundingVolume2, const Transform& transform2,
                                                              ContactInfo*& contactInfo, Vector3D& v) {
     Simplex simplex;
     Vector3D suppA;
@@ -232,11 +238,17 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const NarrowBoundin
     double vDotw;
     double distSquare = DBL_MAX;
     double prevDistSquare;
+
+    // Transform a point from body space of shape 2 to body space of shape 1 (the GJK algorithm is done in body space of shape 1)
+    Transform shape2ToShape1 = transform1.inverse() * transform2;
+
+    // Matrix that transform a direction from body space of shape 1 into body space of shape 2
+    Matrix3x3 rotateToShape2 = transform2.getOrientation().getTranspose() * transform1.getOrientation();
     
     do {
         // Compute the support points for the enlarged object A and B
         suppA = boundingVolume1->getSupportPoint(v.getOpposite(), OBJECT_MARGIN);
-        suppB = boundingVolume2->getSupportPoint(v, OBJECT_MARGIN);
+        suppB = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * v, OBJECT_MARGIN);
 
         // Compute the support point for the Minkowski difference A-B
         w = suppA - suppB;
@@ -272,5 +284,6 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const NarrowBoundin
 
     // Give the simplex computed with GJK algorithm to the EPA algorithm which will compute the correct
     // penetration depth and contact points between the two enlarged objects
-    return algoEPA.computePenetrationDepthAndContactPoints(simplex, boundingVolume1, boundingVolume2, v, contactInfo);
+    return algoEPA.computePenetrationDepthAndContactPoints(simplex, boundingVolume1, transform1,
+                                                           boundingVolume2, transform2, v, contactInfo);
 }
diff --git a/src/collision/GJK/GJKAlgorithm.h b/src/collision/GJK/GJKAlgorithm.h
index 6315b7de..ebe2460c 100644
--- a/src/collision/GJK/GJKAlgorithm.h
+++ b/src/collision/GJK/GJKAlgorithm.h
@@ -62,15 +62,17 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
         EPAAlgorithm algoEPA;             // EPA Algorithm
 
         bool computePenetrationDepthForEnlargedObjects(const NarrowBoundingVolume* const boundingVolume1,
+                                                       const Transform& transform1,
                                                        const NarrowBoundingVolume* const boundingVolume2,
+                                                        const Transform& transform2,
                                                        ContactInfo*& contactInfo, Vector3D& v);             // Compute the penetration depth for enlarged objects
 
     public :
         GJKAlgorithm();           // Constructor
         ~GJKAlgorithm();          // Destructor
 
-        virtual bool testCollision(const NarrowBoundingVolume* const boundingVolume1,
-                                   const NarrowBoundingVolume* const boundingVolume2,
+        virtual bool testCollision(const NarrowBoundingVolume* const boundingVolume1, const Transform& transform1,
+                                   const NarrowBoundingVolume* const boundingVolume2, const Transform& transform2,
                                    ContactInfo*& contactInfo);                          // Return true and compute a contact info if the two bounding volume collide
 };
 
diff --git a/src/collision/NarrowPhaseAlgorithm.h b/src/collision/NarrowPhaseAlgorithm.h
index f71727ca..9cb05309 100644
--- a/src/collision/NarrowPhaseAlgorithm.h
+++ b/src/collision/NarrowPhaseAlgorithm.h
@@ -48,7 +48,9 @@ class NarrowPhaseAlgorithm {
         virtual ~NarrowPhaseAlgorithm();     // Destructor
 
         virtual bool testCollision(const NarrowBoundingVolume* const boundingVolume1,
+                                   const Transform& transform1,
                                    const NarrowBoundingVolume* const boundingVolume2,
+                                   const Transform& transform2,
                                    ContactInfo*& contactInfo)=0;                        // Return true and compute a contact info if the two bounding volume collide
 };
 
diff --git a/src/collision/NoBroadPhaseAlgorithm.cpp b/src/collision/NoBroadPhaseAlgorithm.cpp
index d0ed8790..c2b36632 100644
--- a/src/collision/NoBroadPhaseAlgorithm.cpp
+++ b/src/collision/NoBroadPhaseAlgorithm.cpp
@@ -38,4 +38,37 @@ NoBroadPhaseAlgorithm::~NoBroadPhaseAlgorithm() {
 
 }
 
+// Compute the possible collision pairs of bodies
+// The arguments "addedBodies" and "removedBodies" are respectively the set
+// of bodies that have been added and removed since the last broad-phase
+// computation. Before the call, the argument "possibleCollisionPairs"
+// correspond to the possible colliding pairs of bodies from the last broad-phase
+// computation. This methods computes the current possible collision pairs of
+// bodies and update the "possibleCollisionPairs" argument. This broad-phase
+// algorithm doesn't do anything and therefore the "possibleCollisionPairs" set
+// must contains all the possible pairs of bodies
+void NoBroadPhaseAlgorithm::computePossibleCollisionPairs(std::vector<Body*> addedBodies, std::vector<Body*> removedBodies,
+                                                                 std::vector<std::pair<const Body*, const Body* > >& possibleCollisionPairs) {
+    // Add the new bodies
+    for (std::vector<Body*>::iterator it = addedBodies.begin(); it < addedBodies.end(); it++) {
+        bodies.push_back(*it);
+    }
 
+    // Remove the bodies to be removed
+    for (std::vector<Body*>::iterator it = removedBodies.begin(); it < removedBodies.end(); it++) {
+        bodies.erase(std::find(bodies.begin(), bodies.end(), *it));
+    }
+
+    // If the set of bodies have been changed
+    if (addedBodies.size() + removedBodies.size() > 0) {
+        // Recompute all the possible pairs of bodies
+        possibleCollisionPairs.clear();
+        for (std::vector<Body*>::iterator it1 = addedBodies.begin(); it1 < addedBodies.end(); it1++) {
+            for (std::vector<Body*>::iterator it2 = addedBodies.begin(); it2 < addedBodies.end(); it2++) {
+                if (*it1 != *it2) {
+                    possibleCollisionPairs.push_back(std::make_pair(*it1, *it2));
+                }
+            }
+        }
+    }
+}
diff --git a/src/collision/NoBroadPhaseAlgorithm.h b/src/collision/NoBroadPhaseAlgorithm.h
index c0857e43..74b6f4c5 100644
--- a/src/collision/NoBroadPhaseAlgorithm.h
+++ b/src/collision/NoBroadPhaseAlgorithm.h
@@ -51,42 +51,6 @@ class NoBroadPhaseAlgorithm : public BroadPhaseAlgorithm {
                                                    std::vector<std::pair<const Body*, const Body* > >& possibleCollisionPairs);     // Compute the possible collision pairs of bodies
 };
 
-
-// Compute the possible collision pairs of bodies
-// The arguments "addedBodies" and "removedBodies" are respectively the set
-// of bodies that have been added and removed since the last broad-phase
-// computation. Before the call, the argument "possibleCollisionPairs"
-// correspond to the possible colliding pairs of bodies from the last broad-phase
-// computation. This methods computes the current possible collision pairs of
-// bodies and update the "possibleCollisionPairs" argument. This broad-phase
-// algorithm doesn't do anything and therefore the "possibleCollisionPairs" set
-// must contains all the possible pairs of bodies
-inline void NoBroadPhaseAlgorithm::computePossibleCollisionPairs(std::vector<Body*> addedBodies, std::vector<Body*> removedBodies,
-                                                                 std::vector<std::pair<const Body*, const Body* > >& possibleCollisionPairs) {
-    // Add the new bodies
-    for (std::vector<Body*>::iterator it = addedBodies.begin(); it < addedBodies.end(); it++) {
-        bodies.push_back(*it);
-    }
-
-    // Remove the bodies to be removed
-    for (std::vector<Body*>::iterator it = removedBodies.begin(); it < removedBodies.end(); it++) {
-        bodies.erase(std::find(bodies.begin(), bodies.end(), *it));
-    }
-
-    // If the set of bodies have been changed
-    if (addedBodies.size() + removedBodies.size() > 0) {
-        // Recompute all the possible pairs of bodies
-        possibleCollisionPairs.clear();
-        for (std::vector<Body*>::iterator it1 = addedBodies.begin(); it1 < addedBodies.end(); it1++) {
-            for (std::vector<Body*>::iterator it2 = addedBodies.begin(); it2 < addedBodies.end(); it2++) {
-                if (*it1 != *it2) {
-                    possibleCollisionPairs.push_back(std::make_pair(*it1, *it2));
-                }
-            }
-        }
-    }
-}
-
 } // End of reactphysics3d namespace
 
 #endif
diff --git a/src/collision/SAPAlgorithm.cpp b/src/collision/SAPAlgorithm.cpp
index 801510a1..e6d376b7 100644
--- a/src/collision/SAPAlgorithm.cpp
+++ b/src/collision/SAPAlgorithm.cpp
@@ -50,7 +50,7 @@ void SAPAlgorithm::removeBodiesAABB(vector<Body*> bodies) {
 
     // Removed the AABB of the bodies that have been removed
     for (vector<Body*>::iterator it = bodies.begin(); it != bodies.end(); ++it) {
-        aabb = dynamic_cast<const AABB*>((*it)->getBroadBoundingVolume());
+        aabb = (*it)->getAABB();
         assert(aabb);
         elemToRemove = find(sortedAABBs.begin(), sortedAABBs.end(), aabb);
         assert((*elemToRemove) == aabb);
@@ -64,7 +64,7 @@ void SAPAlgorithm::addBodiesAABB(vector<Body*> bodies) {
     
     for (vector<Body*>::iterator it = bodies.begin(); it != bodies.end(); ++it) {
         aabb = 0;
-        aabb = dynamic_cast<const AABB*>((*it)->getBroadBoundingVolume());
+        aabb = (*it)->getAABB();
         assert(aabb);
         sortedAABBs.push_back(aabb);
     }
@@ -123,7 +123,7 @@ void SAPAlgorithm::computePossibleCollisionPairs(vector<Body*> addedBodies, vect
         // Test collision against all possible overlapping AABBs following the current one
         for (it2 = it + 1; it2 != sortedAABBs.end(); ++it2) {
             // Stop when the tested AABBs are beyond the end of the current AABB
-            if ((*it2)->getMinValueOnAxis(sortAxis) > (*it)->getMaxValueOnAxis(sortAxis)) {
+            if ((*it2)->getMinCoordinates().getValue(sortAxis) > (*it)->getMaxCoordinates().getValue(sortAxis)) {
                 break;
             }
 
diff --git a/src/collision/SAPAlgorithm.h b/src/collision/SAPAlgorithm.h
index bb86f33e..32cc5bd6 100644
--- a/src/collision/SAPAlgorithm.h
+++ b/src/collision/SAPAlgorithm.h
@@ -72,7 +72,7 @@ class SAPAlgorithm : public BroadPhaseAlgorithm {
 // sorting axis (sortAxis) is smaller than the minimum value of "b" on this same
 // axis.
 inline bool SAPAlgorithm::compareAABBs(const AABB* a, const AABB* b) {
-    return (a->getMinValueOnAxis(sortAxis) < b->getMinValueOnAxis(sortAxis));
+    return (a->getMinCoordinates().getValue(sortAxis) < b->getMinCoordinates().getValue(sortAxis));
 }
 
 } // End of reactphysics3d namespace
diff --git a/src/collision/SATAlgorithm.cpp b/src/collision/SATAlgorithm.cpp
index f7dc6479..4e9f2fe3 100644
--- a/src/collision/SATAlgorithm.cpp
+++ b/src/collision/SATAlgorithm.cpp
@@ -32,6 +32,9 @@
 #include <cmath>
 #include <cassert>
 
+// TODO : SAT Algorithm for box-box collision does not work anymore since the use of
+//        transform. Maybe a problem with the computation of the normal vector
+//        Anyway, the GJK algorithm should be used instead
 
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;
@@ -48,8 +51,8 @@ SATAlgorithm::~SATAlgorithm() {
 
 // Return true and compute a contact info if the two bounding volume collide.
 // The method returns false if there is no collision between the two bounding volumes.
-bool SATAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolume1,
-                                 const NarrowBoundingVolume* const boundingVolume2,
+bool SATAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolume1, const Transform& transform1,
+                                 const NarrowBoundingVolume* const boundingVolume2, const Transform& transform2,
                                  ContactInfo*& contactInfo) {
     
     assert(boundingVolume1 != boundingVolume2);
@@ -63,7 +66,7 @@ bool SATAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
     // If the two bounding volumes are OBB
     if (obb1 && obb2) {
         // Compute the collision test between two OBB
-        return computeCollisionTest(obb1, obb2, contactInfo);
+        return computeCollisionTest(obb1, transform1, obb2, transform2, contactInfo);
     }
     else {
         return false;
@@ -78,7 +81,8 @@ bool SATAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
 // OBB are the six face normals (3 for each OBB) and the nine vectors V = Ai x Bj where Ai is the ith face normal
 // vector of OBB 1 and Bj is the jth face normal vector of OBB 2. We will use the notation Ai for the ith face
 // normal of OBB 1 and Bj for the jth face normal of OBB 2.
-bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const obb2, ContactInfo*& contactInfo) const {
+bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform& transform1,
+                                        const OBB* const obb2, const Transform& transform2, ContactInfo*& contactInfo) const {
 
 
     double center;                              // Center of a projection interval
@@ -104,17 +108,18 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
     double udc1[3];                             // DotProduct(obb1.Ai, obb2.center - obb1.center)
     double udc2[3];                             // DotProduct(obb2.Ai, obb2.center - obb1.center)
     
-    Vector3D boxDistance = obb2->getCenter() - obb1->getCenter();   // Vector between the centers of the OBBs
+    //Vector3D boxDistance = obb2->getCenter() - obb1->getCenter();   // Vector between the centers of the OBBs
+    Vector3D boxDistance = transform2.getPosition() - transform1.getPosition();   // Vector between the centers of the OBBs
 
     // Axis A0
     for (int i=0; i<3; ++i) {
-        c[0][i] = obb1->getAxis(0).dot(obb2->getAxis(i));
+        c[0][i] = transform1.getOrientation().getColumn(0).dot(transform2.getOrientation().getColumn(i));
         absC[0][i] = fabs(c[0][i]);
         if (absC[0][i] > cutoff) {
             existsParallelPair = true;
         }
     }
-    udc1[0] = obb1->getAxis(0).dot(boxDistance);
+    udc1[0] = transform1.getOrientation().getColumn(0).dot(boxDistance);
     center = udc1[0];
     radius1 = obb1->getExtent(0);
     radius2 = obb2->getExtent(0)*absC[0][0] + obb2->getExtent(1)*absC[0][1] + obb2->getExtent(2) * absC[0][2];
@@ -133,13 +138,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
 
     // Axis A1
     for (int i=0; i<3; ++i) {
-        c[1][i] = obb1->getAxis(1).dot(obb2->getAxis(i));
+        c[1][i] = transform1.getOrientation().getColumn(1).dot(transform2.getOrientation().getColumn(i));
         absC[1][i] = fabs(c[1][i]);
         if (absC[1][i] > cutoff) {
             existsParallelPair = true;
         }
     }
-    udc1[1] = obb1->getAxis(1).dot(boxDistance);
+    udc1[1] = transform1.getOrientation().getColumn(1).dot(boxDistance);
     center = udc1[1];
     radius1 = obb1->getExtent(1);
     radius2 = obb2->getExtent(0)*absC[1][0] + obb2->getExtent(1)*absC[1][1] + obb2->getExtent(2) * absC[1][2];
@@ -158,13 +163,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
 
     // Axis A2
     for (int i=0; i<3; ++i) {
-        c[2][i] = obb1->getAxis(2).dot(obb2->getAxis(i));
+        c[2][i] = transform1.getOrientation().getColumn(2).dot(transform2.getOrientation().getColumn(i));
         absC[2][i] = fabs(c[2][i]);
         if (absC[2][i] > cutoff) {
             existsParallelPair = true;
         }
     }
-    udc1[2] = obb1->getAxis(2).dot(boxDistance);
+    udc1[2] = transform1.getOrientation().getColumn(2).dot(boxDistance);
     center = udc1[2];
     radius1 = obb1->getExtent(2);
     radius2 = obb2->getExtent(0)*absC[2][0] + obb2->getExtent(1)*absC[2][1] + obb2->getExtent(2)*absC[2][2];
@@ -182,7 +187,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
     }
 
     // Axis B0
-    udc2[0] = obb2->getAxis(0).dot(boxDistance);
+    udc2[0] = transform2.getOrientation().getColumn(0).dot(boxDistance);
     center = udc2[0];
     radius1 = obb1->getExtent(0)*absC[0][0] + obb1->getExtent(1)*absC[1][0] + obb1->getExtent(2) * absC[2][0];
     radius2 = obb2->getExtent(0);
@@ -200,7 +205,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
     }
 
     // Axis B1
-    udc2[1] = obb2->getAxis(1).dot(boxDistance);
+    udc2[1] = transform2.getOrientation().getColumn(1).dot(boxDistance);
     center = udc2[1];
     radius1 = obb1->getExtent(0)*absC[0][1] + obb1->getExtent(1)*absC[1][1] + obb1->getExtent(2) * absC[2][1];
     radius2 = obb2->getExtent(1);
@@ -218,7 +223,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
     }
 
     // Axis B2
-    udc2[2] = obb2->getAxis(2).dot(boxDistance);
+    udc2[2] = transform2.getOrientation().getColumn(2).dot(boxDistance);
     center = udc2[2];
     radius1 = obb1->getExtent(0)*absC[0][2] + obb1->getExtent(1)*absC[1][2] + obb1->getExtent(2)*absC[2][2];
     radius2 = obb2->getExtent(2);
diff --git a/src/collision/SATAlgorithm.h b/src/collision/SATAlgorithm.h
index 72614371..180dd70a 100644
--- a/src/collision/SATAlgorithm.h
+++ b/src/collision/SATAlgorithm.h
@@ -29,6 +29,7 @@
 #include "NarrowPhaseAlgorithm.h"
 #include "../constraint/Contact.h"
 #include "../body/OBB.h"
+#include "../mathematics/Transform.h"
 
 // ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -49,7 +50,9 @@ namespace reactphysics3d {
 */
 class SATAlgorithm : public NarrowPhaseAlgorithm {
     private :
-        bool computeCollisionTest(const OBB* const obb1, const OBB* const obb2, ContactInfo*& contactInfo) const;     // Return true and compute a contact info if the two OBB collide
+        bool computeCollisionTest(const OBB* const obb1, const Transform& transform1,
+                                  const OBB* const obb2, const Transform& transform2,
+                                  ContactInfo*& contactInfo) const;     // Return true and compute a contact info if the two OBB collide
         double computePenetrationDepth(double min1, double max1, double min2, double max2) const;                     // Compute the penetration depth of two projection intervals                             
         Vector3D computeContactNormal(const Vector3D& axis, const Vector3D& distanceOfOBBs) const;                    // Compute a contact normal
 
@@ -58,7 +61,9 @@ class SATAlgorithm : public NarrowPhaseAlgorithm {
         ~SATAlgorithm();          // Destructor
 
         virtual bool testCollision(const NarrowBoundingVolume* const boundingVolume1,
+                                   const Transform& transform1,
                                    const NarrowBoundingVolume* const boundingVolume2,
+                                   const Transform& transform2,
                                    ContactInfo*& contactInfo);                          // Return true and compute a contact info if the two bounding volume collide
 };
 
diff --git a/src/constraint/Contact.cpp b/src/constraint/Contact.cpp
index 815b85b2..d4cc2cc9 100644
--- a/src/constraint/Contact.cpp
+++ b/src/constraint/Contact.cpp
@@ -62,8 +62,8 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const {
     Vector3D r2CrossU1;
     Vector3D r1CrossU2;
     Vector3D r2CrossU2;
-    Vector3D body1Position = rigidBody1->getPosition();
-    Vector3D body2Position = rigidBody2->getPosition();
+    Vector3D body1Position = rigidBody1->getTransform().getPosition();
+    Vector3D body2Position = rigidBody2->getTransform().getPosition();
     int currentIndex = noConstraint;                        // Current constraint index
 
     assert(rigidBody1);
diff --git a/src/engine/PhysicsEngine.cpp b/src/engine/PhysicsEngine.cpp
index 28dbf8df..c321e689 100644
--- a/src/engine/PhysicsEngine.cpp
+++ b/src/engine/PhysicsEngine.cpp
@@ -144,6 +144,7 @@ void PhysicsEngine::updateAllBodiesMotion() {
             updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity);
 
             // If the body state has changed, we have to update some informations in the rigid body
+            // TODO : DELETE THIS
             rigidBody->update();
         }
     }
@@ -159,18 +160,20 @@ void PhysicsEngine::updatePositionAndOrientationOfBody(Body* body, const Vector3
     assert(rigidBody);
 
     // Update the old position and orientation of the body
-    rigidBody->updateOldPositionAndOrientation();
-
-    // Normalize the orientation quaternion
-    rigidBody->setOrientation(rigidBody->getOrientation().getUnit());
+    rigidBody->updateOldTransform();
 
     // Update the linear and angular velocity of the body
     rigidBody->setLinearVelocity(newLinVelocity);
     rigidBody->setAngularVelocity(newAngVelocity);
 
-    // Update the position and the orientation of the body
-    rigidBody->setPosition(rigidBody->getPosition() + newLinVelocity * dt);
-    rigidBody->setOrientation(rigidBody->getOrientation() + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * rigidBody->getOrientation() * 0.5 * dt);
+    // Get current position and orientation of the body
+    Vector3D currentPosition = rigidBody->getTransform().getPosition();
+    Quaternion currentOrientation = Quaternion(rigidBody->getTransform().getOrientation());
+
+    Vector3D newPosition = currentPosition + newLinVelocity * dt;
+    Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt;
+    Transform newTransform(newPosition, newOrientation.getUnit());
+    rigidBody->setTransform(newTransform);
 }
 
 // Compute and set the interpolation factor to all bodies
diff --git a/src/mathematics/Matrix3x3.cpp b/src/mathematics/Matrix3x3.cpp
index 6d627a04..7453d9da 100644
--- a/src/mathematics/Matrix3x3.cpp
+++ b/src/mathematics/Matrix3x3.cpp
@@ -70,20 +70,6 @@ Matrix3x3 Matrix3x3::getInverse() const {
     return (invDeterminant * tempMatrix.getTranspose());
 }
 
-// Overloaded operator for multiplication with a matrix
-Matrix3x3 Matrix3x3::operator*(const Matrix3x3& matrix2) const {
-    // Compute and return the multiplication of the matrices
-    return Matrix3x3(array[0][0]*matrix2.array[0][0] + array[0][1]*matrix2.array[1][0] + array[0][2]*matrix2.array[2][0],
-                     array[0][0]*matrix2.array[0][1] + array[0][1]*matrix2.array[1][1] + array[0][2]*matrix2.array[2][1],
-                     array[0][0]*matrix2.array[0][2] + array[0][1]*matrix2.array[1][2] + array[0][2]*matrix2.array[2][2],
-                     array[1][0]*matrix2.array[0][0] + array[1][1]*matrix2.array[1][0] + array[1][2]*matrix2.array[2][0],
-                     array[1][0]*matrix2.array[0][1] + array[1][1]*matrix2.array[1][1] + array[1][2]*matrix2.array[2][1],
-                     array[1][0]*matrix2.array[0][2] + array[1][1]*matrix2.array[1][2] + array[1][2]*matrix2.array[2][2],
-                     array[2][0]*matrix2.array[0][0] + array[2][1]*matrix2.array[1][0] + array[2][2]*matrix2.array[2][0],
-                     array[2][0]*matrix2.array[0][1] + array[2][1]*matrix2.array[1][1] + array[2][2]*matrix2.array[2][1],
-                     array[2][0]*matrix2.array[0][2] + array[2][1]*matrix2.array[1][2] + array[2][2]*matrix2.array[2][2]);
-}
-
 // Overloaded operator for assignment
 Matrix3x3& Matrix3x3::operator=(const Matrix3x3& matrix2) {
     // Check for self-assignment
diff --git a/src/mathematics/Matrix3x3.h b/src/mathematics/Matrix3x3.h
index eba308f2..865bf882 100644
--- a/src/mathematics/Matrix3x3.h
+++ b/src/mathematics/Matrix3x3.h
@@ -54,10 +54,13 @@ class Matrix3x3 {
         void setValue(int i, int j, double value);                                              // Set a value in the matrix
         void setAllValues(double a1, double a2, double a3, double b1, double b2, double b3,
                   double c1, double c2, double c3);                                             // Set all the values in the matrix
+        Vector3D getColumn(int i) const;                                                        // Return a column
         Matrix3x3 getTranspose() const;                                                         // Return the transpose matrix
         double getDeterminant() const;                                                          // Return the determinant of the matrix
         double getTrace() const;                                                                // Return the trace of the matrix
         Matrix3x3 getInverse() const;                                                           // Return the inverse matrix
+        Matrix3x3 getAbsoluteMatrix() const;                                                    // Return the matrix with absolute values
+        void setToIdentity();                                                                   // Set the matrix to the identity matrix
         static Matrix3x3 identity();                                                            // Return the 3x3 identity matrix
 
         // --- Overloaded operators --- //
@@ -91,6 +94,12 @@ inline void Matrix3x3::setAllValues(double a1, double a2, double a3, double b1,
     array[2][0] = c1; array[2][1] = c2; array[2][2] = c3;
 }
 
+// Return a column
+inline Vector3D Matrix3x3::getColumn(int i) const {
+    assert(i>= 0 && i<3);
+    return Vector3D(array[0][i], array[1][i], array[2][i]);
+}
+
 // Return the transpose matrix
 inline Matrix3x3 Matrix3x3::getTranspose() const {
     // Return the transpose matrix
@@ -133,12 +142,40 @@ inline bool Matrix3x3::operator==(const Matrix3x3& matrix2) const {
             array[2][0] == matrix2.array[2][0] && array[2][1] == matrix2.array[2][1] && array[2][2] == matrix2.array[2][2]);
 }
 
+// Set the matrix to the identity matrix
+inline void Matrix3x3::setToIdentity() {
+    array[0][0] = 1.0; array[0][1] = 0.0; array[0][2] = 0.0;
+    array[1][0] = 0.0; array[1][1] = 1.0; array[1][2] = 0.0;
+    array[2][0] = 0.0; array[2][1] = 0.0; array[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(array[0][0]), fabs(array[0][1]), fabs(array[0][2]),
+                     fabs(array[1][0]), fabs(array[1][1]), fabs(array[1][2]),
+                     fabs(array[2][0]), fabs(array[2][1]), fabs(array[2][2]));
+}
+
+// Overloaded operator for multiplication with a matrix
+inline Matrix3x3 Matrix3x3::operator*(const Matrix3x3& matrix2) const {
+    // Compute and return the multiplication of the matrices
+    return Matrix3x3(array[0][0]*matrix2.array[0][0] + array[0][1]*matrix2.array[1][0] + array[0][2]*matrix2.array[2][0],
+                     array[0][0]*matrix2.array[0][1] + array[0][1]*matrix2.array[1][1] + array[0][2]*matrix2.array[2][1],
+                     array[0][0]*matrix2.array[0][2] + array[0][1]*matrix2.array[1][2] + array[0][2]*matrix2.array[2][2],
+                     array[1][0]*matrix2.array[0][0] + array[1][1]*matrix2.array[1][0] + array[1][2]*matrix2.array[2][0],
+                     array[1][0]*matrix2.array[0][1] + array[1][1]*matrix2.array[1][1] + array[1][2]*matrix2.array[2][1],
+                     array[1][0]*matrix2.array[0][2] + array[1][1]*matrix2.array[1][2] + array[1][2]*matrix2.array[2][2],
+                     array[2][0]*matrix2.array[0][0] + array[2][1]*matrix2.array[1][0] + array[2][2]*matrix2.array[2][0],
+                     array[2][0]*matrix2.array[0][1] + array[2][1]*matrix2.array[1][1] + array[2][2]*matrix2.array[2][1],
+                     array[2][0]*matrix2.array[0][2] + array[2][1]*matrix2.array[1][2] + array[2][2]*matrix2.array[2][2]);
+}
+
 // Overloaded operator for addition
 inline Matrix3x3 Matrix3x3::operator+(const Matrix3x3& matrix2) const {
     // Return the sum matrix
diff --git a/src/mathematics/Transform.cpp b/src/mathematics/Transform.cpp
new file mode 100644
index 00000000..110e76d9
--- /dev/null
+++ b/src/mathematics/Transform.cpp
@@ -0,0 +1,52 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010 Daniel Chappuis                                            *
+*********************************************************************************
+*                                                                               *
+* Permission is hereby granted, free of charge, to any person obtaining a copy  *
+* of this software and associated documentation files (the "Software"), to deal *
+* in the Software without restriction, including without limitation the rights  *
+* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell     *
+* copies of the Software, and to permit persons to whom the Software is         *
+* furnished to do so, subject to the following conditions:                      *
+*                                                                               *
+* The above copyright notice and this permission notice shall be included in    *
+* all copies or substantial portions of the Software.                           *
+*                                                                               *
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR    *
+* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,      *
+* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE   *
+* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER        *
+* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN     *
+* THE SOFTWARE.                                                                 *
+********************************************************************************/
+
+// Libraries
+#include "Transform.h"
+
+// Namespaces
+using namespace reactphysics3d;
+
+// Constructor
+Transform::Transform() {
+    position = Vector3D(0.0, 0.0, 0.0);
+    orientation.setToIdentity();
+}
+
+// Constructor
+Transform::Transform(const Vector3D& position, const Matrix3x3& orientation) {
+    this->position = position;
+    this->orientation = orientation;
+}
+
+// Constructor
+Transform::Transform(const Vector3D& position, const Quaternion& orientation) {
+    this->position = position;
+    this->orientation = orientation.getMatrix();
+}
+
+// Destructor
+Transform::~Transform() {
+    
+}
\ No newline at end of file
diff --git a/src/mathematics/Transform.h b/src/mathematics/Transform.h
new file mode 100644
index 00000000..70e3607b
--- /dev/null
+++ b/src/mathematics/Transform.h
@@ -0,0 +1,138 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010 Daniel Chappuis                                            *
+*********************************************************************************
+*                                                                               *
+* Permission is hereby granted, free of charge, to any person obtaining a copy  *
+* of this software and associated documentation files (the "Software"), to deal *
+* in the Software without restriction, including without limitation the rights  *
+* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell     *
+* copies of the Software, and to permit persons to whom the Software is         *
+* furnished to do so, subject to the following conditions:                      *
+*                                                                               *
+* The above copyright notice and this permission notice shall be included in    *
+* all copies or substantial portions of the Software.                           *
+*                                                                               *
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR    *
+* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,      *
+* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE   *
+* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER        *
+* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN     *
+* THE SOFTWARE.                                                                 *
+********************************************************************************/
+
+#ifndef TRANSFORM_H
+#define	TRANSFORM_H
+
+// Libraries
+#include "Matrix3x3.h"
+#include "Vector3D.h"
+#include "Quaternion.h"
+
+// ReactPhysiscs3D namespace
+namespace reactphysics3d {
+
+/*  -------------------------------------------------------------------
+    Class Transform :
+        This class represents a position and an orientation in 3D. It can
+        also be seen as representing a translation and a rotation
+    -------------------------------------------------------------------
+*/
+class Transform {
+    private :
+        Vector3D position;                      // position
+        Matrix3x3 orientation;                  // orientation matrix
+
+    public :
+        Transform();                                                            // Constructor
+        Transform(const Vector3D& position, const Matrix3x3& orientation);      // Constructor
+        Transform(const Vector3D& position, const Quaternion& orientation);     // Constructor
+        ~Transform();                                                           // Destructor
+
+        const Vector3D& getPosition() const;                                    // Return the origin of the transform
+        void setPosition(const Vector3D& position);                             // Set the origin of the transform
+        const Matrix3x3& getOrientation() const;                                // Return the rotation matrix
+        void setOrientation(const Matrix3x3& orientation);                      // Set the rotation matrix of the transform
+        void setToIdentity();                                                   // Set the transform to the identity transform
+        void setFromOpenGL(double* openglMatrix);                               // Set the transform from an OpenGL transform matrix
+        void getOpenGLMatrix(double* openglMatrix) const;                       // Get the OpenGL matrix of the transform
+        Transform inverse() const;                                              // Return the inverse of the transform
+        static Transform interpolateTransforms(const Transform& oldTransform,
+                                               const Transform& newTransform,
+                                               double interpolationFactor);     // Return an interpolated transform
+
+        Vector3D operator*(const Vector3D& vector) const;          // Return the transformed vector
+        Transform operator*(const Transform& transform2) const;     // Operator of multiplication of a transform with another one
+};
+
+// Return the position of the transform
+inline const Vector3D& Transform::getPosition() const {
+    return position;
+}
+
+// Set the origin of the transform
+inline void Transform::setPosition(const Vector3D& position) {
+    this->position = position;
+}
+
+// Return the rotation matrix
+inline const Matrix3x3& Transform::getOrientation() const {
+    return orientation;
+}
+
+// Set the rotation matrix of the transform
+inline void Transform::setOrientation(const Matrix3x3& orientation) {
+    this->orientation = orientation;
+}
+
+// Set the transform to the identity transform
+inline void Transform::setToIdentity() {
+    position = Vector3D(0.0, 0.0, 0.0);
+    orientation.setToIdentity();
+}
+
+// Set the transform from an OpenGL transform matrix
+inline void Transform::setFromOpenGL(double* openglMatrix) {
+    orientation.setAllValues(openglMatrix[0], openglMatrix[4], openglMatrix[8],
+                             openglMatrix[1], openglMatrix[5], openglMatrix[9],
+                             openglMatrix[2], openglMatrix[6], openglMatrix[10]);
+    position.setAllValues(openglMatrix[12], openglMatrix[13], openglMatrix[14]);
+}
+
+// Get the OpenGL matrix of the transform
+inline void Transform::getOpenGLMatrix(double* openglMatrix) const {
+    openglMatrix[0] = orientation.getValue(0, 0); openglMatrix[1] = orientation.getValue(1, 0); openglMatrix[2] = orientation.getValue(2, 0); openglMatrix[3] = 0.0;
+    openglMatrix[4] = orientation.getValue(0, 1); openglMatrix[5] = orientation.getValue(1, 1); openglMatrix[6] = orientation.getValue(2, 1); openglMatrix[7] = 0.0;
+    openglMatrix[8] = orientation.getValue(0, 2); openglMatrix[9] = orientation.getValue(1, 2); openglMatrix[10] = orientation.getValue(2, 2); openglMatrix[11] = 0.0;
+    openglMatrix[12] = position.getX(); openglMatrix[13] = position.getY(); openglMatrix[14] = position.getZ(); openglMatrix[15] = 1.0;
+}
+
+// Return the inverse of the transform
+inline Transform Transform::inverse() const {
+    Matrix3x3 invMatrix = orientation.getTranspose();
+    return Transform(invMatrix * position.getOpposite(), invMatrix);
+}
+
+// Return an interpolated transform
+inline Transform Transform::interpolateTransforms(const Transform& oldTransform, const Transform& newTransform,
+                                                         double interpolationFactor) {
+    Vector3D interPosition = oldTransform.position * (1.0-interpolationFactor) + newTransform.position * interpolationFactor;
+    Quaternion interOrientation = Quaternion::slerp(oldTransform.orientation, newTransform.orientation, interpolationFactor);
+    return Transform(interPosition, interOrientation);
+}
+
+// Return the transformed vector
+inline Vector3D Transform::operator*(const Vector3D& vector) const {
+    return (orientation * vector) + position;
+}
+
+// Operator of multiplication of a transform with another one
+inline Transform Transform::operator*(const Transform& transform2) const {
+    return Transform(position + orientation * transform2.position, orientation * transform2.orientation);
+}
+
+} // End of the ReactPhysics3D namespace
+
+#endif
+
diff --git a/src/mathematics/mathematics.h b/src/mathematics/mathematics.h
index 5e16ebd0..f13ef901 100644
--- a/src/mathematics/mathematics.h
+++ b/src/mathematics/mathematics.h
@@ -31,6 +31,7 @@
 #include "Quaternion.h"
 #include "Vector.h"
 #include "Vector3D.h"
+#include "Transform.h"
 #include "../constants.h"
 #include "exceptions.h"
 #include "mathematics_functions.h"