To rework

.gitignore

@@ -0,0 +1,27 @@
+# Compiled source #
+###################
+*.com
+*.class
+*.dll
+*.exe
+*.o
+*.so
+
+# Logs #
+######################
+*.log
+
+# OS generated files #
+######################
+.DS_Store
+.DS_Store?
+._*
+.Spotlight-V100
+.Trashes
+Icon?
+ehthumbs.db
+Thumbs.db
+
+# vim swap files
+#####################
+*.*sw*

CMakeLists.txt

@@ -0,0 +1,189 @@
+# Minimum cmake version required
+CMAKE_MINIMUM_REQUIRED(VERSION 2.8.3)
+
+# Project configuration
+PROJECT(REACTPHYSICS3D CXX)
+
+# Build type
+IF (NOT CMAKE_BUILD_TYPE)
+    SET(CMAKE_BUILD_TYPE "Release")
+ENDIF (NOT CMAKE_BUILD_TYPE)
+
+# Where to build the library
+SET(LIBRARY_OUTPUT_PATH "lib")
+
+# Where to build the executables
+SET(OUR_EXECUTABLE_OUTPUT_PATH "${PROJECT_BINARY_DIR}/bin")
+
+ENABLE_TESTING()
+
+# Options
+OPTION(COMPILE_TESTBED "Select this if you want to build the testbed application" OFF)
+OPTION(COMPILE_TESTS "Select this if you want to build the tests" OFF)
+OPTION(PROFILING_ENABLED "Select this if you want to compile with enabled profiling" OFF)
+OPTION(DOUBLE_PRECISION_ENABLED "Select this if you want to compile using double precision floating
+                                 values" OFF)
+
+# Warning Compiler flags
+SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")
+
+# C++11 flags
+IF(CMAKE_CXX_COMPILER_ID MATCHES "Clang" OR CMAKE_CXX_COMPILER_ID MATCHES "GNU")
+	include(CheckCXXCompilerFlag)
+	CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+	IF(COMPILER_SUPPORTS_CXX11)
+		set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+	ELSE()
+		message("The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+	ENDIF()
+ENDIF()
+
+# Headers
+INCLUDE_DIRECTORIES(src)
+
+IF(PROFILING_ENABLED)
+    ADD_DEFINITIONS(-DIS_PROFILING_ACTIVE)
+ENDIF(PROFILING_ENABLED)
+
+IF(DOUBLE_PRECISION_ENABLED)
+    ADD_DEFINITIONS(-DIS_DOUBLE_PRECISION_ENABLED)
+ENDIF(DOUBLE_PRECISION_ENABLED)
+
+# Source files
+SET (REACTPHYSICS3D_SOURCES
+    "src/configuration.h"
+    "src/decimal.h"
+    "src/reactphysics3d.h"
+    "src/body/Body.h"
+    "src/body/Body.cpp"
+    "src/body/CollisionBody.h"
+    "src/body/CollisionBody.cpp"
+    "src/body/RigidBody.h"
+    "src/body/RigidBody.cpp"
+    "src/collision/broadphase/BroadPhaseAlgorithm.h"
+    "src/collision/broadphase/BroadPhaseAlgorithm.cpp"
+    "src/collision/broadphase/DynamicAABBTree.h"
+    "src/collision/broadphase/DynamicAABBTree.cpp"
+    "src/collision/narrowphase/CollisionDispatch.h"
+    "src/collision/narrowphase/DefaultCollisionDispatch.h"
+    "src/collision/narrowphase/DefaultCollisionDispatch.cpp"
+    "src/collision/narrowphase/EPA/EdgeEPA.h"
+    "src/collision/narrowphase/EPA/EdgeEPA.cpp"
+    "src/collision/narrowphase/EPA/EPAAlgorithm.h"
+    "src/collision/narrowphase/EPA/EPAAlgorithm.cpp"
+    "src/collision/narrowphase/EPA/TriangleEPA.h"
+    "src/collision/narrowphase/EPA/TriangleEPA.cpp"
+    "src/collision/narrowphase/EPA/TrianglesStore.h"
+    "src/collision/narrowphase/EPA/TrianglesStore.cpp"
+    "src/collision/narrowphase/GJK/Simplex.h"
+    "src/collision/narrowphase/GJK/Simplex.cpp"
+    "src/collision/narrowphase/GJK/GJKAlgorithm.h"
+    "src/collision/narrowphase/GJK/GJKAlgorithm.cpp"
+    "src/collision/narrowphase/NarrowPhaseAlgorithm.h"
+    "src/collision/narrowphase/NarrowPhaseAlgorithm.cpp"
+    "src/collision/narrowphase/SphereVsSphereAlgorithm.h"
+    "src/collision/narrowphase/SphereVsSphereAlgorithm.cpp"
+    "src/collision/narrowphase/ConcaveVsConvexAlgorithm.h"
+    "src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp"
+    "src/collision/shapes/AABB.h"
+    "src/collision/shapes/AABB.cpp"
+    "src/collision/shapes/ConvexShape.h"
+    "src/collision/shapes/ConvexShape.cpp"
+    "src/collision/shapes/ConcaveShape.h"
+    "src/collision/shapes/ConcaveShape.cpp"
+    "src/collision/shapes/BoxShape.h"
+    "src/collision/shapes/BoxShape.cpp"
+    "src/collision/shapes/CapsuleShape.h"
+    "src/collision/shapes/CapsuleShape.cpp"
+    "src/collision/shapes/CollisionShape.h"
+    "src/collision/shapes/CollisionShape.cpp"
+    "src/collision/shapes/ConeShape.h"
+    "src/collision/shapes/ConeShape.cpp"
+    "src/collision/shapes/ConvexMeshShape.h"
+    "src/collision/shapes/ConvexMeshShape.cpp"
+    "src/collision/shapes/CylinderShape.h"
+    "src/collision/shapes/CylinderShape.cpp"
+    "src/collision/shapes/SphereShape.h"
+    "src/collision/shapes/SphereShape.cpp"
+    "src/collision/shapes/TriangleShape.h"
+    "src/collision/shapes/TriangleShape.cpp"
+    "src/collision/shapes/ConcaveMeshShape.h"
+    "src/collision/shapes/ConcaveMeshShape.cpp"
+    "src/collision/shapes/HeightFieldShape.h"
+    "src/collision/shapes/HeightFieldShape.cpp"
+    "src/collision/RaycastInfo.h"
+    "src/collision/RaycastInfo.cpp"
+    "src/collision/ProxyShape.h"
+    "src/collision/ProxyShape.cpp"
+    "src/collision/TriangleVertexArray.h"
+    "src/collision/TriangleVertexArray.cpp"
+    "src/collision/TriangleMesh.h"
+    "src/collision/TriangleMesh.cpp"
+    "src/collision/CollisionDetection.h"
+    "src/collision/CollisionDetection.cpp"
+    "src/collision/CollisionShapeInfo.h"
+    "src/collision/ContactManifold.h"
+    "src/collision/ContactManifold.cpp"
+    "src/collision/ContactManifoldSet.h"
+    "src/collision/ContactManifoldSet.cpp"
+    "src/constraint/BallAndSocketJoint.h"
+    "src/constraint/BallAndSocketJoint.cpp"
+    "src/constraint/ContactPoint.h"
+    "src/constraint/ContactPoint.cpp"
+    "src/constraint/FixedJoint.h"
+    "src/constraint/FixedJoint.cpp"
+    "src/constraint/HingeJoint.h"
+    "src/constraint/HingeJoint.cpp"
+    "src/constraint/Joint.h"
+    "src/constraint/Joint.cpp"
+    "src/constraint/SliderJoint.h"
+    "src/constraint/SliderJoint.cpp"
+    "src/engine/CollisionWorld.h"
+    "src/engine/CollisionWorld.cpp"
+    "src/engine/ConstraintSolver.h"
+    "src/engine/ConstraintSolver.cpp"
+    "src/engine/ContactSolver.h"
+    "src/engine/ContactSolver.cpp"
+    "src/engine/DynamicsWorld.h"
+    "src/engine/DynamicsWorld.cpp"
+    "src/engine/EventListener.h"
+    "src/engine/Impulse.h"
+    "src/engine/Island.h"
+    "src/engine/Island.cpp"
+    "src/engine/Material.h"
+    "src/engine/Material.cpp"
+    "src/engine/OverlappingPair.h"
+    "src/engine/OverlappingPair.cpp"
+    "src/engine/Profiler.h"
+    "src/engine/Profiler.cpp"
+    "src/engine/Timer.h"
+    "src/engine/Timer.cpp"
+    "src/mathematics/mathematics.h"
+    "src/mathematics/mathematics_functions.h"
+    "src/mathematics/mathematics_functions.cpp"
+    "src/mathematics/Matrix2x2.h"
+    "src/mathematics/Matrix2x2.cpp"
+    "src/mathematics/Matrix3x3.h"
+    "src/mathematics/Matrix3x3.cpp"
+    "src/mathematics/Quaternion.h"
+    "src/mathematics/Quaternion.cpp"
+    "src/mathematics/Transform.h"
+    "src/mathematics/Transform.cpp"
+    "src/mathematics/Vector2.h"
+    "src/mathematics/Vector2.cpp"
+    "src/mathematics/Vector3.h"
+    "src/mathematics/Ray.h"
+    "src/mathematics/Vector3.cpp"
+    "src/memory/MemoryAllocator.h"
+    "src/memory/MemoryAllocator.cpp"
+    "src/memory/Stack.h"
+)
+
+# Create the library
+ADD_LIBRARY(reactphysics3d STATIC ${REACTPHYSICS3D_SOURCES})
+
+# If we need to compile the testbed application
+add_subdirectory(testbed/)
+
+# If we need to compile the tests
+add_subdirectory(test/)

LICENSE

@@ -0,0 +1,20 @@
+ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+Copyright (c) 2010-2016 Daniel Chappuis
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the
+use of this software.
+
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it
+freely, subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim
+   that you wrote the original software. If you use this software in a
+   product, an acknowledgment in the product documentation would be
+   appreciated but is not required.
+
+2. Altered source versions must be plainly marked as such, and must not be
+   misrepresented as being the original software.
+
+3. This notice may not be removed or altered from any source distribution.

README.md

@@ -1 +1,51 @@
-basic start point
+[![Travis Build Status](https://travis-ci.org/DanielChappuis/reactphysics3d.svg?branch=master)](https://travis-ci.org/DanielChappuis/reactphysics3d)
+
+## ReactPhysics3D
+
+ReactPhysics3D is an open source C++ physics engine library that can be used in 3D simulations and games.
+
+Website : [http://www.reactphysics3d.com](http://www.reactphysics3d.com)
+
+Author : Daniel Chappuis
+
+<img src="https://raw.githubusercontent.com/DanielChappuis/reactphysics3d/master/documentation/UserManual/images/testbed.png" alt="Drawing" height="400" />
+
+## Features
+
+ReactPhysics3D has the following features :
+
+- Rigid body dynamics
+- Discrete collision detection
+- Collision shapes (Sphere, Box, Cone, Cylinder, Capsule, Convex Mesh, static Concave Mesh, Height Field)
+- Multiple collision shapes per body
+- Broadphase collision detection (Dynamic AABB tree)
+- Narrowphase collision detection (GJK/EPA)
+- Collision response and friction (Sequential Impulses Solver)
+- Joints (Ball and Socket, Hinge, Slider, Fixed)
+- Collision filtering with categories
+- Ray casting
+- Sleeping technique for inactive bodies
+- Integrated Profiler
+- Multi-platform (Windows, Linux, Mac OS X)
+- No dependencies (only OpenGL for the testbed application)
+- Documentation (User manual and Doxygen API)
+- Testbed application with demo scenes
+- Unit tests
+
+## License
+
+The ReactPhysics3D library is released under the open-source BSD 3 clauses license.
+
+## Documentation
+
+You can find the User Manual and the Doxygen API Documentation [here](http://www.reactphysics3d.com/documentation.html)
+
+## Branches
+
+The "master" branch always contains the last released version of the library and some possible bug fixes. This is the most stable version. On the other side,
+the "develop" branch is used for development. This branch is frequently updated and can be quite unstable. Therefore, if you want to use the library in
+your application, it is recommended to checkout the "master" branch.
+
+## Issues
+
+If you find any issue with the library, you can report it on the issue tracker [here](https://github.com/DanielChappuis/reactphysics3d/issues).

VERSION

@@ -0,0 +1 @@
+0.6.0

cmake/FindGLEW.cmake

@@ -0,0 +1,65 @@
+#
+# Try to find GLEW library and include path.
+# Once done this will define
+#
+# GLEW_FOUND
+# GLEW_INCLUDE_PATH
+# GLEW_LIBRARY
+#
+
+IF (WIN32)
+FIND_PATH( GLEW_INCLUDE_PATH GL/glew.h
+$ENV{PROGRAMFILES}/GLEW/include
+${GLEW_ROOT_DIR}/include
+DOC "The directory where GL/glew.h resides")
+
+IF (NV_SYSTEM_PROCESSOR STREQUAL "AMD64")
+FIND_LIBRARY( GLEW_LIBRARY
+NAMES glew64 glew64s
+PATHS
+$ENV{PROGRAMFILES}/GLEW/lib
+${PROJECT_SOURCE_DIR}/src/nvgl/glew/bin
+${PROJECT_SOURCE_DIR}/src/nvgl/glew/lib
+DOC "The GLEW library (64-bit)"
+)
+ELSE(NV_SYSTEM_PROCESSOR STREQUAL "AMD64")
+FIND_LIBRARY( GLEW_LIBRARY
+NAMES glew GLEW glew32 glew32s
+PATHS
+$ENV{PROGRAMFILES}/GLEW/lib
+${PROJECT_SOURCE_DIR}/src/nvgl/glew/bin
+${PROJECT_SOURCE_DIR}/src/nvgl/glew/lib
+DOC "The GLEW library"
+)
+ENDIF(NV_SYSTEM_PROCESSOR STREQUAL "AMD64")
+ELSE (WIN32)
+FIND_PATH( GLEW_INCLUDE_PATH GL/glew.h
+/usr/include
+/usr/local/include
+/sw/include
+/opt/local/include
+${GLEW_ROOT_DIR}/include
+DOC "The directory where GL/glew.h resides")
+
+FIND_LIBRARY( GLEW_LIBRARY
+NAMES GLEW glew
+PATHS
+/usr/lib64
+/usr/lib
+/usr/local/lib64
+/usr/local/lib
+/sw/lib
+/opt/local/lib
+${GLEW_ROOT_DIR}/lib
+DOC "The GLEW library")
+ENDIF (WIN32)
+
+SET(GLEW_FOUND "NO")
+IF (GLEW_INCLUDE_PATH AND GLEW_LIBRARY)
+SET(GLEW_LIBRARIES ${GLEW_LIBRARY})
+SET(GLEW_FOUND "YES")
+ENDIF (GLEW_INCLUDE_PATH AND GLEW_LIBRARY)
+
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(GLEW DEFAULT_MSG GLEW_LIBRARY GLEW_INCLUDE_PATH)

doc/UserManual/configHTLatex.cfg

@@ -0,0 +1,15 @@
+\Preamble{html}
+\begin{document}
+% Upper case PNG file extensions
+\Configure{graphics*}
+{PNG}
+{\Picture[pict]{\csname Gin@base\endcsname.PNG align=center border=0}}
+% Lower case png extensions
+\Configure{graphics*}
+{png}
+{\Picture[pict]{\csname Gin@base\endcsname.png align=center border=0}}
+% Problems with spanish babel
+\makeatletter
+\let\ifes@LaTeXe\iftrue
+\makeatother
+\EndPreamble

doc/UserManual/generateHTMLManual.sh

@@ -0,0 +1,13 @@
+#!/bin/bash
+
+# Delete the /html folder
+rm -R html/
+
+# Create the /html folder
+mkdir html
+
+# Use the htlatex command to generate the HTML user manual from the .tex file
+htlatex ReactPhysics3D-UserManual.tex "configHTLatex.cfg,html" "" -dhtml/
+
+# Copy the images/ folder into the html/ folder
+cp -R images/ html/images/

doc/UserManual/title.tex

@@ -0,0 +1,27 @@
+% Title page definition
+
+\makeatletter
+\def\maketitle{%
+  \null
+  \thispagestyle{empty}%
+  \begin{center}\leavevmode
+    \normalfont
+    {}
+    \vskip 1.3cm
+    {\Huge \@title\par}%
+    \vskip 0.3cm
+    {\Large Version: 0.6.0\par}%
+    \vskip 0.3cm
+    {\Large \@author\par}%
+    \vskip 2cm
+    {\includegraphics[height=5cm]{images/ReactPhysics3DLogo.png}}
+    \vskip 2cm
+    {\large{\url{http://www.reactphysics3d.com}}}
+    \vskip 0.2cm
+   {\large \@date}%
+  \end{center}%
+  \vfill
+  \null
+  \cleardoublepage
+  }
+\makeatother

src/body/Body.cpp

@@ -0,0 +1,46 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "Body.h"
+#include "collision/shapes/CollisionShape.h"
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Constructor
+/**
+ * @param id ID of the new body
+ */
+Body::Body(bodyindex id)
+     : mID(id), mIsAlreadyInIsland(false), mIsAllowedToSleep(true), mIsActive(true),
+       mIsSleeping(false), mSleepTime(0), mUserData(NULL) {
+
+}
+
+// Destructor
+Body::~Body() {
+
+}

src/body/Body.h

@@ -0,0 +1,243 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_BODY_H
+#define REACTPHYSICS3D_BODY_H
+
+// Libraries
+#include <stdexcept>
+#include <cassert>
+#include "configuration.h"
+
+/// Namespace reactphysics3d
+namespace reactphysics3d {
+
+// TODO : Make this class abstract
+// Class Body
+/**
+ * This class to represent a body of the physics engine. You should not
+ * instantiante this class but instantiate the CollisionBody or RigidBody
+ * classes instead.
+ */
+class Body {
+
+    protected :
+
+        // -------------------- Attributes -------------------- //
+
+        /// ID of the body
+        bodyindex mID;
+
+        /// True if the body has already been added in an island (for sleeping technique)
+        bool mIsAlreadyInIsland;
+
+        /// True if the body is allowed to go to sleep for better efficiency
+        bool mIsAllowedToSleep;
+
+        /// True if the body is active.
+        /// An inactive body does not participate in collision detection,
+        /// is not simulated and will not be hit in a ray casting query.
+        /// A body is active by default. If you set this
+        /// value to "false", all the proxy shapes of this body will be
+        /// removed from the broad-phase. If you set this value to "true",
+        /// all the proxy shapes will be added to the broad-phase. A joint
+        /// connected to an inactive body will also be inactive.
+        bool mIsActive;
+
+        /// True if the body is sleeping (for sleeping technique)
+        bool mIsSleeping;
+
+        /// Elapsed time since the body velocity was bellow the sleep velocity
+        decimal mSleepTime;
+
+        /// Pointer that can be used to attach user data to the body
+        void* mUserData;
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        Body(const Body& body);
+
+        /// Private assignment operator
+        Body& operator=(const Body& body);
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        Body(bodyindex id);
+
+        /// Destructor
+        virtual ~Body();
+
+        /// Return the ID of the body
+        bodyindex getID() const;
+
+        /// Return whether or not the body is allowed to sleep
+        bool isAllowedToSleep() const;
+
+        /// Set whether or not the body is allowed to go to sleep
+        void setIsAllowedToSleep(bool isAllowedToSleep);
+
+        /// Set the variable to know whether or not the body is sleeping
+        virtual void setIsSleeping(bool isSleeping);
+
+        /// Return whether or not the body is sleeping
+        bool isSleeping() const;
+
+        /// Return true if the body is active
+        bool isActive() const;
+
+        /// Set whether or not the body is active
+        virtual void setIsActive(bool isActive);
+
+        /// Return a pointer to the user data attached to this body
+        void* getUserData() const;
+
+        /// Attach user data to this body
+        void setUserData(void* userData);
+
+        /// Smaller than operator
+        bool operator<(const Body& body2) const;
+
+        /// Larger than operator
+        bool operator>(const Body& body2) const;
+
+        /// Equal operator
+        bool operator==(const Body& body2) const;
+
+        /// Not equal operator
+        bool operator!=(const Body& body2) const;
+
+        // -------------------- Friendship -------------------- //
+
+        friend class DynamicsWorld;
+};
+
+// Return the id of the body
+/**
+ * @return The ID of the body
+ */
+inline bodyindex Body::getID() const {
+    return mID;
+}
+
+// Return whether or not the body is allowed to sleep
+/**
+ * @return True if the body is allowed to sleep and false otherwise
+ */
+inline bool Body::isAllowedToSleep() const {
+    return mIsAllowedToSleep;
+}
+
+// Set whether or not the body is allowed to go to sleep
+/**
+ * @param isAllowedToSleep True if the body is allowed to sleep
+ */
+inline void Body::setIsAllowedToSleep(bool isAllowedToSleep) {
+    mIsAllowedToSleep = isAllowedToSleep;
+
+    if (!mIsAllowedToSleep) setIsSleeping(false);
+}
+
+// Return whether or not the body is sleeping
+/**
+ * @return True if the body is currently sleeping and false otherwise
+ */
+inline bool Body::isSleeping() const {
+    return mIsSleeping;
+}
+
+// Return true if the body is active
+/**
+ * @return True if the body currently active and false otherwise
+ */
+inline bool Body::isActive() const {
+    return mIsActive;
+}
+
+// Set whether or not the body is active
+/**
+ * @param isActive True if you want to activate the body
+ */
+inline void Body::setIsActive(bool isActive) {
+    mIsActive = isActive;
+}
+
+// Set the variable to know whether or not the body is sleeping
+inline void Body::setIsSleeping(bool isSleeping) {
+
+    if (isSleeping) {
+        mSleepTime = decimal(0.0);
+    }
+    else {
+        if (mIsSleeping) {
+            mSleepTime = decimal(0.0);
+        }
+    }
+
+    mIsSleeping = isSleeping;
+}
+
+// Return a pointer to the user data attached to this body
+/**
+ * @return A pointer to the user data you have attached to the body
+ */
+inline void* Body::getUserData() const {
+    return mUserData;
+}
+
+// Attach user data to this body
+/**
+ * @param userData A pointer to the user data you want to attach to the body
+ */
+inline void Body::setUserData(void* userData) {
+    mUserData = userData;
+}
+
+// Smaller than operator
+inline bool Body::operator<(const Body& body2) const {
+    return (mID < body2.mID);
+} 
+
+// Larger than operator
+inline bool Body::operator>(const Body& body2) const {
+    return (mID > body2.mID);
+} 
+
+// Equal operator
+inline bool Body::operator==(const Body& body2) const {
+    return (mID == body2.mID);
+}
+        
+// Not equal operator
+inline bool Body::operator!=(const Body& body2) const {
+    return (mID != body2.mID);
+}               
+
+}
+
+ #endif

src/body/CollisionBody.cpp

@@ -0,0 +1,354 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+ // Libraries
+#include "CollisionBody.h"
+#include "engine/CollisionWorld.h"
+#include "collision/ContactManifold.h"
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Constructor
+/**
+ * @param transform The transform of the body
+ * @param world The physics world where the body is created
+ * @param id ID of the body
+ */
+CollisionBody::CollisionBody(const Transform& transform, CollisionWorld& world, bodyindex id)
+              : Body(id), mType(DYNAMIC), mTransform(transform), mProxyCollisionShapes(NULL),
+                mNbCollisionShapes(0), mContactManifoldsList(NULL), mWorld(world) {
+
+}
+
+// Destructor
+CollisionBody::~CollisionBody() {
+    assert(mContactManifoldsList == NULL);
+
+    // Remove all the proxy collision shapes of the body
+    removeAllCollisionShapes();
+}
+
+// Add a collision shape to the body. Note that you can share a collision
+// shape between several bodies using the same collision shape instance to
+// when you add the shape to the different bodies. Do not forget to delete
+// the collision shape you have created at the end of your program.
+/// This method will return a pointer to a new proxy shape. A proxy shape is
+/// an object that links a collision shape and a given body. You can use the
+/// returned proxy shape to get and set information about the corresponding
+/// collision shape for that body.
+/**
+ * @param collisionShape A pointer to the collision shape you want to add to the body
+ * @param transform The transformation of the collision shape that transforms the
+ *        local-space of the collision shape into the local-space of the body
+ * @return A pointer to the proxy shape that has been created to link the body to
+ *         the new collision shape you have added.
+ */
+ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape,
+                                             const Transform& transform) {
+
+    // Create a new proxy collision shape to attach the collision shape to the body
+    ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate(
+                                      sizeof(ProxyShape))) ProxyShape(this, collisionShape,
+                                                                      transform, decimal(1));
+
+    // Add it to the list of proxy collision shapes of the body
+    if (mProxyCollisionShapes == NULL) {
+        mProxyCollisionShapes = proxyShape;
+    }
+    else {
+        proxyShape->mNext = mProxyCollisionShapes;
+        mProxyCollisionShapes = proxyShape;
+    }
+
+    // Compute the world-space AABB of the new collision shape
+    AABB aabb;
+    collisionShape->computeAABB(aabb, mTransform * transform);
+
+    // Notify the collision detection about this new collision shape
+    mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape, aabb);
+
+    mNbCollisionShapes++;
+
+    // Return a pointer to the collision shape
+    return proxyShape;
+}
+
+// Remove a collision shape from the body
+/// To remove a collision shape, you need to specify the pointer to the proxy
+/// shape that has been returned when you have added the collision shape to the
+/// body
+/**
+ * @param proxyShape The pointer of the proxy shape you want to remove
+ */
+void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
+
+    ProxyShape* current = mProxyCollisionShapes;
+
+    // If the the first proxy shape is the one to remove
+    if (current == proxyShape) {
+        mProxyCollisionShapes = current->mNext;
+
+        if (mIsActive) {
+            mWorld.mCollisionDetection.removeProxyCollisionShape(current);
+        }
+
+        current->~ProxyShape();
+        mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape));
+        mNbCollisionShapes--;
+        return;
+    }
+
+    // Look for the proxy shape that contains the collision shape in parameter
+    while(current->mNext != NULL) {
+
+        // If we have found the collision shape to remove
+        if (current->mNext == proxyShape) {
+
+            // Remove the proxy collision shape
+            ProxyShape* elementToRemove = current->mNext;
+            current->mNext = elementToRemove->mNext;
+
+            if (mIsActive) {
+                mWorld.mCollisionDetection.removeProxyCollisionShape(elementToRemove);
+            }
+
+            elementToRemove->~ProxyShape();
+            mWorld.mMemoryAllocator.release(elementToRemove, sizeof(ProxyShape));
+            mNbCollisionShapes--;
+            return;
+        }
+
+        // Get the next element in the list
+        current = current->mNext;
+    }
+}
+
+// Remove all the collision shapes
+void CollisionBody::removeAllCollisionShapes() {
+
+    ProxyShape* current = mProxyCollisionShapes;
+
+    // Look for the proxy shape that contains the collision shape in parameter
+    while(current != NULL) {
+
+        // Remove the proxy collision shape
+        ProxyShape* nextElement = current->mNext;
+
+        if (mIsActive) {
+            mWorld.mCollisionDetection.removeProxyCollisionShape(current);
+        }
+
+        current->~ProxyShape();
+        mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape));
+
+        // Get the next element in the list
+        current = nextElement;
+    }
+
+    mProxyCollisionShapes = NULL;
+}
+
+// Reset the contact manifold lists
+void CollisionBody::resetContactManifoldsList() {
+
+    // Delete the linked list of contact manifolds of that body
+    ContactManifoldListElement* currentElement = mContactManifoldsList;
+    while (currentElement != NULL) {
+        ContactManifoldListElement* nextElement = currentElement->next;
+
+        // Delete the current element
+        currentElement->~ContactManifoldListElement();
+        mWorld.mMemoryAllocator.release(currentElement, sizeof(ContactManifoldListElement));
+
+        currentElement = nextElement;
+    }
+    mContactManifoldsList = NULL;
+}
+
+// Update the broad-phase state for this body (because it has moved for instance)
+void CollisionBody::updateBroadPhaseState() const {
+
+    // For all the proxy collision shapes of the body
+    for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
+
+        // Update the proxy
+        updateProxyShapeInBroadPhase(shape);
+    }
+}
+
+// Update the broad-phase state of a proxy collision shape of the body
+void CollisionBody::updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool forceReinsert) const {
+
+    // Recompute the world-space AABB of the collision shape
+    AABB aabb;
+    proxyShape->getCollisionShape()->computeAABB(aabb, mTransform * proxyShape->getLocalToBodyTransform());
+
+    // Update the broad-phase state for the proxy collision shape
+    mWorld.mCollisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3(0, 0, 0), forceReinsert);
+}
+
+// Set whether or not the body is active
+/**
+ * @param isActive True if you want to activate the body
+ */
+void CollisionBody::setIsActive(bool isActive) {
+
+    // If the state does not change
+    if (mIsActive == isActive) return;
+
+    Body::setIsActive(isActive);
+
+    // If we have to activate the body
+    if (isActive) {
+
+        // For each proxy shape of the body
+        for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
+
+            // Compute the world-space AABB of the new collision shape
+            AABB aabb;
+            shape->getCollisionShape()->computeAABB(aabb, mTransform * shape->mLocalToBodyTransform);
+
+            // Add the proxy shape to the collision detection
+            mWorld.mCollisionDetection.addProxyCollisionShape(shape, aabb);
+        }
+    }
+    else {  // If we have to deactivate the body
+
+        // For each proxy shape of the body
+        for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
+
+            // Remove the proxy shape from the collision detection
+            mWorld.mCollisionDetection.removeProxyCollisionShape(shape);
+        }
+
+        // Reset the contact manifold list of the body
+        resetContactManifoldsList();
+    }
+}
+
+// Ask the broad-phase to test again the collision shapes of the body for collision
+// (as if the body has moved).
+void CollisionBody::askForBroadPhaseCollisionCheck() const {
+
+    // For all the proxy collision shapes of the body
+    for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
+
+        mWorld.mCollisionDetection.askForBroadPhaseCollisionCheck(shape);  
+    }
+}
+
+// Reset the mIsAlreadyInIsland variable of the body and contact manifolds.
+/// This method also returns the number of contact manifolds of the body.
+int CollisionBody::resetIsAlreadyInIslandAndCountManifolds() {
+
+    mIsAlreadyInIsland = false;
+
+    int nbManifolds = 0;
+
+    // Reset the mIsAlreadyInIsland variable of the contact manifolds for
+    // this body
+    ContactManifoldListElement* currentElement = mContactManifoldsList;
+    while (currentElement != NULL) {
+        currentElement->contactManifold->mIsAlreadyInIsland = false;
+        currentElement = currentElement->next;
+        nbManifolds++;
+    }
+
+    return nbManifolds;
+}
+
+// Return true if a point is inside the collision body
+/// This method returns true if a point is inside any collision shape of the body
+/**
+ * @param worldPoint The point to test (in world-space coordinates)
+ * @return True if the point is inside the body
+ */
+bool CollisionBody::testPointInside(const Vector3& worldPoint) const {
+
+    // For each collision shape of the body
+    for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
+
+        // Test if the point is inside the collision shape
+        if (shape->testPointInside(worldPoint)) return true;
+    }
+
+    return false;
+}
+
+// Raycast method with feedback information
+/// The method returns the closest hit among all the collision shapes of the body
+/**
+* @param ray The ray used to raycast agains the body
+* @param[out] raycastInfo Structure that contains the result of the raycasting
+*                         (valid only if the method returned true)
+* @return True if the ray hit the body and false otherwise
+*/
+bool CollisionBody::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
+
+    // If the body is not active, it cannot be hit by rays
+    if (!mIsActive) return false;
+
+    bool isHit = false;
+    Ray rayTemp(ray);
+
+    // For each collision shape of the body
+    for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
+
+        // Test if the ray hits the collision shape
+        if (shape->raycast(rayTemp, raycastInfo)) {
+            rayTemp.maxFraction = raycastInfo.hitFraction;
+            isHit = true;
+        }
+    }
+
+    return isHit;
+}
+
+// Compute and return the AABB of the body by merging all proxy shapes AABBs
+/**
+* @return The axis-aligned bounding box (AABB) of the body in world-space coordinates
+*/
+AABB CollisionBody::getAABB() const {
+
+    AABB bodyAABB;
+
+    if (mProxyCollisionShapes == NULL) return bodyAABB;
+
+    mProxyCollisionShapes->getCollisionShape()->computeAABB(bodyAABB, mTransform * mProxyCollisionShapes->getLocalToBodyTransform());
+
+    // For each proxy shape of the body
+    for (ProxyShape* shape = mProxyCollisionShapes->mNext; shape != NULL; shape = shape->mNext) {
+
+        // Compute the world-space AABB of the collision shape
+        AABB aabb;
+        shape->getCollisionShape()->computeAABB(aabb, mTransform * shape->getLocalToBodyTransform());
+
+        // Merge the proxy shape AABB with the current body AABB
+        bodyAABB.mergeWithAABB(aabb);
+    }
+
+    return bodyAABB;
+}

src/body/CollisionBody.h

@@ -0,0 +1,306 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_COLLISION_BODY_H
+#define REACTPHYSICS3D_COLLISION_BODY_H
+
+// Libraries
+#include <stdexcept>
+#include <cassert>
+#include "Body.h"
+#include "mathematics/Transform.h"
+#include "collision/shapes/AABB.h"
+#include "collision/shapes/CollisionShape.h"
+#include "collision/RaycastInfo.h"
+#include "memory/MemoryAllocator.h"
+#include "configuration.h"
+
+/// Namespace reactphysics3d
+namespace reactphysics3d {
+
+// Class declarations
+struct ContactManifoldListElement;
+class ProxyShape;
+class CollisionWorld;
+
+/// Enumeration for the type of a body
+/// STATIC : A static body has infinite mass, zero velocity but the position can be
+///          changed manually. A static body does not collide with other static or kinematic bodies.
+/// KINEMATIC : A kinematic body has infinite mass, the velocity can be changed manually and its
+///             position is computed by the physics engine. A kinematic body does not collide with
+///             other static or kinematic bodies.
+/// DYNAMIC : A dynamic body has non-zero mass, non-zero velocity determined by forces and its
+///           position is determined by the physics engine. A dynamic body can collide with other
+///           dynamic, static or kinematic bodies.
+enum BodyType {STATIC, KINEMATIC, DYNAMIC};
+
+// Class CollisionBody
+/**
+ * This class represents a body that is able to collide with others
+ * bodies. This class inherits from the Body class.
+ */
+class CollisionBody : public Body {
+
+    protected :
+
+        // -------------------- Attributes -------------------- //
+
+        /// Type of body (static, kinematic or dynamic)
+        BodyType mType;
+
+        /// Position and orientation of the body
+        Transform mTransform;
+
+        /// First element of the linked list of proxy collision shapes of this body
+        ProxyShape* mProxyCollisionShapes;
+
+        /// Number of collision shapes
+        uint mNbCollisionShapes;
+
+        /// First element of the linked list of contact manifolds involving this body
+        ContactManifoldListElement* mContactManifoldsList;
+
+        /// Reference to the world the body belongs to
+        CollisionWorld& mWorld;
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        CollisionBody(const CollisionBody& body);
+
+        /// Private assignment operator
+        CollisionBody& operator=(const CollisionBody& body);
+
+        /// Reset the contact manifold lists
+        void resetContactManifoldsList();
+
+        /// Remove all the collision shapes
+        void removeAllCollisionShapes();
+
+        /// Update the broad-phase state for this body (because it has moved for instance)
+        virtual void updateBroadPhaseState() const;
+
+        /// Update the broad-phase state of a proxy collision shape of the body
+        void updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool forceReinsert = false) const;
+
+        /// Ask the broad-phase to test again the collision shapes of the body for collision
+        /// (as if the body has moved).
+        void askForBroadPhaseCollisionCheck() const;
+
+        /// Reset the mIsAlreadyInIsland variable of the body and contact manifolds
+        int resetIsAlreadyInIslandAndCountManifolds();
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        CollisionBody(const Transform& transform, CollisionWorld& world, bodyindex id);
+
+        /// Destructor
+        virtual ~CollisionBody();
+
+        /// Return the type of the body
+        BodyType getType() const;
+
+        /// Set the type of the body
+        void setType(BodyType type);
+
+        /// Set whether or not the body is active
+        virtual void setIsActive(bool isActive);
+
+        /// Return the current position and orientation
+        const Transform& getTransform() const;
+
+        /// Set the current position and orientation
+        virtual void setTransform(const Transform& transform);
+
+        /// Add a collision shape to the body.
+        virtual ProxyShape* addCollisionShape(CollisionShape* collisionShape,
+                                              const Transform& transform);
+
+        /// Remove a collision shape from the body
+        virtual void removeCollisionShape(const ProxyShape* proxyShape);
+
+        /// Return the first element of the linked list of contact manifolds involving this body
+        const ContactManifoldListElement* getContactManifoldsList() const;
+
+        /// Return true if a point is inside the collision body
+        bool testPointInside(const Vector3& worldPoint) const;
+
+        /// Raycast method with feedback information
+        bool raycast(const Ray& ray, RaycastInfo& raycastInfo);
+
+        /// Compute and return the AABB of the body by merging all proxy shapes AABBs
+        AABB getAABB() const;
+
+        /// Return the linked list of proxy shapes of that body
+        ProxyShape* getProxyShapesList();
+
+        /// Return the linked list of proxy shapes of that body
+        const ProxyShape* getProxyShapesList() const;
+
+        /// Return the world-space coordinates of a point given the local-space coordinates of the body
+        Vector3 getWorldPoint(const Vector3& localPoint) const;
+
+        /// Return the world-space vector of a vector given in local-space coordinates of the body
+        Vector3 getWorldVector(const Vector3& localVector) const;
+
+        /// Return the body local-space coordinates of a point given in the world-space coordinates
+        Vector3 getLocalPoint(const Vector3& worldPoint) const;
+
+        /// Return the body local-space coordinates of a vector given in the world-space coordinates
+        Vector3 getLocalVector(const Vector3& worldVector) const;
+
+        // -------------------- Friendship -------------------- //
+
+        friend class CollisionWorld;
+        friend class DynamicsWorld;
+        friend class CollisionDetection;
+        friend class BroadPhaseAlgorithm;
+        friend class ConvexMeshShape;
+        friend class ProxyShape;
+};
+
+// Return the type of the body
+/**
+ * @return the type of the body (STATIC, KINEMATIC, DYNAMIC)
+ */
+inline BodyType CollisionBody::getType() const {
+    return mType;
+}
+
+// Set the type of the body
+/// The type of the body can either STATIC, KINEMATIC or DYNAMIC as described bellow:
+/// STATIC : A static body has infinite mass, zero velocity but the position can be
+///          changed manually. A static body does not collide with other static or kinematic bodies.
+/// KINEMATIC : A kinematic body has infinite mass, the velocity can be changed manually and its
+///             position is computed by the physics engine. A kinematic body does not collide with
+///             other static or kinematic bodies.
+/// DYNAMIC : A dynamic body has non-zero mass, non-zero velocity determined by forces and its
+///           position is determined by the physics engine. A dynamic body can collide with other
+///           dynamic, static or kinematic bodies.
+/**
+ * @param type The type of the body (STATIC, KINEMATIC, DYNAMIC)
+ */
+inline void CollisionBody::setType(BodyType type) {
+    mType = type;
+
+    if (mType == STATIC) {
+
+        // Update the broad-phase state of the body
+        updateBroadPhaseState();
+    }
+}
+
+// Return the current position and orientation
+/**
+ * @return The current transformation of the body that transforms the local-space
+ *         of the body into world-space
+ */
+inline const Transform& CollisionBody::getTransform() const {
+    return mTransform;
+}
+
+// Set the current position and orientation
+/**
+ * @param transform The transformation of the body that transforms the local-space
+ *                  of the body into world-space
+ */
+inline void CollisionBody::setTransform(const Transform& transform) {
+
+    // Update the transform of the body
+    mTransform = transform;
+
+    // Update the broad-phase state of the body
+    updateBroadPhaseState();
+}
+
+// Return the first element of the linked list of contact manifolds involving this body
+/**
+ * @return A pointer to the first element of the linked-list with the contact
+ *         manifolds of this body
+ */
+inline const ContactManifoldListElement* CollisionBody::getContactManifoldsList() const {
+    return mContactManifoldsList;
+}
+
+// Return the linked list of proxy shapes of that body
+/**
+* @return The pointer of the first proxy shape of the linked-list of all the
+*         proxy shapes of the body
+*/
+inline ProxyShape* CollisionBody::getProxyShapesList() {
+    return mProxyCollisionShapes;
+}
+
+// Return the linked list of proxy shapes of that body
+/**
+* @return The pointer of the first proxy shape of the linked-list of all the
+*         proxy shapes of the body
+*/
+inline const ProxyShape* CollisionBody::getProxyShapesList() const {
+    return mProxyCollisionShapes;
+}
+
+// Return the world-space coordinates of a point given the local-space coordinates of the body
+/**
+* @param localPoint A point in the local-space coordinates of the body
+* @return The point in world-space coordinates
+*/
+inline Vector3 CollisionBody::getWorldPoint(const Vector3& localPoint) const {
+    return mTransform * localPoint;
+}
+
+// Return the world-space vector of a vector given in local-space coordinates of the body
+/**
+* @param localVector A vector in the local-space coordinates of the body
+* @return The vector in world-space coordinates
+*/
+inline Vector3 CollisionBody::getWorldVector(const Vector3& localVector) const {
+    return mTransform.getOrientation() * localVector;
+}
+
+// Return the body local-space coordinates of a point given in the world-space coordinates
+/**
+* @param worldPoint A point in world-space coordinates
+* @return The point in the local-space coordinates of the body
+*/
+inline Vector3 CollisionBody::getLocalPoint(const Vector3& worldPoint) const {
+    return mTransform.getInverse() * worldPoint;
+}
+
+// Return the body local-space coordinates of a vector given in the world-space coordinates
+/**
+* @param worldVector A vector in world-space coordinates
+* @return The vector in the local-space coordinates of the body
+*/
+inline Vector3 CollisionBody::getLocalVector(const Vector3& worldVector) const {
+    return mTransform.getOrientation().getInverse() * worldVector;
+}
+
+}
+
+ #endif

src/body/RigidBody.cpp

@@ -0,0 +1,414 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "RigidBody.h"
+#include "constraint/Joint.h"
+#include "collision/shapes/CollisionShape.h"
+#include "engine/DynamicsWorld.h"
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Constructor
+/**
+* @param transform The transformation of the body
+* @param world The world where the body has been added
+* @param id The ID of the body
+*/
+RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id)
+          : CollisionBody(transform, world, id), mInitMass(decimal(1.0)),
+            mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()),
+            mIsGravityEnabled(true), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)),
+            mJointsList(NULL) {
+
+    // Compute the inverse mass
+    mMassInverse = decimal(1.0) / mInitMass;
+}
+
+// Destructor
+RigidBody::~RigidBody() {
+    assert(mJointsList == NULL);
+}
+
+// Set the type of the body
+/// The type of the body can either STATIC, KINEMATIC or DYNAMIC as described bellow:
+/// STATIC : A static body has infinite mass, zero velocity but the position can be
+///          changed manually. A static body does not collide with other static or kinematic bodies.
+/// KINEMATIC : A kinematic body has infinite mass, the velocity can be changed manually and its
+///             position is computed by the physics engine. A kinematic body does not collide with
+///             other static or kinematic bodies.
+/// DYNAMIC : A dynamic body has non-zero mass, non-zero velocity determined by forces and its
+///           position is determined by the physics engine. A dynamic body can collide with other
+///           dynamic, static or kinematic bodies.
+/**
+ * @param type The type of the body (STATIC, KINEMATIC, DYNAMIC)
+ */
+void RigidBody::setType(BodyType type) {
+
+    if (mType == type) return;
+
+    CollisionBody::setType(type);
+
+    // Recompute the total mass, center of mass and inertia tensor
+    recomputeMassInformation();
+
+    // If it is a static body
+    if (mType == STATIC) {
+
+        // Reset the velocity to zero
+        mLinearVelocity.setToZero();
+        mAngularVelocity.setToZero();
+    }
+
+    // If it is a static or a kinematic body
+    if (mType == STATIC || mType == KINEMATIC) {
+
+        // Reset the inverse mass and inverse inertia tensor to zero
+        mMassInverse = decimal(0.0);
+        mInertiaTensorLocal.setToZero();
+        mInertiaTensorLocalInverse.setToZero();
+
+    }
+    else {  // If it is a dynamic body
+        mMassInverse = decimal(1.0) / mInitMass;
+        mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
+    }
+
+    // Awake the body
+    setIsSleeping(false);
+
+    // Remove all the contacts with this body
+    resetContactManifoldsList();
+
+    // Ask the broad-phase to test again the collision shapes of the body for collision
+    // detection (as if the body has moved)
+    askForBroadPhaseCollisionCheck();
+
+    // Reset the force and torque on the body
+    mExternalForce.setToZero();
+    mExternalTorque.setToZero();
+}
+
+// Set the local inertia tensor of the body (in local-space coordinates)
+/**
+ * @param inertiaTensorLocal The 3x3 inertia tensor matrix of the body in local-space
+ *                           coordinates
+ */
+void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
+
+    if (mType != DYNAMIC) return;
+
+    mInertiaTensorLocal = inertiaTensorLocal;
+
+    // Compute the inverse local inertia tensor
+    mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
+}
+
+// Set the local center of mass of the body (in local-space coordinates)
+/**
+ * @param centerOfMassLocal The center of mass of the body in local-space
+ *                          coordinates
+ */
+void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) {
+
+    if (mType != DYNAMIC) return;
+
+    const Vector3 oldCenterOfMass = mCenterOfMassWorld;
+    mCenterOfMassLocal = centerOfMassLocal;
+
+    // Compute the center of mass in world-space coordinates
+    mCenterOfMassWorld = mTransform * mCenterOfMassLocal;
+
+    // Update the linear velocity of the center of mass
+    mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
+}
+
+// Set the mass of the rigid body
+/**
+ * @param mass The mass (in kilograms) of the body
+ */
+void RigidBody::setMass(decimal mass) {
+
+    if (mType != DYNAMIC) return;
+
+    mInitMass = mass;
+
+    if (mInitMass > decimal(0.0)) {
+        mMassInverse = decimal(1.0) / mInitMass;
+    }
+    else {
+        mInitMass = decimal(1.0);
+        mMassInverse = decimal(1.0);
+    }
+}
+
+// Remove a joint from the joints list
+void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint) {
+
+    assert(joint != NULL);
+    assert(mJointsList != NULL);
+
+    // Remove the joint from the linked list of the joints of the first body
+    if (mJointsList->joint == joint) {   // If the first element is the one to remove
+        JointListElement* elementToRemove = mJointsList;
+        mJointsList = elementToRemove->next;
+        elementToRemove->~JointListElement();
+        memoryAllocator.release(elementToRemove, sizeof(JointListElement));
+    }
+    else {  // If the element to remove is not the first one in the list
+        JointListElement* currentElement = mJointsList;
+        while (currentElement->next != NULL) {
+            if (currentElement->next->joint == joint) {
+                JointListElement* elementToRemove = currentElement->next;
+                currentElement->next = elementToRemove->next;
+                elementToRemove->~JointListElement();
+                memoryAllocator.release(elementToRemove, sizeof(JointListElement));
+                break;
+            }
+            currentElement = currentElement->next;
+        }
+    }
+}
+
+// Add a collision shape to the body.
+/// When you add a collision shape to the body, an internal copy of this
+/// collision shape will be created internally. Therefore, you can delete it
+/// right after calling this method or use it later to add it to another body.
+/// This method will return a pointer to a new proxy shape. A proxy shape is
+/// an object that links a collision shape and a given body. You can use the
+/// returned proxy shape to get and set information about the corresponding
+/// collision shape for that body.
+/**
+ * @param collisionShape The collision shape you want to add to the body
+ * @param transform The transformation of the collision shape that transforms the
+ *        local-space of the collision shape into the local-space of the body
+ * @param mass Mass (in kilograms) of the collision shape you want to add
+ * @return A pointer to the proxy shape that has been created to link the body to
+ *         the new collision shape you have added.
+ */
+ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape,
+                                         const Transform& transform,
+                                         decimal mass) {
+
+    assert(mass > decimal(0.0));
+
+    // Create a new proxy collision shape to attach the collision shape to the body
+    ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate(
+                                      sizeof(ProxyShape))) ProxyShape(this, collisionShape,
+                                                                      transform, mass);
+
+    // Add it to the list of proxy collision shapes of the body
+    if (mProxyCollisionShapes == NULL) {
+        mProxyCollisionShapes = proxyShape;
+    }
+    else {
+        proxyShape->mNext = mProxyCollisionShapes;
+        mProxyCollisionShapes = proxyShape;
+    }
+
+    // Compute the world-space AABB of the new collision shape
+    AABB aabb;
+    collisionShape->computeAABB(aabb, mTransform * transform);
+
+    // Notify the collision detection about this new collision shape
+    mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape, aabb);
+
+    mNbCollisionShapes++;
+
+    // Recompute the center of mass, total mass and inertia tensor of the body with the new
+    // collision shape
+    recomputeMassInformation();
+
+    // Return a pointer to the proxy collision shape
+    return proxyShape;
+}
+
+// Remove a collision shape from the body
+/// To remove a collision shape, you need to specify the pointer to the proxy
+/// shape that has been returned when you have added the collision shape to the
+/// body
+/**
+ * @param proxyShape The pointer of the proxy shape you want to remove
+ */
+void RigidBody::removeCollisionShape(const ProxyShape* proxyShape) {
+
+    // Remove the collision shape
+    CollisionBody::removeCollisionShape(proxyShape);
+
+    // Recompute the total mass, center of mass and inertia tensor
+    recomputeMassInformation();
+}
+
+// Set the linear velocity of the rigid body.
+/**
+ * @param linearVelocity Linear velocity vector of the body
+ */
+void RigidBody::setLinearVelocity(const Vector3& linearVelocity) {
+
+    // If it is a static body, we do nothing
+    if (mType == STATIC) return;
+
+    // Update the linear velocity of the current body state
+    mLinearVelocity = linearVelocity;
+
+    // If the linear velocity is not zero, awake the body
+    if (mLinearVelocity.lengthSquare() > decimal(0.0)) {
+        setIsSleeping(false);
+    }
+}
+
+// Set the angular velocity.
+/**
+* @param angularVelocity The angular velocity vector of the body
+*/
+void RigidBody::setAngularVelocity(const Vector3& angularVelocity) {
+
+    // If it is a static body, we do nothing
+    if (mType == STATIC) return;
+
+    // Set the angular velocity
+    mAngularVelocity = angularVelocity;
+
+    // If the velocity is not zero, awake the body
+    if (mAngularVelocity.lengthSquare() > decimal(0.0)) {
+        setIsSleeping(false);
+    }
+}
+
+// Set the current position and orientation
+/**
+ * @param transform The transformation of the body that transforms the local-space
+ *                  of the body into world-space
+ */
+void RigidBody::setTransform(const Transform& transform) {
+
+    // Update the transform of the body
+    mTransform = transform;
+
+    const Vector3 oldCenterOfMass = mCenterOfMassWorld;
+
+    // Compute the new center of mass in world-space coordinates
+    mCenterOfMassWorld = mTransform * mCenterOfMassLocal;
+
+    // Update the linear velocity of the center of mass
+    mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
+
+    // Update the broad-phase state of the body
+    updateBroadPhaseState();
+}
+
+// Recompute the center of mass, total mass and inertia tensor of the body using all
+// the collision shapes attached to the body.
+void RigidBody::recomputeMassInformation() {
+
+    mInitMass = decimal(0.0);
+    mMassInverse = decimal(0.0);
+    mInertiaTensorLocal.setToZero();
+    mInertiaTensorLocalInverse.setToZero();
+    mCenterOfMassLocal.setToZero();
+
+    // If it is STATIC or KINEMATIC body
+    if (mType == STATIC || mType == KINEMATIC) {
+        mCenterOfMassWorld = mTransform.getPosition();
+        return;
+    }
+
+    assert(mType == DYNAMIC);
+
+    // Compute the total mass of the body
+    for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
+        mInitMass += shape->getMass();
+        mCenterOfMassLocal += shape->getLocalToBodyTransform().getPosition() * shape->getMass();
+    }
+
+    if (mInitMass > decimal(0.0)) {
+        mMassInverse = decimal(1.0) / mInitMass;
+    }
+    else {
+        mInitMass = decimal(1.0);
+        mMassInverse = decimal(1.0);
+    }
+
+    // Compute the center of mass
+    const Vector3 oldCenterOfMass = mCenterOfMassWorld;
+    mCenterOfMassLocal *= mMassInverse;
+    mCenterOfMassWorld = mTransform * mCenterOfMassLocal;
+
+    // Compute the total mass and inertia tensor using all the collision shapes
+    for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
+
+        // Get the inertia tensor of the collision shape in its local-space
+        Matrix3x3 inertiaTensor;
+        shape->getCollisionShape()->computeLocalInertiaTensor(inertiaTensor, shape->getMass());
+
+        // Convert the collision shape inertia tensor into the local-space of the body
+        const Transform& shapeTransform = shape->getLocalToBodyTransform();
+        Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix();
+        inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose();
+
+        // Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape
+        // center into a inertia tensor w.r.t to the body origin.
+        Vector3 offset = shapeTransform.getPosition() - mCenterOfMassLocal;
+        decimal offsetSquare = offset.lengthSquare();
+        Matrix3x3 offsetMatrix;
+        offsetMatrix[0].setAllValues(offsetSquare, decimal(0.0), decimal(0.0));
+        offsetMatrix[1].setAllValues(decimal(0.0), offsetSquare, decimal(0.0));
+        offsetMatrix[2].setAllValues(decimal(0.0), decimal(0.0), offsetSquare);
+        offsetMatrix[0] += offset * (-offset.x);
+        offsetMatrix[1] += offset * (-offset.y);
+        offsetMatrix[2] += offset * (-offset.z);
+        offsetMatrix *= shape->getMass();
+
+        mInertiaTensorLocal += inertiaTensor + offsetMatrix;
+    }
+
+    // Compute the local inverse inertia tensor
+    mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
+
+    // Update the linear velocity of the center of mass
+    mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
+}
+
+// Update the broad-phase state for this body (because it has moved for instance)
+void RigidBody::updateBroadPhaseState() const {
+
+    PROFILE("RigidBody::updateBroadPhaseState()");
+
+    DynamicsWorld& world = static_cast<DynamicsWorld&>(mWorld);
+ 	 const Vector3 displacement = world.mTimeStep * mLinearVelocity;
+
+    // For all the proxy collision shapes of the body
+    for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
+
+        // Recompute the world-space AABB of the collision shape
+        AABB aabb;
+        shape->getCollisionShape()->computeAABB(aabb, mTransform *shape->getLocalToBodyTransform());
+
+        // Update the broad-phase state for the proxy collision shape
+        mWorld.mCollisionDetection.updateProxyCollisionShape(shape, aabb, displacement);
+    }
+}
+

src/body/RigidBody.h

@@ -0,0 +1,478 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_RIGID_BODY_H
+#define REACTPHYSICS3D_RIGID_BODY_H
+
+// Libraries
+#include <cassert>
+#include "CollisionBody.h"
+#include "engine/Material.h"
+#include "mathematics/mathematics.h"
+#include "memory/MemoryAllocator.h"
+
+/// Namespace reactphysics3d
+namespace reactphysics3d {
+
+// Class declarations
+struct JointListElement;
+class Joint;
+class DynamicsWorld;
+
+// Class RigidBody
+/**
+ * This class represents a rigid body of the physics
+ * engine. A rigid body is a non-deformable body that
+ * has a constant mass. This class inherits from the
+ * CollisionBody class.
+  */
+class RigidBody : public CollisionBody {
+
+    protected :
+
+        // -------------------- Attributes -------------------- //
+
+        /// Intial mass of the body
+        decimal mInitMass;
+
+        /// Center of mass of the body in local-space coordinates.
+        /// The center of mass can therefore be different from the body origin
+        Vector3 mCenterOfMassLocal;
+
+        /// Center of mass of the body in world-space coordinates
+        Vector3 mCenterOfMassWorld;
+
+        /// Linear velocity of the body
+        Vector3 mLinearVelocity;
+
+        /// Angular velocity of the body
+        Vector3 mAngularVelocity;
+
+        /// Current external force on the body
+        Vector3 mExternalForce;
+
+        /// Current external torque on the body
+        Vector3 mExternalTorque;
+
+        /// Local inertia tensor of the body (in local-space) with respect to the
+        /// center of mass of the body
+        Matrix3x3 mInertiaTensorLocal;
+
+        /// Inverse of the inertia tensor of the body
+        Matrix3x3 mInertiaTensorLocalInverse;
+
+        /// Inverse of the mass of the body
+        decimal mMassInverse;
+
+        /// True if the gravity needs to be applied to this rigid body
+        bool mIsGravityEnabled;
+
+        /// Material properties of the rigid body
+        Material mMaterial;
+
+        /// Linear velocity damping factor
+        decimal mLinearDamping;
+
+        /// Angular velocity damping factor
+        decimal mAngularDamping;
+
+        /// First element of the linked list of joints involving this body
+        JointListElement* mJointsList;        
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        RigidBody(const RigidBody& body);
+
+        /// Private assignment operator
+        RigidBody& operator=(const RigidBody& body);
+
+        /// Remove a joint from the joints list
+        void removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint);
+
+        /// Update the transform of the body after a change of the center of mass
+        void updateTransformWithCenterOfMass();
+
+        /// Update the broad-phase state for this body (because it has moved for instance)
+        virtual void updateBroadPhaseState() const;
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id);
+
+        /// Destructor
+        virtual ~RigidBody();
+
+        /// Set the type of the body (static, kinematic or dynamic)
+        void setType(BodyType type);
+
+        /// Set the current position and orientation
+        virtual void setTransform(const Transform& transform);
+
+        /// Return the mass of the body
+        decimal getMass() const;
+
+        /// Return the linear velocity
+        Vector3 getLinearVelocity() const;
+
+        /// Set the linear velocity of the body.
+        void setLinearVelocity(const Vector3& linearVelocity);
+
+        /// Return the angular velocity
+        Vector3 getAngularVelocity() const;
+
+        /// Set the angular velocity.
+        void setAngularVelocity(const Vector3& angularVelocity);
+
+        /// Set the variable to know whether or not the body is sleeping
+        virtual void setIsSleeping(bool isSleeping);
+
+        /// Return the local inertia tensor of the body (in body coordinates)
+        const Matrix3x3& getInertiaTensorLocal() const;
+
+        /// Set the local inertia tensor of the body (in body coordinates)
+        void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal);
+
+        /// Set the local center of mass of the body (in local-space coordinates)
+        void setCenterOfMassLocal(const Vector3& centerOfMassLocal);
+
+        /// Set the mass of the rigid body
+        void setMass(decimal mass);
+
+        /// Return the inertia tensor in world coordinates.
+        Matrix3x3 getInertiaTensorWorld() const;
+
+        /// Return the inverse of the inertia tensor in world coordinates.
+        Matrix3x3 getInertiaTensorInverseWorld() const;
+
+        /// Return true if the gravity needs to be applied to this rigid body
+        bool isGravityEnabled() const;
+
+        /// Set the variable to know if the gravity is applied to this rigid body
+        void enableGravity(bool isEnabled);
+
+        /// Return a reference to the material properties of the rigid body
+        Material& getMaterial();
+
+        /// Set a new material for this rigid body
+        void setMaterial(const Material& material);
+
+        /// Return the linear velocity damping factor
+        decimal getLinearDamping() const;
+
+        /// Set the linear damping factor
+        void setLinearDamping(decimal linearDamping);
+
+        /// Return the angular velocity damping factor
+        decimal getAngularDamping() const;
+
+        /// Set the angular damping factor
+        void setAngularDamping(decimal angularDamping);
+
+        /// Return the first element of the linked list of joints involving this body
+        const JointListElement* getJointsList() const;
+
+        /// Return the first element of the linked list of joints involving this body
+        JointListElement* getJointsList();
+
+        /// Apply an external force to the body at its center of mass.
+        void applyForceToCenterOfMass(const Vector3& force);
+
+        /// Apply an external force to the body at a given point (in world-space coordinates).
+        void applyForce(const Vector3& force, const Vector3& point);
+
+        /// Apply an external torque to the body.
+        void applyTorque(const Vector3& torque);
+
+        /// Add a collision shape to the body.
+        virtual ProxyShape* addCollisionShape(CollisionShape* collisionShape,
+                                              const Transform& transform,
+                                              decimal mass);
+
+        /// Remove a collision shape from the body
+        virtual void removeCollisionShape(const ProxyShape* proxyShape);
+
+        /// Recompute the center of mass, total mass and inertia tensor of the body using all
+        /// the collision shapes attached to the body.
+        void recomputeMassInformation();
+
+        // -------------------- Friendship -------------------- //
+
+        friend class DynamicsWorld;
+        friend class ContactSolver;
+        friend class BallAndSocketJoint;
+        friend class SliderJoint;
+        friend class HingeJoint;
+        friend class FixedJoint;
+};
+
+// Method that return the mass of the body
+/**
+ * @return The mass (in kilograms) of the body
+ */
+inline decimal RigidBody::getMass() const {
+    return mInitMass;
+}
+
+// Return the linear velocity
+/**
+ * @return The linear velocity vector of the body
+ */
+inline Vector3 RigidBody::getLinearVelocity() const {
+    return mLinearVelocity;
+}
+
+// Return the angular velocity of the body
+/**
+ * @return The angular velocity vector of the body
+ */
+inline Vector3 RigidBody::getAngularVelocity() const {
+    return mAngularVelocity;
+}
+
+// Return the local inertia tensor of the body (in local-space coordinates)
+/**
+ * @return The 3x3 inertia tensor matrix of the body (in local-space coordinates)
+ */
+inline const Matrix3x3& RigidBody::getInertiaTensorLocal() const {
+    return mInertiaTensorLocal;
+}
+
+// Return the inertia tensor in world coordinates.
+/// The inertia tensor I_w in world coordinates is computed
+/// with the local inertia tensor I_b in body coordinates
+/// by I_w = R * I_b * R^T
+/// where R is the rotation matrix (and R^T its transpose) of
+/// the current orientation quaternion of the body
+/**
+ * @return The 3x3 inertia tensor matrix of the body in world-space coordinates
+ */
+inline Matrix3x3 RigidBody::getInertiaTensorWorld() const {
+
+    // Compute and return the inertia tensor in world coordinates
+    return mTransform.getOrientation().getMatrix() * mInertiaTensorLocal *
+           mTransform.getOrientation().getMatrix().getTranspose();
+}
+
+// Return the inverse of the inertia tensor in world coordinates.
+/// The inertia tensor I_w in world coordinates is computed with the
+/// local inverse inertia tensor I_b^-1 in body coordinates
+/// by I_w = R * I_b^-1 * R^T
+/// where R is the rotation matrix (and R^T its transpose) of the
+/// current orientation quaternion of the body
+/**
+ * @return The 3x3 inverse inertia tensor matrix of the body in world-space
+ *         coordinates
+ */
+inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
+
+    // TODO : DO NOT RECOMPUTE THE MATRIX MULTIPLICATION EVERY TIME. WE NEED TO STORE THE
+    //        INVERSE WORLD TENSOR IN THE CLASS AND UPLDATE IT WHEN THE ORIENTATION OF THE BODY CHANGES
+
+    // Compute and return the inertia tensor in world coordinates
+    return mTransform.getOrientation().getMatrix() * mInertiaTensorLocalInverse *
+           mTransform.getOrientation().getMatrix().getTranspose();
+}
+
+// Return true if the gravity needs to be applied to this rigid body
+/**
+ * @return True if the gravity is applied to the body
+ */
+inline bool RigidBody::isGravityEnabled() const {
+    return mIsGravityEnabled;
+}
+
+// Set the variable to know if the gravity is applied to this rigid body
+/**
+ * @param isEnabled True if you want the gravity to be applied to this body
+ */
+inline void RigidBody::enableGravity(bool isEnabled) {
+    mIsGravityEnabled = isEnabled;
+}
+
+// Return a reference to the material properties of the rigid body
+/**
+ * @return A reference to the material of the body
+ */
+inline Material& RigidBody::getMaterial() {
+    return mMaterial;
+}
+
+// Set a new material for this rigid body
+/**
+ * @param material The material you want to set to the body
+ */
+inline void RigidBody::setMaterial(const Material& material) {
+    mMaterial = material;
+}
+
+// Return the linear velocity damping factor
+/**
+ * @return The linear damping factor of this body
+ */
+inline decimal RigidBody::getLinearDamping() const {
+    return mLinearDamping;
+}
+
+// Set the linear damping factor. This is the ratio of the linear velocity
+// that the body will lose every at seconds of simulation.
+/**
+ * @param linearDamping The linear damping factor of this body
+ */
+inline void RigidBody::setLinearDamping(decimal linearDamping) {
+    assert(linearDamping >= decimal(0.0));
+    mLinearDamping = linearDamping;
+}
+
+// Return the angular velocity damping factor
+/**
+ * @return The angular damping factor of this body
+ */
+inline decimal RigidBody::getAngularDamping() const {
+    return mAngularDamping;
+}
+
+// Set the angular damping factor. This is the ratio of the angular velocity
+// that the body will lose at every seconds of simulation.
+/**
+ * @param angularDamping The angular damping factor of this body
+ */
+inline void RigidBody::setAngularDamping(decimal angularDamping) {
+    assert(angularDamping >= decimal(0.0));
+    mAngularDamping = angularDamping;
+}
+
+// Return the first element of the linked list of joints involving this body
+/**
+ * @return The first element of the linked-list of all the joints involving this body
+ */
+inline const JointListElement* RigidBody::getJointsList() const {
+    return mJointsList;
+}
+
+// Return the first element of the linked list of joints involving this body
+/**
+ * @return The first element of the linked-list of all the joints involving this body
+ */
+inline JointListElement* RigidBody::getJointsList() {
+    return mJointsList;
+}
+
+// Set the variable to know whether or not the body is sleeping
+inline void RigidBody::setIsSleeping(bool isSleeping) {
+
+    if (isSleeping) {
+        mLinearVelocity.setToZero();
+        mAngularVelocity.setToZero();
+        mExternalForce.setToZero();
+        mExternalTorque.setToZero();
+    }
+
+    Body::setIsSleeping(isSleeping);
+}
+
+// Apply an external force to the body at its center of mass.
+/// If the body is sleeping, calling this method will wake it up. Note that the
+/// force will we added to the sum of the applied forces and that this sum will be
+/// reset to zero at the end of each call of the DynamicsWorld::update() method.
+/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
+/**
+ * @param force The external force to apply on the center of mass of the body
+ */
+inline void RigidBody::applyForceToCenterOfMass(const Vector3& force) {
+
+    // If it is not a dynamic body, we do nothing
+    if (mType != DYNAMIC) return;
+
+    // Awake the body if it was sleeping
+    if (mIsSleeping) {
+        setIsSleeping(false);
+    }
+
+    // Add the force
+    mExternalForce += force;
+}
+
+// Apply an external force to the body at a given point (in world-space coordinates).
+/// If the point is not at the center of mass of the body, it will also
+/// generate some torque and therefore, change the angular velocity of the body.
+/// If the body is sleeping, calling this method will wake it up. Note that the
+/// force will we added to the sum of the applied forces and that this sum will be
+/// reset to zero at the end of each call of the DynamicsWorld::update() method.
+/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
+/**
+ * @param force The force to apply on the body
+ * @param point The point where the force is applied (in world-space coordinates)
+ */
+inline void RigidBody::applyForce(const Vector3& force, const Vector3& point) {
+
+    // If it is not a dynamic body, we do nothing
+    if (mType != DYNAMIC) return;
+
+    // Awake the body if it was sleeping
+    if (mIsSleeping) {
+        setIsSleeping(false);
+    }
+
+    // Add the force and torque
+    mExternalForce += force;
+    mExternalTorque += (point - mCenterOfMassWorld).cross(force);
+}
+
+// Apply an external torque to the body.
+/// If the body is sleeping, calling this method will wake it up. Note that the
+/// force will we added to the sum of the applied torques and that this sum will be
+/// reset to zero at the end of each call of the DynamicsWorld::update() method.
+/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
+/**
+ * @param torque The external torque to apply on the body
+ */
+inline void RigidBody::applyTorque(const Vector3& torque) {
+
+    // If it is not a dynamic body, we do nothing
+    if (mType != DYNAMIC) return;
+
+    // Awake the body if it was sleeping
+    if (mIsSleeping) {
+        setIsSleeping(false);
+    }
+
+    // Add the torque
+    mExternalTorque += torque;
+}
+
+/// Update the transform of the body after a change of the center of mass
+inline void RigidBody::updateTransformWithCenterOfMass() {
+
+    // Translate the body according to the translation of the center of mass position
+    mTransform.setPosition(mCenterOfMassWorld - mTransform.getOrientation() * mCenterOfMassLocal);
+}
+
+}
+
+ #endif

src/collision/CollisionDetection.cpp

@@ -0,0 +1,532 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "CollisionDetection.h"
+#include "engine/CollisionWorld.h"
+#include "body/Body.h"
+#include "collision/shapes/BoxShape.h"
+#include "body/RigidBody.h"
+#include "configuration.h"
+#include <cassert>
+#include <complex>
+#include <set>
+#include <utility>
+#include <utility>
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+using namespace std;
+
+// Constructor
+CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator)
+                   : mMemoryAllocator(memoryAllocator),
+                     mWorld(world), mBroadPhaseAlgorithm(*this),
+                     mIsCollisionShapesAdded(false) {
+
+    // Set the default collision dispatch configuration
+    setCollisionDispatch(&mDefaultCollisionDispatch);
+
+    // Fill-in the collision detection matrix with algorithms
+    fillInCollisionMatrix();
+}
+
+// Destructor
+CollisionDetection::~CollisionDetection() {
+
+}
+
+// Compute the collision detection
+void CollisionDetection::computeCollisionDetection() {
+
+    PROFILE("CollisionDetection::computeCollisionDetection()");
+	    
+    // Compute the broad-phase collision detection
+    computeBroadPhase();
+    
+    // Compute the narrow-phase collision detection
+    computeNarrowPhase();
+}
+
+// Compute the collision detection
+void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback,
+                                                    const std::set<uint>& shapes1,
+                                                    const std::set<uint>& shapes2) {
+
+    // Compute the broad-phase collision detection
+    computeBroadPhase();
+
+    // Delete all the contact points in the currently overlapping pairs
+    clearContactPoints();
+
+    // Compute the narrow-phase collision detection among given sets of shapes
+    computeNarrowPhaseBetweenShapes(callback, shapes1, shapes2);
+}
+
+// Report collision between two sets of shapes
+void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback,
+                                                      const std::set<uint>& shapes1,
+                                                      const std::set<uint>& shapes2) {
+
+    // For each possible collision pair of bodies
+    map<overlappingpairid, OverlappingPair*>::iterator it;
+    for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
+
+        OverlappingPair* pair = it->second;
+
+        const ProxyShape* shape1 = pair->getShape1();
+        const ProxyShape* shape2 = pair->getShape2();
+
+        assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
+
+        // If both shapes1 and shapes2 sets are non-empty, we check that
+        // shape1 is among on set and shape2 is among the other one
+        if (!shapes1.empty() && !shapes2.empty() &&
+            (shapes1.count(shape1->mBroadPhaseID) == 0 || shapes2.count(shape2->mBroadPhaseID) == 0) &&
+            (shapes1.count(shape2->mBroadPhaseID) == 0 || shapes2.count(shape1->mBroadPhaseID) == 0)) {
+            continue;
+        }
+        if (!shapes1.empty() && shapes2.empty() &&
+            shapes1.count(shape1->mBroadPhaseID) == 0 && shapes1.count(shape2->mBroadPhaseID) == 0)
+        {
+            continue;
+        }
+        if (!shapes2.empty() && shapes1.empty() &&
+            shapes2.count(shape1->mBroadPhaseID) == 0 && shapes2.count(shape2->mBroadPhaseID) == 0)
+        {
+            continue;
+        }
+
+        // For each contact manifold set of the overlapping pair
+        const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
+        for (int j=0; j<manifoldSet.getNbContactManifolds(); j++) {
+
+            const ContactManifold* manifold = manifoldSet.getContactManifold(j);
+
+            // For each contact manifold of the manifold set
+            for (uint i=0; i<manifold->getNbContactPoints(); i++) {
+
+                ContactPoint* contactPoint = manifold->getContactPoint(i);
+
+                // Create the contact info object for the contact
+                ContactPointInfo contactInfo(manifold->getShape1(), manifold->getShape2(),
+                                             manifold->getShape1()->getCollisionShape(),
+                                             manifold->getShape2()->getCollisionShape(),
+                                             contactPoint->getNormal(),
+                                             contactPoint->getPenetrationDepth(),
+                                             contactPoint->getLocalPointOnBody1(),
+                                             contactPoint->getLocalPointOnBody2());
+
+                // Notify the collision callback about this new contact
+                if (callback != NULL) callback->notifyContact(contactInfo);
+            }
+        }
+    }
+}
+
+// Compute the broad-phase collision detection
+void CollisionDetection::computeBroadPhase() {
+
+    PROFILE("CollisionDetection::computeBroadPhase()");
+
+    // If new collision shapes have been added to bodies
+    if (mIsCollisionShapesAdded) {
+
+        // Ask the broad-phase to recompute the overlapping pairs of collision
+        // shapes. This call can only add new overlapping pairs in the collision
+        // detection.
+        mBroadPhaseAlgorithm.computeOverlappingPairs();
+    }
+}
+
+// Compute the narrow-phase collision detection
+void CollisionDetection::computeNarrowPhase() {
+
+    PROFILE("CollisionDetection::computeNarrowPhase()");
+
+    // Clear the set of overlapping pairs in narrow-phase contact
+    mContactOverlappingPairs.clear();
+    
+    // For each possible collision pair of bodies
+    map<overlappingpairid, OverlappingPair*>::iterator it;
+    for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
+
+        OverlappingPair* pair = it->second;
+
+        ProxyShape* shape1 = pair->getShape1();
+        ProxyShape* shape2 = pair->getShape2();
+
+        assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
+
+        // Check if the collision filtering allows collision between the two shapes and
+        // that the two shapes are still overlapping. Otherwise, we destroy the
+        // overlapping pair
+        if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 ||
+             (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) ||
+             !mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
+
+            std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
+            ++it;
+
+            // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
+
+            // Destroy the overlapping pair
+            itToRemove->second->~OverlappingPair();
+            mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair));
+            mOverlappingPairs.erase(itToRemove);
+            continue;
+        }
+        else {
+            ++it;
+        }
+
+        CollisionBody* const body1 = shape1->getBody();
+        CollisionBody* const body2 = shape2->getBody();
+        
+        // Update the contact cache of the overlapping pair
+        pair->update();
+
+        // Check that at least one body is awake and not static
+        bool isBody1Active = !body1->isSleeping() && body1->getType() != STATIC;
+        bool isBody2Active = !body2->isSleeping() && body2->getType() != STATIC;
+        if (!isBody1Active && !isBody2Active) continue;
+
+        // Check if the bodies are in the set of bodies that cannot collide between each other
+        bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
+        if (mNoCollisionPairs.count(bodiesIndex) > 0) continue;
+        
+        // Select the narrow phase algorithm to use according to the two collision shapes
+        const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
+        const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
+        NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Type][shape2Type];
+
+        // If there is no collision algorithm between those two kinds of shapes
+        if (narrowPhaseAlgorithm == NULL) continue;
+        
+        // Notify the narrow-phase algorithm about the overlapping pair we are going to test
+        narrowPhaseAlgorithm->setCurrentOverlappingPair(pair);
+
+        // Create the CollisionShapeInfo objects
+        CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(),
+                                      pair, shape1->getCachedCollisionData());
+        CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(),
+                                      pair, shape2->getCachedCollisionData());
+        
+        // Use the narrow-phase collision detection algorithm to check
+        // if there really is a collision. If a collision occurs, the
+        // notifyContact() callback method will be called.
+        narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, this);
+    }
+
+    // Add all the contact manifolds (between colliding bodies) to the bodies
+    addAllContactManifoldsToBodies();
+}
+
+// Compute the narrow-phase collision detection
+void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
+                                                         const std::set<uint>& shapes1,
+                                                         const std::set<uint>& shapes2) {
+
+    mContactOverlappingPairs.clear();
+
+    // For each possible collision pair of bodies
+    map<overlappingpairid, OverlappingPair*>::iterator it;
+    for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
+
+        OverlappingPair* pair = it->second;
+
+        ProxyShape* shape1 = pair->getShape1();
+        ProxyShape* shape2 = pair->getShape2();
+
+        assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
+
+        // If both shapes1 and shapes2 sets are non-empty, we check that
+        // shape1 is among on set and shape2 is among the other one
+        if (!shapes1.empty() && !shapes2.empty() &&
+            (shapes1.count(shape1->mBroadPhaseID) == 0 || shapes2.count(shape2->mBroadPhaseID) == 0) &&
+            (shapes1.count(shape2->mBroadPhaseID) == 0 || shapes2.count(shape1->mBroadPhaseID) == 0)) {
+            ++it;
+            continue;
+        }
+        if (!shapes1.empty() && shapes2.empty() &&
+            shapes1.count(shape1->mBroadPhaseID) == 0 && shapes1.count(shape2->mBroadPhaseID) == 0)
+        {
+            ++it;
+            continue;
+        }
+        if (!shapes2.empty() && shapes1.empty() &&
+            shapes2.count(shape1->mBroadPhaseID) == 0 && shapes2.count(shape2->mBroadPhaseID) == 0)
+        {
+            ++it;
+            continue;
+        }
+
+        // Check if the collision filtering allows collision between the two shapes and
+        // that the two shapes are still overlapping. Otherwise, we destroy the
+        // overlapping pair
+        if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 ||
+             (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) ||
+             !mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
+
+            std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
+            ++it;
+
+            // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
+
+            // Destroy the overlapping pair
+            itToRemove->second->~OverlappingPair();
+            mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair));
+            mOverlappingPairs.erase(itToRemove);
+            continue;
+        }
+        else {
+            ++it;
+        }
+
+        CollisionBody* const body1 = shape1->getBody();
+        CollisionBody* const body2 = shape2->getBody();
+
+        // Update the contact cache of the overlapping pair
+        pair->update();
+
+        // Check if the two bodies are allowed to collide, otherwise, we do not test for collision
+        if (body1->getType() != DYNAMIC && body2->getType() != DYNAMIC) continue;
+        bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
+        if (mNoCollisionPairs.count(bodiesIndex) > 0) continue;
+
+        // Check if the two bodies are sleeping, if so, we do no test collision between them
+        if (body1->isSleeping() && body2->isSleeping()) continue;
+
+        // Select the narrow phase algorithm to use according to the two collision shapes
+        const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
+        const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
+        NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Type][shape2Type];
+
+        // If there is no collision algorithm between those two kinds of shapes
+        if (narrowPhaseAlgorithm == NULL) continue;
+
+        // Notify the narrow-phase algorithm about the overlapping pair we are going to test
+        narrowPhaseAlgorithm->setCurrentOverlappingPair(pair);
+
+        // Create the CollisionShapeInfo objects
+        CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(),
+                                      pair, shape1->getCachedCollisionData());
+        CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(),
+                                      pair, shape2->getCachedCollisionData());
+
+        TestCollisionBetweenShapesCallback narrowPhaseCallback(callback);
+
+        // Use the narrow-phase collision detection algorithm to check
+        // if there really is a collision
+        narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, &narrowPhaseCallback);
+    }
+
+    // Add all the contact manifolds (between colliding bodies) to the bodies
+    addAllContactManifoldsToBodies();
+}
+
+// Allow the broadphase to notify the collision detection about an overlapping pair.
+/// This method is called by the broad-phase collision detection algorithm
+void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) {
+
+    assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
+
+    // If the two proxy collision shapes are from the same body, skip it
+    if (shape1->getBody()->getID() == shape2->getBody()->getID()) return;
+
+    // Check if the collision filtering allows collision between the two shapes
+    if ((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 ||
+        (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) return;
+
+    // Compute the overlapping pair ID
+    overlappingpairid pairID = OverlappingPair::computeID(shape1, shape2);
+
+    // Check if the overlapping pair already exists
+    if (mOverlappingPairs.find(pairID) != mOverlappingPairs.end()) return;
+
+    // Compute the maximum number of contact manifolds for this pair
+    int nbMaxManifolds = CollisionShape::computeNbMaxContactManifolds(shape1->getCollisionShape()->getType(),
+                                                                      shape2->getCollisionShape()->getType());
+
+    // Create the overlapping pair and add it into the set of overlapping pairs
+    OverlappingPair* newPair = new (mWorld->mMemoryAllocator.allocate(sizeof(OverlappingPair)))
+                              OverlappingPair(shape1, shape2, nbMaxManifolds, mWorld->mMemoryAllocator);
+    assert(newPair != NULL);
+
+#ifndef NDEBUG
+    std::pair<map<overlappingpairid, OverlappingPair*>::iterator, bool> check =
+#endif
+    mOverlappingPairs.insert(make_pair(pairID, newPair));
+    assert(check.second);
+
+    // Wake up the two bodies
+    shape1->getBody()->setIsSleeping(false);
+    shape2->getBody()->setIsSleeping(false);
+}
+
+// Remove a body from the collision detection
+void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
+
+    // Remove all the overlapping pairs involving this proxy shape
+    std::map<overlappingpairid, OverlappingPair*>::iterator it;
+    for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
+        if (it->second->getShape1()->mBroadPhaseID == proxyShape->mBroadPhaseID||
+            it->second->getShape2()->mBroadPhaseID == proxyShape->mBroadPhaseID) {
+            std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
+            ++it;
+
+            // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
+
+            // Destroy the overlapping pair
+            itToRemove->second->~OverlappingPair();
+            mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair));
+            mOverlappingPairs.erase(itToRemove);
+        }
+        else {
+            ++it;
+        }
+    }
+
+    // Remove the body from the broad-phase
+    mBroadPhaseAlgorithm.removeProxyCollisionShape(proxyShape);
+}
+
+// Called by a narrow-phase collision algorithm when a new contact has been found
+void CollisionDetection::notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo) {
+
+    // If it is the first contact since the pairs are overlapping
+    if (overlappingPair->getNbContactPoints() == 0) {
+
+        // Trigger a callback event
+        if (mWorld->mEventListener != NULL) mWorld->mEventListener->beginContact(contactInfo);
+    }
+
+    // Create a new contact
+    createContact(overlappingPair, contactInfo);
+
+    // Trigger a callback event for the new contact
+    if (mWorld->mEventListener != NULL) mWorld->mEventListener->newContact(contactInfo);
+}
+
+// Create a new contact
+void CollisionDetection::createContact(OverlappingPair* overlappingPair,
+                                       const ContactPointInfo& contactInfo) {
+
+    // Create a new contact
+    ContactPoint* contact = new (mWorld->mMemoryAllocator.allocate(sizeof(ContactPoint)))
+                                 ContactPoint(contactInfo);
+
+    // Add the contact to the contact manifold set of the corresponding overlapping pair
+    overlappingPair->addContact(contact);
+
+    // Add the overlapping pair into the set of pairs in contact during narrow-phase
+    overlappingpairid pairId = OverlappingPair::computeID(overlappingPair->getShape1(),
+                                                          overlappingPair->getShape2());
+    mContactOverlappingPairs[pairId] = overlappingPair;
+}
+
+void CollisionDetection::addAllContactManifoldsToBodies() {
+
+    // For each overlapping pairs in contact during the narrow-phase
+    std::map<overlappingpairid, OverlappingPair*>::iterator it;
+    for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) {
+
+        // Add all the contact manifolds of the pair into the list of contact manifolds
+        // of the two bodies involved in the contact
+        addContactManifoldToBody(it->second);
+    }
+}
+
+// Add a contact manifold to the linked list of contact manifolds of the two bodies involved
+// in the corresponding contact
+void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
+
+    assert(pair != NULL);
+
+    CollisionBody* body1 = pair->getShape1()->getBody();
+    CollisionBody* body2 = pair->getShape2()->getBody();
+    const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
+
+    // For each contact manifold in the set of manifolds in the pair
+    for (int i=0; i<manifoldSet.getNbContactManifolds(); i++) {
+
+        ContactManifold* contactManifold = manifoldSet.getContactManifold(i);
+
+        assert(contactManifold->getNbContactPoints() > 0);
+
+        // Add the contact manifold at the beginning of the linked
+        // list of contact manifolds of the first body
+        void* allocatedMemory1 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
+        ContactManifoldListElement* listElement1 = new (allocatedMemory1)
+                                                      ContactManifoldListElement(contactManifold,
+                                                                         body1->mContactManifoldsList);
+        body1->mContactManifoldsList = listElement1;
+
+        // Add the contact manifold at the beginning of the linked
+        // list of the contact manifolds of the second body
+        void* allocatedMemory2 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
+        ContactManifoldListElement* listElement2 = new (allocatedMemory2)
+                                                      ContactManifoldListElement(contactManifold,
+                                                                         body2->mContactManifoldsList);
+        body2->mContactManifoldsList = listElement2;
+    }
+}
+
+// Delete all the contact points in the currently overlapping pairs
+void CollisionDetection::clearContactPoints() {
+
+    // For each overlapping pair
+    std::map<overlappingpairid, OverlappingPair*>::iterator it;
+    for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
+        it->second->clearContactPoints();
+    }
+}
+
+// Fill-in the collision detection matrix
+void CollisionDetection::fillInCollisionMatrix() {
+
+    // For each possible type of collision shape
+    for (int i=0; i<NB_COLLISION_SHAPE_TYPES; i++) {
+        for (int j=0; j<NB_COLLISION_SHAPE_TYPES; j++) {
+            mCollisionMatrix[i][j] = mCollisionDispatch->selectAlgorithm(i, j);
+        }
+    }
+}
+
+// Return the world event listener
+EventListener* CollisionDetection::getWorldEventListener() {
+   return mWorld->mEventListener;
+}
+
+/// Return a reference to the world memory allocator
+MemoryAllocator& CollisionDetection::getWorldMemoryAllocator() {
+  return mWorld->mMemoryAllocator;
+}
+
+// Called by a narrow-phase collision algorithm when a new contact has been found
+void TestCollisionBetweenShapesCallback::notifyContact(OverlappingPair* overlappingPair,
+                           const ContactPointInfo& contactInfo) {
+    mCollisionCallback->notifyContact(contactInfo);
+}

src/collision/CollisionDetection.h

@@ -0,0 +1,318 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_COLLISION_DETECTION_H
+#define REACTPHYSICS3D_COLLISION_DETECTION_H
+
+// Libraries
+#include "body/CollisionBody.h"
+#include "broadphase/BroadPhaseAlgorithm.h"
+#include "engine/OverlappingPair.h"
+#include "engine/EventListener.h"
+#include "narrowphase/DefaultCollisionDispatch.h"
+#include "memory/MemoryAllocator.h"
+#include "constraint/ContactPoint.h"
+#include <vector>
+#include <map>
+#include <set>
+#include <utility>
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Declarations
+class BroadPhaseAlgorithm;
+class CollisionWorld;
+class CollisionCallback;
+
+// Class TestCollisionBetweenShapesCallback
+class TestCollisionBetweenShapesCallback : public NarrowPhaseCallback {
+
+    private:
+
+        CollisionCallback* mCollisionCallback;
+
+    public:
+
+        // Constructor
+        TestCollisionBetweenShapesCallback(CollisionCallback* callback)
+            : mCollisionCallback(callback) {
+
+        }
+
+        // Called by a narrow-phase collision algorithm when a new contact has been found
+        virtual void notifyContact(OverlappingPair* overlappingPair,
+                                   const ContactPointInfo& contactInfo);
+};
+
+// Class CollisionDetection
+/**
+ * This class computes the collision detection algorithms. We first
+ * perform a broad-phase algorithm to know which pairs of bodies can
+ * collide and then we run a narrow-phase algorithm to compute the
+ * collision contacts between bodies.
+ */
+class CollisionDetection : public NarrowPhaseCallback {
+
+    private :
+
+        // -------------------- Attributes -------------------- //
+
+        /// Collision Detection Dispatch configuration
+        CollisionDispatch* mCollisionDispatch;
+
+        /// Default collision dispatch configuration
+        DefaultCollisionDispatch mDefaultCollisionDispatch;
+
+        /// Collision detection matrix (algorithms to use)
+        NarrowPhaseAlgorithm* mCollisionMatrix[NB_COLLISION_SHAPE_TYPES][NB_COLLISION_SHAPE_TYPES];
+
+        /// Reference to the memory allocator
+        MemoryAllocator& mMemoryAllocator;
+
+        /// Pointer to the physics world
+        CollisionWorld* mWorld;
+
+        /// Broad-phase overlapping pairs
+        std::map<overlappingpairid, OverlappingPair*> mOverlappingPairs;
+
+        /// Overlapping pairs in contact (during the current Narrow-phase collision detection)
+        std::map<overlappingpairid, OverlappingPair*> mContactOverlappingPairs;
+
+        /// Broad-phase algorithm
+        BroadPhaseAlgorithm mBroadPhaseAlgorithm;
+
+        /// Narrow-phase GJK algorithm
+        // TODO : Delete this
+        GJKAlgorithm mNarrowPhaseGJKAlgorithm;
+
+        /// Set of pair of bodies that cannot collide between each other
+        std::set<bodyindexpair> mNoCollisionPairs;
+
+        /// True if some collision shapes have been added previously
+        bool mIsCollisionShapesAdded;
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        CollisionDetection(const CollisionDetection& collisionDetection);
+
+        /// Private assignment operator
+        CollisionDetection& operator=(const CollisionDetection& collisionDetection);
+
+        /// Compute the broad-phase collision detection
+        void computeBroadPhase();
+
+        /// Compute the narrow-phase collision detection
+        void computeNarrowPhase();
+
+        /// Add a contact manifold to the linked list of contact manifolds of the two bodies
+        /// involed in the corresponding contact.
+        void addContactManifoldToBody(OverlappingPair* pair);
+
+        /// Delete all the contact points in the currently overlapping pairs
+        void clearContactPoints();
+
+        /// Fill-in the collision detection matrix
+        void fillInCollisionMatrix();
+
+        /// Add all the contact manifold of colliding pairs to their bodies
+        void addAllContactManifoldsToBodies();
+   
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator);
+
+        /// Destructor
+        ~CollisionDetection();
+
+        /// Set the collision dispatch configuration
+        void setCollisionDispatch(CollisionDispatch* collisionDispatch);
+
+        /// Return the Narrow-phase collision detection algorithm to use between two types of shapes
+        NarrowPhaseAlgorithm* getCollisionAlgorithm(CollisionShapeType shape1Type,
+                                                    CollisionShapeType shape2Type) const;
+
+        /// Add a proxy collision shape to the collision detection
+        void addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb);
+
+        /// Remove a proxy collision shape from the collision detection
+        void removeProxyCollisionShape(ProxyShape* proxyShape);
+
+        /// Update a proxy collision shape (that has moved for instance)
+        void updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb,
+                                       const Vector3& displacement = Vector3(0, 0, 0), bool forceReinsert = false);
+
+        /// Add a pair of bodies that cannot collide with each other
+        void addNoCollisionPair(CollisionBody* body1, CollisionBody* body2);
+
+        /// Remove a pair of bodies that cannot collide with each other
+        void removeNoCollisionPair(CollisionBody* body1, CollisionBody* body2);
+
+        /// Ask for a collision shape to be tested again during broad-phase.
+        void askForBroadPhaseCollisionCheck(ProxyShape* shape);
+
+        /// Compute the collision detection
+        void computeCollisionDetection();
+
+        /// Compute the collision detection
+        void testCollisionBetweenShapes(CollisionCallback* callback,
+                                        const std::set<uint>& shapes1,
+                                        const std::set<uint>& shapes2);
+
+        /// Report collision between two sets of shapes
+        void reportCollisionBetweenShapes(CollisionCallback* callback,
+                                          const std::set<uint>& shapes1,
+                                          const std::set<uint>& shapes2) ;
+
+        /// Ray casting method
+        void raycast(RaycastCallback* raycastCallback, const Ray& ray,
+                     unsigned short raycastWithCategoryMaskBits) const;
+
+        /// Test if the AABBs of two bodies overlap
+        bool testAABBOverlap(const CollisionBody* body1,
+                             const CollisionBody* body2) const;
+
+        /// Test if the AABBs of two proxy shapes overlap
+        bool testAABBOverlap(const ProxyShape* shape1,
+                             const ProxyShape* shape2) const;
+
+        /// Allow the broadphase to notify the collision detection about an overlapping pair.
+        void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
+
+        /// Compute the narrow-phase collision detection
+        void computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
+                                             const std::set<uint>& shapes1,
+                                             const std::set<uint>& shapes2);
+
+        /// Return a pointer to the world
+        CollisionWorld* getWorld();
+
+        /// Return the world event listener
+        EventListener* getWorldEventListener();
+
+        /// Return a reference to the world memory allocator
+        MemoryAllocator& getWorldMemoryAllocator();
+
+        /// Called by a narrow-phase collision algorithm when a new contact has been found
+        virtual void notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo);
+
+        /// Create a new contact
+        void createContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo);
+
+        // -------------------- Friendship -------------------- //
+
+        friend class DynamicsWorld;
+        friend class ConvexMeshShape;
+};
+
+// Return the Narrow-phase collision detection algorithm to use between two types of shapes
+inline NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(CollisionShapeType shape1Type,
+                                                                       CollisionShapeType shape2Type) const {
+    return mCollisionMatrix[shape1Type][shape2Type];
+}
+
+// Set the collision dispatch configuration
+inline void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisionDispatch) {
+    mCollisionDispatch = collisionDispatch;
+
+    mCollisionDispatch->init(this, &mMemoryAllocator);
+
+    // Fill-in the collision matrix with the new algorithms to use
+    fillInCollisionMatrix();
+}
+
+// Add a body to the collision detection
+inline void CollisionDetection::addProxyCollisionShape(ProxyShape* proxyShape,
+                                                       const AABB& aabb) {
+    
+    // Add the body to the broad-phase
+    mBroadPhaseAlgorithm.addProxyCollisionShape(proxyShape, aabb);
+
+    mIsCollisionShapesAdded = true;
+}  
+
+// Add a pair of bodies that cannot collide with each other
+inline void CollisionDetection::addNoCollisionPair(CollisionBody* body1,
+                                                   CollisionBody* body2) {
+    mNoCollisionPairs.insert(OverlappingPair::computeBodiesIndexPair(body1, body2));
+}
+
+// Remove a pair of bodies that cannot collide with each other
+inline void CollisionDetection::removeNoCollisionPair(CollisionBody* body1,
+                                                      CollisionBody* body2) {
+    mNoCollisionPairs.erase(OverlappingPair::computeBodiesIndexPair(body1, body2));
+}
+
+// Ask for a collision shape to be tested again during broad-phase.
+/// We simply put the shape in the list of collision shape that have moved in the
+/// previous frame so that it is tested for collision again in the broad-phase.
+inline void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape) {
+    mBroadPhaseAlgorithm.addMovedCollisionShape(shape->mBroadPhaseID);
+}
+
+// Update a proxy collision shape (that has moved for instance)
+inline void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb,
+                                                          const Vector3& displacement, bool forceReinsert) {
+    mBroadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement);
+}
+
+// Ray casting method
+inline void CollisionDetection::raycast(RaycastCallback* raycastCallback,
+                                        const Ray& ray,
+                                        unsigned short raycastWithCategoryMaskBits) const {
+
+    PROFILE("CollisionDetection::raycast()");
+
+    RaycastTest rayCastTest(raycastCallback);
+
+    // Ask the broad-phase algorithm to call the testRaycastAgainstShape()
+    // callback method for each proxy shape hit by the ray in the broad-phase
+    mBroadPhaseAlgorithm.raycast(ray, rayCastTest, raycastWithCategoryMaskBits);
+}
+
+// Test if the AABBs of two proxy shapes overlap
+inline bool CollisionDetection::testAABBOverlap(const ProxyShape* shape1,
+                                                const ProxyShape* shape2) const {
+
+    // If one of the shape's body is not active, we return no overlap
+    if (!shape1->getBody()->isActive() || !shape2->getBody()->isActive()) {
+        return false;
+    }
+
+    return mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2);
+}
+
+// Return a pointer to the world
+inline CollisionWorld* CollisionDetection::getWorld() {
+    return mWorld;
+}
+
+}
+
+#endif

src/collision/CollisionShapeInfo.h

@@ -0,0 +1,75 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_COLLISION_SHAPE_INFO_H
+#define REACTPHYSICS3D_COLLISION_SHAPE_INFO_H
+
+// Libraries
+#include "shapes/CollisionShape.h"
+
+/// Namespace ReactPhysics3D
+namespace reactphysics3d {
+
+class OverlappingPair;
+
+// Class CollisionShapeInfo
+/**
+ * This structure regroups different things about a collision shape. This is
+ * used to pass information about a collision shape to a collision algorithm.
+ */
+struct CollisionShapeInfo {
+
+    public:
+
+        /// Broadphase overlapping pair
+        OverlappingPair* overlappingPair;
+
+        /// Proxy shape
+        ProxyShape* proxyShape;
+
+        /// Pointer to the collision shape
+        const CollisionShape* collisionShape;
+
+        /// Transform that maps from collision shape local-space to world-space
+        Transform shapeToWorldTransform;
+
+        /// Cached collision data of the proxy shape
+        void** cachedCollisionData;
+
+        /// Constructor
+        CollisionShapeInfo(ProxyShape* proxyCollisionShape, const CollisionShape* shape,
+                           const Transform& shapeLocalToWorldTransform, OverlappingPair* pair,
+                           void** cachedData)
+              : overlappingPair(pair), proxyShape(proxyCollisionShape), collisionShape(shape),
+                shapeToWorldTransform(shapeLocalToWorldTransform),
+                cachedCollisionData(cachedData) {
+
+        }
+};
+
+}
+
+#endif
+

src/collision/ContactManifold.cpp

@@ -0,0 +1,263 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include <iostream>
+#include "ContactManifold.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
+                                 MemoryAllocator& memoryAllocator, short normalDirectionId)
+                : mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId),
+                  mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
+                  mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
+                  mMemoryAllocator(memoryAllocator) {
+    
+}
+
+// Destructor
+ContactManifold::~ContactManifold() {
+    clear();
+}
+
+// Add a contact point in the manifold
+void ContactManifold::addContactPoint(ContactPoint* contact) {
+	
+    // For contact already in the manifold
+    for (uint i=0; i<mNbContactPoints; i++) {
+
+		// Check if the new point point does not correspond to a same contact point
+        // already in the manifold.
+        decimal distance = (mContactPoints[i]->getWorldPointOnBody1() -
+                            contact->getWorldPointOnBody1()).lengthSquare();
+        if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) {
+
+            // Delete the new contact
+            contact->~ContactPoint();
+            mMemoryAllocator.release(contact, sizeof(ContactPoint));
+
+            assert(mNbContactPoints > 0);
+
+            return;
+		}
+	}
+    
+    // If the contact manifold is full
+    if (mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) {
+        int indexMaxPenetration = getIndexOfDeepestPenetration(contact);
+        int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1());
+        removeContactPoint(indexToRemove);
+    }
+
+    // Add the new contact point in the manifold
+    mContactPoints[mNbContactPoints] = contact;
+    mNbContactPoints++;
+
+    assert(mNbContactPoints > 0);
+}
+
+// Remove a contact point from the manifold
+void ContactManifold::removeContactPoint(uint index) {
+    assert(index < mNbContactPoints);
+    assert(mNbContactPoints > 0);
+	
+    // Call the destructor explicitly and tell the memory allocator that
+	// the corresponding memory block is now free
+    mContactPoints[index]->~ContactPoint();
+    mMemoryAllocator.release(mContactPoints[index], sizeof(ContactPoint));
+	
+    // If we don't remove the last index
+    if (index < mNbContactPoints - 1) {
+        mContactPoints[index] = mContactPoints[mNbContactPoints - 1];
+    }
+
+    mNbContactPoints--;
+}
+
+// Update the contact manifold
+/// First the world space coordinates of the current contacts in the manifold are recomputed from
+/// the corresponding transforms of the bodies because they have moved. Then we remove the contacts
+/// with a negative penetration depth (meaning that the bodies are not penetrating anymore) and also
+/// 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 (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()));
+    }
+
+    const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD *
+                                                     PERSISTENT_CONTACT_DIST_THRESHOLD;
+
+    // Remove the contact points that don't represent very well the contact manifold
+    for (int i=static_cast<int>(mNbContactPoints)-1; i>=0; i--) {
+        assert(i < static_cast<int>(mNbContactPoints));
+
+        // Compute the distance between contact points in the normal direction
+        decimal distanceNormal = -mContactPoints[i]->getPenetrationDepth();
+        
+        // If the contacts points are too far from each other in the normal direction
+        if (distanceNormal > squarePersistentContactThreshold) {
+            removeContactPoint(i);
+        }
+        else {
+            // Compute the distance of the two contact points in the plane
+            // orthogonal to the contact normal
+            Vector3 projOfPoint1 = mContactPoints[i]->getWorldPointOnBody1() +
+                                   mContactPoints[i]->getNormal() * distanceNormal;
+            Vector3 projDifference = mContactPoints[i]->getWorldPointOnBody2() - projOfPoint1;
+
+            // If the orthogonal distance is larger than the valid distance
+            // threshold, we remove the contact
+            if (projDifference.lengthSquare() > squarePersistentContactThreshold) {
+                removeContactPoint(i);
+            }
+        }
+    }    
+}
+
+// Return the index of the contact point with the larger penetration depth.
+/// This corresponding contact will be kept in the cache. The method returns -1 is
+/// the new contact is the deepest.
+int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) const {
+    assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
+    int indexMaxPenetrationDepth = -1;
+    decimal maxPenetrationDepth = newContact->getPenetrationDepth();
+
+    // For each contact in the cache
+    for (uint i=0; i<mNbContactPoints; i++) {
+
+        // If the current contact has a larger penetration depth
+        if (mContactPoints[i]->getPenetrationDepth() > maxPenetrationDepth) {
+            maxPenetrationDepth = mContactPoints[i]->getPenetrationDepth();
+            indexMaxPenetrationDepth = i;
+        }
+    }
+
+    // Return the index of largest penetration depth
+    return indexMaxPenetrationDepth;
+}
+
+// Return the index that will be removed.
+/// The index of the contact point with the larger penetration
+/// depth is given as a parameter. This contact won't be removed. Given this contact, we compute
+/// the different area and we want to keep the contacts with the largest area. The new point is also
+/// kept. In order to compute the area of a quadrilateral, we use the formula :
+/// Area = 0.5 * | AC x BD | where AC and BD form the diagonals of the quadrilateral. Note that
+/// when we compute this area, we do not calculate it exactly but we
+/// only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore,
+/// this is only a guess that is faster to compute. This idea comes from the Bullet Physics library
+/// by Erwin Coumans (http://wwww.bulletphysics.org).
+int ContactManifold::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const {
+
+    assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
+
+    decimal area0 = 0.0;       // Area with contact 1,2,3 and newPoint
+    decimal area1 = 0.0;       // Area with contact 0,2,3 and newPoint
+    decimal area2 = 0.0;       // Area with contact 0,1,3 and newPoint
+    decimal area3 = 0.0;       // Area with contact 0,1,2 and newPoint
+
+    if (indexMaxPenetration != 0) {
+        // Compute the area
+        Vector3 vector1 = newPoint - mContactPoints[1]->getLocalPointOnBody1();
+        Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() -
+                          mContactPoints[2]->getLocalPointOnBody1();
+        Vector3 crossProduct = vector1.cross(vector2);
+        area0 = crossProduct.lengthSquare();
+    }
+    if (indexMaxPenetration != 1) {
+        // Compute the area
+        Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
+        Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() -
+                          mContactPoints[2]->getLocalPointOnBody1();
+        Vector3 crossProduct = vector1.cross(vector2);
+        area1 = crossProduct.lengthSquare();
+    }
+    if (indexMaxPenetration != 2) {
+        // Compute the area
+        Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
+        Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() -
+                          mContactPoints[1]->getLocalPointOnBody1();
+        Vector3 crossProduct = vector1.cross(vector2);
+        area2 = crossProduct.lengthSquare();
+    }
+    if (indexMaxPenetration != 3) {
+        // Compute the area
+        Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
+        Vector3 vector2 = mContactPoints[2]->getLocalPointOnBody1() -
+                          mContactPoints[1]->getLocalPointOnBody1();
+        Vector3 crossProduct = vector1.cross(vector2);
+        area3 = crossProduct.lengthSquare();
+    }
+    
+    // Return the index of the contact to remove
+    return getMaxArea(area0, area1, area2, area3);
+}
+
+// Return the index of maximum area
+int ContactManifold::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const {
+    if (area0 < area1) {
+        if (area1 < area2) {
+            if (area2 < area3) return 3;
+            else return 2;
+        }
+        else {
+            if (area1 < area3) return 3;
+            else return 1;
+        }
+    }
+    else {
+        if (area0 < area2) {
+            if (area2 < area3) return 3;
+            else return 2;
+        }
+        else {
+            if (area0 < area3) return 3;
+            else return 0;
+        }
+    }
+}
+
+// Clear the contact manifold
+void ContactManifold::clear() {
+    for (uint i=0; i<mNbContactPoints; i++) {
+		
+        // Call the destructor explicitly and tell the memory allocator that
+		// the corresponding memory block is now free
+        mContactPoints[i]->~ContactPoint();
+        mMemoryAllocator.release(mContactPoints[i], sizeof(ContactPoint));
+    }
+    mNbContactPoints = 0;
+}

src/collision/ContactManifold.h

@@ -0,0 +1,364 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_CONTACT_MANIFOLD_H
+#define	REACTPHYSICS3D_CONTACT_MANIFOLD_H
+
+// Libraries
+#include <vector>
+#include "body/CollisionBody.h"
+#include "collision/ProxyShape.h"
+#include "constraint/ContactPoint.h"
+#include "memory/MemoryAllocator.h"
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Constants
+const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4;   // Maximum number of contacts in the manifold
+
+// Class declarations
+class ContactManifold;
+
+// Structure ContactManifoldListElement
+/**
+ * This structure represents a single element of a linked list of contact manifolds
+ */
+struct ContactManifoldListElement {
+
+    public:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Pointer to the actual contact manifold
+        ContactManifold* contactManifold;
+
+        /// Next element of the list
+        ContactManifoldListElement* next;
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        ContactManifoldListElement(ContactManifold* initContactManifold,
+                                   ContactManifoldListElement* initNext)
+                                  :contactManifold(initContactManifold), next(initNext) {
+
+        }
+};
+
+// Class ContactManifold
+/**
+ * This class represents the set of contact points between two bodies.
+ * The contact manifold is implemented in a way to cache the contact
+ * points among the frames for better stability following the
+ * "Contact Generation" presentation of Erwin Coumans at GDC 2010
+ * conference (bullet.googlecode.com/files/GDC10_Coumans_Erwin_Contact.pdf).
+ * Some code of this class is based on the implementation of the
+ * btPersistentManifold class from Bullet physics engine (www.http://bulletphysics.org).
+ * The contacts between two bodies are added one after the other in the cache.
+ * When the cache is full, we have to remove one point. The idea is to keep
+ * the point with the deepest penetration depth and also to keep the
+ * points producing the larger area (for a more stable contact manifold).
+ * The new added point is always kept.
+ */
+class ContactManifold {
+
+    private:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Pointer to the first proxy shape of the contact
+        ProxyShape* mShape1;
+
+        /// Pointer to the second proxy shape of the contact
+        ProxyShape* mShape2;
+
+        /// Contact points in the manifold
+        ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD];
+
+        /// Normal direction Id (Unique Id representing the normal direction)
+        short int mNormalDirectionId;
+
+        /// Number of contacts in the cache
+        uint mNbContactPoints;
+
+        /// First friction vector of the contact manifold
+        Vector3 mFrictionVector1;
+
+        /// Second friction vector of the contact manifold
+        Vector3 mFrictionVector2;
+
+        /// First friction constraint accumulated impulse
+        decimal mFrictionImpulse1;
+
+        /// Second friction constraint accumulated impulse
+        decimal mFrictionImpulse2;
+
+        /// Twist friction constraint accumulated impulse
+        decimal mFrictionTwistImpulse;
+
+        /// Accumulated rolling resistance impulse
+        Vector3 mRollingResistanceImpulse;
+
+        /// True if the contact manifold has already been added into an island
+        bool mIsAlreadyInIsland;
+
+        /// Reference to the memory allocator
+        MemoryAllocator& mMemoryAllocator;
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        ContactManifold(const ContactManifold& contactManifold);
+
+        /// Private assignment operator
+        ContactManifold& operator=(const ContactManifold& contactManifold);
+
+        /// Return the index of maximum area
+        int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const;
+
+        /// Return the index of the contact with the larger penetration depth.
+        int getIndexOfDeepestPenetration(ContactPoint* newContact) const;
+
+        /// Return the index that will be removed.
+        int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const;
+
+        /// Remove a contact point from the manifold
+        void removeContactPoint(uint index);
+
+        /// Return true if the contact manifold has already been added into an island
+        bool isAlreadyInIsland() const;
+        
+    public:
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
+                        MemoryAllocator& memoryAllocator, short int normalDirectionId);
+
+        /// Destructor
+        ~ContactManifold();
+
+        /// Return a pointer to the first proxy shape of the contact
+        ProxyShape* getShape1() const;
+
+        /// Return a pointer to the second proxy shape of the contact
+        ProxyShape* getShape2() const;
+
+        /// Return a pointer to the first body of the contact manifold
+        CollisionBody* getBody1() const;
+
+        /// Return a pointer to the second body of the contact manifold
+        CollisionBody* getBody2() const;
+
+        /// Return the normal direction Id
+        short int getNormalDirectionId() const;
+
+        /// Add a contact point to the manifold
+        void addContactPoint(ContactPoint* contact);
+
+        /// Update the contact manifold.
+        void update(const Transform& transform1, const Transform& transform2);
+
+        /// Clear the contact manifold
+        void clear();
+
+        /// Return the number of contact points in the manifold
+        uint getNbContactPoints() const;
+
+        /// Return the first friction vector at the center of the contact manifold
+        const Vector3& getFrictionVector1() const;
+
+        /// set the first friction vector at the center of the contact manifold
+        void setFrictionVector1(const Vector3& mFrictionVector1);
+
+        /// Return the second friction vector at the center of the contact manifold
+        const Vector3& getFrictionVector2() const;
+
+        /// set the second friction vector at the center of the contact manifold
+        void setFrictionVector2(const Vector3& mFrictionVector2);
+
+        /// Return the first friction accumulated impulse
+        decimal getFrictionImpulse1() const;
+
+        /// Set the first friction accumulated impulse
+        void setFrictionImpulse1(decimal frictionImpulse1);
+
+        /// Return the second friction accumulated impulse
+        decimal getFrictionImpulse2() const;
+
+        /// Set the second friction accumulated impulse
+        void setFrictionImpulse2(decimal frictionImpulse2);
+
+        /// Return the friction twist accumulated impulse
+        decimal getFrictionTwistImpulse() const;
+
+        /// Set the friction twist accumulated impulse
+        void setFrictionTwistImpulse(decimal frictionTwistImpulse);
+
+        /// Set the accumulated rolling resistance impulse
+        void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse);
+
+        /// Return a contact point of the manifold
+        ContactPoint* getContactPoint(uint index) const;
+
+        /// Return the normalized averaged normal vector
+        Vector3 getAverageContactNormal() const;
+
+        /// Return the largest depth of all the contact points
+        decimal getLargestContactDepth() const;
+
+        // -------------------- Friendship -------------------- //
+
+        friend class DynamicsWorld;
+        friend class Island;
+        friend class CollisionBody;
+};
+
+// Return a pointer to the first proxy shape of the contact
+inline ProxyShape* ContactManifold::getShape1() const {
+    return mShape1;
+}
+
+// Return a pointer to the second proxy shape of the contact
+inline ProxyShape* ContactManifold::getShape2() const {
+    return mShape2;
+}
+
+// Return a pointer to the first body of the contact manifold
+inline CollisionBody* ContactManifold::getBody1() const {
+    return mShape1->getBody();
+}
+
+// Return a pointer to the second body of the contact manifold
+inline CollisionBody* ContactManifold::getBody2() const {
+    return mShape2->getBody();
+}
+
+// Return the normal direction Id
+inline short int ContactManifold::getNormalDirectionId() const {
+    return mNormalDirectionId;
+}
+
+// Return the number of contact points in the manifold
+inline uint ContactManifold::getNbContactPoints() const {
+    return mNbContactPoints;
+}
+
+// Return the first friction vector at the center of the contact manifold
+inline const Vector3& ContactManifold::getFrictionVector1() const {
+    return mFrictionVector1;
+}
+
+// set the first friction vector at the center of the contact manifold
+inline void ContactManifold::setFrictionVector1(const Vector3& frictionVector1) {
+    mFrictionVector1 = frictionVector1;
+}
+
+// Return the second friction vector at the center of the contact manifold
+inline const Vector3& ContactManifold::getFrictionVector2() const {
+    return mFrictionVector2;
+}
+
+// set the second friction vector at the center of the contact manifold
+inline void ContactManifold::setFrictionVector2(const Vector3& frictionVector2) {
+    mFrictionVector2 = frictionVector2;
+}
+
+// Return the first friction accumulated impulse
+inline decimal ContactManifold::getFrictionImpulse1() const {
+    return mFrictionImpulse1;
+}
+
+// Set the first friction accumulated impulse
+inline void ContactManifold::setFrictionImpulse1(decimal frictionImpulse1) {
+    mFrictionImpulse1 = frictionImpulse1;
+}
+
+// Return the second friction accumulated impulse
+inline decimal ContactManifold::getFrictionImpulse2() const {
+    return mFrictionImpulse2;
+}
+
+// Set the second friction accumulated impulse
+inline void ContactManifold::setFrictionImpulse2(decimal frictionImpulse2) {
+    mFrictionImpulse2 = frictionImpulse2;
+}
+
+// Return the friction twist accumulated impulse
+inline decimal ContactManifold::getFrictionTwistImpulse() const {
+    return mFrictionTwistImpulse;
+}
+
+// Set the friction twist accumulated impulse
+inline void ContactManifold::setFrictionTwistImpulse(decimal frictionTwistImpulse) {
+    mFrictionTwistImpulse = frictionTwistImpulse;
+}
+
+// Set the accumulated rolling resistance impulse
+inline void ContactManifold::setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse) {
+    mRollingResistanceImpulse = rollingResistanceImpulse;
+}
+
+// Return a contact point of the manifold
+inline ContactPoint* ContactManifold::getContactPoint(uint index) const {
+    assert(index < mNbContactPoints);
+    return mContactPoints[index];
+}
+
+// Return true if the contact manifold has already been added into an island
+inline bool ContactManifold::isAlreadyInIsland() const {
+    return mIsAlreadyInIsland;
+}
+
+// Return the normalized averaged normal vector
+inline Vector3 ContactManifold::getAverageContactNormal() const {
+    Vector3 averageNormal;
+
+    for (uint i=0; i<mNbContactPoints; i++) {
+        averageNormal += mContactPoints[i]->getNormal();
+    }
+
+    return averageNormal.getUnit();
+}
+
+// Return the largest depth of all the contact points
+inline decimal ContactManifold::getLargestContactDepth() const {
+    decimal largestDepth = 0.0f;
+
+    for (uint i=0; i<mNbContactPoints; i++) {
+        decimal depth = mContactPoints[i]->getPenetrationDepth();
+        if (depth > largestDepth) {
+            largestDepth = depth;
+        }
+    }
+
+    return largestDepth;
+}
+
+}
+#endif
+

src/collision/ContactManifoldSet.cpp

@@ -0,0 +1,236 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "ContactManifoldSet.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
+                                       MemoryAllocator& memoryAllocator, int nbMaxManifolds)
+                   : mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1),
+                     mShape2(shape2), mMemoryAllocator(memoryAllocator) {
+    assert(nbMaxManifolds >= 1);
+}
+
+// Destructor
+ContactManifoldSet::~ContactManifoldSet() {
+
+    // Clear all the contact manifolds
+    clear();
+}
+
+// Add a contact point to the manifold set
+void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
+
+    // Compute an Id corresponding to the normal direction (using a cubemap)
+    short int normalDirectionId = computeCubemapNormalId(contact->getNormal());
+
+    // If there is no contact manifold yet
+    if (mNbManifolds == 0) {
+
+        createManifold(normalDirectionId);
+        mManifolds[0]->addContactPoint(contact);
+        assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0);
+        for (int i=0; i<mNbManifolds; i++) {
+            assert(mManifolds[i]->getNbContactPoints() > 0);
+        }
+
+        return;
+    }
+
+    // Select the manifold with the most similar normal (if exists)
+    int similarManifoldIndex = 0;
+    if (mNbMaxManifolds > 1) {
+        similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId);
+    }
+
+    // If a similar manifold has been found
+    if (similarManifoldIndex != -1) {
+
+        // Add the contact point to that similar manifold
+        mManifolds[similarManifoldIndex]->addContactPoint(contact);
+        assert(mManifolds[similarManifoldIndex]->getNbContactPoints() > 0);
+
+        return;
+    }
+
+    // If the maximum number of manifold has not been reached yet
+    if (mNbManifolds < mNbMaxManifolds) {
+
+        // Create a new manifold for the contact point
+        createManifold(normalDirectionId);
+        mManifolds[mNbManifolds-1]->addContactPoint(contact);
+        for (int i=0; i<mNbManifolds; i++) {
+            assert(mManifolds[i]->getNbContactPoints() > 0);
+        }
+
+        return;
+    }
+
+    // The contact point will be in a new contact manifold, we now have too much
+    // manifolds condidates. We need to remove one. We choose to keep the manifolds
+    // with the largest contact depth among their points
+    int smallestDepthIndex = -1;
+    decimal minDepth = contact->getPenetrationDepth();
+    assert(mNbManifolds == mNbMaxManifolds);
+    for (int i=0; i<mNbManifolds; i++) {
+        decimal depth = mManifolds[i]->getLargestContactDepth();
+        if (depth < minDepth) {
+            minDepth = depth;
+            smallestDepthIndex = i;
+        }
+    }
+
+    // If we do not want to keep to new manifold (not created yet) with the
+    // new contact point
+    if (smallestDepthIndex == -1) {
+
+        // Delete the new contact
+        contact->~ContactPoint();
+        mMemoryAllocator.release(contact, sizeof(ContactPoint));
+
+        return;
+    }
+
+    assert(smallestDepthIndex >= 0 && smallestDepthIndex < mNbManifolds);
+
+    // Here we need to replace an existing manifold with a new one (that contains
+    // the new contact point)
+    removeManifold(smallestDepthIndex);
+    createManifold(normalDirectionId);
+    mManifolds[mNbManifolds-1]->addContactPoint(contact);
+    assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0);
+    for (int i=0; i<mNbManifolds; i++) {
+        assert(mManifolds[i]->getNbContactPoints() > 0);
+    }
+
+    return;
+}
+
+// Return the index of the contact manifold with a similar average normal.
+// If no manifold has close enough average normal, it returns -1
+int ContactManifoldSet::selectManifoldWithSimilarNormal(short int normalDirectionId) const {
+
+    // Return the Id of the manifold with the same normal direction id (if exists)
+    for (int i=0; i<mNbManifolds; i++) {
+        if (normalDirectionId == mManifolds[i]->getNormalDirectionId()) {
+            return i;
+        }
+    }
+
+    return -1;
+}
+
+// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
+// Each face of the cube is divided into 4x4 buckets. This method maps the
+// normal vector into of the of the bucket and returns a unique Id for the bucket
+short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) const {
+
+    assert(normal.lengthSquare() > MACHINE_EPSILON);
+
+    int faceNo;
+    decimal u, v;
+    decimal max = max3(fabs(normal.x), fabs(normal.y), fabs(normal.z));
+    Vector3 normalScaled = normal / max;
+
+    if (normalScaled.x >= normalScaled.y && normalScaled.x >= normalScaled.z) {
+        faceNo = normalScaled.x > 0 ? 0 : 1;
+        u = normalScaled.y;
+        v = normalScaled.z;
+    }
+    else if (normalScaled.y >= normalScaled.x && normalScaled.y >= normalScaled.z) {
+        faceNo = normalScaled.y > 0 ? 2 : 3;
+        u = normalScaled.x;
+        v = normalScaled.z;
+    }
+    else {
+        faceNo = normalScaled.z > 0 ? 4 : 5;
+        u = normalScaled.x;
+        v = normalScaled.y;
+    }
+
+    int indexU = floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
+    int indexV = floor(((v + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
+    if (indexU == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexU--;
+    if (indexV == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexV--;
+
+    const int nbSubDivInFace = CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS;
+    return faceNo * 200 + indexU * nbSubDivInFace + indexV;
+}
+
+// Update the contact manifolds
+void ContactManifoldSet::update() {
+
+    for (int i=mNbManifolds-1; i>=0; i--) {
+
+        // Update the contact manifold
+        mManifolds[i]->update(mShape1->getBody()->getTransform() * mShape1->getLocalToBodyTransform(),
+                              mShape2->getBody()->getTransform() * mShape2->getLocalToBodyTransform());
+
+        // Remove the contact manifold if has no contact points anymore
+        if (mManifolds[i]->getNbContactPoints() == 0) {
+            removeManifold(i);
+        }
+    }
+}
+
+// Clear the contact manifold set
+void ContactManifoldSet::clear() {
+
+    // Destroy all the contact manifolds
+    for (int i=mNbManifolds-1; i>=0; i--) {
+        removeManifold(i);
+    }
+
+    assert(mNbManifolds == 0);
+}
+
+// Create a new contact manifold and add it to the set
+void ContactManifoldSet::createManifold(short int normalDirectionId) {
+    assert(mNbManifolds < mNbMaxManifolds);
+
+    mManifolds[mNbManifolds] = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
+                                    ContactManifold(mShape1, mShape2, mMemoryAllocator, normalDirectionId);
+    mNbManifolds++;
+}
+
+// Remove a contact manifold from the set
+void ContactManifoldSet::removeManifold(int index) {
+
+    assert(mNbManifolds > 0);
+    assert(index >= 0 && index < mNbManifolds);
+
+    // Delete the new contact
+    mManifolds[index]->~ContactManifold();
+    mMemoryAllocator.release(mManifolds[index], sizeof(ContactManifold));
+
+    for (int i=index; (i+1) < mNbManifolds; i++) {
+        mManifolds[i] = mManifolds[i+1];
+    }
+
+    mNbManifolds--;
+}

src/collision/ContactManifoldSet.h

@@ -0,0 +1,154 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_CONTACT_MANIFOLD_SET_H
+#define REACTPHYSICS3D_CONTACT_MANIFOLD_SET_H
+
+// Libraries
+#include "ContactManifold.h"
+
+namespace reactphysics3d {
+
+// Constants
+const int MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET = 3;   // Maximum number of contact manifolds in the set
+const int CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS = 3;    // N Number for the N x N subdivisions of the cubemap
+
+// Class ContactManifoldSet
+/**
+ * This class represents a set of one or several contact manifolds. Typically a
+ * convex/convex collision will have a set with a single manifold and a convex-concave
+ * collision can have more than one manifolds. Note that a contact manifold can
+ * contains several contact points.
+ */
+class ContactManifoldSet {
+
+    private:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Maximum number of contact manifolds in the set
+        int mNbMaxManifolds;
+
+        /// Current number of contact manifolds in the set
+        int mNbManifolds;
+
+        /// Pointer to the first proxy shape of the contact
+        ProxyShape* mShape1;
+
+        /// Pointer to the second proxy shape of the contact
+        ProxyShape* mShape2;
+
+        /// Reference to the memory allocator
+        MemoryAllocator& mMemoryAllocator;
+
+        /// Contact manifolds of the set
+        ContactManifold* mManifolds[MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET];
+
+        // -------------------- Methods -------------------- //
+
+        /// Create a new contact manifold and add it to the set
+        void createManifold(short normalDirectionId);
+
+        /// Remove a contact manifold from the set
+        void removeManifold(int index);
+
+        // Return the index of the contact manifold with a similar average normal.
+        int selectManifoldWithSimilarNormal(short int normalDirectionId) const;
+
+        // Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
+        // Each face of the cube is divided into 4x4 buckets. This method maps the
+        // normal vector into of the of the bucket and returns a unique Id for the bucket
+        short int computeCubemapNormalId(const Vector3& normal) const;
+
+    public:
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
+                           MemoryAllocator& memoryAllocator, int nbMaxManifolds);
+
+        /// Destructor
+        ~ContactManifoldSet();
+
+        /// Return the first proxy shape
+        ProxyShape* getShape1() const;
+
+        /// Return the second proxy shape
+        ProxyShape* getShape2() const;
+
+        /// Add a contact point to the manifold set
+        void addContactPoint(ContactPoint* contact);
+
+        /// Update the contact manifolds
+        void update();
+
+        /// Clear the contact manifold set
+        void clear();
+
+        /// Return the number of manifolds in the set
+        int getNbContactManifolds() const;
+
+        /// Return a given contact manifold
+        ContactManifold* getContactManifold(int index) const;
+
+        /// Return the total number of contact points in the set of manifolds
+        int getTotalNbContactPoints() const;
+};
+
+// Return the first proxy shape
+inline ProxyShape* ContactManifoldSet::getShape1() const {
+    return mShape1;
+}
+
+// Return the second proxy shape
+inline ProxyShape* ContactManifoldSet::getShape2() const {
+    return mShape2;
+}
+
+// Return the number of manifolds in the set
+inline int ContactManifoldSet::getNbContactManifolds() const {
+    return mNbManifolds;
+}
+
+// Return a given contact manifold
+inline ContactManifold* ContactManifoldSet::getContactManifold(int index) const {
+    assert(index >= 0 && index < mNbManifolds);
+    return mManifolds[index];
+}
+
+// Return the total number of contact points in the set of manifolds
+inline int ContactManifoldSet::getTotalNbContactPoints() const {
+    int nbPoints = 0;
+    for (int i=0; i<mNbManifolds; i++) {
+        nbPoints += mManifolds[i]->getNbContactPoints();
+    }
+    return nbPoints;
+}
+
+}
+
+#endif
+

src/collision/ProxyShape.cpp

@@ -0,0 +1,92 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "ProxyShape.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+/**
+ * @param body Pointer to the parent body
+ * @param shape Pointer to the collision shape
+ * @param transform Transformation from collision shape local-space to body local-space
+ * @param mass Mass of the collision shape (in kilograms)
+ */
+ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass)
+           :mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass),
+            mNext(NULL), mBroadPhaseID(-1), mCachedCollisionData(NULL), mUserData(NULL),
+            mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) {
+
+}
+
+// Destructor
+ProxyShape::~ProxyShape() {
+
+    // Release the cached collision data memory
+    if (mCachedCollisionData != NULL) {
+        free(mCachedCollisionData);
+    }
+}
+
+// Return true if a point is inside the collision shape
+/**
+ * @param worldPoint Point to test in world-space coordinates
+ * @return True if the point is inside the collision shape
+ */
+bool ProxyShape::testPointInside(const Vector3& worldPoint) {
+    const Transform localToWorld = mBody->getTransform() * mLocalToBodyTransform;
+    const Vector3 localPoint = localToWorld.getInverse() * worldPoint;
+    return mCollisionShape->testPointInside(localPoint, this);
+}
+
+// Raycast method with feedback information
+/**
+ * @param ray Ray to use for the raycasting
+ * @param[out] raycastInfo Result of the raycasting that is valid only if the
+ *             methods returned true
+ * @return True if the ray hit the collision shape
+ */
+bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
+
+    // If the corresponding body is not active, it cannot be hit by rays
+    if (!mBody->isActive()) return false;
+
+    // Convert the ray into the local-space of the collision shape
+    const Transform localToWorldTransform = getLocalToWorldTransform();
+    const Transform worldToLocalTransform = localToWorldTransform.getInverse();
+    Ray rayLocal(worldToLocalTransform * ray.point1,
+                 worldToLocalTransform * ray.point2,
+                 ray.maxFraction);
+
+    bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this);
+
+    // Convert the raycast info into world-space
+    raycastInfo.worldPoint = localToWorldTransform * raycastInfo.worldPoint;
+    raycastInfo.worldNormal = localToWorldTransform.getOrientation() * raycastInfo.worldNormal;
+    raycastInfo.worldNormal.normalize();
+
+    return isHit;
+}

src/collision/ProxyShape.h

@@ -0,0 +1,327 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_PROXY_SHAPE_H
+#define REACTPHYSICS3D_PROXY_SHAPE_H
+
+// Libraries
+#include "body/CollisionBody.h"
+#include "shapes/CollisionShape.h"
+
+namespace  reactphysics3d {
+
+// Class ProxyShape
+/**
+ * The CollisionShape instances are supposed to be unique for memory optimization. For instance,
+ * consider two rigid bodies with the same sphere collision shape. In this situation, we will have
+ * a unique instance of SphereShape but we need to differentiate between the two instances during
+ * the collision detection. They do not have the same position in the world and they do not
+ * belong to the same rigid body. The ProxyShape class is used for that purpose by attaching a
+ * rigid body with one of its collision shape. A body can have multiple proxy shapes (one for
+ * each collision shape attached to the body).
+ */
+class ProxyShape {
+
+    protected:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Pointer to the parent body
+        CollisionBody* mBody;
+
+        /// Internal collision shape
+        CollisionShape* mCollisionShape;
+
+        /// Local-space to parent body-space transform (does not change over time)
+        Transform mLocalToBodyTransform;
+
+        /// Mass (in kilogramms) of the corresponding collision shape
+        decimal mMass;
+
+        /// Pointer to the next proxy shape of the body (linked list)
+        ProxyShape* mNext;
+
+        /// Broad-phase ID (node ID in the dynamic AABB tree)
+        int mBroadPhaseID;
+
+        /// Cached collision data
+        void* mCachedCollisionData;
+
+        /// Pointer to user data
+        void* mUserData;
+
+        /// Bits used to define the collision category of this shape.
+        /// You can set a single bit to one to define a category value for this
+        /// shape. This value is one (0x0001) by default. This variable can be used
+        /// together with the mCollideWithMaskBits variable so that given
+        /// categories of shapes collide with each other and do not collide with
+        /// other categories.
+        unsigned short mCollisionCategoryBits;
+
+        /// Bits mask used to state which collision categories this shape can
+        /// collide with. This value is 0xFFFF by default. It means that this
+        /// proxy shape will collide with every collision categories by default.
+        unsigned short mCollideWithMaskBits;
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        ProxyShape(const ProxyShape& proxyShape);
+
+        /// Private assignment operator
+        ProxyShape& operator=(const ProxyShape& proxyShape);
+
+    public:
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        ProxyShape(CollisionBody* body, CollisionShape* shape,
+                   const Transform& transform, decimal mass);
+
+        /// Destructor
+        ~ProxyShape();
+
+        /// Return the collision shape
+        const CollisionShape* getCollisionShape() const;
+
+        /// Return the parent body
+        CollisionBody* getBody() const;
+
+        /// Return the mass of the collision shape
+        decimal getMass() const;
+
+        /// Return a pointer to the user data attached to this body
+        void* getUserData() const;
+
+        /// Attach user data to this body
+        void setUserData(void* userData);
+
+        /// Return the local to parent body transform
+        const Transform& getLocalToBodyTransform() const;
+
+        /// Set the local to parent body transform
+        void setLocalToBodyTransform(const Transform& transform);
+
+        /// Return the local to world transform
+        const Transform getLocalToWorldTransform() const;
+
+        /// Return true if a point is inside the collision shape
+        bool testPointInside(const Vector3& worldPoint);
+
+        /// Raycast method with feedback information
+        bool raycast(const Ray& ray, RaycastInfo& raycastInfo);
+
+        /// Return the collision bits mask
+        unsigned short getCollideWithMaskBits() const;
+
+        /// Set the collision bits mask
+        void setCollideWithMaskBits(unsigned short collideWithMaskBits);
+
+        /// Return the collision category bits
+        unsigned short getCollisionCategoryBits() const;
+
+        /// Set the collision category bits
+        void setCollisionCategoryBits(unsigned short collisionCategoryBits);
+
+        /// Return the next proxy shape in the linked list of proxy shapes
+        ProxyShape* getNext();
+
+        /// Return the next proxy shape in the linked list of proxy shapes
+        const ProxyShape* getNext() const;
+
+        /// Return the pointer to the cached collision data
+        void** getCachedCollisionData();
+
+        /// Return the local scaling vector of the collision shape
+        Vector3 getLocalScaling() const;
+
+        /// Set the local scaling vector of the collision shape
+        virtual void setLocalScaling(const Vector3& scaling);
+
+        // -------------------- Friendship -------------------- //
+
+        friend class OverlappingPair;
+        friend class CollisionBody;
+        friend class RigidBody;
+        friend class BroadPhaseAlgorithm;
+        friend class DynamicAABBTree;
+        friend class CollisionDetection;
+        friend class CollisionWorld;
+        friend class DynamicsWorld;
+        friend class EPAAlgorithm;
+        friend class GJKAlgorithm;
+        friend class ConvexMeshShape;
+
+};
+
+// Return the pointer to the cached collision data
+inline void** ProxyShape::getCachedCollisionData()  {
+    return &mCachedCollisionData;
+}
+
+// Return the collision shape
+/**
+ * @return Pointer to the internal collision shape
+ */
+inline const CollisionShape* ProxyShape::getCollisionShape() const {
+    return mCollisionShape;
+}
+
+// Return the parent body
+/**
+ * @return Pointer to the parent body
+ */
+inline CollisionBody* ProxyShape::getBody() const {
+    return mBody;
+}
+
+// Return the mass of the collision shape
+/**
+ * @return Mass of the collision shape (in kilograms)
+ */
+inline decimal ProxyShape::getMass() const {
+    return mMass;
+}
+
+// Return a pointer to the user data attached to this body
+/**
+ * @return A pointer to the user data stored into the proxy shape
+ */
+inline void* ProxyShape::getUserData() const {
+    return mUserData;
+}
+
+// Attach user data to this body
+/**
+ * @param userData Pointer to the user data you want to store within the proxy shape
+ */
+inline void ProxyShape::setUserData(void* userData) {
+    mUserData = userData;
+}
+
+// Return the local to parent body transform
+/**
+ * @return The transformation that transforms the local-space of the collision shape
+ *         to the local-space of the parent body
+ */
+inline const Transform& ProxyShape::getLocalToBodyTransform() const {
+    return mLocalToBodyTransform;
+}
+
+// Set the local to parent body transform
+inline void ProxyShape::setLocalToBodyTransform(const Transform& transform) {
+
+    mLocalToBodyTransform = transform;
+
+    mBody->setIsSleeping(false);
+
+    // Notify the body that the proxy shape has to be updated in the broad-phase
+    mBody->updateProxyShapeInBroadPhase(this, true);
+}
+
+// Return the local to world transform
+/**
+ * @return The transformation that transforms the local-space of the collision
+ *         shape to the world-space
+ */
+inline const Transform ProxyShape::getLocalToWorldTransform() const {
+    return mBody->mTransform * mLocalToBodyTransform;
+}
+
+// Return the next proxy shape in the linked list of proxy shapes
+/**
+ * @return Pointer to the next proxy shape in the linked list of proxy shapes
+ */
+inline ProxyShape* ProxyShape::getNext() {
+    return mNext;
+}
+
+// Return the next proxy shape in the linked list of proxy shapes
+/**
+ * @return Pointer to the next proxy shape in the linked list of proxy shapes
+ */
+inline const ProxyShape* ProxyShape::getNext() const {
+    return mNext;
+}
+
+// Return the collision category bits
+/**
+ * @return The collision category bits mask of the proxy shape
+ */
+inline unsigned short ProxyShape::getCollisionCategoryBits() const {
+    return mCollisionCategoryBits;
+}
+
+// Set the collision category bits
+/**
+ * @param collisionCategoryBits The collision category bits mask of the proxy shape
+ */
+inline void ProxyShape::setCollisionCategoryBits(unsigned short collisionCategoryBits) {
+    mCollisionCategoryBits = collisionCategoryBits;
+}
+
+// Return the collision bits mask
+/**
+ * @return The bits mask that specifies with which collision category this shape will collide
+ */
+inline unsigned short ProxyShape::getCollideWithMaskBits() const {
+    return mCollideWithMaskBits;
+}
+
+// Set the collision bits mask
+/**
+ * @param collideWithMaskBits The bits mask that specifies with which collision category this shape will collide
+ */
+inline void ProxyShape::setCollideWithMaskBits(unsigned short collideWithMaskBits) {
+    mCollideWithMaskBits = collideWithMaskBits;
+}
+
+// Return the local scaling vector of the collision shape
+/**
+ * @return The local scaling vector
+ */
+inline Vector3 ProxyShape::getLocalScaling() const {
+    return mCollisionShape->getScaling();
+}
+
+// Set the local scaling vector of the collision shape
+/**
+ * @param scaling The new local scaling vector
+ */
+inline void ProxyShape::setLocalScaling(const Vector3& scaling) {
+
+    // Set the local scaling of the collision shape
+    mCollisionShape->setLocalScaling(scaling);
+
+    mBody->setIsSleeping(false);
+
+    // Notify the body that the proxy shape has to be updated in the broad-phase
+    mBody->updateProxyShapeInBroadPhase(this, true);
+}
+
+}
+
+#endif

src/collision/RaycastInfo.cpp

@@ -0,0 +1,49 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "decimal.h"
+#include "RaycastInfo.h"
+#include "ProxyShape.h"
+
+using namespace reactphysics3d;
+
+// Ray cast test against a proxy shape
+decimal RaycastTest::raycastAgainstShape(ProxyShape* shape, const Ray& ray) {
+
+    // Ray casting test against the collision shape
+    RaycastInfo raycastInfo;
+    bool isHit = shape->raycast(ray, raycastInfo);
+
+    // If the ray hit the collision shape
+    if (isHit) {
+
+        // Report the hit to the user and return the
+        // user hit fraction value
+        return userCallback->notifyRaycastHit(raycastInfo);
+    }
+
+    return ray.maxFraction;
+}

src/collision/RaycastInfo.h

@@ -0,0 +1,152 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_RAYCAST_INFO_H
+#define REACTPHYSICS3D_RAYCAST_INFO_H
+
+// Libraries
+#include "mathematics/Vector3.h"
+#include "mathematics/Ray.h"
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Declarations
+class CollisionBody;
+class ProxyShape;
+class CollisionShape;
+
+// Structure RaycastInfo
+/**
+ * This structure contains the information about a raycast hit.
+ */
+struct RaycastInfo {
+
+    private:
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy constructor
+        RaycastInfo(const RaycastInfo& raycastInfo);
+
+        /// Private assignment operator
+        RaycastInfo& operator=(const RaycastInfo& raycastInfo);
+
+    public:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Hit point in world-space coordinates
+        Vector3 worldPoint;
+
+        /// Surface normal at hit point in world-space coordinates
+        Vector3 worldNormal;
+
+        /// Fraction distance of the hit point between point1 and point2 of the ray
+        /// The hit point "p" is such that p = point1 + hitFraction * (point2 - point1)
+        decimal hitFraction;
+
+        /// Mesh subpart index that has been hit (only used for triangles mesh and -1 otherwise)
+        int meshSubpart;
+
+        /// Hit triangle index (only used for triangles mesh and -1 otherwise)
+        int triangleIndex;
+
+        /// Pointer to the hit collision body
+        CollisionBody* body;
+
+        /// Pointer to the hit proxy collision shape
+        ProxyShape* proxyShape;
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        RaycastInfo() : meshSubpart(-1), triangleIndex(-1), body(NULL), proxyShape(NULL) {
+
+        }
+
+        /// Destructor
+        ~RaycastInfo() {
+
+        }
+};
+
+// Class RaycastCallback
+/**
+ * This class can be used to register a callback for ray casting queries.
+ * You should implement your own class inherited from this one and implement
+ * the notifyRaycastHit() method. This method will be called for each ProxyShape
+ * that is hit by the ray.
+ */
+class RaycastCallback {
+
+    public:
+
+        // -------------------- Methods -------------------- //
+
+        /// Destructor
+        virtual ~RaycastCallback() {
+
+        }
+
+        /// This method will be called for each ProxyShape that is hit by the
+        /// ray. You cannot make any assumptions about the order of the
+        /// calls. You should use the return value to control the continuation
+        /// of the ray. The returned value is the next maxFraction value to use.
+        /// If you return a fraction of 0.0, it means that the raycast should
+        /// terminate. If you return a fraction of 1.0, it indicates that the
+        /// ray is not clipped and the ray cast should continue as if no hit
+        /// occurred. If you return the fraction in the parameter (hitFraction
+        /// value in the RaycastInfo object), the current ray will be clipped
+        /// to this fraction in the next queries. If you return -1.0, it will
+        /// ignore this ProxyShape and continue the ray cast.
+        /**
+         * @param raycastInfo Information about the raycast hit
+         * @return Value that controls the continuation of the ray after a hit
+         */
+        virtual decimal notifyRaycastHit(const RaycastInfo& raycastInfo)=0;
+
+};
+
+/// Structure RaycastTest
+struct RaycastTest {
+
+    public:
+
+        /// User callback class
+        RaycastCallback* userCallback;
+
+        /// Constructor
+        RaycastTest(RaycastCallback* callback) {
+            userCallback = callback;
+        }
+
+        /// Ray cast test against a proxy shape
+        decimal raycastAgainstShape(ProxyShape* shape, const Ray& ray);
+};
+
+}
+
+#endif

src/collision/TriangleMesh.cpp

@@ -0,0 +1,39 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "TriangleMesh.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+TriangleMesh::TriangleMesh() {
+
+}
+
+// Destructor
+TriangleMesh::~TriangleMesh() {
+
+}

src/collision/TriangleMesh.h

@@ -0,0 +1,88 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_TRIANGLE_MESH_H
+#define REACTPHYSICS3D_TRIANGLE_MESH_H
+
+// Libraries
+#include <vector>
+#include <cassert>
+#include "TriangleVertexArray.h"
+
+namespace reactphysics3d {
+
+// Class TriangleMesh
+/**
+ * This class represents a mesh made of triangles. A TriangleMesh contains
+ * one or several parts. Each part is a set of triangles represented in a
+ * TriangleVertexArray object describing all the triangles vertices of the part.
+ * A TriangleMesh object is used to create a ConcaveMeshShape from a triangle
+ * mesh for instance.
+ */
+class TriangleMesh {
+
+    protected:
+
+        /// All the triangle arrays of the mesh (one triangle array per part)
+        std::vector<TriangleVertexArray*> mTriangleArrays;
+
+    public:
+
+        /// Constructor
+        TriangleMesh();
+
+        /// Destructor
+        ~TriangleMesh();
+
+        /// Add a subpart of the mesh
+        void addSubpart(TriangleVertexArray* triangleVertexArray);
+
+        /// Return a pointer to a given subpart (triangle vertex array) of the mesh
+        TriangleVertexArray* getSubpart(uint indexSubpart) const;
+
+        /// Return the number of subparts of the mesh
+        uint getNbSubparts() const;
+};
+
+// Add a subpart of the mesh
+inline void TriangleMesh::addSubpart(TriangleVertexArray* triangleVertexArray) {
+    mTriangleArrays.push_back(triangleVertexArray );
+}
+
+// Return a pointer to a given subpart (triangle vertex array) of the mesh
+inline TriangleVertexArray* TriangleMesh::getSubpart(uint indexSubpart) const {
+   assert(indexSubpart < mTriangleArrays.size());
+   return mTriangleArrays[indexSubpart];
+}
+
+// Return the number of subparts of the mesh
+inline uint TriangleMesh::getNbSubparts() const {
+    return mTriangleArrays.size();
+}
+
+}
+
+#endif
+

src/collision/TriangleVertexArray.cpp

@@ -0,0 +1,61 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "TriangleVertexArray.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+/// Note that your data will not be copied into the TriangleVertexArray and
+/// therefore, you need to make sure that those data are always valid during
+/// the lifetime of the TriangleVertexArray.
+/**
+ * @param nbVertices Number of vertices in the array
+ * @param verticesStart Pointer to the first vertices of the array
+ * @param verticesStride Number of bytes between the beginning of two consecutive vertices
+ * @param nbTriangles Number of triangles in the array
+ * @param indexesStart Pointer to the first triangle index
+ * @param indexesStride Number of bytes between the beginning of two consecutive triangle indices
+ * @param vertexDataType Type of data for the vertices (float, double)
+ * @param indexDataType Type of data for the indices (short, int)
+ */
+TriangleVertexArray::TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
+                                         uint nbTriangles, void* indexesStart, int indexesStride,
+                                         VertexDataType vertexDataType, IndexDataType indexDataType) {
+    mNbVertices = nbVertices;
+    mVerticesStart = reinterpret_cast<unsigned char*>(verticesStart);
+    mVerticesStride = verticesStride;
+    mNbTriangles = nbTriangles;
+    mIndicesStart = reinterpret_cast<unsigned char*>(indexesStart);
+    mIndicesStride = indexesStride;
+    mVertexDataType = vertexDataType;
+    mIndexDataType = indexDataType;
+}
+
+// Destructor
+TriangleVertexArray::~TriangleVertexArray() {
+
+}

src/collision/TriangleVertexArray.h

@@ -0,0 +1,160 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_TRIANGLE_VERTEX_ARRAY_H
+#define REACTPHYSICS3D_TRIANGLE_VERTEX_ARRAY_H
+
+// Libraries
+#include "configuration.h"
+
+namespace reactphysics3d {
+
+// Class TriangleVertexArray
+/**
+ * This class is used to describe the vertices and faces of a triangular mesh.
+ * A TriangleVertexArray represents a continuous array of vertices and indexes
+ * of a triangular mesh. When you create a TriangleVertexArray, no data is copied
+ * into the array. It only stores pointer to the data. The purpose is to allow
+ * the user to share vertices data between the physics engine and the rendering
+ * part. Therefore, make sure that the data pointed by a TriangleVertexArray
+ * remains valid during the TriangleVertexArray life.
+ */
+class TriangleVertexArray {
+
+    public:
+
+        /// Data type for the vertices in the array
+        enum VertexDataType {VERTEX_FLOAT_TYPE, VERTEX_DOUBLE_TYPE};
+
+        /// Data type for the indices in the array
+        enum IndexDataType {INDEX_INTEGER_TYPE, INDEX_SHORT_TYPE};
+
+    protected:
+
+        /// Number of vertices in the array
+        uint mNbVertices;
+
+        /// Pointer to the first vertex value in the array
+        unsigned char* mVerticesStart;
+
+        /// Stride (number of bytes) between the beginning of two vertices
+        /// values in the array
+        int mVerticesStride;
+
+        /// Number of triangles in the array
+        uint mNbTriangles;
+
+        /// Pointer to the first vertex index of the array
+        unsigned char* mIndicesStart;
+
+        /// Stride (number of bytes) between the beginning of two indices in
+        /// the array
+        int mIndicesStride;
+
+        /// Data type of the vertices in the array
+        VertexDataType mVertexDataType;
+
+        /// Data type of the indices in the array
+        IndexDataType mIndexDataType;
+
+    public:
+
+        /// Constructor
+        TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
+                            uint nbTriangles, void* indexesStart, int indexesStride,
+                            VertexDataType vertexDataType, IndexDataType indexDataType);
+
+        /// Destructor
+        ~TriangleVertexArray();
+
+        /// Return the vertex data type
+        VertexDataType getVertexDataType() const;
+
+        /// Return the index data type
+        IndexDataType getIndexDataType() const;
+
+        /// Return the number of vertices
+        uint getNbVertices() const;
+
+        /// Return the number of triangles
+        uint getNbTriangles() const;
+
+        /// Return the vertices stride (number of bytes)
+        int getVerticesStride() const;
+
+        /// Return the indices stride (number of bytes)
+        int getIndicesStride() const;
+
+        /// Return the pointer to the start of the vertices array
+        unsigned char* getVerticesStart() const;
+
+        /// Return the pointer to the start of the indices array
+        unsigned char* getIndicesStart() const;
+};
+
+// Return the vertex data type
+inline TriangleVertexArray::VertexDataType TriangleVertexArray::getVertexDataType() const {
+    return mVertexDataType;
+}
+
+// Return the index data type
+inline TriangleVertexArray::IndexDataType TriangleVertexArray::getIndexDataType() const {
+   return mIndexDataType;
+}
+
+// Return the number of vertices
+inline uint TriangleVertexArray::getNbVertices() const {
+    return mNbVertices;
+}
+
+// Return the number of triangles
+inline uint TriangleVertexArray::getNbTriangles() const {
+    return mNbTriangles;
+}
+
+// Return the vertices stride (number of bytes)
+inline int TriangleVertexArray::getVerticesStride() const {
+    return mVerticesStride;
+}
+
+// Return the indices stride (number of bytes)
+inline int TriangleVertexArray::getIndicesStride() const {
+    return mIndicesStride;
+}
+
+// Return the pointer to the start of the vertices array
+inline unsigned char* TriangleVertexArray::getVerticesStart() const {
+    return mVerticesStart;
+}
+
+// Return the pointer to the start of the indices array
+inline unsigned char* TriangleVertexArray::getIndicesStart() const {
+    return mIndicesStart;
+}
+
+}
+
+#endif
+

src/collision/broadphase/BroadPhaseAlgorithm.cpp

@@ -0,0 +1,292 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "BroadPhaseAlgorithm.h"
+#include "collision/CollisionDetection.h"
+#include "engine/Profiler.h"
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Constructor
+BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
+                    :mDynamicAABBTree(DYNAMIC_TREE_AABB_GAP), mNbMovedShapes(0), mNbAllocatedMovedShapes(8),
+                     mNbNonUsedMovedShapes(0), mNbPotentialPairs(0), mNbAllocatedPotentialPairs(8),
+                     mCollisionDetection(collisionDetection) {
+
+    // Allocate memory for the array of non-static proxy shapes IDs
+    mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
+    assert(mMovedShapes != NULL);
+
+    // Allocate memory for the array of potential overlapping pairs
+    mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
+    assert(mPotentialPairs != NULL);
+}
+
+// Destructor
+BroadPhaseAlgorithm::~BroadPhaseAlgorithm() {
+
+    // Release the memory for the array of non-static proxy shapes IDs
+    free(mMovedShapes);
+
+    // Release the memory for the array of potential overlapping pairs
+    free(mPotentialPairs);
+}
+
+// Add a collision shape in the array of shapes that have moved in the last simulation step
+// and that need to be tested again for broad-phase overlapping.
+void BroadPhaseAlgorithm::addMovedCollisionShape(int broadPhaseID) {
+
+    // Allocate more elements in the array of shapes that have moved if necessary
+    if (mNbAllocatedMovedShapes == mNbMovedShapes) {
+        mNbAllocatedMovedShapes *= 2;
+        int* oldArray = mMovedShapes;
+        mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
+        assert(mMovedShapes != NULL);
+        memcpy(mMovedShapes, oldArray, mNbMovedShapes * sizeof(int));
+        free(oldArray);
+    }
+
+    // Store the broad-phase ID into the array of shapes that have moved
+    assert(mNbMovedShapes < mNbAllocatedMovedShapes);
+    assert(mMovedShapes != NULL);
+    mMovedShapes[mNbMovedShapes] = broadPhaseID;
+    mNbMovedShapes++;
+}
+
+// Remove a collision shape from the array of shapes that have moved in the last simulation step
+// and that need to be tested again for broad-phase overlapping.
+void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
+
+    assert(mNbNonUsedMovedShapes <= mNbMovedShapes);
+
+    // If less than the quarter of allocated elements of the non-static shapes IDs array
+    // are used, we release some allocated memory
+    if ((mNbMovedShapes - mNbNonUsedMovedShapes) < mNbAllocatedMovedShapes / 4 &&
+            mNbAllocatedMovedShapes > 8) {
+
+        mNbAllocatedMovedShapes /= 2;
+        int* oldArray = mMovedShapes;
+        mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
+        assert(mMovedShapes != NULL);
+        uint nbElements = 0;
+        for (uint i=0; i<mNbMovedShapes; i++) {
+            if (oldArray[i] != -1) {
+                mMovedShapes[nbElements] = oldArray[i];
+                nbElements++;
+            }
+        }
+        mNbMovedShapes = nbElements;
+        mNbNonUsedMovedShapes = 0;
+        free(oldArray);
+    }
+
+    // Remove the broad-phase ID from the array
+    for (uint i=0; i<mNbMovedShapes; i++) {
+        if (mMovedShapes[i] == broadPhaseID) {
+            mMovedShapes[i] = -1;
+            mNbNonUsedMovedShapes++;
+            break;
+        }
+    }
+}
+
+// Add a proxy collision shape into the broad-phase collision detection
+void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) {
+
+    // Add the collision shape into the dynamic AABB tree and get its broad-phase ID
+    int nodeId = mDynamicAABBTree.addObject(aabb, proxyShape);
+
+    // Set the broad-phase ID of the proxy shape
+    proxyShape->mBroadPhaseID = nodeId;
+
+    // Add the collision shape into the array of bodies that have moved (or have been created)
+    // during the last simulation step
+    addMovedCollisionShape(proxyShape->mBroadPhaseID);
+}
+
+// Remove a proxy collision shape from the broad-phase collision detection
+void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) {
+
+    int broadPhaseID = proxyShape->mBroadPhaseID;
+
+    // Remove the collision shape from the dynamic AABB tree
+    mDynamicAABBTree.removeObject(broadPhaseID);
+
+    // Remove the collision shape into the array of shapes that have moved (or have been created)
+    // during the last simulation step
+    removeMovedCollisionShape(broadPhaseID);
+}
+
+// Notify the broad-phase that a collision shape has moved and need to be updated
+void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb,
+                                                    const Vector3& displacement, bool forceReinsert) {
+
+    int broadPhaseID = proxyShape->mBroadPhaseID;
+
+    assert(broadPhaseID >= 0);
+
+    // Update the dynamic AABB tree according to the movement of the collision shape
+    bool hasBeenReInserted = mDynamicAABBTree.updateObject(broadPhaseID, aabb, displacement, forceReinsert);
+
+    // If the collision shape has moved out of its fat AABB (and therefore has been reinserted
+    // into the tree).
+    if (hasBeenReInserted) {
+
+        // Add the collision shape into the array of shapes that have moved (or have been created)
+        // during the last simulation step
+        addMovedCollisionShape(broadPhaseID);
+    }
+}
+
+// Compute all the overlapping pairs of collision shapes
+void BroadPhaseAlgorithm::computeOverlappingPairs() {
+
+    // Reset the potential overlapping pairs
+    mNbPotentialPairs = 0;
+
+    // For all collision shapes that have moved (or have been created) during the
+    // last simulation step
+    for (uint i=0; i<mNbMovedShapes; i++) {
+        int shapeID = mMovedShapes[i];
+
+        if (shapeID == -1) continue;
+
+        AABBOverlapCallback callback(*this, shapeID);
+
+        // Get the AABB of the shape
+        const AABB& shapeAABB = mDynamicAABBTree.getFatAABB(shapeID);
+
+        // Ask the dynamic AABB tree to report all collision shapes that overlap with
+        // this AABB. The method BroadPhase::notifiyOverlappingPair() will be called
+        // by the dynamic AABB tree for each potential overlapping pair.
+        mDynamicAABBTree.reportAllShapesOverlappingWithAABB(shapeAABB, callback);
+    }
+
+    // Reset the array of collision shapes that have move (or have been created) during the
+    // last simulation step
+    mNbMovedShapes = 0;
+
+    // Sort the array of potential overlapping pairs in order to remove duplicate pairs
+    std::sort(mPotentialPairs, mPotentialPairs + mNbPotentialPairs, BroadPhasePair::smallerThan);
+
+    // Check all the potential overlapping pairs avoiding duplicates to report unique
+    // overlapping pairs
+    uint i=0;
+    while (i < mNbPotentialPairs) {
+
+        // Get a potential overlapping pair
+        BroadPhasePair* pair = mPotentialPairs + i;
+        i++;
+
+        assert(pair->collisionShape1ID != pair->collisionShape2ID);
+
+        // Get the two collision shapes of the pair
+        ProxyShape* shape1 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(pair->collisionShape1ID));
+        ProxyShape* shape2 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(pair->collisionShape2ID));
+
+        // Notify the collision detection about the overlapping pair
+        mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2);
+
+        // Skip the duplicate overlapping pairs
+        while (i < mNbPotentialPairs) {
+
+            // Get the next pair
+            BroadPhasePair* nextPair = mPotentialPairs + i;
+
+            // If the next pair is different from the previous one, we stop skipping pairs
+            if (nextPair->collisionShape1ID != pair->collisionShape1ID ||
+                nextPair->collisionShape2ID != pair->collisionShape2ID) {
+                break;
+            }
+            i++;
+        }
+    }
+
+    // If the number of potential overlapping pairs is less than the quarter of allocated
+    // number of overlapping pairs
+    if (mNbPotentialPairs < mNbAllocatedPotentialPairs / 4 && mNbPotentialPairs > 8) {
+
+        // Reduce the number of allocated potential overlapping pairs
+        BroadPhasePair* oldPairs = mPotentialPairs;
+        mNbAllocatedPotentialPairs /= 2;
+        mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
+        assert(mPotentialPairs);
+        memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair));
+        free(oldPairs);
+    }
+}
+
+// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
+void BroadPhaseAlgorithm::notifyOverlappingNodes(int node1ID, int node2ID) {
+
+    // If both the nodes are the same, we do not create store the overlapping pair
+    if (node1ID == node2ID) return;
+
+    // If we need to allocate more memory for the array of potential overlapping pairs
+    if (mNbPotentialPairs == mNbAllocatedPotentialPairs) {
+
+        // Allocate more memory for the array of potential pairs
+        BroadPhasePair* oldPairs = mPotentialPairs;
+        mNbAllocatedPotentialPairs *= 2;
+        mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
+        assert(mPotentialPairs);
+        memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair));
+        free(oldPairs);
+    }
+
+    // Add the new potential pair into the array of potential overlapping pairs
+    mPotentialPairs[mNbPotentialPairs].collisionShape1ID = std::min(node1ID, node2ID);
+    mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(node1ID, node2ID);
+    mNbPotentialPairs++;
+}
+
+// Called when a overlapping node has been found during the call to
+// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
+void AABBOverlapCallback::notifyOverlappingNode(int nodeId) {
+
+    mBroadPhaseAlgorithm.notifyOverlappingNodes(mReferenceNodeId, nodeId);
+}
+
+// Called for a broad-phase shape that has to be tested for raycast
+decimal BroadPhaseRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) {
+
+    decimal hitFraction = decimal(-1.0);
+
+    // Get the proxy shape from the node
+    ProxyShape* proxyShape = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(nodeId));
+
+    // Check if the raycast filtering mask allows raycast against this shape
+    if ((mRaycastWithCategoryMaskBits & proxyShape->getCollisionCategoryBits()) != 0) {
+
+        // Ask the collision detection to perform a ray cast test against
+        // the proxy shape of this node because the ray is overlapping
+        // with the shape in the broad-phase
+        hitFraction = mRaycastTest.raycastAgainstShape(proxyShape, ray);
+    }
+
+    return hitFraction;
+}

src/collision/broadphase/BroadPhaseAlgorithm.h

@@ -0,0 +1,248 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_BROAD_PHASE_ALGORITHM_H
+#define REACTPHYSICS3D_BROAD_PHASE_ALGORITHM_H
+
+// Libraries
+#include <vector>
+#include "body/CollisionBody.h"
+#include "collision/ProxyShape.h"
+#include "DynamicAABBTree.h"
+#include "engine/Profiler.h"
+
+/// Namespace ReactPhysics3D
+namespace reactphysics3d {
+
+// Declarations
+class CollisionDetection;
+class BroadPhaseAlgorithm;
+
+// Structure BroadPhasePair
+/**
+ * This structure represent a potential overlapping pair during the
+ * broad-phase collision detection.
+ */
+struct BroadPhasePair {
+
+    // -------------------- Attributes -------------------- //
+
+    /// Broad-phase ID of the first collision shape
+    int collisionShape1ID;
+
+    /// Broad-phase ID of the second collision shape
+    int collisionShape2ID;
+
+    // -------------------- Methods -------------------- //
+
+    /// Method used to compare two pairs for sorting algorithm
+    static bool smallerThan(const BroadPhasePair& pair1, const BroadPhasePair& pair2);
+};
+
+// class AABBOverlapCallback
+class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback {
+
+    private:
+
+        BroadPhaseAlgorithm& mBroadPhaseAlgorithm;
+
+        int mReferenceNodeId;
+
+    public:
+
+        // Constructor
+        AABBOverlapCallback(BroadPhaseAlgorithm& broadPhaseAlgo, int referenceNodeId)
+             : mBroadPhaseAlgorithm(broadPhaseAlgo), mReferenceNodeId(referenceNodeId) {
+
+        }
+
+        // Called when a overlapping node has been found during the call to
+        // DynamicAABBTree:reportAllShapesOverlappingWithAABB()
+        virtual void notifyOverlappingNode(int nodeId);
+
+};
+
+// Class BroadPhaseRaycastCallback
+/**
+ * Callback called when the AABB of a leaf node is hit by a ray the
+ * broad-phase Dynamic AABB Tree.
+ */
+class BroadPhaseRaycastCallback : public DynamicAABBTreeRaycastCallback {
+
+    private :
+
+        const DynamicAABBTree& mDynamicAABBTree;
+
+        unsigned short mRaycastWithCategoryMaskBits;
+
+        RaycastTest& mRaycastTest;
+
+    public:
+
+        // Constructor
+        BroadPhaseRaycastCallback(const DynamicAABBTree& dynamicAABBTree, unsigned short raycastWithCategoryMaskBits,
+                                  RaycastTest& raycastTest)
+            : mDynamicAABBTree(dynamicAABBTree), mRaycastWithCategoryMaskBits(raycastWithCategoryMaskBits),
+              mRaycastTest(raycastTest) {
+
+        }
+
+        // Called for a broad-phase shape that has to be tested for raycast
+        virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray);
+
+};
+
+// Class BroadPhaseAlgorithm
+/**
+ * This class represents the broad-phase collision detection. The
+ * goal of the broad-phase collision detection is to compute the pairs of proxy shapes
+ * that have their AABBs overlapping. Only those pairs of bodies will be tested
+ * later for collision during the narrow-phase collision detection. A dynamic AABB
+ * tree data structure is used for fast broad-phase collision detection.
+ */
+class BroadPhaseAlgorithm {
+
+    protected :
+
+        // -------------------- Attributes -------------------- //
+
+        /// Dynamic AABB tree
+        DynamicAABBTree mDynamicAABBTree;
+
+        /// Array with the broad-phase IDs of all collision shapes that have moved (or have been
+        /// created) during the last simulation step. Those are the shapes that need to be tested
+        /// for overlapping in the next simulation step.
+        int* mMovedShapes;
+
+        /// Number of collision shapes in the array of shapes that have moved during the last
+        /// simulation step.
+        uint mNbMovedShapes;
+
+        /// Number of allocated elements for the array of shapes that have moved during the last
+        /// simulation step.
+        uint mNbAllocatedMovedShapes;
+
+        /// Number of non-used elements in the array of shapes that have moved during the last
+        /// simulation step.
+        uint mNbNonUsedMovedShapes;
+
+        /// Temporary array of potential overlapping pairs (with potential duplicates)
+        BroadPhasePair* mPotentialPairs;
+
+        /// Number of potential overlapping pairs
+        uint mNbPotentialPairs;
+
+        /// Number of allocated elements for the array of potential overlapping pairs
+        uint mNbAllocatedPotentialPairs;
+
+        /// Reference to the collision detection object
+        CollisionDetection& mCollisionDetection;
+        
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        BroadPhaseAlgorithm(const BroadPhaseAlgorithm& algorithm);
+
+        /// Private assignment operator
+        BroadPhaseAlgorithm& operator=(const BroadPhaseAlgorithm& algorithm);
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        BroadPhaseAlgorithm(CollisionDetection& collisionDetection);
+
+        /// Destructor
+        ~BroadPhaseAlgorithm();
+        
+        /// Add a proxy collision shape into the broad-phase collision detection
+        void addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb);
+
+        /// Remove a proxy collision shape from the broad-phase collision detection
+        void removeProxyCollisionShape(ProxyShape* proxyShape);
+
+        /// Notify the broad-phase that a collision shape has moved and need to be updated
+        void updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb,
+                                       const Vector3& displacement, bool forceReinsert = false);
+
+        /// Add a collision shape in the array of shapes that have moved in the last simulation step
+        /// and that need to be tested again for broad-phase overlapping.
+        void addMovedCollisionShape(int broadPhaseID);
+
+        /// Remove a collision shape from the array of shapes that have moved in the last simulation
+        /// step and that need to be tested again for broad-phase overlapping.
+        void removeMovedCollisionShape(int broadPhaseID);
+
+        /// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
+        void notifyOverlappingNodes(int broadPhaseId1, int broadPhaseId2);
+
+        /// Compute all the overlapping pairs of collision shapes
+        void computeOverlappingPairs();
+
+        /// Return true if the two broad-phase collision shapes are overlapping
+        bool testOverlappingShapes(const ProxyShape* shape1, const ProxyShape* shape2) const;
+
+        /// Ray casting method
+        void raycast(const Ray& ray, RaycastTest& raycastTest,
+                     unsigned short raycastWithCategoryMaskBits) const;
+};
+
+// Method used to compare two pairs for sorting algorithm
+inline bool BroadPhasePair::smallerThan(const BroadPhasePair& pair1, const BroadPhasePair& pair2) {
+
+    if (pair1.collisionShape1ID < pair2.collisionShape1ID) return true;
+    if (pair1.collisionShape1ID == pair2.collisionShape1ID) {
+        return pair1.collisionShape2ID < pair2.collisionShape2ID;
+    }
+    return false;
+}
+
+// Return true if the two broad-phase collision shapes are overlapping
+inline bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* shape1,
+                                                       const ProxyShape* shape2) const {
+    // Get the two AABBs of the collision shapes
+    const AABB& aabb1 = mDynamicAABBTree.getFatAABB(shape1->mBroadPhaseID);
+    const AABB& aabb2 = mDynamicAABBTree.getFatAABB(shape2->mBroadPhaseID);
+
+    // Check if the two AABBs are overlapping
+    return aabb1.testCollision(aabb2);
+}
+
+// Ray casting method
+inline void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTest,
+                                         unsigned short raycastWithCategoryMaskBits) const {
+
+    PROFILE("BroadPhaseAlgorithm::raycast()");
+
+    BroadPhaseRaycastCallback broadPhaseRaycastCallback(mDynamicAABBTree, raycastWithCategoryMaskBits, raycastTest);
+
+    mDynamicAABBTree.raycast(ray, broadPhaseRaycastCallback);
+}
+
+}
+
+#endif
+

src/collision/broadphase/DynamicAABBTree.cpp

@@ -0,0 +1,792 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "DynamicAABBTree.h"
+#include "BroadPhaseAlgorithm.h"
+#include "memory/Stack.h"
+#include "engine/Profiler.h"
+
+using namespace reactphysics3d;
+
+// Initialization of static variables
+const int TreeNode::NULL_TREE_NODE = -1;
+
+// Constructor
+DynamicAABBTree::DynamicAABBTree(decimal extraAABBGap) : mExtraAABBGap(extraAABBGap) {
+
+    init();
+}
+
+// Destructor
+DynamicAABBTree::~DynamicAABBTree() {
+
+    // Free the allocated memory for the nodes
+    free(mNodes);
+}
+
+// Initialize the tree
+void DynamicAABBTree::init() {
+
+    mRootNodeID = TreeNode::NULL_TREE_NODE;
+    mNbNodes = 0;
+    mNbAllocatedNodes = 8;
+
+    // Allocate memory for the nodes of the tree
+    mNodes = (TreeNode*) malloc(mNbAllocatedNodes * sizeof(TreeNode));
+    assert(mNodes);
+    memset(mNodes, 0, mNbAllocatedNodes * sizeof(TreeNode));
+
+    // Initialize the allocated nodes
+    for (int i=0; i<mNbAllocatedNodes - 1; i++) {
+        mNodes[i].nextNodeID = i + 1;
+        mNodes[i].height = -1;
+    }
+    mNodes[mNbAllocatedNodes - 1].nextNodeID = TreeNode::NULL_TREE_NODE;
+    mNodes[mNbAllocatedNodes - 1].height = -1;
+    mFreeNodeID = 0;
+}
+
+// Clear all the nodes and reset the tree
+void DynamicAABBTree::reset() {
+
+    // Free the allocated memory for the nodes
+    free(mNodes);
+
+    // Initialize the tree
+    init();
+}
+
+// Allocate and return a new node in the tree
+int DynamicAABBTree::allocateNode() {
+
+    // If there is no more allocated node to use
+    if (mFreeNodeID == TreeNode::NULL_TREE_NODE) {
+
+        assert(mNbNodes == mNbAllocatedNodes);
+
+        // Allocate more nodes in the tree
+        mNbAllocatedNodes *= 2;
+        TreeNode* oldNodes = mNodes;
+        mNodes = (TreeNode*) malloc(mNbAllocatedNodes * sizeof(TreeNode));
+        assert(mNodes);
+        memcpy(mNodes, oldNodes, mNbNodes * sizeof(TreeNode));
+        free(oldNodes);
+
+        // Initialize the allocated nodes
+        for (int i=mNbNodes; i<mNbAllocatedNodes - 1; i++) {
+            mNodes[i].nextNodeID = i + 1;
+            mNodes[i].height = -1;
+        }
+        mNodes[mNbAllocatedNodes - 1].nextNodeID = TreeNode::NULL_TREE_NODE;
+        mNodes[mNbAllocatedNodes - 1].height = -1;
+        mFreeNodeID = mNbNodes;
+    }
+
+    // Get the next free node
+    int freeNodeID = mFreeNodeID;
+    mFreeNodeID = mNodes[freeNodeID].nextNodeID;
+    mNodes[freeNodeID].parentID = TreeNode::NULL_TREE_NODE;
+    mNodes[freeNodeID].height = 0;
+    mNbNodes++;
+
+    return freeNodeID;
+}
+
+// Release a node
+void DynamicAABBTree::releaseNode(int nodeID) {
+
+    assert(mNbNodes > 0);
+    assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
+    assert(mNodes[nodeID].height >= 0);
+    mNodes[nodeID].nextNodeID = mFreeNodeID;
+    mNodes[nodeID].height = -1;
+    mFreeNodeID = nodeID;
+    mNbNodes--;
+}
+
+// Internally add an object into the tree
+int DynamicAABBTree::addObjectInternal(const AABB& aabb) {
+
+    // Get the next available node (or allocate new ones if necessary)
+    int nodeID = allocateNode();
+
+    // Create the fat aabb to use in the tree
+    const Vector3 gap(mExtraAABBGap, mExtraAABBGap, mExtraAABBGap);
+    mNodes[nodeID].aabb.setMin(aabb.getMin() - gap);
+    mNodes[nodeID].aabb.setMax(aabb.getMax() + gap);
+
+    // Set the height of the node in the tree
+    mNodes[nodeID].height = 0;
+
+    // Insert the new leaf node in the tree
+    insertLeafNode(nodeID);
+    assert(mNodes[nodeID].isLeaf());
+
+    assert(nodeID >= 0);
+
+    // Return the Id of the node
+    return nodeID;
+}
+
+// Remove an object from the tree
+void DynamicAABBTree::removeObject(int nodeID) {
+
+    assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
+    assert(mNodes[nodeID].isLeaf());
+
+    // Remove the node from the tree
+    removeLeafNode(nodeID);
+    releaseNode(nodeID);
+}
+
+// Update the dynamic tree after an object has moved.
+/// If the new AABB of the object that has moved is still inside its fat AABB, then
+/// nothing is done. Otherwise, the corresponding node is removed and reinserted into the tree.
+/// The method returns true if the object has been reinserted into the tree. The "displacement"
+/// argument is the linear velocity of the AABB multiplied by the elapsed time between two
+/// frames. If the "forceReinsert" parameter is true, we force a removal and reinsertion of the node
+/// (this can be useful if the shape AABB has become much smaller than the previous one for instance).
+bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector3& displacement, bool forceReinsert) {
+
+    PROFILE("DynamicAABBTree::updateObject()");
+
+    assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
+    assert(mNodes[nodeID].isLeaf());
+    assert(mNodes[nodeID].height >= 0);
+
+    // If the new AABB is still inside the fat AABB of the node
+    if (!forceReinsert && mNodes[nodeID].aabb.contains(newAABB)) {
+        return false;
+    }
+
+    // If the new AABB is outside the fat AABB, we remove the corresponding node
+    removeLeafNode(nodeID);
+
+    // Compute the fat AABB by inflating the AABB with a constant gap
+    mNodes[nodeID].aabb = newAABB;
+    const Vector3 gap(mExtraAABBGap, mExtraAABBGap, mExtraAABBGap);
+    mNodes[nodeID].aabb.mMinCoordinates -= gap;
+    mNodes[nodeID].aabb.mMaxCoordinates += gap;
+
+    // Inflate the fat AABB in direction of the linear motion of the AABB
+    if (displacement.x < decimal(0.0)) {
+      mNodes[nodeID].aabb.mMinCoordinates.x += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x;
+    }
+    else {
+      mNodes[nodeID].aabb.mMaxCoordinates.x += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x;
+    }
+    if (displacement.y < decimal(0.0)) {
+      mNodes[nodeID].aabb.mMinCoordinates.y += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y;
+    }
+    else {
+      mNodes[nodeID].aabb.mMaxCoordinates.y += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y;
+    }
+    if (displacement.z < decimal(0.0)) {
+      mNodes[nodeID].aabb.mMinCoordinates.z += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z;
+    }
+    else {
+      mNodes[nodeID].aabb.mMaxCoordinates.z += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z;
+    }
+
+    assert(mNodes[nodeID].aabb.contains(newAABB));
+
+    // Reinsert the node into the tree
+    insertLeafNode(nodeID);
+
+    return true;
+}
+
+// Insert a leaf node in the tree. The process of inserting a new leaf node
+// in the dynamic tree is described in the book "Introduction to Game Physics
+// with Box2D" by Ian Parberry.
+void DynamicAABBTree::insertLeafNode(int nodeID) {
+
+    // If the tree is empty
+    if (mRootNodeID == TreeNode::NULL_TREE_NODE) {
+        mRootNodeID = nodeID;
+        mNodes[mRootNodeID].parentID = TreeNode::NULL_TREE_NODE;
+        return;
+    }
+
+    assert(mRootNodeID != TreeNode::NULL_TREE_NODE);
+
+    // Find the best sibling node for the new node
+    AABB newNodeAABB = mNodes[nodeID].aabb;
+    int currentNodeID = mRootNodeID;
+    while (!mNodes[currentNodeID].isLeaf()) {
+
+        int leftChild = mNodes[currentNodeID].children[0];
+        int rightChild = mNodes[currentNodeID].children[1];
+
+        // Compute the merged AABB
+        decimal volumeAABB = mNodes[currentNodeID].aabb.getVolume();
+        AABB mergedAABBs;
+        mergedAABBs.mergeTwoAABBs(mNodes[currentNodeID].aabb, newNodeAABB);
+        decimal mergedVolume = mergedAABBs.getVolume();
+
+        // Compute the cost of making the current node the sibbling of the new node
+        decimal costS = decimal(2.0) * mergedVolume;
+
+        // Compute the minimum cost of pushing the new node further down the tree (inheritance cost)
+        decimal costI = decimal(2.0) * (mergedVolume - volumeAABB);
+
+        // Compute the cost of descending into the left child
+        decimal costLeft;
+        AABB currentAndLeftAABB;
+        currentAndLeftAABB.mergeTwoAABBs(newNodeAABB, mNodes[leftChild].aabb);
+        if (mNodes[leftChild].isLeaf()) {   // If the left child is a leaf
+            costLeft = currentAndLeftAABB.getVolume() + costI;
+        }
+        else {
+            decimal leftChildVolume = mNodes[leftChild].aabb.getVolume();
+            costLeft = costI + currentAndLeftAABB.getVolume() - leftChildVolume;
+        }
+
+        // Compute the cost of descending into the right child
+        decimal costRight;
+        AABB currentAndRightAABB;
+        currentAndRightAABB.mergeTwoAABBs(newNodeAABB, mNodes[rightChild].aabb);
+        if (mNodes[rightChild].isLeaf()) {   // If the right child is a leaf
+            costRight = currentAndRightAABB.getVolume() + costI;
+        }
+        else {
+            decimal rightChildVolume = mNodes[rightChild].aabb.getVolume();
+            costRight = costI + currentAndRightAABB.getVolume() - rightChildVolume;
+        }
+
+        // If the cost of making the current node a sibbling of the new node is smaller than
+        // the cost of going down into the left or right child
+        if (costS < costLeft && costS < costRight) break;
+
+        // It is cheaper to go down into a child of the current node, choose the best child
+        if (costLeft < costRight) {
+            currentNodeID = leftChild;
+        }
+        else {
+            currentNodeID = rightChild;
+        }
+    }
+
+    int siblingNode = currentNodeID;
+
+    // Create a new parent for the new node and the sibling node
+    int oldParentNode = mNodes[siblingNode].parentID;
+    int newParentNode = allocateNode();
+    mNodes[newParentNode].parentID = oldParentNode;
+    mNodes[newParentNode].aabb.mergeTwoAABBs(mNodes[siblingNode].aabb, newNodeAABB);
+    mNodes[newParentNode].height = mNodes[siblingNode].height + 1;
+    assert(mNodes[newParentNode].height > 0);
+
+    // If the sibling node was not the root node
+    if (oldParentNode != TreeNode::NULL_TREE_NODE) {
+        assert(!mNodes[oldParentNode].isLeaf());
+        if (mNodes[oldParentNode].children[0] == siblingNode) {
+            mNodes[oldParentNode].children[0] = newParentNode;
+        }
+        else {
+            mNodes[oldParentNode].children[1] = newParentNode;
+        }
+        mNodes[newParentNode].children[0] = siblingNode;
+        mNodes[newParentNode].children[1] = nodeID;
+        mNodes[siblingNode].parentID = newParentNode;
+        mNodes[nodeID].parentID = newParentNode;
+    }
+    else {  // If the sibling node was the root node
+        mNodes[newParentNode].children[0] = siblingNode;
+        mNodes[newParentNode].children[1] = nodeID;
+        mNodes[siblingNode].parentID = newParentNode;
+        mNodes[nodeID].parentID = newParentNode;
+        mRootNodeID = newParentNode;
+    }
+
+    // Move up in the tree to change the AABBs that have changed
+    currentNodeID = mNodes[nodeID].parentID;
+    assert(!mNodes[currentNodeID].isLeaf());
+    while (currentNodeID != TreeNode::NULL_TREE_NODE) {
+
+        // Balance the sub-tree of the current node if it is not balanced
+        currentNodeID = balanceSubTreeAtNode(currentNodeID);
+        assert(mNodes[nodeID].isLeaf());
+
+        assert(!mNodes[currentNodeID].isLeaf());
+        int leftChild = mNodes[currentNodeID].children[0];
+        int rightChild = mNodes[currentNodeID].children[1];
+        assert(leftChild != TreeNode::NULL_TREE_NODE);
+        assert(rightChild != TreeNode::NULL_TREE_NODE);
+
+        // Recompute the height of the node in the tree
+        mNodes[currentNodeID].height = std::max(mNodes[leftChild].height,
+                                                mNodes[rightChild].height) + 1;
+        assert(mNodes[currentNodeID].height > 0);
+
+        // Recompute the AABB of the node
+        mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChild].aabb, mNodes[rightChild].aabb);
+
+        currentNodeID = mNodes[currentNodeID].parentID;
+    }
+
+    assert(mNodes[nodeID].isLeaf());
+}
+
+// Remove a leaf node from the tree
+void DynamicAABBTree::removeLeafNode(int nodeID) {
+
+    assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
+    assert(mNodes[nodeID].isLeaf());
+
+    // If we are removing the root node (root node is a leaf in this case)
+    if (mRootNodeID == nodeID) {
+        mRootNodeID = TreeNode::NULL_TREE_NODE;
+        return;
+    }
+
+    int parentNodeID = mNodes[nodeID].parentID;
+    int grandParentNodeID = mNodes[parentNodeID].parentID;
+    int siblingNodeID;
+    if (mNodes[parentNodeID].children[0] == nodeID) {
+        siblingNodeID = mNodes[parentNodeID].children[1];
+    }
+    else {
+        siblingNodeID = mNodes[parentNodeID].children[0];
+    }
+
+    // If the parent of the node to remove is not the root node
+    if (grandParentNodeID != TreeNode::NULL_TREE_NODE) {
+
+        // Destroy the parent node
+        if (mNodes[grandParentNodeID].children[0] == parentNodeID) {
+            mNodes[grandParentNodeID].children[0] = siblingNodeID;
+        }
+        else {
+            assert(mNodes[grandParentNodeID].children[1] == parentNodeID);
+            mNodes[grandParentNodeID].children[1] = siblingNodeID;
+        }
+        mNodes[siblingNodeID].parentID = grandParentNodeID;
+        releaseNode(parentNodeID);
+
+        // Now, we need to recompute the AABBs of the node on the path back to the root
+        // and make sure that the tree is still balanced
+        int currentNodeID = grandParentNodeID;
+        while(currentNodeID != TreeNode::NULL_TREE_NODE) {
+
+            // Balance the current sub-tree if necessary
+            currentNodeID = balanceSubTreeAtNode(currentNodeID);
+
+            assert(!mNodes[currentNodeID].isLeaf());
+
+            // Get the two children of the current node
+            int leftChildID = mNodes[currentNodeID].children[0];
+            int rightChildID = mNodes[currentNodeID].children[1];
+
+            // Recompute the AABB and the height of the current node
+            mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChildID].aabb,
+                                                     mNodes[rightChildID].aabb);
+            mNodes[currentNodeID].height = std::max(mNodes[leftChildID].height,
+                                                    mNodes[rightChildID].height) + 1;
+            assert(mNodes[currentNodeID].height > 0);
+
+            currentNodeID = mNodes[currentNodeID].parentID;
+        }
+    }
+    else { // If the parent of the node to remove is the root node
+
+        // The sibling node becomes the new root node
+        mRootNodeID = siblingNodeID;
+        mNodes[siblingNodeID].parentID = TreeNode::NULL_TREE_NODE;
+        releaseNode(parentNodeID);
+    }
+}
+
+// Balance the sub-tree of a given node using left or right rotations.
+/// The rotation schemes are described in the book "Introduction to Game Physics
+/// with Box2D" by Ian Parberry. This method returns the new root node ID.
+int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
+
+    assert(nodeID != TreeNode::NULL_TREE_NODE);
+
+    TreeNode* nodeA = mNodes + nodeID;
+
+    // If the node is a leaf or the height of A's sub-tree is less than 2
+    if (nodeA->isLeaf() || nodeA->height < 2) {
+
+        // Do not perform any rotation
+        return nodeID;
+    }
+
+    // Get the two children nodes
+    int nodeBID = nodeA->children[0];
+    int nodeCID = nodeA->children[1];
+    assert(nodeBID >= 0 && nodeBID < mNbAllocatedNodes);
+    assert(nodeCID >= 0 && nodeCID < mNbAllocatedNodes);
+    TreeNode* nodeB = mNodes + nodeBID;
+    TreeNode* nodeC = mNodes + nodeCID;
+
+    // Compute the factor of the left and right sub-trees
+    int balanceFactor = nodeC->height - nodeB->height;
+
+    // If the right node C is 2 higher than left node B
+    if (balanceFactor > 1) {
+
+        assert(!nodeC->isLeaf());
+
+        int nodeFID = nodeC->children[0];
+        int nodeGID = nodeC->children[1];
+        assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes);
+        assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes);
+        TreeNode* nodeF = mNodes + nodeFID;
+        TreeNode* nodeG = mNodes + nodeGID;
+
+        nodeC->children[0] = nodeID;
+        nodeC->parentID = nodeA->parentID;
+        nodeA->parentID = nodeCID;
+
+        if (nodeC->parentID != TreeNode::NULL_TREE_NODE) {
+
+            if (mNodes[nodeC->parentID].children[0] == nodeID) {
+                mNodes[nodeC->parentID].children[0] = nodeCID;
+            }
+            else {
+                assert(mNodes[nodeC->parentID].children[1] == nodeID);
+                mNodes[nodeC->parentID].children[1] = nodeCID;
+            }
+        }
+        else {
+            mRootNodeID = nodeCID;
+        }
+
+        assert(!nodeC->isLeaf());
+        assert(!nodeA->isLeaf());
+
+        // If the right node C was higher than left node B because of the F node
+        if (nodeF->height > nodeG->height) {
+
+            nodeC->children[1] = nodeFID;
+            nodeA->children[1] = nodeGID;
+            nodeG->parentID = nodeID;
+
+            // Recompute the AABB of node A and C
+            nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeG->aabb);
+            nodeC->aabb.mergeTwoAABBs(nodeA->aabb, nodeF->aabb);
+
+            // Recompute the height of node A and C
+            nodeA->height = std::max(nodeB->height, nodeG->height) + 1;
+            nodeC->height = std::max(nodeA->height, nodeF->height) + 1;
+            assert(nodeA->height > 0);
+            assert(nodeC->height > 0);
+        }
+        else {  // If the right node C was higher than left node B because of node G
+            nodeC->children[1] = nodeGID;
+            nodeA->children[1] = nodeFID;
+            nodeF->parentID = nodeID;
+
+            // Recompute the AABB of node A and C
+            nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeF->aabb);
+            nodeC->aabb.mergeTwoAABBs(nodeA->aabb, nodeG->aabb);
+
+            // Recompute the height of node A and C
+            nodeA->height = std::max(nodeB->height, nodeF->height) + 1;
+            nodeC->height = std::max(nodeA->height, nodeG->height) + 1;
+            assert(nodeA->height > 0);
+            assert(nodeC->height > 0);
+        }
+
+        // Return the new root of the sub-tree
+        return nodeCID;
+    }
+
+    // If the left node B is 2 higher than right node C
+    if (balanceFactor < -1) {
+
+        assert(!nodeB->isLeaf());
+
+        int nodeFID = nodeB->children[0];
+        int nodeGID = nodeB->children[1];
+        assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes);
+        assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes);
+        TreeNode* nodeF = mNodes + nodeFID;
+        TreeNode* nodeG = mNodes + nodeGID;
+
+        nodeB->children[0] = nodeID;
+        nodeB->parentID = nodeA->parentID;
+        nodeA->parentID = nodeBID;
+
+        if (nodeB->parentID != TreeNode::NULL_TREE_NODE) {
+
+            if (mNodes[nodeB->parentID].children[0] == nodeID) {
+                mNodes[nodeB->parentID].children[0] = nodeBID;
+            }
+            else {
+                assert(mNodes[nodeB->parentID].children[1] == nodeID);
+                mNodes[nodeB->parentID].children[1] = nodeBID;
+            }
+        }
+        else {
+            mRootNodeID = nodeBID;
+        }
+
+        assert(!nodeB->isLeaf());
+        assert(!nodeA->isLeaf());
+
+        // If the left node B was higher than right node C because of the F node
+        if (nodeF->height > nodeG->height) {
+
+            nodeB->children[1] = nodeFID;
+            nodeA->children[0] = nodeGID;
+            nodeG->parentID = nodeID;
+
+            // Recompute the AABB of node A and B
+            nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeG->aabb);
+            nodeB->aabb.mergeTwoAABBs(nodeA->aabb, nodeF->aabb);
+
+            // Recompute the height of node A and B
+            nodeA->height = std::max(nodeC->height, nodeG->height) + 1;
+            nodeB->height = std::max(nodeA->height, nodeF->height) + 1;
+            assert(nodeA->height > 0);
+            assert(nodeB->height > 0);
+        }
+        else {  // If the left node B was higher than right node C because of node G
+            nodeB->children[1] = nodeGID;
+            nodeA->children[0] = nodeFID;
+            nodeF->parentID = nodeID;
+
+            // Recompute the AABB of node A and B
+            nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeF->aabb);
+            nodeB->aabb.mergeTwoAABBs(nodeA->aabb, nodeG->aabb);
+
+            // Recompute the height of node A and B
+            nodeA->height = std::max(nodeC->height, nodeF->height) + 1;
+            nodeB->height = std::max(nodeA->height, nodeG->height) + 1;
+            assert(nodeA->height > 0);
+            assert(nodeB->height > 0);
+        }
+
+        // Return the new root of the sub-tree
+        return nodeBID;
+    }
+
+    // If the sub-tree is balanced, return the current root node
+    return nodeID;
+}
+
+/// Report all shapes overlapping with the AABB given in parameter.
+void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb,
+                                                         DynamicAABBTreeOverlapCallback& callback) const {
+
+    // Create a stack with the nodes to visit
+    Stack<int, 64> stack;
+    stack.push(mRootNodeID);
+
+    // While there are still nodes to visit
+    while(stack.getNbElements() > 0) {
+
+        // Get the next node ID to visit
+        int nodeIDToVisit = stack.pop();
+
+        // Skip it if it is a null node
+        if (nodeIDToVisit == TreeNode::NULL_TREE_NODE) continue;
+
+        // Get the corresponding node
+        const TreeNode* nodeToVisit = mNodes + nodeIDToVisit;
+
+        // If the AABB in parameter overlaps with the AABB of the node to visit
+        if (aabb.testCollision(nodeToVisit->aabb)) {
+
+            // If the node is a leaf
+            if (nodeToVisit->isLeaf()) {
+
+                // Notify the broad-phase about a new potential overlapping pair
+                callback.notifyOverlappingNode(nodeIDToVisit);
+            }
+            else {  // If the node is not a leaf
+
+                // We need to visit its children
+                stack.push(nodeToVisit->children[0]);
+                stack.push(nodeToVisit->children[1]);
+            }
+        }
+    }
+}
+
+// Ray casting method
+void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &callback) const {
+
+    PROFILE("DynamicAABBTree::raycast()");
+
+    decimal maxFraction = ray.maxFraction;
+
+    Stack<int, 128> stack;
+    stack.push(mRootNodeID);
+
+    // Walk through the tree from the root looking for proxy shapes
+    // that overlap with the ray AABB
+    while (stack.getNbElements() > 0) {
+
+        // Get the next node in the stack
+        int nodeID = stack.pop();
+
+        // If it is a null node, skip it
+        if (nodeID == TreeNode::NULL_TREE_NODE) continue;
+
+        // Get the corresponding node
+        const TreeNode* node = mNodes + nodeID;
+
+        Ray rayTemp(ray.point1, ray.point2, maxFraction);
+
+        // Test if the ray intersects with the current node AABB
+        if (!node->aabb.testRayIntersect(rayTemp)) continue;
+
+        // If the node is a leaf of the tree
+        if (node->isLeaf()) {
+
+            // Call the callback that will raycast again the broad-phase shape
+            decimal hitFraction = callback.raycastBroadPhaseShape(nodeID, rayTemp);
+
+            // If the user returned a hitFraction of zero, it means that
+            // the raycasting should stop here
+            if (hitFraction == decimal(0.0)) {
+                return;
+            }
+
+            // If the user returned a positive fraction
+            if (hitFraction > decimal(0.0)) {
+
+                // We update the maxFraction value and the ray
+                // AABB using the new maximum fraction
+                if (hitFraction < maxFraction) {
+                    maxFraction = hitFraction;
+                }
+            }
+
+            // If the user returned a negative fraction, we continue
+            // the raycasting as if the proxy shape did not exist
+        }
+        else {  // If the node has children
+
+            // Push its children in the stack of nodes to explore
+            stack.push(node->children[0]);
+            stack.push(node->children[1]);
+        }
+    }
+}
+
+#ifndef NDEBUG
+
+// Check if the tree structure is valid (for debugging purpose)
+void DynamicAABBTree::check() const {
+
+    // Recursively check each node
+    checkNode(mRootNodeID);
+
+    int nbFreeNodes = 0;
+    int freeNodeID = mFreeNodeID;
+
+    // Check the free nodes
+    while(freeNodeID != TreeNode::NULL_TREE_NODE) {
+        assert(0 <= freeNodeID && freeNodeID < mNbAllocatedNodes);
+        freeNodeID = mNodes[freeNodeID].nextNodeID;
+        nbFreeNodes++;
+    }
+
+    assert(mNbNodes + nbFreeNodes == mNbAllocatedNodes);
+}
+
+// Check if the node structure is valid (for debugging purpose)
+void DynamicAABBTree::checkNode(int nodeID) const {
+
+    if (nodeID == TreeNode::NULL_TREE_NODE) return;
+
+    // If it is the root
+    if (nodeID == mRootNodeID) {
+        assert(mNodes[nodeID].parentID == TreeNode::NULL_TREE_NODE);
+    }
+
+    // Get the children nodes
+    TreeNode* pNode = mNodes + nodeID;
+    assert(!pNode->isLeaf());
+    int leftChild = pNode->children[0];
+    int rightChild = pNode->children[1];
+
+    assert(pNode->height >= 0);
+    assert(pNode->aabb.getVolume() > 0);
+
+    // If the current node is a leaf
+    if (pNode->isLeaf()) {
+
+        // Check that there are no children
+        assert(leftChild == TreeNode::NULL_TREE_NODE);
+        assert(rightChild == TreeNode::NULL_TREE_NODE);
+        assert(pNode->height == 0);
+    }
+    else {
+
+        // Check that the children node IDs are valid
+        assert(0 <= leftChild && leftChild < mNbAllocatedNodes);
+        assert(0 <= rightChild && rightChild < mNbAllocatedNodes);
+
+        // Check that the children nodes have the correct parent node
+        assert(mNodes[leftChild].parentID == nodeID);
+        assert(mNodes[rightChild].parentID == nodeID);
+
+        // Check the height of node
+        int height = 1 + std::max(mNodes[leftChild].height, mNodes[rightChild].height);
+        assert(mNodes[nodeID].height == height);
+
+        // Check the AABB of the node
+        AABB aabb;
+        aabb.mergeTwoAABBs(mNodes[leftChild].aabb, mNodes[rightChild].aabb);
+        assert(aabb.getMin() == mNodes[nodeID].aabb.getMin());
+        assert(aabb.getMax() == mNodes[nodeID].aabb.getMax());
+
+        // Recursively check the children nodes
+        checkNode(leftChild);
+        checkNode(rightChild);
+    }
+}
+
+// Compute the height of the tree
+int DynamicAABBTree::computeHeight() {
+   return computeHeight(mRootNodeID);
+}
+
+// Compute the height of a given node in the tree
+int DynamicAABBTree::computeHeight(int nodeID) {
+    assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
+    TreeNode* node = mNodes + nodeID;
+
+    // If the node is a leaf, its height is zero
+    if (node->isLeaf()) {
+        return 0;
+    }
+
+    // Compute the height of the left and right sub-tree
+    int leftHeight = computeHeight(node->children[0]);
+    int rightHeight = computeHeight(node->children[1]);
+
+    // Return the height of the node
+    return 1 + std::max(leftHeight, rightHeight);
+}
+
+#endif

src/collision/broadphase/DynamicAABBTree.h

@@ -0,0 +1,292 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_DYNAMIC_AABB_TREE_H
+#define REACTPHYSICS3D_DYNAMIC_AABB_TREE_H
+
+// Libraries
+#include "configuration.h"
+#include "collision/shapes/AABB.h"
+#include "body/CollisionBody.h"
+
+/// Namespace ReactPhysics3D
+namespace reactphysics3d {
+
+// Declarations
+class BroadPhaseAlgorithm;
+class BroadPhaseRaycastTestCallback;
+class DynamicAABBTreeOverlapCallback;
+struct RaycastTest;
+
+// Structure TreeNode
+/**
+ * This structure represents a node of the dynamic AABB tree.
+ */
+struct TreeNode {
+
+    // -------------------- Constants -------------------- //
+
+    /// Null tree node constant
+    const static int NULL_TREE_NODE;
+
+    // -------------------- Attributes -------------------- //
+
+    // A node is either in the tree (has a parent) or in the free nodes list
+    // (has a next node)
+    union {
+
+        /// Parent node ID
+        int32 parentID;
+
+        /// Next allocated node ID
+        int32 nextNodeID;
+    };
+
+    // A node is either a leaf (has data) or is an internal node (has children)
+    union {
+
+        /// Left and right child of the node (children[0] = left child)
+        int32 children[2];
+
+        /// Two pieces of data stored at that node (in case the node is a leaf)
+        union {
+            int32 dataInt[2];
+            void* dataPointer;
+        };
+    };
+
+    /// Height of the node in the tree
+    int16 height;
+
+    /// Fat axis aligned bounding box (AABB) corresponding to the node
+    AABB aabb;
+
+    // -------------------- Methods -------------------- //
+
+    /// Return true if the node is a leaf of the tree
+    bool isLeaf() const;
+};
+
+// Class DynamicAABBTreeOverlapCallback
+/**
+ * Overlapping callback method that has to be used as parameter of the
+ * reportAllShapesOverlappingWithNode() method.
+ */
+class DynamicAABBTreeOverlapCallback {
+
+    public :
+
+        // Called when a overlapping node has been found during the call to
+        // DynamicAABBTree:reportAllShapesOverlappingWithAABB()
+        virtual void notifyOverlappingNode(int nodeId)=0;
+};
+
+// Class DynamicAABBTreeRaycastCallback
+/**
+ * Raycast callback in the Dynamic AABB Tree called when the AABB of a leaf
+ * node is hit by the ray.
+ */
+class DynamicAABBTreeRaycastCallback {
+
+    public:
+
+        // Called when the AABB of a leaf node is hit by a ray
+        virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray)=0;
+
+};
+
+// Class DynamicAABBTree
+/**
+ * This class implements a dynamic AABB tree that is used for broad-phase
+ * collision detection. This data structure is inspired by Nathanael Presson's
+ * dynamic tree implementation in BulletPhysics. The following implementation is
+ * based on the one from Erin Catto in Box2D as described in the book
+ * "Introduction to Game Physics with Box2D" by Ian Parberry.
+ */
+class DynamicAABBTree {
+
+    private:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Pointer to the memory location of the nodes of the tree
+        TreeNode* mNodes;
+
+        /// ID of the root node of the tree
+        int mRootNodeID;
+
+        /// ID of the first node of the list of free (allocated) nodes in the tree that we can use
+        int mFreeNodeID;
+
+        /// Number of allocated nodes in the tree
+        int mNbAllocatedNodes;
+
+        /// Number of nodes in the tree
+        int mNbNodes;
+
+        /// Extra AABB Gap used to allow the collision shape to move a little bit
+        /// without triggering a large modification of the tree which can be costly
+        decimal mExtraAABBGap;
+
+        // -------------------- Methods -------------------- //
+
+        /// Allocate and return a node to use in the tree
+        int allocateNode();
+
+        /// Release a node
+        void releaseNode(int nodeID);
+
+        /// Insert a leaf node in the tree
+        void insertLeafNode(int nodeID);
+
+        /// Remove a leaf node from the tree
+        void removeLeafNode(int nodeID);
+
+        /// Balance the sub-tree of a given node using left or right rotations.
+        int balanceSubTreeAtNode(int nodeID);
+
+        /// Compute the height of a given node in the tree
+        int computeHeight(int nodeID);
+
+        /// Internally add an object into the tree
+        int addObjectInternal(const AABB& aabb);
+
+        /// Initialize the tree
+        void init();
+
+#ifndef NDEBUG
+
+        /// Check if the tree structure is valid (for debugging purpose)
+        void check() const;
+
+        /// Check if the node structure is valid (for debugging purpose)
+        void checkNode(int nodeID) const;
+
+#endif
+
+    public:
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        DynamicAABBTree(decimal extraAABBGap = decimal(0.0));
+
+        /// Destructor
+        ~DynamicAABBTree();
+
+        /// Add an object into the tree (where node data are two integers)
+        int addObject(const AABB& aabb, int32 data1, int32 data2);
+
+        /// Add an object into the tree (where node data is a pointer)
+        int addObject(const AABB& aabb, void* data);
+
+        /// Remove an object from the tree
+        void removeObject(int nodeID);
+
+        /// Update the dynamic tree after an object has moved.
+        bool updateObject(int nodeID, const AABB& newAABB, const Vector3& displacement, bool forceReinsert = false);
+
+        /// Return the fat AABB corresponding to a given node ID
+        const AABB& getFatAABB(int nodeID) const;
+
+        /// Return the pointer to the data array of a given leaf node of the tree
+        int32* getNodeDataInt(int nodeID) const;
+
+        /// Return the data pointer of a given leaf node of the tree
+        void* getNodeDataPointer(int nodeID) const;
+
+        /// Report all shapes overlapping with the AABB given in parameter.
+        void reportAllShapesOverlappingWithAABB(const AABB& aabb,
+                                                DynamicAABBTreeOverlapCallback& callback) const;
+
+        /// Ray casting method
+        void raycast(const Ray& ray, DynamicAABBTreeRaycastCallback& callback) const;
+
+        /// Compute the height of the tree
+        int computeHeight();
+
+        /// Return the root AABB of the tree
+        AABB getRootAABB() const;
+
+        /// Clear all the nodes and reset the tree
+        void reset();
+};
+
+// Return true if the node is a leaf of the tree
+inline bool TreeNode::isLeaf() const {
+    return (height == 0);
+}
+
+// Return the fat AABB corresponding to a given node ID
+inline const AABB& DynamicAABBTree::getFatAABB(int nodeID) const {
+    assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
+    return mNodes[nodeID].aabb;
+}
+
+// Return the pointer to the data array of a given leaf node of the tree
+inline int32* DynamicAABBTree::getNodeDataInt(int nodeID) const {
+    assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
+    assert(mNodes[nodeID].isLeaf());
+    return mNodes[nodeID].dataInt;
+}
+
+// Return the pointer to the data pointer of a given leaf node of the tree
+inline void* DynamicAABBTree::getNodeDataPointer(int nodeID) const {
+    assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
+    assert(mNodes[nodeID].isLeaf());
+    return mNodes[nodeID].dataPointer;
+}
+
+// Return the root AABB of the tree
+inline AABB DynamicAABBTree::getRootAABB() const {
+    return getFatAABB(mRootNodeID);
+}
+
+// Add an object into the tree. This method creates a new leaf node in the tree and
+// returns the ID of the corresponding node.
+inline int DynamicAABBTree::addObject(const AABB& aabb, int32 data1, int32 data2) {
+
+    int nodeId = addObjectInternal(aabb);
+
+    mNodes[nodeId].dataInt[0] = data1;
+    mNodes[nodeId].dataInt[1] = data2;
+
+    return nodeId;
+}
+
+// Add an object into the tree. This method creates a new leaf node in the tree and
+// returns the ID of the corresponding node.
+inline int DynamicAABBTree::addObject(const AABB& aabb, void* data) {
+
+    int nodeId = addObjectInternal(aabb);
+
+    mNodes[nodeId].dataPointer = data;
+
+    return nodeId;
+}
+
+}
+
+#endif

src/collision/narrowphase/CollisionDispatch.h

@@ -0,0 +1,67 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_COLLISION_DISPATCH_H
+#define	REACTPHYSICS3D_COLLISION_DISPATCH_H
+
+// Libraries
+#include "NarrowPhaseAlgorithm.h"
+
+namespace reactphysics3d {
+
+// Class CollisionDispatch
+/**
+ * This is the abstract base class for dispatching the narrow-phase
+ * collision detection algorithm. Collision dispatching decides which collision
+ * algorithm to use given two types of proxy collision shapes.
+ */
+class CollisionDispatch {
+
+    protected:
+
+    public:
+
+        /// Constructor
+        CollisionDispatch() {}
+
+        /// Destructor
+        virtual ~CollisionDispatch() {}
+
+        /// Initialize the collision dispatch configuration
+        virtual void init(CollisionDetection* collisionDetection,
+                          MemoryAllocator* memoryAllocator) {
+
+        }
+
+        /// Select and return the narrow-phase collision detection algorithm to
+        /// use between two types of collision shapes.
+        virtual NarrowPhaseAlgorithm* selectAlgorithm(int shape1Type,
+                                                      int shape2Type)=0;
+};
+
+}
+
+#endif
+

src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp

@@ -0,0 +1,291 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "collision/shapes/ConcaveShape.h"
+#include "collision/shapes/TriangleShape.h"
+#include "ConcaveVsConvexAlgorithm.h"
+#include "collision/CollisionDetection.h"
+#include "engine/CollisionWorld.h"
+#include <algorithm>
+
+using namespace reactphysics3d;
+
+// Constructor
+ConcaveVsConvexAlgorithm::ConcaveVsConvexAlgorithm() {
+
+}
+
+// Destructor
+ConcaveVsConvexAlgorithm::~ConcaveVsConvexAlgorithm() {
+
+}
+
+// Return true and compute a contact info if the two bounding volumes collide
+void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
+                                             const CollisionShapeInfo& shape2Info,
+                                             NarrowPhaseCallback* narrowPhaseCallback) {
+
+    ProxyShape* convexProxyShape;
+    ProxyShape* concaveProxyShape;
+    const ConvexShape* convexShape;
+    const ConcaveShape* concaveShape;
+
+    // Collision shape 1 is convex, collision shape 2 is concave
+    if (shape1Info.collisionShape->isConvex()) {
+        convexProxyShape = shape1Info.proxyShape;
+        convexShape = static_cast<const ConvexShape*>(shape1Info.collisionShape);
+        concaveProxyShape = shape2Info.proxyShape;
+        concaveShape = static_cast<const ConcaveShape*>(shape2Info.collisionShape);
+    }
+    else {  // Collision shape 2 is convex, collision shape 1 is concave
+        convexProxyShape = shape2Info.proxyShape;
+        convexShape = static_cast<const ConvexShape*>(shape2Info.collisionShape);
+        concaveProxyShape = shape1Info.proxyShape;
+        concaveShape = static_cast<const ConcaveShape*>(shape1Info.collisionShape);
+    }
+
+    // Set the parameters of the callback object
+    ConvexVsTriangleCallback convexVsTriangleCallback;
+    convexVsTriangleCallback.setCollisionDetection(mCollisionDetection);
+    convexVsTriangleCallback.setConvexShape(convexShape);
+    convexVsTriangleCallback.setConcaveShape(concaveShape);
+    convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape);
+    convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair);
+
+    // Compute the convex shape AABB in the local-space of the convex shape
+    AABB aabb;
+    convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform());
+
+    // If smooth mesh collision is enabled for the concave mesh
+    if (concaveShape->getIsSmoothMeshCollisionEnabled()) {
+
+        std::vector<SmoothMeshContactInfo> contactPoints;
+
+        SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints);
+
+        convexVsTriangleCallback.setNarrowPhaseCallback(&smoothNarrowPhaseCallback);
+
+        // Call the convex vs triangle callback for each triangle of the concave shape
+        concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
+
+        // Run the smooth mesh collision algorithm
+        processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, narrowPhaseCallback);
+    }
+    else {
+
+        convexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback);
+
+        // Call the convex vs triangle callback for each triangle of the concave shape
+        concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
+    }
+}
+
+// Test collision between a triangle and the convex mesh shape
+void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) {
+
+    // Create a triangle collision shape
+    decimal margin = mConcaveShape->getTriangleMargin();
+    TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
+
+    // Select the collision algorithm to use between the triangle and the convex shape
+    NarrowPhaseAlgorithm* algo = mCollisionDetection->getCollisionAlgorithm(triangleShape.getType(),
+                                                                            mConvexShape->getType());
+
+    // If there is no collision algorithm between those two kinds of shapes
+    if (algo == NULL) return;
+
+    // Notify the narrow-phase algorithm about the overlapping pair we are going to test
+    algo->setCurrentOverlappingPair(mOverlappingPair);
+
+    // Create the CollisionShapeInfo objects
+    CollisionShapeInfo shapeConvexInfo(mConvexProxyShape, mConvexShape, mConvexProxyShape->getLocalToWorldTransform(),
+                                       mOverlappingPair, mConvexProxyShape->getCachedCollisionData());
+    CollisionShapeInfo shapeConcaveInfo(mConcaveProxyShape, &triangleShape,
+                                        mConcaveProxyShape->getLocalToWorldTransform(),
+                                        mOverlappingPair, mConcaveProxyShape->getCachedCollisionData());
+
+    // Use the collision algorithm to test collision between the triangle and the other convex shape
+    algo->testCollision(shapeConvexInfo, shapeConcaveInfo, mNarrowPhaseCallback);
+}
+
+// Process the concave triangle mesh collision using the smooth mesh collision algorithm described
+// by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision
+// issue with some internal edges.
+void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair,
+                                                          std::vector<SmoothMeshContactInfo> contactPoints,
+                                                          NarrowPhaseCallback* narrowPhaseCallback) {
+
+    // Set with the triangle vertices already processed to void further contacts with same triangle
+    std::unordered_multimap<int, Vector3> processTriangleVertices;
+
+    // Sort the list of narrow-phase contacts according to their penetration depth
+    std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare());
+
+    // For each contact point (from smaller penetration depth to larger)
+    std::vector<SmoothMeshContactInfo>::const_iterator it;
+    for (it = contactPoints.begin(); it != contactPoints.end(); ++it) {
+
+        const SmoothMeshContactInfo info = *it;
+        const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
+
+        // Compute the barycentric coordinates of the point in the triangle
+        decimal u, v, w;
+        computeBarycentricCoordinatesInTriangle(info.triangleVertices[0],
+                                                info.triangleVertices[1],
+                                                info.triangleVertices[2],
+                                                contactPoint, u, v, w);
+        int nbZeros = 0;
+        bool isUZero = approxEqual(u, 0, 0.0001);
+        bool isVZero = approxEqual(v, 0, 0.0001);
+        bool isWZero = approxEqual(w, 0, 0.0001);
+        if (isUZero) nbZeros++;
+        if (isVZero) nbZeros++;
+        if (isWZero) nbZeros++;
+
+        // If it is a vertex contact
+        if (nbZeros == 2) {
+
+            Vector3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]);
+
+            // Check that this triangle vertex has not been processed yet
+            if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) {
+
+                // Keep the contact as it is and report it
+                narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
+            }
+        }
+        else if (nbZeros == 1) {  // If it is an edge contact
+
+            Vector3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]);
+            Vector3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]);
+
+            // Check that this triangle edge has not been processed yet
+            if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) &&
+                !hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) {
+
+                // Keep the contact as it is and report it
+                narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
+            }
+
+        }
+        else {  // If it is a face contact
+
+            ContactPointInfo newContactInfo(info.contactInfo);
+
+            ProxyShape* firstShape;
+            ProxyShape* secondShape;
+            if (info.isFirstShapeTriangle) {
+                firstShape = overlappingPair->getShape1();
+                secondShape = overlappingPair->getShape2();
+            }
+            else {
+                firstShape = overlappingPair->getShape2();
+                secondShape = overlappingPair->getShape1();
+            }
+
+            // We use the triangle normal as the contact normal
+            Vector3 a = info.triangleVertices[1] - info.triangleVertices[0];
+            Vector3 b = info.triangleVertices[2] - info.triangleVertices[0];
+            Vector3 localNormal = a.cross(b);
+            newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal;
+            Vector3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
+            Vector3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint;
+            newContactInfo.normal.normalize();
+            if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) {
+                newContactInfo.normal = -newContactInfo.normal;
+            }
+
+            // We recompute the contact point on the second body with the new normal as described in
+            // the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and
+            // Dirk Gregorius) to avoid adding torque
+            Transform worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse();
+            if (info.isFirstShapeTriangle) {
+                Vector3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal;
+                newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint;
+            }
+            else {
+                Vector3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal;
+                newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint;
+            }
+
+            // Report the contact
+            narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo);
+        }
+
+        // Add the three vertices of the triangle to the set of processed
+        // triangle vertices
+        addProcessedVertex(processTriangleVertices, info.triangleVertices[0]);
+        addProcessedVertex(processTriangleVertices, info.triangleVertices[1]);
+        addProcessedVertex(processTriangleVertices, info.triangleVertices[2]);
+    }
+}
+
+// Return true if the vertex is in the set of already processed vertices
+bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multimap<int, Vector3>& processTriangleVertices, const Vector3& vertex) const {
+
+    int key = int(vertex.x * vertex.y * vertex.z);
+
+    auto range = processTriangleVertices.equal_range(key);
+    for (auto it = range.first; it != range.second; ++it) {
+        if (vertex.x == it->second.x && vertex.y == it->second.y && vertex.z == it->second.z) return true;
+    }
+
+    return false;
+}
+
+// Called by a narrow-phase collision algorithm when a new contact has been found
+void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair,
+                                                       const ContactPointInfo& contactInfo) {
+    Vector3 triangleVertices[3];
+    bool isFirstShapeTriangle;
+
+    // If the collision shape 1 is the triangle
+    if (contactInfo.collisionShape1->getType() == TRIANGLE) {
+        assert(contactInfo.collisionShape2->getType() != TRIANGLE);
+
+        const TriangleShape* triangleShape = static_cast<const TriangleShape*>(contactInfo.collisionShape1);
+        triangleVertices[0] = triangleShape->getVertex(0);
+        triangleVertices[1] = triangleShape->getVertex(1);
+        triangleVertices[2] = triangleShape->getVertex(2);
+
+        isFirstShapeTriangle = true;
+    }
+    else {  // If the collision shape 2 is the triangle
+        assert(contactInfo.collisionShape2->getType() == TRIANGLE);
+
+        const TriangleShape* triangleShape = static_cast<const TriangleShape*>(contactInfo.collisionShape2);
+        triangleVertices[0] = triangleShape->getVertex(0);
+        triangleVertices[1] = triangleShape->getVertex(1);
+        triangleVertices[2] = triangleShape->getVertex(2);
+
+        isFirstShapeTriangle = false;
+    }
+    SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]);
+
+    // Add the narrow-phase contact into the list of contact to process for
+    // smooth mesh collision
+    mContactPoints.push_back(smoothContactInfo);
+}

src/collision/narrowphase/ConcaveVsConvexAlgorithm.h

@@ -0,0 +1,234 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_CONCAVE_VS_CONVEX_ALGORITHM_H
+#define	REACTPHYSICS3D_CONCAVE_VS_CONVEX_ALGORITHM_H
+
+// Libraries
+#include "NarrowPhaseAlgorithm.h"
+#include "collision/shapes/ConvexShape.h"
+#include "collision/shapes/ConcaveShape.h"
+#include <unordered_map>
+
+/// Namespace ReactPhysics3D
+namespace reactphysics3d {
+
+// Class ConvexVsTriangleCallback
+/**
+ * This class is used to encapsulate a callback method for
+ * collision detection between the triangle of a concave mesh shape
+ * and a convex shape.
+ */
+class ConvexVsTriangleCallback : public TriangleCallback {
+
+    protected:
+
+        /// Pointer to the collision detection object
+        CollisionDetection* mCollisionDetection;
+
+        /// Narrow-phase collision callback
+        NarrowPhaseCallback* mNarrowPhaseCallback;
+
+        /// Convex collision shape to test collision with
+        const ConvexShape* mConvexShape;
+
+        /// Concave collision shape
+        const ConcaveShape* mConcaveShape;
+
+        /// Proxy shape of the convex collision shape
+        ProxyShape* mConvexProxyShape;
+
+        /// Proxy shape of the concave collision shape
+        ProxyShape* mConcaveProxyShape;
+
+        /// Broadphase overlapping pair
+        OverlappingPair* mOverlappingPair;
+
+        /// Used to sort ContactPointInfos according to their penetration depth
+        static bool contactsDepthCompare(const ContactPointInfo& contact1,
+                                         const ContactPointInfo& contact2);
+
+    public:
+
+        /// Set the collision detection pointer
+        void setCollisionDetection(CollisionDetection* collisionDetection) {
+            mCollisionDetection = collisionDetection;
+        }
+
+        /// Set the narrow-phase collision callback
+        void setNarrowPhaseCallback(NarrowPhaseCallback* callback) {
+            mNarrowPhaseCallback = callback;
+        }
+
+        /// Set the convex collision shape to test collision with
+        void setConvexShape(const ConvexShape* convexShape) {
+            mConvexShape = convexShape;
+        }
+
+        /// Set the concave collision shape
+        void setConcaveShape(const ConcaveShape* concaveShape) {
+            mConcaveShape = concaveShape;
+        }
+
+        /// Set the broadphase overlapping pair
+        void setOverlappingPair(OverlappingPair* overlappingPair) {
+            mOverlappingPair = overlappingPair;
+        }
+
+        /// Set the proxy shapes of the two collision shapes
+        void setProxyShapes(ProxyShape* convexProxyShape, ProxyShape* concaveProxyShape) {
+            mConvexProxyShape = convexProxyShape;
+            mConcaveProxyShape = concaveProxyShape;
+        }
+
+        /// Test collision between a triangle and the convex mesh shape
+        virtual void testTriangle(const Vector3* trianglePoints);
+};
+
+// Class SmoothMeshContactInfo
+/**
+ * This class is used to store data about a contact with a triangle for the smooth
+ * mesh algorithm.
+ */
+class SmoothMeshContactInfo {
+
+    public:
+
+        ContactPointInfo contactInfo;
+        bool isFirstShapeTriangle;
+        Vector3 triangleVertices[3];
+
+        /// Constructor
+        SmoothMeshContactInfo(const ContactPointInfo& contact, bool firstShapeTriangle, const Vector3& trianglePoint1,
+                              const Vector3& trianglePoint2, const Vector3& trianglePoint3)
+            : contactInfo(contact) {
+            isFirstShapeTriangle = firstShapeTriangle;
+            triangleVertices[0] = trianglePoint1;
+            triangleVertices[1] = trianglePoint2;
+            triangleVertices[2] = trianglePoint3;
+        }
+
+};
+
+struct ContactsDepthCompare {
+    bool operator()(const SmoothMeshContactInfo& contact1, const SmoothMeshContactInfo& contact2)
+    {
+        return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth;
+    }
+};
+
+/// Method used to compare two smooth mesh contact info to sort them
+//inline static bool contactsDepthCompare(const SmoothMeshContactInfo& contact1,
+//                                        const SmoothMeshContactInfo& contact2) {
+//    return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth;
+//}
+
+// Class SmoothCollisionNarrowPhaseCallback
+/**
+ * This class is used as a narrow-phase callback to get narrow-phase contacts
+ * of the concave triangle mesh to temporary store them in order to be used in
+ * the smooth mesh collision algorithm if this one is enabled.
+ */
+class SmoothCollisionNarrowPhaseCallback : public NarrowPhaseCallback {
+
+    private:
+
+        std::vector<SmoothMeshContactInfo>& mContactPoints;
+
+
+    public:
+
+        // Constructor
+        SmoothCollisionNarrowPhaseCallback(std::vector<SmoothMeshContactInfo>& contactPoints)
+          : mContactPoints(contactPoints) {
+
+        }
+
+
+        /// Called by a narrow-phase collision algorithm when a new contact has been found
+        virtual void notifyContact(OverlappingPair* overlappingPair,
+                                   const ContactPointInfo& contactInfo);
+
+};
+
+// Class ConcaveVsConvexAlgorithm
+/**
+ * This class is used to compute the narrow-phase collision detection
+ * between a concave collision shape and a convex collision shape. The idea is
+ * to use the GJK collision detection algorithm to compute the collision between
+ * the convex shape and each of the triangles in the concave shape.
+ */
+class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm {
+
+    protected :
+
+        // -------------------- Attributes -------------------- //        
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        ConcaveVsConvexAlgorithm(const ConcaveVsConvexAlgorithm& algorithm);
+
+        /// Private assignment operator
+        ConcaveVsConvexAlgorithm& operator=(const ConcaveVsConvexAlgorithm& algorithm);
+
+        /// Process the concave triangle mesh collision using the smooth mesh collision algorithm
+        void processSmoothMeshCollision(OverlappingPair* overlappingPair,
+                                        std::vector<SmoothMeshContactInfo> contactPoints,
+                                        NarrowPhaseCallback* narrowPhaseCallback);
+
+        /// Add a triangle vertex into the set of processed triangles
+        void addProcessedVertex(std::unordered_multimap<int, Vector3>& processTriangleVertices,
+                                const Vector3& vertex);
+
+        /// Return true if the vertex is in the set of already processed vertices
+        bool hasVertexBeenProcessed(const std::unordered_multimap<int, Vector3>& processTriangleVertices,
+                                    const Vector3& vertex) const;
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        ConcaveVsConvexAlgorithm();
+
+        /// Destructor
+        virtual ~ConcaveVsConvexAlgorithm();
+
+        /// Compute a contact info if the two bounding volume collide
+        virtual void testCollision(const CollisionShapeInfo& shape1Info,
+                                   const CollisionShapeInfo& shape2Info,
+                                   NarrowPhaseCallback* narrowPhaseCallback);
+};
+
+// Add a triangle vertex into the set of processed triangles
+inline void ConcaveVsConvexAlgorithm::addProcessedVertex(std::unordered_multimap<int, Vector3>& processTriangleVertices, const Vector3& vertex) {
+    processTriangleVertices.insert(std::make_pair(int(vertex.x * vertex.y * vertex.z), vertex));
+}
+
+}
+
+#endif
+

src/collision/narrowphase/DefaultCollisionDispatch.cpp

@@ -0,0 +1,75 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "DefaultCollisionDispatch.h"
+#include "collision/shapes/CollisionShape.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+DefaultCollisionDispatch::DefaultCollisionDispatch() {
+
+}
+
+// Destructor
+DefaultCollisionDispatch::~DefaultCollisionDispatch() {
+
+}
+
+/// Initialize the collision dispatch configuration
+void DefaultCollisionDispatch::init(CollisionDetection* collisionDetection,
+                                    MemoryAllocator* memoryAllocator) {
+
+    // Initialize the collision algorithms
+    mSphereVsSphereAlgorithm.init(collisionDetection, memoryAllocator);
+    mGJKAlgorithm.init(collisionDetection, memoryAllocator);
+    mConcaveVsConvexAlgorithm.init(collisionDetection, memoryAllocator);
+}
+
+// Select and return the narrow-phase collision detection algorithm to
+// use between two types of collision shapes.
+NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int type2) {
+
+    CollisionShapeType shape1Type = static_cast<CollisionShapeType>(type1);
+    CollisionShapeType shape2Type = static_cast<CollisionShapeType>(type2);
+
+    // Sphere vs Sphere algorithm
+    if (shape1Type == SPHERE && shape2Type == SPHERE) {
+        return &mSphereVsSphereAlgorithm;
+    }
+    // Concave vs Convex algorithm
+    else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) ||
+             (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) {
+        return &mConcaveVsConvexAlgorithm;
+    }
+    // Convex vs Convex algorithm (GJK algorithm)
+    else if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) {
+        return &mGJKAlgorithm;
+    }
+    else {
+        return NULL;
+    }
+}

src/collision/narrowphase/DefaultCollisionDispatch.h

@@ -0,0 +1,78 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_DEFAULT_COLLISION_DISPATCH_H
+#define	REACTPHYSICS3D_DEFAULT_COLLISION_DISPATCH_H
+
+// Libraries
+#include "CollisionDispatch.h"
+#include "ConcaveVsConvexAlgorithm.h"
+#include "SphereVsSphereAlgorithm.h"
+#include "GJK/GJKAlgorithm.h"
+
+namespace reactphysics3d {
+
+// Class DefaultCollisionDispatch
+/**
+ * This is the default collision dispatch configuration use in ReactPhysics3D.
+ * Collision dispatching decides which collision
+ * algorithm to use given two types of proxy collision shapes.
+ */
+class DefaultCollisionDispatch : public CollisionDispatch {
+
+    protected:
+
+        /// Sphere vs Sphere collision algorithm
+        SphereVsSphereAlgorithm mSphereVsSphereAlgorithm;
+
+        /// Concave vs Convex collision algorithm
+        ConcaveVsConvexAlgorithm mConcaveVsConvexAlgorithm;
+
+        /// GJK Algorithm
+        GJKAlgorithm mGJKAlgorithm;
+
+    public:
+
+        /// Constructor
+        DefaultCollisionDispatch();
+
+        /// Destructor
+        virtual ~DefaultCollisionDispatch();
+
+        /// Initialize the collision dispatch configuration
+        virtual void init(CollisionDetection* collisionDetection,
+                          MemoryAllocator* memoryAllocator);
+
+        /// Select and return the narrow-phase collision detection algorithm to
+        /// use between two types of collision shapes.
+        virtual NarrowPhaseAlgorithm* selectAlgorithm(int type1, int type2);
+};
+
+}
+
+#endif
+
+
+

src/collision/narrowphase/EPA/EPAAlgorithm.cpp

@@ -0,0 +1,439 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "EPAAlgorithm.h"
+#include "engine/Profiler.h"
+#include "collision/narrowphase//GJK/GJKAlgorithm.h"
+#include "TrianglesStore.h"
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Constructor
+EPAAlgorithm::EPAAlgorithm() {
+
+}
+
+// Destructor
+EPAAlgorithm::~EPAAlgorithm() {
+    
+}
+
+// Decide if the origin is in the tetrahedron.
+/// Return 0 if the origin is in the tetrahedron and return the number (1,2,3 or 4) of
+/// the vertex that is wrong if the origin is not in the tetrahedron
+int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
+                                        const Vector3& p3, const Vector3& p4) const {
+
+    // Check vertex 1
+    Vector3 normal1 = (p2-p1).cross(p3-p1);
+    if ((normal1.dot(p1) > 0.0) == (normal1.dot(p4) > 0.0)) {
+        return 4;
+    }
+
+    // Check vertex 2
+    Vector3 normal2 = (p4-p2).cross(p3-p2);
+    if ((normal2.dot(p2) > 0.0) == (normal2.dot(p1) > 0.0)) {
+        return 1;
+    }
+
+    // Check vertex 3
+    Vector3 normal3 = (p4-p3).cross(p1-p3);
+    if ((normal3.dot(p3) > 0.0) == (normal3.dot(p2) > 0.0)) {
+        return 2;
+    }
+
+    // Check vertex 4
+    Vector3 normal4 = (p2-p4).cross(p1-p4);
+    if ((normal4.dot(p4) > 0.0) == (normal4.dot(p3) > 0.0)) {
+        return 3;
+    }
+
+    // The origin is in the tetrahedron, we return 0
+    return 0;
+}
+
+// Compute the penetration depth with the EPA algorithm.
+/// This method computes the penetration depth and contact points between two
+/// enlarged objects (with margin) where the original objects (without margin)
+/// intersect. An initial simplex that contains origin has been computed with
+/// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find
+/// the correct penetration depth
+void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex,
+                                                           CollisionShapeInfo shape1Info,
+                                                           const Transform& transform1,
+                                                           CollisionShapeInfo shape2Info,
+                                                           const Transform& transform2,
+                                                           Vector3& v,
+                                                           NarrowPhaseCallback* narrowPhaseCallback) {
+
+    PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()");
+
+    assert(shape1Info.collisionShape->isConvex());
+    assert(shape2Info.collisionShape->isConvex());
+
+    const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
+    const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
+
+    void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
+    void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
+
+    Vector3 suppPointsA[MAX_SUPPORT_POINTS];  // Support points of object A in local coordinates
+    Vector3 suppPointsB[MAX_SUPPORT_POINTS];  // Support points of object B in local coordinates
+    Vector3 points[MAX_SUPPORT_POINTS];       // Current points
+    TrianglesStore triangleStore;             // Store the triangles
+    TriangleEPA* triangleHeap[MAX_FACETS];    // Heap that contains the face
+                                              // candidate of the EPA algorithm
+
+    // Transform a point from local space of body 2 to local
+    // space of body 1 (the GJK algorithm is done in local space of body 1)
+    Transform body2Tobody1 = transform1.getInverse() * transform2;
+
+    // Matrix that transform a direction from local
+    // space of body 1 into local space of body 2
+    Quaternion rotateToBody2 = transform2.getOrientation().getInverse() *
+                              transform1.getOrientation();
+
+    // Get the simplex computed previously by the GJK algorithm
+    unsigned int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
+
+    // Compute the tolerance
+    decimal tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint();
+
+    // Number of triangles in the polytope
+    unsigned int nbTriangles = 0;
+
+    // Clear the storing of triangles
+    triangleStore.clear();
+
+    // Select an action according to the number of points in the simplex
+    // computed with GJK algorithm in order to obtain an initial polytope for
+    // The EPA algorithm.
+    switch(nbVertices) {
+        case 1:
+            // Only one point in the simplex (which should be the origin).
+            // We have a touching contact with zero penetration depth.
+            // We drop that kind of contact. Therefore, we return false
+            return;
+
+        case 2: {
+            // The simplex returned by GJK is a line segment d containing the origin.
+            // We add two additional support points to construct a hexahedron (two tetrahedron
+            // glued together with triangle faces. The idea is to compute three different vectors
+            // v1, v2 and v3 that are orthogonal to the segment d. The three vectors are relatively
+            // rotated of 120 degree around the d segment. The the three new points to
+            // construct the polytope are the three support points in those three directions
+            // v1, v2 and v3.
+
+            // Direction of the segment
+            Vector3 d = (points[1] - points[0]).getUnit();
+
+            // Choose the coordinate axis from the minimal absolute component of the vector d
+            int minAxis = d.getAbsoluteVector().getMinAxis();
+
+            // Compute sin(60)
+            const decimal sin60 = decimal(sqrt(3.0)) * decimal(0.5);
+
+            // Create a rotation quaternion to rotate the vector v1 to get the vectors
+            // v2 and v3
+            Quaternion rotationQuat(d.x * sin60, d.y * sin60, d.z * sin60, 0.5);
+
+            // Compute the vector v1, v2, v3
+            Vector3 v1 = d.cross(Vector3(minAxis == 0, minAxis == 1, minAxis == 2));
+            Vector3 v2 = rotationQuat * v1;
+            Vector3 v3 = rotationQuat * v2;
+
+            // Compute the support point in the direction of v1
+            suppPointsA[2] = shape1->getLocalSupportPointWithMargin(v1, shape1CachedCollisionData);
+            suppPointsB[2] = body2Tobody1 *
+                       shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v1), shape2CachedCollisionData);
+            points[2] = suppPointsA[2] - suppPointsB[2];
+
+            // Compute the support point in the direction of v2
+            suppPointsA[3] = shape1->getLocalSupportPointWithMargin(v2, shape1CachedCollisionData);
+            suppPointsB[3] = body2Tobody1 *
+                     shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v2), shape2CachedCollisionData);
+            points[3] = suppPointsA[3] - suppPointsB[3];
+
+            // Compute the support point in the direction of v3
+            suppPointsA[4] = shape1->getLocalSupportPointWithMargin(v3, shape1CachedCollisionData);
+            suppPointsB[4] = body2Tobody1 *
+                            shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v3), shape2CachedCollisionData);
+            points[4] = suppPointsA[4] - suppPointsB[4];
+
+            // Now we have an hexahedron (two tetrahedron glued together). We can simply keep the
+            // tetrahedron that contains the origin in order that the initial polytope of the
+            // EPA algorithm is a tetrahedron, which is simpler to deal with.
+
+            // If the origin is in the tetrahedron of points 0, 2, 3, 4
+            if (isOriginInTetrahedron(points[0], points[2], points[3], points[4]) == 0) {
+                // We use the point 4 instead of point 1 for the initial tetrahedron
+                suppPointsA[1] = suppPointsA[4];
+                suppPointsB[1] = suppPointsB[4];
+                points[1] = points[4];
+            }
+            // If the origin is in the tetrahedron of points 1, 2, 3, 4
+            else if (isOriginInTetrahedron(points[1], points[2], points[3], points[4]) == 0) {
+                // We use the point 4 instead of point 0 for the initial tetrahedron
+                suppPointsA[0] = suppPointsA[4];
+                suppPointsB[0] = suppPointsB[4];
+                points[0] = points[4];
+            }
+            else {
+                // The origin is not in the initial polytope
+                return;
+            }
+
+            // The polytope contains now 4 vertices
+            nbVertices = 4;
+        }
+        case 4: {
+            // The simplex computed by the GJK algorithm is a tetrahedron. Here we check
+            // if this tetrahedron contains the origin. If it is the case, we keep it and
+            // otherwise we remove the wrong vertex of the tetrahedron and go in the case
+            // where the GJK algorithm compute a simplex of three vertices.
+
+            // Check if the tetrahedron contains the origin (or wich is the wrong vertex otherwise)
+            int badVertex = isOriginInTetrahedron(points[0], points[1], points[2], points[3]);
+
+            // If the origin is in the tetrahedron
+            if (badVertex == 0) {
+                // The tetrahedron is a correct initial polytope for the EPA algorithm.
+                // Therefore, we construct the tetrahedron.
+
+                // Comstruct the 4 triangle faces of the tetrahedron
+                TriangleEPA* face0 = triangleStore.newTriangle(points, 0, 1, 2);
+                TriangleEPA* face1 = triangleStore.newTriangle(points, 0, 3, 1);
+                TriangleEPA* face2 = triangleStore.newTriangle(points, 0, 2, 3);
+                TriangleEPA* face3 = triangleStore.newTriangle(points, 1, 3, 2);
+
+                // If the constructed tetrahedron is not correct
+                if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL)
+                   && face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0
+                   && face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) {
+                    return;
+                }
+
+                // Associate the edges of neighbouring triangle faces
+                link(EdgeEPA(face0, 0), EdgeEPA(face1, 2));
+                link(EdgeEPA(face0, 1), EdgeEPA(face3, 2));
+                link(EdgeEPA(face0, 2), EdgeEPA(face2, 0));
+                link(EdgeEPA(face1, 0), EdgeEPA(face2, 2));
+                link(EdgeEPA(face1, 1), EdgeEPA(face3, 0));
+                link(EdgeEPA(face2, 1), EdgeEPA(face3, 1));
+
+                // Add the triangle faces in the candidate heap
+                addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST);
+                addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST);
+                addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST);
+                addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST);
+
+                break;
+            }
+
+            // The tetrahedron contains a wrong vertex (the origin is not inside the tetrahedron)
+            // Remove the wrong vertex and continue to the next case with the
+            // three remaining vertices
+            if (badVertex < 4) {
+
+                suppPointsA[badVertex-1] = suppPointsA[3];
+                suppPointsB[badVertex-1] = suppPointsB[3];
+                points[badVertex-1] = points[3];
+            }
+
+            // We have removed the wrong vertex
+            nbVertices = 3;
+        }
+        case 3: {
+            // The GJK algorithm returned a triangle that contains the origin.
+            // We need two new vertices to create two tetrahedron. The two new
+            // vertices are the support points in the "n" and "-n" direction
+            // where "n" is the normal of the triangle. Then, we use only the
+            // tetrahedron that contains the origin.
+
+            // Compute the normal of the triangle
+            Vector3 v1 = points[1] - points[0];
+            Vector3 v2 = points[2] - points[0];
+            Vector3 n = v1.cross(v2);
+
+            // Compute the two new vertices to obtain a hexahedron
+            suppPointsA[3] = shape1->getLocalSupportPointWithMargin(n, shape1CachedCollisionData);
+            suppPointsB[3] = body2Tobody1 *
+                     shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-n), shape2CachedCollisionData);
+            points[3] = suppPointsA[3] - suppPointsB[3];
+            suppPointsA[4] = shape1->getLocalSupportPointWithMargin(-n, shape1CachedCollisionData);
+            suppPointsB[4] = body2Tobody1 *
+                     shape2->getLocalSupportPointWithMargin(rotateToBody2 * n, shape2CachedCollisionData);
+            points[4] = suppPointsA[4] - suppPointsB[4];
+
+            TriangleEPA* face0 = NULL;
+            TriangleEPA* face1 = NULL;
+            TriangleEPA* face2 = NULL;
+            TriangleEPA* face3 = NULL;
+
+            // If the origin is in the first tetrahedron
+            if (isOriginInTetrahedron(points[0], points[1],
+                                      points[2], points[3]) == 0) {
+                // The tetrahedron is a correct initial polytope for the EPA algorithm.
+                // Therefore, we construct the tetrahedron.
+
+                // Comstruct the 4 triangle faces of the tetrahedron
+                face0 = triangleStore.newTriangle(points, 0, 1, 2);
+                face1 = triangleStore.newTriangle(points, 0, 3, 1);
+                face2 = triangleStore.newTriangle(points, 0, 2, 3);
+                face3 = triangleStore.newTriangle(points, 1, 3, 2);
+            }
+            else if (isOriginInTetrahedron(points[0], points[1],
+                                           points[2], points[4]) == 0) {
+
+                // The tetrahedron is a correct initial polytope for the EPA algorithm.
+                // Therefore, we construct the tetrahedron.
+
+                // Comstruct the 4 triangle faces of the tetrahedron
+                face0 = triangleStore.newTriangle(points, 0, 1, 2);
+                face1 = triangleStore.newTriangle(points, 0, 4, 1);
+                face2 = triangleStore.newTriangle(points, 0, 2, 4);
+                face3 = triangleStore.newTriangle(points, 1, 4, 2);
+            }
+            else {
+                return;
+            }
+
+            // If the constructed tetrahedron is not correct
+            if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL)
+               && face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0
+               && face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) {
+                return;
+            }
+
+            // Associate the edges of neighbouring triangle faces
+            link(EdgeEPA(face0, 0), EdgeEPA(face1, 2));
+            link(EdgeEPA(face0, 1), EdgeEPA(face3, 2));
+            link(EdgeEPA(face0, 2), EdgeEPA(face2, 0));
+            link(EdgeEPA(face1, 0), EdgeEPA(face2, 2));
+            link(EdgeEPA(face1, 1), EdgeEPA(face3, 0));
+            link(EdgeEPA(face2, 1), EdgeEPA(face3, 1));
+
+            // Add the triangle faces in the candidate heap
+            addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST);
+            addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST);
+            addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST);
+            addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST);
+
+            nbVertices = 4;
+
+        }
+        break;
+    }
+
+    // At this point, we have a polytope that contains the origin. Therefore, we
+    // can run the EPA algorithm.
+
+    if (nbTriangles == 0) {
+        return;
+    }
+
+    TriangleEPA* triangle = 0;
+    decimal upperBoundSquarePenDepth = DECIMAL_LARGEST;
+
+    do {
+        triangle = triangleHeap[0];
+
+        // Get the next candidate face (the face closest to the origin)
+        std::pop_heap(&triangleHeap[0], &triangleHeap[nbTriangles], mTriangleComparison);
+        nbTriangles--;
+
+        // If the candidate face in the heap is not obsolete
+        if (!triangle->getIsObsolete()) {
+            // If we have reached the maximum number of support points
+            if (nbVertices == MAX_SUPPORT_POINTS) {
+                assert(false);
+                break;
+            }
+
+            // Compute the support point of the Minkowski
+            // difference (A-B) in the closest point direction
+            suppPointsA[nbVertices] = shape1->getLocalSupportPointWithMargin(
+                                        triangle->getClosestPoint(), shape1CachedCollisionData);
+            suppPointsB[nbVertices] = body2Tobody1 *
+                               shape2->getLocalSupportPointWithMargin(rotateToBody2 *
+                                                                (-triangle->getClosestPoint()), shape2CachedCollisionData);
+            points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices];
+
+            int indexNewVertex = nbVertices;
+            nbVertices++;
+
+            // Update the upper bound of the penetration depth
+            decimal wDotv = points[indexNewVertex].dot(triangle->getClosestPoint());
+            assert(wDotv > 0.0);
+            decimal wDotVSquare = wDotv * wDotv / triangle->getDistSquare();
+            if (wDotVSquare < upperBoundSquarePenDepth) {
+                upperBoundSquarePenDepth = wDotVSquare;
+            }
+
+            // Compute the error
+            decimal error = wDotv - triangle->getDistSquare();
+            if (error <= std::max(tolerance, REL_ERROR_SQUARE * wDotv) ||
+                points[indexNewVertex] == points[(*triangle)[0]] ||
+                points[indexNewVertex] == points[(*triangle)[1]] ||
+                points[indexNewVertex] == points[(*triangle)[2]]) {
+                break;
+            }
+
+            // Now, we compute the silhouette cast by the new vertex. The current triangle
+            // face will not be in the convex hull. We start the local recursive silhouette
+            // algorithm from the current triangle face.
+            int i = triangleStore.getNbTriangles();
+            if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) {
+                break;
+            }
+
+            // Add all the new triangle faces computed with the silhouette algorithm
+            // to the candidates list of faces of the current polytope
+            while(i != triangleStore.getNbTriangles()) {
+                TriangleEPA* newTriangle = &triangleStore[i];
+                addFaceCandidate(newTriangle, triangleHeap, nbTriangles, upperBoundSquarePenDepth);
+                i++;
+            }
+        }
+    } while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
+
+    // Compute the contact info
+    v = transform1.getOrientation() * triangle->getClosestPoint();
+    Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA);
+    Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB);
+    Vector3 normal = v.getUnit();
+    decimal penetrationDepth = v.length();
+    assert(penetrationDepth > 0.0);
+
+    if (normal.lengthSquare() < MACHINE_EPSILON) return;
+    
+    // Create the contact info object
+    ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
+                                 shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal);
+
+    narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
+}

src/collision/narrowphase/EPA/EPAAlgorithm.h

@@ -0,0 +1,161 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_EPA_ALGORITHM_H
+#define REACTPHYSICS3D_EPA_ALGORITHM_H
+
+// Libraries
+#include "collision/narrowphase/GJK/Simplex.h"
+#include "collision/shapes/CollisionShape.h"
+#include "collision/CollisionShapeInfo.h"
+#include "constraint/ContactPoint.h"
+#include "collision/narrowphase/NarrowPhaseAlgorithm.h"
+#include "mathematics/mathematics.h"
+#include "TriangleEPA.h"
+#include "memory/MemoryAllocator.h"
+#include <algorithm>
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// ---------- Constants ---------- //
+
+/// Maximum number of support points of the polytope
+const unsigned int MAX_SUPPORT_POINTS = 100;
+
+/// Maximum number of facets of the polytope
+const unsigned int MAX_FACETS = 200;
+
+
+// Class TriangleComparison
+/**
+ * This class allows the comparison of two triangles in the heap
+ * The comparison between two triangles is made using their square distance to the closest
+ * point to the origin. The goal is that in the heap, the first triangle is the one with the
+ * smallest square distance.
+ */
+class TriangleComparison {
+
+    public:
+
+        /// Comparison operator
+        bool operator()(const TriangleEPA* face1, const TriangleEPA* face2) {
+            return (face1->getDistSquare() > face2->getDistSquare());
+        }
+};
+
+
+// Class EPAAlgorithm
+/**
+ * This class is the implementation of the Expanding Polytope Algorithm (EPA).
+ * The EPA algorithm computes the penetration depth and contact points between
+ * two enlarged objects (with margin) where the original objects (without margin)
+ * intersect. The penetration depth of a pair of intersecting objects A and B is
+ * the length of a point on the boundary of the Minkowski sum (A-B) closest to the
+ * origin. The goal of the EPA algorithm is to start with an initial simplex polytope
+ * that contains the origin and expend it in order to find the point on the boundary
+ * of (A-B) that is closest to the origin. An initial simplex that contains origin
+ * has been computed wit GJK algorithm. The EPA Algorithm will extend this simplex
+ * polytope to find the correct penetration depth. The implementation of the EPA
+ * algorithm is based on the book "Collision Detection in 3D Environments".
+ */
+class EPAAlgorithm {
+
+    private:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Reference to the memory allocator
+        MemoryAllocator* mMemoryAllocator;
+
+        /// Triangle comparison operator
+        TriangleComparison mTriangleComparison;
+        
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        EPAAlgorithm(const EPAAlgorithm& algorithm);
+
+        /// Private assignment operator
+        EPAAlgorithm& operator=(const EPAAlgorithm& algorithm);
+
+        /// Add a triangle face in the candidate triangle heap
+        void addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap, uint& nbTriangles,
+                              decimal upperBoundSquarePenDepth);
+
+        /// Decide if the origin is in the tetrahedron.
+        int isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
+                                  const Vector3& p3, const Vector3& p4) const;
+
+    public:
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        EPAAlgorithm();
+
+        /// Destructor
+        ~EPAAlgorithm();
+
+        /// Initalize the algorithm
+        void init(MemoryAllocator* memoryAllocator);
+
+        /// Compute the penetration depth with EPA algorithm.
+        void computePenetrationDepthAndContactPoints(const Simplex& simplex,
+                                                     CollisionShapeInfo shape1Info,
+                                                     const Transform& transform1,
+                                                     CollisionShapeInfo shape2Info,
+                                                     const Transform& transform2,
+                                                     Vector3& v,
+                                                    NarrowPhaseCallback* narrowPhaseCallback);
+};
+
+// Add a triangle face in the candidate triangle heap in the EPA algorithm
+inline void EPAAlgorithm::addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap,
+                                           uint& nbTriangles, decimal upperBoundSquarePenDepth) {
+    
+    // If the closest point of the affine hull of triangle
+    // points is internal to the triangle and if the distance
+    // of the closest point from the origin is at most the
+    // penetration depth upper bound
+    if (triangle->isClosestPointInternalToTriangle() &&
+        triangle->getDistSquare() <= upperBoundSquarePenDepth) {
+
+        // Add the triangle face to the list of candidates
+        heap[nbTriangles] = triangle;
+        nbTriangles++;
+        std::push_heap(&heap[0], &heap[nbTriangles], mTriangleComparison);
+    }
+}
+
+// Initalize the algorithm
+inline void EPAAlgorithm::init(MemoryAllocator* memoryAllocator) {
+    mMemoryAllocator = memoryAllocator;
+}
+
+}
+
+#endif
+

src/collision/narrowphase/EPA/EdgeEPA.cpp

@@ -0,0 +1,136 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "EdgeEPA.h"
+#include "TriangleEPA.h"
+#include "TrianglesStore.h"
+#include <cassert>
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+
+// Constructor
+EdgeEPA::EdgeEPA() {
+    
+}
+
+// Constructor
+EdgeEPA::EdgeEPA(TriangleEPA* ownerTriangle, int index)
+        : mOwnerTriangle(ownerTriangle), mIndex(index) {
+    assert(index >= 0 && index < 3);
+}
+
+// Copy-constructor
+EdgeEPA::EdgeEPA(const EdgeEPA& edge) {
+    mOwnerTriangle = edge.mOwnerTriangle;
+    mIndex = edge.mIndex;
+}
+
+// Destructor
+EdgeEPA::~EdgeEPA() {
+
+}
+
+// Return the index of the source vertex of the edge (vertex starting the edge)
+uint EdgeEPA::getSourceVertexIndex() const {
+    return (*mOwnerTriangle)[mIndex];
+}
+
+// Return the index of the target vertex of the edge (vertex ending the edge)
+uint EdgeEPA::getTargetVertexIndex() const {
+    return (*mOwnerTriangle)[indexOfNextCounterClockwiseEdge(mIndex)];
+}
+
+// Execute the recursive silhouette algorithm from this edge
+bool EdgeEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex,
+                                TrianglesStore& triangleStore) {
+    // If the edge has not already been visited
+    if (!mOwnerTriangle->getIsObsolete()) {
+
+        // If the triangle of this edge is not visible from the given point
+        if (!mOwnerTriangle->isVisibleFromVertex(vertices, indexNewVertex)) {
+            TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
+                                                              getTargetVertexIndex(),
+                                                              getSourceVertexIndex());
+
+            // If the triangle has been created
+            if (triangle != NULL) {
+                halfLink(EdgeEPA(triangle, 1), *this);
+                return true;
+            }
+
+            return false;
+        }
+        else {
+
+            // The current triangle is visible and therefore obsolete
+            mOwnerTriangle->setIsObsolete(true);
+
+            int backup = triangleStore.getNbTriangles();
+
+            if(!mOwnerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge(
+                                                this->mIndex)).computeSilhouette(vertices,
+                                                                                 indexNewVertex,
+                                                                                 triangleStore)) {
+                mOwnerTriangle->setIsObsolete(false);
+
+                TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
+                                                                  getTargetVertexIndex(),
+                                                                  getSourceVertexIndex());
+
+                // If the triangle has been created
+                if (triangle != NULL) {
+                    halfLink(EdgeEPA(triangle, 1), *this);
+                    return true;
+                }
+
+                return false;
+            }
+            else if (!mOwnerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(
+                                                  this->mIndex)).computeSilhouette(vertices,
+                                                                                   indexNewVertex,
+                                                                                   triangleStore)) {
+                mOwnerTriangle->setIsObsolete(false);
+
+                triangleStore.setNbTriangles(backup);
+
+                TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
+                                                                  getTargetVertexIndex(),
+                                                                  getSourceVertexIndex());
+
+                if (triangle != NULL) {
+                    halfLink(EdgeEPA(triangle, 1), *this);
+                    return true;
+                }
+
+                return false;
+            }
+        }
+    }
+
+    return true;
+}

src/collision/narrowphase/EPA/EdgeEPA.h

@@ -0,0 +1,122 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_EDGE_EPA_H
+#define REACTPHYSICS3D_EDGE_EPA_H
+
+
+// Libraries
+#include "mathematics/mathematics.h"
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Class declarations
+class TriangleEPA;
+class TrianglesStore;
+
+// Class EdgeEPA
+/**
+ * This class represents an edge of the current polytope in the EPA algorithm.
+ */
+class EdgeEPA {
+
+    private:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Pointer to the triangle that contains this edge
+        TriangleEPA* mOwnerTriangle;
+
+        /// Index of the edge in the triangle (between 0 and 2).
+        /// The edge with index i connect triangle vertices i and (i+1 % 3)
+        int mIndex;
+
+    public:
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        EdgeEPA();
+
+        /// Constructor
+        EdgeEPA(TriangleEPA* ownerTriangle, int index);
+
+        /// Copy-constructor
+        EdgeEPA(const EdgeEPA& edge);
+
+        /// Destructor
+        ~EdgeEPA();
+
+        /// Return the pointer to the owner triangle
+        TriangleEPA* getOwnerTriangle() const;
+
+        /// Return the index of the edge in the triangle
+        int getIndex() const;
+
+        /// Return index of the source vertex of the edge
+        uint getSourceVertexIndex() const;
+
+        /// Return the index of the target vertex of the edge
+        uint getTargetVertexIndex() const;
+
+        /// Execute the recursive silhouette algorithm from this edge
+        bool computeSilhouette(const Vector3* vertices, uint index, TrianglesStore& triangleStore);
+
+        /// Assignment operator
+        EdgeEPA& operator=(const EdgeEPA& edge);
+};
+
+// Return the pointer to the owner triangle
+inline TriangleEPA* EdgeEPA::getOwnerTriangle() const {
+    return mOwnerTriangle;
+}
+
+// Return the edge index
+inline int EdgeEPA::getIndex() const {
+    return mIndex;
+}
+
+// Assignment operator
+inline EdgeEPA& EdgeEPA::operator=(const EdgeEPA& edge) {
+    mOwnerTriangle = edge.mOwnerTriangle;
+    mIndex = edge.mIndex;
+    return *this;
+}
+
+// Return the index of the next counter-clockwise edge of the ownver triangle
+inline int indexOfNextCounterClockwiseEdge(int i) {
+    return (i + 1) % 3;
+}
+
+// Return the index of the previous counter-clockwise edge of the ownver triangle
+inline int indexOfPreviousCounterClockwiseEdge(int i) {
+    return (i + 2) % 3;
+}
+
+}
+
+#endif
+

src/collision/narrowphase/EPA/TriangleEPA.cpp

@@ -0,0 +1,155 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+
+// Libraries
+#include "TriangleEPA.h"
+#include "EdgeEPA.h"
+#include "TrianglesStore.h"
+
+// We use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Constructor
+TriangleEPA::TriangleEPA() {
+    
+}
+
+// Constructor
+TriangleEPA::TriangleEPA(uint indexVertex1, uint indexVertex2, uint indexVertex3)
+            : mIsObsolete(false) {
+    mIndicesVertices[0] = indexVertex1;
+    mIndicesVertices[1] = indexVertex2;
+    mIndicesVertices[2] = indexVertex3;
+}
+
+// Destructor
+TriangleEPA::~TriangleEPA() {
+
+}
+
+// Compute the point v closest to the origin of this triangle
+bool TriangleEPA::computeClosestPoint(const Vector3* vertices) {
+    const Vector3& p0 = vertices[mIndicesVertices[0]];
+
+    Vector3 v1 = vertices[mIndicesVertices[1]] - p0;
+    Vector3 v2 = vertices[mIndicesVertices[2]] - p0;
+    decimal v1Dotv1 = v1.dot(v1);
+    decimal v1Dotv2 = v1.dot(v2);
+    decimal v2Dotv2 = v2.dot(v2);
+    decimal p0Dotv1 = p0.dot(v1);
+    decimal p0Dotv2 = p0.dot(v2);
+
+    // Compute determinant
+    mDet = v1Dotv1 * v2Dotv2 - v1Dotv2 * v1Dotv2;
+
+    // Compute lambda values
+    mLambda1 = p0Dotv2 * v1Dotv2 - p0Dotv1 * v2Dotv2;
+    mLambda2 = p0Dotv1 * v1Dotv2 - p0Dotv2 * v1Dotv1;
+
+    // If the determinant is positive
+    if (mDet > 0.0) {
+        // Compute the closest point v
+        mClosestPoint = p0 + decimal(1.0) / mDet * (mLambda1 * v1 + mLambda2 * v2);
+
+        // Compute the square distance of closest point to the origin
+        mDistSquare = mClosestPoint.dot(mClosestPoint);
+
+        return true;
+    }
+
+    return false;
+}
+
+/// Link an edge with another one. It means that the current edge of a triangle will
+/// be associated with the edge of another triangle in order that both triangles
+/// are neighbour along both edges).
+bool reactphysics3d::link(const EdgeEPA& edge0, const EdgeEPA& edge1) {
+    bool isPossible = (edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() &&
+                       edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex());
+
+    if (isPossible) {
+        edge0.getOwnerTriangle()->mAdjacentEdges[edge0.getIndex()] = edge1;
+        edge1.getOwnerTriangle()->mAdjacentEdges[edge1.getIndex()] = edge0;
+    }
+
+    return isPossible;
+}
+
+/// Make an half link of an edge with another one from another triangle. An half-link
+/// between an edge "edge0" and an edge "edge1" represents the fact that "edge1" is an
+/// adjacent edge of "edge0" but not the opposite. The opposite edge connection will
+/// be made later.
+void reactphysics3d::halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1) {
+    assert(edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() &&
+           edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex());
+
+    // Link
+    edge0.getOwnerTriangle()->mAdjacentEdges[edge0.getIndex()] = edge1;
+}
+
+// Execute the recursive silhouette algorithm from this triangle face.
+/// The parameter "vertices" is an array that contains the vertices of the current polytope and the
+/// parameter "indexNewVertex" is the index of the new vertex in this array. The goal of the
+/// silhouette algorithm is to add the new vertex in the polytope by keeping it convex. Therefore,
+/// the triangle faces that are visible from the new vertex must be removed from the polytope and we
+/// need to add triangle faces where each face contains the new vertex and an edge of the silhouette.
+/// The silhouette is the connected set of edges that are part of the border between faces that
+/// are seen and faces that are not seen from the new vertex. This method starts from the nearest
+/// face from the new vertex, computes the silhouette and create the new faces from the new vertex in
+/// order that we always have a convex polytope. The faces visible from the new vertex are set
+/// obselete and will not be considered as being a candidate face in the future.
+bool TriangleEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex,
+                                    TrianglesStore& triangleStore) {
+    
+    uint first = triangleStore.getNbTriangles();
+
+    // Mark the current triangle as obsolete because it
+    setIsObsolete(true);
+
+    // Execute recursively the silhouette algorithm for the adjacent edges of neighboring
+    // triangles of the current triangle
+    bool result = mAdjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) &&
+                  mAdjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore) &&
+                  mAdjacentEdges[2].computeSilhouette(vertices, indexNewVertex, triangleStore);
+
+    if (result) {
+        int i,j;
+
+        // For each triangle face that contains the new vertex and an edge of the silhouette
+        for (i=first, j=triangleStore.getNbTriangles()-1;
+             i != triangleStore.getNbTriangles(); j = i++) {
+            TriangleEPA* triangle = &triangleStore[i];
+            halfLink(triangle->getAdjacentEdge(1), EdgeEPA(triangle, 1));
+
+            if (!link(EdgeEPA(triangle, 0), EdgeEPA(&triangleStore[j], 2))) {
+                return false;
+            }
+        }
+
+    }
+
+    return result;
+}

src/collision/narrowphase/EPA/TriangleEPA.h

@@ -0,0 +1,199 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_TRIANGLE_EPA_H
+#define REACTPHYSICS3D_TRIANGLE_EPA_H
+
+// Libraries
+#include "mathematics/mathematics.h"
+#include "configuration.h"
+#include "EdgeEPA.h"
+#include <cassert>
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Prototypes
+bool link(const EdgeEPA& edge0, const EdgeEPA& edge1);
+void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1);
+
+
+// Class TriangleEPA
+/**
+ * This class represents a triangle face of the current polytope in the EPA algorithm.
+ */
+class TriangleEPA {
+
+    private:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Indices of the vertices y_i of the triangle
+        uint mIndicesVertices[3];
+
+        /// Three adjacent edges of the triangle (edges of other triangles)
+        EdgeEPA mAdjacentEdges[3];
+
+        /// True if the triangle face is visible from the new support point
+        bool mIsObsolete;
+
+        /// Determinant
+        decimal mDet;
+
+        /// Point v closest to the origin on the affine hull of the triangle
+        Vector3 mClosestPoint;
+
+        /// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
+        decimal mLambda1;
+
+        /// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
+        decimal mLambda2;
+
+        /// Square distance of the point closest point v to the origin
+        decimal mDistSquare;
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        TriangleEPA(const TriangleEPA& triangle);
+
+        /// Private assignment operator
+        TriangleEPA& operator=(const TriangleEPA& triangle);
+
+    public:
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        TriangleEPA();
+
+        /// Constructor
+        TriangleEPA(uint v1, uint v2, uint v3);
+
+        /// Destructor
+        ~TriangleEPA();
+
+        /// Return an adjacent edge of the triangle
+        EdgeEPA& getAdjacentEdge(int index);
+
+        /// Set an adjacent edge of the triangle
+        void setAdjacentEdge(int index, EdgeEPA& edge);
+
+        /// Return the square distance of the closest point to origin
+        decimal getDistSquare() const;
+
+        /// Set the isObsolete value
+        void setIsObsolete(bool isObsolete);
+
+        /// Return true if the triangle face is obsolete
+        bool getIsObsolete() const;
+
+        /// Return the point closest to the origin
+        const Vector3& getClosestPoint() const;
+
+        // Return true if the closest point on affine hull is inside the triangle
+        bool isClosestPointInternalToTriangle() const;
+
+        /// Return true if the triangle is visible from a given vertex
+        bool isVisibleFromVertex(const Vector3* vertices, uint index) const;
+
+        /// Compute the point v closest to the origin of this triangle
+        bool computeClosestPoint(const Vector3* vertices);
+
+        /// Compute the point of an object closest to the origin
+        Vector3 computeClosestPointOfObject(const Vector3* supportPointsOfObject) const;
+
+        /// Execute the recursive silhouette algorithm from this triangle face.
+        bool computeSilhouette(const Vector3* vertices, uint index, TrianglesStore& triangleStore);
+
+        /// Access operator
+        uint operator[](int i) const;
+
+        /// Associate two edges
+        friend bool link(const EdgeEPA& edge0, const EdgeEPA& edge1);
+
+        /// Make a half-link between two edges
+        friend void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1);
+};
+
+// Return an edge of the triangle
+inline EdgeEPA& TriangleEPA::getAdjacentEdge(int index) {
+    assert(index >= 0 && index < 3);
+    return mAdjacentEdges[index];
+}
+
+// Set an adjacent edge of the triangle
+inline void TriangleEPA::setAdjacentEdge(int index, EdgeEPA& edge) {
+    assert(index >=0 && index < 3);
+    mAdjacentEdges[index] = edge;
+}
+
+// Return the square distance  of the closest point to origin
+inline decimal TriangleEPA::getDistSquare() const {
+    return mDistSquare;
+}
+
+// Set the isObsolete value
+inline void TriangleEPA::setIsObsolete(bool isObsolete) {
+    mIsObsolete = isObsolete;
+}
+
+// Return true if the triangle face is obsolete
+inline bool TriangleEPA::getIsObsolete() const {
+    return mIsObsolete;
+}
+
+// Return the point closest to the origin
+inline const Vector3& TriangleEPA::getClosestPoint() const {
+    return mClosestPoint;
+}
+
+// Return true if the closest point on affine hull is inside the triangle
+inline bool TriangleEPA::isClosestPointInternalToTriangle() const {
+    return (mLambda1 >= 0.0 && mLambda2 >= 0.0 && (mLambda1 + mLambda2) <= mDet);
+}
+
+// Return true if the triangle is visible from a given vertex
+inline bool TriangleEPA::isVisibleFromVertex(const Vector3* vertices, uint index) const {
+    Vector3 closestToVert = vertices[index] - mClosestPoint;
+    return (mClosestPoint.dot(closestToVert) > 0.0);
+}
+
+// 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 + decimal(1.0)/mDet * (mLambda1 * (supportPointsOfObject[mIndicesVertices[1]] - p0) +
+                           mLambda2 * (supportPointsOfObject[mIndicesVertices[2]] - p0));
+}
+
+// Access operator
+inline uint TriangleEPA::operator[](int i) const {
+    assert(i >= 0 && i <3);
+    return mIndicesVertices[i];
+}
+
+}
+
+#endif

src/collision/narrowphase/EPA/TrianglesStore.cpp

@@ -0,0 +1,40 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "TrianglesStore.h"
+
+// We use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Constructor
+TrianglesStore::TrianglesStore() : mNbTriangles(0) {
+    
+}
+
+// Destructor
+TrianglesStore::~TrianglesStore() {
+
+}

src/collision/narrowphase/EPA/TrianglesStore.h

@@ -0,0 +1,141 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_TRIANGLES_STORE_H
+#define REACTPHYSICS3D_TRIANGLES_STORE_H
+
+#include "TriangleEPA.h"
+
+
+// Libraries
+#include <cassert>
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Constants
+const unsigned int MAX_TRIANGLES = 200;     // Maximum number of triangles
+
+// Class TriangleStore
+/**
+ * This class stores several triangles of the polytope in the EPA algorithm.
+ */
+class TrianglesStore {
+
+    private:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Triangles
+        TriangleEPA mTriangles[MAX_TRIANGLES];
+
+        /// Number of triangles
+        int mNbTriangles;
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        TrianglesStore(const TrianglesStore& triangleStore);
+
+        /// Private assignment operator
+        TrianglesStore& operator=(const TrianglesStore& triangleStore);
+        
+    public:
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        TrianglesStore();
+
+        /// Destructor
+        ~TrianglesStore();
+
+        /// Clear all the storage
+        void clear();
+
+        /// Return the number of triangles
+        int getNbTriangles() const;
+
+        /// Set the number of triangles
+        void setNbTriangles(int backup);
+
+        /// Return the last triangle
+        TriangleEPA& last();
+
+        /// Create a new triangle
+        TriangleEPA* newTriangle(const Vector3* vertices, uint v0, uint v1, uint v2);
+
+        /// Access operator
+        TriangleEPA& operator[](int i);
+};
+
+// Clear all the storage
+inline void TrianglesStore::clear() {
+    mNbTriangles = 0;
+}
+
+// Return the number of triangles
+inline int TrianglesStore::getNbTriangles() const {
+    return mNbTriangles;
+}
+
+
+inline void TrianglesStore::setNbTriangles(int backup) {
+    mNbTriangles = backup;
+}
+
+// Return the last triangle
+inline TriangleEPA& TrianglesStore::last() {
+    assert(mNbTriangles > 0);
+    return mTriangles[mNbTriangles - 1];
+}
+
+// Create a new triangle
+inline TriangleEPA* TrianglesStore::newTriangle(const Vector3* vertices,
+                                                uint v0,uint v1, uint v2) {
+    TriangleEPA* newTriangle = NULL;
+
+    // If we have not reached the maximum number of triangles
+    if (mNbTriangles != MAX_TRIANGLES) {
+        newTriangle = &mTriangles[mNbTriangles++];
+        new (newTriangle) TriangleEPA(v0, v1, v2);
+        if (!newTriangle->computeClosestPoint(vertices)) {
+            mNbTriangles--;
+            newTriangle = NULL;
+        }
+    }
+
+    // Return the new triangle
+    return newTriangle;
+}
+
+// Access operator
+inline TriangleEPA& TrianglesStore::operator[](int i) {
+    return mTriangles[i];
+}
+
+}
+
+#endif

src/collision/narrowphase/GJK/GJKAlgorithm.cpp

@@ -0,0 +1,528 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "GJKAlgorithm.h"
+#include "Simplex.h"
+#include "constraint/ContactPoint.h"
+#include "configuration.h"
+#include "engine/Profiler.h"
+#include <algorithm>
+#include <cmath>
+#include <cfloat>
+#include <cassert>
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Constructor
+GJKAlgorithm::GJKAlgorithm() : NarrowPhaseAlgorithm() {
+
+}
+
+// Destructor
+GJKAlgorithm::~GJKAlgorithm() {
+
+}
+
+// Compute a contact info if the two collision shapes collide.
+/// This method implements the Hybrid Technique for computing the penetration depth by
+/// running the GJK algorithm on original objects (without margin). If the shapes intersect
+/// only in the margins, the method compute the penetration depth and contact points
+/// (of enlarged objects). If the original objects (without margin) intersect, we
+/// call the computePenetrationDepthForEnlargedObjects() method that run the GJK
+/// 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.
+void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
+                                 const CollisionShapeInfo& shape2Info,
+                                 NarrowPhaseCallback* narrowPhaseCallback) {
+
+    PROFILE("GJKAlgorithm::testCollision()");
+    
+    Vector3 suppA;             // Support point of object A
+    Vector3 suppB;             // Support point of object B
+    Vector3 w;                 // Support point of Minkowski difference A-B
+    Vector3 pA;                // Closest point of object A
+    Vector3 pB;                // Closest point of object B
+    decimal vDotw;
+    decimal prevDistSquare;
+
+    assert(shape1Info.collisionShape->isConvex());
+    assert(shape2Info.collisionShape->isConvex());
+
+    const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
+    const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
+
+    void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
+    void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
+
+    // Get the local-space to world-space transforms
+    const Transform transform1 = shape1Info.shapeToWorldTransform;
+    const Transform transform2 = shape2Info.shapeToWorldTransform;
+
+    // Transform a point from local space of body 2 to local
+    // space of body 1 (the GJK algorithm is done in local space of body 1)
+    Transform body2Tobody1 = transform1.getInverse() * transform2;
+
+    // Matrix that transform a direction from local
+    // space of body 1 into local space of body 2
+    Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *
+                              transform1.getOrientation().getMatrix();
+
+    // Initialize the margin (sum of margins of both objects)
+    decimal margin = shape1->getMargin() + shape2->getMargin();
+    decimal marginSquare = margin * margin;
+    assert(margin > 0.0);
+
+    // Create a simplex set
+    Simplex simplex;
+
+    // Get the previous point V (last cached separating axis)
+    Vector3 v = mCurrentOverlappingPair->getCachedSeparatingAxis();
+
+    // Initialize the upper bound for the square distance
+    decimal distSquare = DECIMAL_LARGEST;
+    
+    do {
+              
+        // Compute the support points for original objects (without margins) A and B
+        suppA = shape1->getLocalSupportPointWithoutMargin(-v, shape1CachedCollisionData);
+        suppB = body2Tobody1 *
+                     shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData);
+
+        // Compute the support point for the Minkowski difference A-B
+        w = suppA - suppB;
+        
+        vDotw = v.dot(w);
+        
+        // If the enlarge objects (with margins) do not intersect
+        if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) {
+                        
+            // Cache the current separating axis for frame coherence
+            mCurrentOverlappingPair->setCachedSeparatingAxis(v);
+            
+            // No intersection, we return
+            return;
+        }
+
+        // If the objects intersect only in the margins
+        if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) {
+
+            // Compute the closet points of both objects (without the margins)
+            simplex.computeClosestPointsOfAandB(pA, pB);
+
+            // Project those two points on the margins to have the closest points of both
+            // object with the margins
+            decimal dist = sqrt(distSquare);
+            assert(dist > 0.0);
+            pA = (pA - (shape1->getMargin() / dist) * v);
+            pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
+
+            // Compute the contact info
+            Vector3 normal = transform1.getOrientation() * (-v.getUnit());
+            decimal penetrationDepth = margin - dist;
+			
+			// Reject the contact if the penetration depth is negative (due too numerical errors)
+            if (penetrationDepth <= 0.0) return;
+			
+            // Create the contact info object
+            ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
+                                         shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
+
+            narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
+
+            // There is an intersection, therefore we return
+            return;
+        }
+
+        // Add the new support point to the simplex
+        simplex.addPoint(w, suppA, suppB);
+
+        // If the simplex is affinely dependent
+        if (simplex.isAffinelyDependent()) {
+
+            // Compute the closet points of both objects (without the margins)
+            simplex.computeClosestPointsOfAandB(pA, pB);
+
+            // Project those two points on the margins to have the closest points of both
+            // object with the margins
+            decimal dist = sqrt(distSquare);
+            assert(dist > 0.0);
+            pA = (pA - (shape1->getMargin() / dist) * v);
+            pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
+
+            // Compute the contact info
+            Vector3 normal = transform1.getOrientation() * (-v.getUnit());
+            decimal penetrationDepth = margin - dist;
+			
+			// Reject the contact if the penetration depth is negative (due too numerical errors)
+            if (penetrationDepth <= 0.0) return;
+			
+            // Create the contact info object
+            ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
+                                         shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
+
+            narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
+
+            // There is an intersection, therefore we return
+            return;
+        }
+
+        // Compute the point of the simplex closest to the origin
+        // If the computation of the closest point fail
+        if (!simplex.computeClosestPoint(v)) {
+
+            // Compute the closet points of both objects (without the margins)
+            simplex.computeClosestPointsOfAandB(pA, pB);
+
+            // Project those two points on the margins to have the closest points of both
+            // object with the margins
+            decimal dist = sqrt(distSquare);
+            assert(dist > 0.0);
+            pA = (pA - (shape1->getMargin() / dist) * v);
+            pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
+
+            // Compute the contact info
+            Vector3 normal = transform1.getOrientation() * (-v.getUnit());
+            decimal penetrationDepth = margin - dist;
+			
+			// Reject the contact if the penetration depth is negative (due too numerical errors)
+            if (penetrationDepth <= 0.0) return;
+			
+            // Create the contact info object
+            ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
+                                         shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
+
+            narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
+
+            // There is an intersection, therefore we return
+            return;
+        }
+
+        // Store and update the squared distance of the closest point
+        prevDistSquare = distSquare;
+        distSquare = v.lengthSquare();
+
+        // If the distance to the closest point doesn't improve a lot
+        if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
+            simplex.backupClosestPointInSimplex(v);
+            
+            // Get the new squared distance
+            distSquare = v.lengthSquare();
+
+            // Compute the closet points of both objects (without the margins)
+            simplex.computeClosestPointsOfAandB(pA, pB);
+
+            // Project those two points on the margins to have the closest points of both
+            // object with the margins
+            decimal dist = sqrt(distSquare);
+            assert(dist > 0.0);
+            pA = (pA - (shape1->getMargin() / dist) * v);
+            pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
+
+            // Compute the contact info
+            Vector3 normal = transform1.getOrientation() * (-v.getUnit());
+            decimal penetrationDepth = margin - dist;
+			
+			// Reject the contact if the penetration depth is negative (due too numerical errors)
+            if (penetrationDepth <= 0.0) return;
+			
+            // Create the contact info object
+            ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
+                                         shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
+
+            narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
+
+            // There is an intersection, therefore we return
+            return;
+        }
+    } while(!simplex.isFull() && distSquare > MACHINE_EPSILON *
+                                 simplex.getMaxLengthSquareOfAPoint());
+
+    // The objects (without margins) intersect. Therefore, we run the GJK algorithm
+    // again but on the 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(shape1Info, transform1, shape2Info,
+                                                     transform2, narrowPhaseCallback, v);
+}
+
+/// This method runs the GJK algorithm on the two enlarged objects (with margin)
+/// to compute a simplex polytope that contains the origin. The two objects are
+/// 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.
+void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
+                                                             const Transform& transform1,
+                                                             const CollisionShapeInfo& shape2Info,
+                                                             const Transform& transform2,
+                                                             NarrowPhaseCallback* narrowPhaseCallback,
+                                                             Vector3& v) {
+    PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()");
+
+    Simplex simplex;
+    Vector3 suppA;
+    Vector3 suppB;
+    Vector3 w;
+    decimal vDotw;
+    decimal distSquare = DECIMAL_LARGEST;
+    decimal prevDistSquare;
+
+    assert(shape1Info.collisionShape->isConvex());
+    assert(shape2Info.collisionShape->isConvex());
+
+    const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
+    const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
+
+    void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
+    void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
+
+    // Transform a point from local space of body 2 to local space
+    // of body 1 (the GJK algorithm is done in local space of body 1)
+    Transform body2ToBody1 = transform1.getInverse() * transform2;
+
+    // Matrix that transform a direction from local space of body 1 into local space of body 2
+    Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *
+                              transform1.getOrientation().getMatrix();
+    
+    do {
+        // Compute the support points for the enlarged object A and B
+        suppA = shape1->getLocalSupportPointWithMargin(-v, shape1CachedCollisionData);
+        suppB = body2ToBody1 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * v, shape2CachedCollisionData);
+
+        // Compute the support point for the Minkowski difference A-B
+        w = suppA - suppB;
+
+        vDotw = v.dot(w);
+
+        // If the enlarge objects do not intersect
+        if (vDotw > 0.0) {
+
+            // No intersection, we return
+            return;
+        }
+
+        // Add the new support point to the simplex
+        simplex.addPoint(w, suppA, suppB);
+
+        if (simplex.isAffinelyDependent()) {
+            return;
+        }
+
+        if (!simplex.computeClosestPoint(v)) {
+            return;
+        }
+
+        // Store and update the square distance
+        prevDistSquare = distSquare;
+        distSquare = v.lengthSquare();
+
+        if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
+            return;
+        }
+
+    } while(!simplex.isFull() && distSquare > MACHINE_EPSILON *
+                                 simplex.getMaxLengthSquareOfAPoint());
+
+    // 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 mAlgoEPA.computePenetrationDepthAndContactPoints(simplex, shape1Info,
+                                                            transform1, shape2Info, transform2,
+                                                            v, narrowPhaseCallback);
+}
+
+// Use the GJK Algorithm to find if a point is inside a convex collision shape
+bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) {
+
+    Vector3 suppA;             // Support point of object A
+    Vector3 w;                 // Support point of Minkowski difference A-B
+    decimal prevDistSquare;
+
+    assert(proxyShape->getCollisionShape()->isConvex());
+
+    const ConvexShape* shape = static_cast<const ConvexShape*>(proxyShape->getCollisionShape());
+
+    void** shapeCachedCollisionData = proxyShape->getCachedCollisionData();
+
+    // Support point of object B (object B is a single point)
+    const Vector3 suppB(localPoint);
+
+    // Create a simplex set
+    Simplex simplex;
+
+    // Initial supporting direction
+    Vector3 v(1, 1, 1);
+
+    // Initialize the upper bound for the square distance
+    decimal distSquare = DECIMAL_LARGEST;
+
+    do {
+
+        // Compute the support points for original objects (without margins) A and B
+        suppA = shape->getLocalSupportPointWithoutMargin(-v, shapeCachedCollisionData);
+
+        // Compute the support point for the Minkowski difference A-B
+        w = suppA - suppB;
+
+        // Add the new support point to the simplex
+        simplex.addPoint(w, suppA, suppB);
+
+        // If the simplex is affinely dependent
+        if (simplex.isAffinelyDependent()) {
+
+            return false;
+        }
+
+        // Compute the point of the simplex closest to the origin
+        // If the computation of the closest point fail
+        if (!simplex.computeClosestPoint(v)) {
+
+            return false;
+        }
+
+        // Store and update the squared distance of the closest point
+        prevDistSquare = distSquare;
+        distSquare = v.lengthSquare();
+
+        // If the distance to the closest point doesn't improve a lot
+        if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
+
+            return false;
+        }
+    } while(!simplex.isFull() && distSquare > MACHINE_EPSILON *
+                                 simplex.getMaxLengthSquareOfAPoint());
+
+    // The point is inside the collision shape
+    return true;
+}
+
+
+// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
+/// This method implements the GJK ray casting algorithm described by Gino Van Den Bergen in
+/// "Ray Casting against General Convex Objects with Application to Continuous Collision Detection".
+bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo) {
+
+    assert(proxyShape->getCollisionShape()->isConvex());
+
+    const ConvexShape* shape = static_cast<const ConvexShape*>(proxyShape->getCollisionShape());
+
+    void** shapeCachedCollisionData = proxyShape->getCachedCollisionData();
+
+    Vector3 suppA;      // Current lower bound point on the ray (starting at ray's origin)
+    Vector3 suppB;      // Support point on the collision shape
+    const decimal machineEpsilonSquare = MACHINE_EPSILON * MACHINE_EPSILON;
+    const decimal epsilon = decimal(0.0001);
+
+    // Convert the ray origin and direction into the local-space of the collision shape
+    Vector3 rayDirection = ray.point2 - ray.point1;
+
+    // If the points of the segment are two close, return no hit
+    if (rayDirection.lengthSquare() < machineEpsilonSquare) return false;
+
+    Vector3 w;
+
+    // Create a simplex set
+    Simplex simplex;
+
+    Vector3 n(decimal(0.0), decimal(0.0), decimal(0.0));
+    decimal lambda = decimal(0.0);
+    suppA = ray.point1;    // Current lower bound point on the ray (starting at ray's origin)
+    suppB = shape->getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData);
+    Vector3 v = suppA - suppB;
+    decimal vDotW, vDotR;
+    decimal distSquare = v.lengthSquare();
+    int nbIterations = 0;
+
+    // GJK Algorithm loop
+    while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) {
+
+        // Compute the support points
+        suppB = shape->getLocalSupportPointWithoutMargin(v, shapeCachedCollisionData);
+        w = suppA - suppB;
+
+        vDotW = v.dot(w);
+
+        if (vDotW > decimal(0)) {
+
+            vDotR = v.dot(rayDirection);
+
+            if (vDotR >= -machineEpsilonSquare) {
+                return false;
+            }
+            else {
+
+                // We have found a better lower bound for the hit point along the ray
+                lambda = lambda - vDotW / vDotR;
+                suppA = ray.point1 + lambda * rayDirection;
+                w = suppA - suppB;
+                n = v;
+            }
+        }
+
+        // Add the new support point to the simplex
+        if (!simplex.isPointInSimplex(w)) {
+            simplex.addPoint(w, suppA, suppB);
+        }
+
+        // Compute the closest point
+        if (simplex.computeClosestPoint(v)) {
+
+            distSquare = v.lengthSquare();
+        }
+        else {
+            distSquare = decimal(0.0);
+        }
+
+        // If the current lower bound distance is larger than the maximum raycasting distance
+        if (lambda > ray.maxFraction) return false;
+
+        nbIterations++;
+    }
+
+    // If the origin was inside the shape, we return no hit
+    if (lambda < MACHINE_EPSILON) return false;
+
+    // Compute the closet points of both objects (without the margins)
+    Vector3 pointA;
+    Vector3 pointB;
+    simplex.computeClosestPointsOfAandB(pointA, pointB);
+
+    // A raycast hit has been found, we fill in the raycast info
+    raycastInfo.hitFraction = lambda;
+    raycastInfo.worldPoint = pointB;
+    raycastInfo.body = proxyShape->getBody();
+    raycastInfo.proxyShape = proxyShape;
+
+    if (n.lengthSquare() >= machineEpsilonSquare) { // The normal vector is valid
+        raycastInfo.worldNormal = n;
+    }
+    else {  // Degenerated normal vector, we return a zero normal vector
+        raycastInfo.worldNormal = Vector3(decimal(0), decimal(0), decimal(0));
+    }
+
+    return true;
+}

src/collision/narrowphase/GJK/GJKAlgorithm.h

@@ -0,0 +1,120 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_GJK_ALGORITHM_H
+#define REACTPHYSICS3D_GJK_ALGORITHM_H
+
+// Libraries
+#include "collision/narrowphase/NarrowPhaseAlgorithm.h"
+#include "constraint/ContactPoint.h"
+#include "collision/shapes/ConvexShape.h"
+#include "collision/narrowphase/EPA/EPAAlgorithm.h"
+
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Constants
+const decimal REL_ERROR = decimal(1.0e-3);
+const decimal REL_ERROR_SQUARE = REL_ERROR * REL_ERROR;
+const int MAX_ITERATIONS_GJK_RAYCAST = 32;
+
+// Class GJKAlgorithm
+/**
+ * This class implements a narrow-phase collision detection algorithm. This
+ * algorithm uses the ISA-GJK algorithm and the EPA algorithm. This
+ * implementation is based on the implementation discussed in the book
+ * "Collision Detection in Interactive 3D Environments" by Gino van den Bergen.
+ * This method implements the Hybrid Technique for calculating the
+ * penetration depth. The two objects are enlarged with a small margin. If
+ * the object intersects in their margins, the penetration depth is quickly
+ * computed using the GJK algorithm on the original objects (without margin).
+ * If the original objects (without margin) intersect, we run again the GJK
+ * algorithm on the enlarged objects (with margin) to compute simplex
+ * polytope that contains the origin and give it to the EPA (Expanding
+ * Polytope Algorithm) to compute the correct penetration depth between the
+ * enlarged objects.
+ */
+class GJKAlgorithm : public NarrowPhaseAlgorithm {
+
+    private :
+
+        // -------------------- Attributes -------------------- //
+
+        /// EPA Algorithm
+        EPAAlgorithm mAlgoEPA;
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        GJKAlgorithm(const GJKAlgorithm& algorithm);
+
+        /// Private assignment operator
+        GJKAlgorithm& operator=(const GJKAlgorithm& algorithm);
+
+        /// Compute the penetration depth for enlarged objects.
+        void computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
+                                                       const Transform& transform1,
+                                                       const CollisionShapeInfo& shape2Info,
+                                                       const Transform& transform2,
+                                                       NarrowPhaseCallback* narrowPhaseCallback,
+                                                       Vector3& v);
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        GJKAlgorithm();
+
+        /// Destructor
+        ~GJKAlgorithm();
+
+        /// Initalize the algorithm
+        virtual void init(CollisionDetection* collisionDetection,
+                          MemoryAllocator* memoryAllocator);
+
+        /// Compute a contact info if the two bounding volumes collide.
+        virtual void testCollision(const CollisionShapeInfo& shape1Info,
+                                   const CollisionShapeInfo& shape2Info,
+                                   NarrowPhaseCallback* narrowPhaseCallback);
+
+        /// Use the GJK Algorithm to find if a point is inside a convex collision shape
+        bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape);
+
+        /// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
+        bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo);
+};
+
+// Initalize the algorithm
+inline void GJKAlgorithm::init(CollisionDetection* collisionDetection,
+                               MemoryAllocator* memoryAllocator) {
+    NarrowPhaseAlgorithm::init(collisionDetection, memoryAllocator);
+    mAlgoEPA.init(memoryAllocator);
+}
+
+}
+
+#endif

src/collision/narrowphase/GJK/Simplex.cpp

@@ -0,0 +1,394 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "Simplex.h"
+#include <cfloat>
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Constructor
+Simplex::Simplex() : mBitsCurrentSimplex(0x0), mAllBits(0x0) {
+
+}
+
+// Destructor
+Simplex::~Simplex() {
+
+}
+
+// Add a new support point of (A-B) into the simplex
+/// suppPointA : support point of object A in a direction -v
+/// suppPointB : support point of object B in a direction v
+/// point      : support point of object (A-B) => point = suppPointA - suppPointB
+void Simplex::addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB) {
+    assert(!isFull());
+
+    mLastFound = 0;
+    mLastFoundBit = 0x1;
+
+    // Look for the bit corresponding to one of the four point that is not in
+    // the current simplex
+    while (overlap(mBitsCurrentSimplex, mLastFoundBit)) {
+        mLastFound++;
+        mLastFoundBit <<= 1;
+    }
+
+    assert(mLastFound < 4);
+
+    // Add the point into the simplex
+    mPoints[mLastFound] = point;
+    mPointsLengthSquare[mLastFound] = point.dot(point);
+    mAllBits = mBitsCurrentSimplex | mLastFoundBit;
+
+    // Update the cached values
+    updateCache();
+    
+    // Compute the cached determinant values
+    computeDeterminants();
+    
+    // Add the support points of objects A and B
+    mSuppPointsA[mLastFound] = suppPointA;
+    mSuppPointsB[mLastFound] = suppPointB;
+}
+
+// Return true if the point is in the simplex
+bool Simplex::isPointInSimplex(const Vector3& point) const {
+    int i;
+    Bits bit;
+
+    // For each four possible points in the simplex
+    for (i=0, bit = 0x1; i<4; i++, bit <<= 1) {
+        // Check if the current point is in the simplex
+        if (overlap(mAllBits, bit) && point == mPoints[i]) return true;
+    }
+
+    return false;
+}
+
+// Update the cached values used during the GJK algorithm
+void Simplex::updateCache() {
+    int i;
+    Bits bit;
+
+    // For each of the four possible points of the simplex
+    for (i=0, bit = 0x1; i<4; i++, bit <<= 1) {
+        // If the current points is in the simplex
+        if (overlap(mBitsCurrentSimplex, bit)) {
+            
+            // Compute the distance between two points in the possible simplex set
+            mDiffLength[i][mLastFound] = mPoints[i] - mPoints[mLastFound];
+            mDiffLength[mLastFound][i] = -mDiffLength[i][mLastFound];
+
+            // Compute the squared length of the vector
+            // distances from points in the possible simplex set
+            mNormSquare[i][mLastFound] = mNormSquare[mLastFound][i] =
+                                         mDiffLength[i][mLastFound].dot(mDiffLength[i][mLastFound]);
+        }
+    }
+}
+
+// Return the points of the simplex
+unsigned int Simplex::getSimplex(Vector3* suppPointsA, Vector3* suppPointsB,
+                                 Vector3* points) const {
+    unsigned int nbVertices = 0;
+    int i;
+    Bits bit;
+
+    // For each four point in the possible simplex
+    for (i=0, bit=0x1; i<4; i++, bit <<=1) {
+
+        // If the current point is in the simplex
+        if (overlap(mBitsCurrentSimplex, bit)) {
+
+            // Store the points
+            suppPointsA[nbVertices] = this->mSuppPointsA[nbVertices];
+            suppPointsB[nbVertices] = this->mSuppPointsB[nbVertices];
+            points[nbVertices] = this->mPoints[nbVertices];
+
+            nbVertices++;
+        }
+    }
+
+    // Return the number of points in the simplex
+    return nbVertices;
+}
+
+// Compute the cached determinant values
+void Simplex::computeDeterminants() {
+    mDet[mLastFoundBit][mLastFound] = 1.0;
+
+    // If the current simplex is not empty
+    if (!isEmpty()) {
+        int i;
+        Bits bitI;
+
+        // For each possible four points in the simplex set
+        for (i=0, bitI = 0x1; i<4; i++, bitI <<= 1) {
+
+            // If the current point is in the simplex
+            if (overlap(mBitsCurrentSimplex, bitI)) {
+                Bits bit2 = bitI | mLastFoundBit;
+
+                mDet[bit2][i] = mDiffLength[mLastFound][i].dot(mPoints[mLastFound]);
+                mDet[bit2][mLastFound] = mDiffLength[i][mLastFound].dot(mPoints[i]);
+      
+
+                int j;
+                Bits bitJ;
+
+                for (j=0, bitJ = 0x1; j<i; j++, bitJ <<= 1) {
+                    if (overlap(mBitsCurrentSimplex, bitJ)) {
+                        int k;
+                        Bits bit3 = bitJ | bit2;
+
+                        k = mNormSquare[i][j] < mNormSquare[mLastFound][j] ? i : mLastFound;
+                        mDet[bit3][j] = mDet[bit2][i] * mDiffLength[k][j].dot(mPoints[i]) +
+                                       mDet[bit2][mLastFound] *
+                                       mDiffLength[k][j].dot(mPoints[mLastFound]);
+
+                        k = mNormSquare[j][i] < mNormSquare[mLastFound][i] ? j : mLastFound;
+                        mDet[bit3][i] = mDet[bitJ | mLastFoundBit][j] *
+                                        mDiffLength[k][i].dot(mPoints[j]) +
+                                        mDet[bitJ | mLastFoundBit][mLastFound] *
+                                        mDiffLength[k][i].dot(mPoints[mLastFound]);
+
+                        k = mNormSquare[i][mLastFound] < mNormSquare[j][mLastFound] ? i : j;
+                        mDet[bit3][mLastFound] = mDet[bitJ | bitI][j] *
+                                                 mDiffLength[k][mLastFound].dot(mPoints[j]) +
+                                                 mDet[bitJ | bitI][i] *
+                                                 mDiffLength[k][mLastFound].dot(mPoints[i]);
+                    }
+                }
+            }
+        }
+
+        if (mAllBits == 0xf) {
+            int k;
+
+            k = mNormSquare[1][0] < mNormSquare[2][0] ?
+                                   (mNormSquare[1][0] < mNormSquare[3][0] ? 1 : 3) :
+                                   (mNormSquare[2][0] < mNormSquare[3][0] ? 2 : 3);
+            mDet[0xf][0] = mDet[0xe][1] * mDiffLength[k][0].dot(mPoints[1]) +
+                        mDet[0xe][2] * mDiffLength[k][0].dot(mPoints[2]) +
+                        mDet[0xe][3] * mDiffLength[k][0].dot(mPoints[3]);
+
+            k = mNormSquare[0][1] < mNormSquare[2][1] ?
+                                   (mNormSquare[0][1] < mNormSquare[3][1] ? 0 : 3) :
+                                   (mNormSquare[2][1] < mNormSquare[3][1] ? 2 : 3);
+            mDet[0xf][1] = mDet[0xd][0] * mDiffLength[k][1].dot(mPoints[0]) +
+                        mDet[0xd][2] * mDiffLength[k][1].dot(mPoints[2]) +
+                        mDet[0xd][3] * mDiffLength[k][1].dot(mPoints[3]);
+
+            k = mNormSquare[0][2] < mNormSquare[1][2] ?
+                                   (mNormSquare[0][2] < mNormSquare[3][2] ? 0 : 3) :
+                                   (mNormSquare[1][2] < mNormSquare[3][2] ? 1 : 3);
+            mDet[0xf][2] = mDet[0xb][0] * mDiffLength[k][2].dot(mPoints[0]) +
+                        mDet[0xb][1] * mDiffLength[k][2].dot(mPoints[1]) +
+                        mDet[0xb][3] * mDiffLength[k][2].dot(mPoints[3]);
+
+            k = mNormSquare[0][3] < mNormSquare[1][3] ?
+                                   (mNormSquare[0][3] < mNormSquare[2][3] ? 0 : 2) :
+                                   (mNormSquare[1][3] < mNormSquare[2][3] ? 1 : 2);
+            mDet[0xf][3] = mDet[0x7][0] * mDiffLength[k][3].dot(mPoints[0]) +
+                        mDet[0x7][1] * mDiffLength[k][3].dot(mPoints[1]) +
+                        mDet[0x7][2] * mDiffLength[k][3].dot(mPoints[2]);
+        }
+    }
+}
+
+// Return true if the subset is a proper subset.
+/// A proper subset X is a subset where for all point "y_i" in X, we have
+/// detX_i value bigger than zero
+bool Simplex::isProperSubset(Bits subset) const {
+    int i;
+    Bits bit;
+
+    // For each four point of the possible simplex set
+    for (i=0, bit=0x1; i<4; i++, bit <<=1) {
+        if (overlap(subset, bit) && mDet[subset][i] <= 0.0) {
+            return false;
+        }
+    }
+
+    return true;
+}
+
+// Return true if the set is affinely dependent.
+/// A set if affinely dependent if a point of the set
+/// is an affine combination of other points in the set
+bool Simplex::isAffinelyDependent() const {
+    decimal sum = 0.0;
+    int i;
+    Bits bit;
+
+    // For each four point of the possible simplex set
+    for (i=0, bit=0x1; i<4; i++, bit <<= 1) {
+        if (overlap(mAllBits, bit)) {
+            sum += mDet[mAllBits][i];
+        }
+    }
+
+    return (sum <= 0.0);
+}
+
+// Return true if the subset is a valid one for the closest point computation.
+/// A subset X is valid if :
+///    1. delta(X)_i > 0 for each "i" in I_x and
+///    2. delta(X U {y_j})_j <= 0 for each "j" not in I_x_
+bool Simplex::isValidSubset(Bits subset) const {
+    int i;
+    Bits bit;
+
+    // For each four point in the possible simplex set
+    for (i=0, bit=0x1; i<4; i++, bit <<= 1) {
+        if (overlap(mAllBits, bit)) {
+            // If the current point is in the subset
+            if (overlap(subset, bit)) {
+                // If one delta(X)_i is smaller or equal to zero
+                if (mDet[subset][i] <= 0.0) {
+                    // The subset is not valid
+                    return false;
+                }
+            }
+            // If the point is not in the subset and the value delta(X U {y_j})_j
+            // is bigger than zero
+            else if (mDet[subset | bit][i] > 0.0) {
+                // The subset is not valid
+                return false;
+            }
+        }
+    }
+
+    return true;
+}
+
+// Compute the closest points "pA" and "pB" of object A and B.
+/// The points are computed as follows :
+///      pA = sum(lambda_i * a_i)    where "a_i" are the support points of object A
+///      pB = sum(lambda_i * b_i)    where "b_i" are the support points of object B
+///      with lambda_i = deltaX_i / deltaX
+void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const {
+    decimal deltaX = 0.0;
+    pA.setAllValues(0.0, 0.0, 0.0);
+    pB.setAllValues(0.0, 0.0, 0.0);
+    int i;
+    Bits bit;
+
+    // For each four points in the possible simplex set
+    for (i=0, bit=0x1; i<4; i++, bit <<= 1) {
+        // If the current point is part of the simplex
+        if (overlap(mBitsCurrentSimplex, bit)) {
+            deltaX += mDet[mBitsCurrentSimplex][i];
+            pA += mDet[mBitsCurrentSimplex][i] * mSuppPointsA[i];
+            pB += mDet[mBitsCurrentSimplex][i] * mSuppPointsB[i];
+        }
+    }
+
+    assert(deltaX > 0.0);
+    decimal factor = decimal(1.0) / deltaX;
+    pA *= factor;
+    pB *= factor;
+}
+
+// Compute the closest point "v" to the origin of the current simplex.
+/// This method executes the Jonhnson's algorithm for computing the point
+/// "v" of simplex that is closest to the origin. The method returns true
+/// if a closest point has been found.
+bool Simplex::computeClosestPoint(Vector3& v) {
+    Bits subset;
+
+    // For each possible simplex set
+    for (subset=mBitsCurrentSimplex; subset != 0x0; subset--) {
+        // If the simplex is a subset of the current simplex and is valid for the Johnson's
+        // algorithm test
+        if (isSubset(subset, mBitsCurrentSimplex) && isValidSubset(subset | mLastFoundBit)) {
+           mBitsCurrentSimplex = subset | mLastFoundBit;              // Add the last added point to the current simplex
+           v = computeClosestPointForSubset(mBitsCurrentSimplex);    // Compute the closest point in the simplex
+           return true;
+        }
+    }
+
+    // If the simplex that contains only the last added point is valid for the Johnson's algorithm test
+    if (isValidSubset(mLastFoundBit)) {
+        mBitsCurrentSimplex = mLastFoundBit;                  // Set the current simplex to the set that contains only the last added point
+        mMaxLengthSquare = mPointsLengthSquare[mLastFound];    // Update the maximum square length
+        v = mPoints[mLastFound];                              // The closest point of the simplex "v" is the last added point
+        return true;
+    }
+
+    // The algorithm failed to found a point
+    return false;
+}
+
+// Backup the closest point
+void Simplex::backupClosestPointInSimplex(Vector3& v) {
+    decimal minDistSquare = DECIMAL_LARGEST;
+    Bits bit;
+
+    for (bit = mAllBits; bit != 0x0; bit--) {
+        if (isSubset(bit, mAllBits) && isProperSubset(bit)) {
+            Vector3 u = computeClosestPointForSubset(bit);
+            decimal distSquare = u.dot(u);
+            if (distSquare < minDistSquare) {
+                minDistSquare = distSquare;
+                mBitsCurrentSimplex = bit;
+                v = u;
+            }
+        }
+    }
+}
+
+// Return the closest point "v" in the convex hull of the points in the subset
+// represented by the bits "subset"
+Vector3 Simplex::computeClosestPointForSubset(Bits subset) {
+    Vector3 v(0.0, 0.0, 0.0);      // Closet point v = sum(lambda_i * points[i])
+    mMaxLengthSquare = 0.0;
+    decimal deltaX = 0.0;            // deltaX = sum of all det[subset][i]
+    int i;
+    Bits bit;
+
+    // For each four point in the possible simplex set
+    for (i=0, bit=0x1; i<4; i++, bit <<= 1) {
+        // If the current point is in the subset
+        if (overlap(subset, bit)) {
+            // deltaX = sum of all det[subset][i]
+            deltaX += mDet[subset][i];
+
+            if (mMaxLengthSquare < mPointsLengthSquare[i]) {
+                mMaxLengthSquare = mPointsLengthSquare[i];
+            }
+
+            // Closest point v = sum(lambda_i * points[i])
+            v += mDet[subset][i] * mPoints[i];
+        }
+    }
+
+    assert(deltaX > 0.0);
+
+    // Return the closet point "v" in the convex hull for the given subset
+    return (decimal(1.0) / deltaX) * v;
+}

src/collision/narrowphase/GJK/Simplex.h

@@ -0,0 +1,189 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_SIMPLEX_H
+#define	REACTPHYSICS3D_SIMPLEX_H
+
+// Libraries
+#include "mathematics/mathematics.h"
+#include <vector>
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Type definitions
+typedef unsigned int Bits;
+
+// Class Simplex
+/**
+ * This class represents a simplex which is a set of 3D points. This
+ * class is used in the GJK algorithm. This implementation is based on
+ * the implementation discussed in the book "Collision Detection in 3D
+ * Environments". This class implements the Johnson's algorithm for
+ * computing the point of a simplex that is closest to the origin and also
+ * the smallest simplex needed to represent that closest point.
+ */
+class Simplex {
+
+    private:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Current points
+        Vector3 mPoints[4];
+
+        /// pointsLengthSquare[i] = (points[i].length)^2
+        decimal mPointsLengthSquare[4];
+
+        /// Maximum length of pointsLengthSquare[i]
+        decimal mMaxLengthSquare;
+
+        /// Support points of object A in local coordinates
+        Vector3 mSuppPointsA[4];
+
+        /// Support points of object B in local coordinates
+        Vector3 mSuppPointsB[4];
+
+        /// diff[i][j] contains points[i] - points[j]
+        Vector3 mDiffLength[4][4];
+
+        /// Cached determinant values
+        decimal mDet[16][4];
+
+        /// norm[i][j] = (diff[i][j].length())^2
+        decimal mNormSquare[4][4];
+
+        /// 4 bits that identify the current points of the simplex
+        /// For instance, 0101 means that points[1] and points[3] are in the simplex
+        Bits mBitsCurrentSimplex;
+
+        /// Number between 1 and 4 that identify the last found support point
+        Bits mLastFound;
+
+        /// Position of the last found support point (lastFoundBit = 0x1 << lastFound)
+        Bits mLastFoundBit;
+
+        /// allBits = bitsCurrentSimplex | lastFoundBit;
+        Bits mAllBits;
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        Simplex(const Simplex& simplex);
+
+        /// Private assignment operator
+        Simplex& operator=(const Simplex& simplex);
+
+        /// Return true if some bits of "a" overlap with bits of "b"
+        bool overlap(Bits a, Bits b) const;
+
+        /// Return true if the bits of "b" is a subset of the bits of "a"
+        bool isSubset(Bits a, Bits b) const;
+
+        /// Return true if the subset is a valid one for the closest point computation.
+        bool isValidSubset(Bits subset) const;
+
+        /// Return true if the subset is a proper subset.
+        bool isProperSubset(Bits subset) const;
+
+        /// Update the cached values used during the GJK algorithm
+        void updateCache();
+
+        /// Compute the cached determinant values
+        void computeDeterminants();
+
+        /// Return the closest point "v" in the convex hull of a subset of points
+        Vector3 computeClosestPointForSubset(Bits subset);
+
+    public:
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        Simplex();
+
+        /// Destructor
+        ~Simplex();
+
+        /// Return true if the simplex contains 4 points
+        bool isFull() const;
+
+        /// Return true if the simplex is empty
+        bool isEmpty() const;
+
+        /// Return the points of the simplex
+        unsigned int getSimplex(Vector3* mSuppPointsA, Vector3* mSuppPointsB,
+                                Vector3* mPoints) const;
+
+        /// Return the maximum squared length of a point
+        decimal getMaxLengthSquareOfAPoint() const;
+
+        /// Add a new support point of (A-B) into the simplex.
+        void addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB);
+
+        /// Return true if the point is in the simplex
+        bool isPointInSimplex(const Vector3& point) const;
+
+        /// Return true if the set is affinely dependent
+        bool isAffinelyDependent() const;
+
+        /// Backup the closest point
+        void backupClosestPointInSimplex(Vector3& point);
+
+        /// Compute the closest points "pA" and "pB" of object A and B.
+        void computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const;
+
+        /// Compute the closest point to the origin of the current simplex.
+        bool computeClosestPoint(Vector3& v);
+};
+
+// Return true if some bits of "a" overlap with bits of "b"
+inline bool Simplex::overlap(Bits a, Bits b) const {
+    return ((a & b) != 0x0);
+}
+
+// Return true if the bits of "b" is a subset of the bits of "a"
+inline bool Simplex::isSubset(Bits a, Bits b) const {
+    return ((a & b) == a);
+}
+
+// Return true if the simplex contains 4 points
+inline bool Simplex::isFull() const {
+    return (mBitsCurrentSimplex == 0xf);
+}
+
+// Return true if the simple is empty
+inline bool Simplex::isEmpty() const {
+    return (mBitsCurrentSimplex == 0x0);
+}
+
+// Return the maximum squared length of a point
+inline decimal Simplex::getMaxLengthSquareOfAPoint() const {
+    return mMaxLengthSquare;
+}
+
+}
+
+#endif

src/collision/narrowphase/NarrowPhaseAlgorithm.cpp

@@ -0,0 +1,47 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "NarrowPhaseAlgorithm.h"
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Constructor
+NarrowPhaseAlgorithm::NarrowPhaseAlgorithm()
+                     : mMemoryAllocator(NULL), mCurrentOverlappingPair(NULL) {
+
+}
+
+// Destructor
+NarrowPhaseAlgorithm::~NarrowPhaseAlgorithm() {
+
+}
+
+// Initalize the algorithm
+void NarrowPhaseAlgorithm::init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator) {
+    mCollisionDetection = collisionDetection;
+    mMemoryAllocator = memoryAllocator;
+}

src/collision/narrowphase/NarrowPhaseAlgorithm.h

@@ -0,0 +1,116 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_NARROW_PHASE_ALGORITHM_H
+#define REACTPHYSICS3D_NARROW_PHASE_ALGORITHM_H
+
+// Libraries
+#include "body/Body.h"
+#include "constraint/ContactPoint.h"
+#include "memory/MemoryAllocator.h"
+#include "engine/OverlappingPair.h"
+#include "collision/CollisionShapeInfo.h"
+
+/// Namespace ReactPhysics3D
+namespace reactphysics3d {
+
+class CollisionDetection;
+
+// Class NarrowPhaseCallback
+/**
+ * This abstract class is the base class for a narrow-phase collision
+ * callback class.
+ */
+class NarrowPhaseCallback {
+
+    public:
+
+        /// Called by a narrow-phase collision algorithm when a new contact has been found
+        virtual void notifyContact(OverlappingPair* overlappingPair,
+                                   const ContactPointInfo& contactInfo)=0;
+
+};
+
+// Class NarrowPhaseAlgorithm
+/**
+ * This abstract class is the base class for a  narrow-phase collision
+ * detection algorithm. The goal of the narrow phase algorithm is to
+ * compute information about the contact between two proxy shapes.
+ */
+class NarrowPhaseAlgorithm {
+
+    protected :
+
+        // -------------------- Attributes -------------------- //
+
+        /// Pointer to the collision detection object
+        CollisionDetection* mCollisionDetection;
+
+        /// Pointer to the memory allocator
+        MemoryAllocator* mMemoryAllocator;
+
+        /// Overlapping pair of the bodies currently tested for collision
+        OverlappingPair* mCurrentOverlappingPair;
+        
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        NarrowPhaseAlgorithm(const NarrowPhaseAlgorithm& algorithm);
+
+        /// Private assignment operator
+        NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm);
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        NarrowPhaseAlgorithm();
+
+        /// Destructor
+        virtual ~NarrowPhaseAlgorithm();
+
+        /// Initalize the algorithm
+        virtual void init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator);
+        
+        /// Set the current overlapping pair of bodies
+        void setCurrentOverlappingPair(OverlappingPair* overlappingPair);
+
+        /// Compute a contact info if the two bounding volume collide
+        virtual void testCollision(const CollisionShapeInfo& shape1Info,
+                                   const CollisionShapeInfo& shape2Info,
+                                   NarrowPhaseCallback* narrowPhaseCallback)=0;
+};
+
+// Set the current overlapping pair of bodies
+inline void NarrowPhaseAlgorithm::setCurrentOverlappingPair(OverlappingPair* overlappingPair) {
+    mCurrentOverlappingPair = overlappingPair;
+}      
+
+}
+
+#endif
+
+

src/collision/narrowphase/SphereVsSphereAlgorithm.cpp

@@ -0,0 +1,80 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "SphereVsSphereAlgorithm.h"
+#include "collision/shapes/SphereShape.h"
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Constructor
+SphereVsSphereAlgorithm::SphereVsSphereAlgorithm() : NarrowPhaseAlgorithm() {
+    
+}
+
+// Destructor
+SphereVsSphereAlgorithm::~SphereVsSphereAlgorithm() {
+    
+}   
+
+void SphereVsSphereAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
+                                            const CollisionShapeInfo& shape2Info,
+                                            NarrowPhaseCallback* narrowPhaseCallback) {
+    
+    // Get the sphere collision shapes
+    const SphereShape* sphereShape1 = static_cast<const SphereShape*>(shape1Info.collisionShape);
+    const SphereShape* sphereShape2 = static_cast<const SphereShape*>(shape2Info.collisionShape);
+
+    // Get the local-space to world-space transforms
+    const Transform& transform1 = shape1Info.shapeToWorldTransform;
+    const Transform& transform2 = shape2Info.shapeToWorldTransform;
+
+    // Compute the distance between the centers
+    Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
+    decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare();
+
+    // Compute the sum of the radius
+    decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius();
+    
+    // If the sphere collision shapes intersect
+    if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) {
+        Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
+        Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
+        Vector3 intersectionOnBody1 = sphereShape1->getRadius() *
+                                      centerSphere2InBody1LocalSpace.getUnit();
+        Vector3 intersectionOnBody2 = sphereShape2->getRadius() *
+                                      centerSphere1InBody2LocalSpace.getUnit();
+        decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
+        
+        // Create the contact info object
+        ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
+                                     shape2Info.collisionShape, vectorBetweenCenters.getUnit(), penetrationDepth,
+                                     intersectionOnBody1, intersectionOnBody2);
+
+        // Notify about the new contact
+        narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
+    }
+}

src/collision/narrowphase/SphereVsSphereAlgorithm.h

@@ -0,0 +1,74 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H
+#define	REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H
+
+// Libraries
+#include "body/Body.h"
+#include "constraint/ContactPoint.h"
+#include "NarrowPhaseAlgorithm.h"
+
+
+/// Namespace ReactPhysics3D
+namespace reactphysics3d {
+
+// Class SphereVsSphereAlgorithm
+/**
+ * This class is used to compute the narrow-phase collision detection
+ * between two sphere collision shapes.
+ */
+class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
+
+    protected :
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        SphereVsSphereAlgorithm(const SphereVsSphereAlgorithm& algorithm);
+
+        /// Private assignment operator
+        SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm);
+        
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        SphereVsSphereAlgorithm();
+
+        /// Destructor
+        virtual ~SphereVsSphereAlgorithm();
+
+        /// Compute a contact info if the two bounding volume collide
+        virtual void testCollision(const CollisionShapeInfo& shape1Info,
+                                   const CollisionShapeInfo& shape2Info,
+                                   NarrowPhaseCallback* narrowPhaseCallback);
+};
+
+}
+
+#endif
+

src/collision/shapes/AABB.cpp

@@ -0,0 +1,150 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "AABB.h"
+#include "configuration.h"
+#include <cassert>
+
+using namespace reactphysics3d;
+using namespace std;
+
+// Constructor
+AABB::AABB() {
+
+}
+
+// Constructor
+AABB::AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates)
+     :mMinCoordinates(minCoordinates), mMaxCoordinates(maxCoordinates) {
+
+}
+
+// Copy-constructor
+AABB::AABB(const AABB& aabb)
+     : mMinCoordinates(aabb.mMinCoordinates), mMaxCoordinates(aabb.mMaxCoordinates) {
+
+}
+
+// Destructor
+AABB::~AABB() {
+
+}
+
+// Merge the AABB in parameter with the current one
+void AABB::mergeWithAABB(const AABB& aabb) {
+    mMinCoordinates.x = std::min(mMinCoordinates.x, aabb.mMinCoordinates.x);
+    mMinCoordinates.y = std::min(mMinCoordinates.y, aabb.mMinCoordinates.y);
+    mMinCoordinates.z = std::min(mMinCoordinates.z, aabb.mMinCoordinates.z);
+
+    mMaxCoordinates.x = std::max(mMaxCoordinates.x, aabb.mMaxCoordinates.x);
+    mMaxCoordinates.y = std::max(mMaxCoordinates.y, aabb.mMaxCoordinates.y);
+    mMaxCoordinates.z = std::max(mMaxCoordinates.z, aabb.mMaxCoordinates.z);
+}
+
+// Replace the current AABB with a new AABB that is the union of two AABBs in parameters
+void AABB::mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2) {
+    mMinCoordinates.x = std::min(aabb1.mMinCoordinates.x, aabb2.mMinCoordinates.x);
+    mMinCoordinates.y = std::min(aabb1.mMinCoordinates.y, aabb2.mMinCoordinates.y);
+    mMinCoordinates.z = std::min(aabb1.mMinCoordinates.z, aabb2.mMinCoordinates.z);
+
+    mMaxCoordinates.x = std::max(aabb1.mMaxCoordinates.x, aabb2.mMaxCoordinates.x);
+    mMaxCoordinates.y = std::max(aabb1.mMaxCoordinates.y, aabb2.mMaxCoordinates.y);
+    mMaxCoordinates.z = std::max(aabb1.mMaxCoordinates.z, aabb2.mMaxCoordinates.z);
+}
+
+// Return true if the current AABB contains the AABB given in parameter
+bool AABB::contains(const AABB& aabb) const {
+
+    bool isInside = true;
+    isInside = isInside && mMinCoordinates.x <= aabb.mMinCoordinates.x;
+    isInside = isInside && mMinCoordinates.y <= aabb.mMinCoordinates.y;
+    isInside = isInside && mMinCoordinates.z <= aabb.mMinCoordinates.z;
+
+    isInside = isInside && mMaxCoordinates.x >= aabb.mMaxCoordinates.x;
+    isInside = isInside && mMaxCoordinates.y >= aabb.mMaxCoordinates.y;
+    isInside = isInside && mMaxCoordinates.z >= aabb.mMaxCoordinates.z;
+    return isInside;
+}
+
+// Create and return an AABB for a triangle
+AABB AABB::createAABBForTriangle(const Vector3* trianglePoints) {
+
+    Vector3 minCoords(trianglePoints[0].x, trianglePoints[0].y, trianglePoints[0].z);
+    Vector3 maxCoords(trianglePoints[0].x, trianglePoints[0].y, trianglePoints[0].z);
+
+    if (trianglePoints[1].x < minCoords.x) minCoords.x = trianglePoints[1].x;
+    if (trianglePoints[1].y < minCoords.y) minCoords.y = trianglePoints[1].y;
+    if (trianglePoints[1].z < minCoords.z) minCoords.z = trianglePoints[1].z;
+
+    if (trianglePoints[2].x < minCoords.x) minCoords.x = trianglePoints[2].x;
+    if (trianglePoints[2].y < minCoords.y) minCoords.y = trianglePoints[2].y;
+    if (trianglePoints[2].z < minCoords.z) minCoords.z = trianglePoints[2].z;
+
+    if (trianglePoints[1].x > maxCoords.x) maxCoords.x = trianglePoints[1].x;
+    if (trianglePoints[1].y > maxCoords.y) maxCoords.y = trianglePoints[1].y;
+    if (trianglePoints[1].z > maxCoords.z) maxCoords.z = trianglePoints[1].z;
+
+    if (trianglePoints[2].x > maxCoords.x) maxCoords.x = trianglePoints[2].x;
+    if (trianglePoints[2].y > maxCoords.y) maxCoords.y = trianglePoints[2].y;
+    if (trianglePoints[2].z > maxCoords.z) maxCoords.z = trianglePoints[2].z;
+
+    return AABB(minCoords, maxCoords);
+}
+
+// Return true if the ray intersects the AABB
+/// This method use the line vs AABB raycasting technique described in
+/// Real-time Collision Detection by Christer Ericson.
+bool AABB::testRayIntersect(const Ray& ray) const {
+
+    const Vector3 point2 = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
+    const Vector3 e = mMaxCoordinates - mMinCoordinates;
+    const Vector3 d = point2 - ray.point1;
+    const Vector3 m = ray.point1 + point2 - mMinCoordinates - mMaxCoordinates;
+
+    // Test if the AABB face normals are separating axis
+    decimal adx = std::abs(d.x);
+    if (std::abs(m.x) > e.x + adx) return false;
+    decimal ady = std::abs(d.y);
+    if (std::abs(m.y) > e.y + ady) return false;
+    decimal adz = std::abs(d.z);
+    if (std::abs(m.z) > e.z + adz) return false;
+
+    // Add in an epsilon term to counteract arithmetic errors when segment is
+    // (near) parallel to a coordinate axis (see text for detail)
+    const decimal epsilon = 0.00001;
+    adx += epsilon;
+    ady += epsilon;
+    adz += epsilon;
+
+    // Test if the cross products between face normals and ray direction are
+    // separating axis
+    if (std::abs(m.y * d.z - m.z * d.y) > e.y * adz + e.z * ady) return false;
+    if (std::abs(m.z * d.x - m.x * d.z) > e.x * adz + e.z * adx) return false;
+    if (std::abs(m.x * d.y - m.y * d.x) > e.x * ady + e.y * adx) return false;
+
+    // No separating axis has been found
+    return true;
+}

src/collision/shapes/AABB.h

@@ -0,0 +1,213 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_AABB_H
+#define REACTPHYSICS3D_AABB_H
+
+// Libraries
+#include "mathematics/mathematics.h"
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+    
+// Class AABB
+/**
+ * This class represents a bounding volume of type "Axis Aligned
+ * Bounding Box". It's a box where all the edges are always aligned
+ * with the world coordinate system. The AABB is defined by the
+ * minimum and maximum world coordinates of the three axis.
+ */
+class AABB {
+
+    private :
+
+        // -------------------- Attributes -------------------- //
+
+        /// Minimum world coordinates of the AABB on the x,y and z axis
+        Vector3 mMinCoordinates;
+
+        /// Maximum world coordinates of the AABB on the x,y and z axis
+        Vector3 mMaxCoordinates;
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        AABB();
+
+        /// Constructor
+        AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates);
+
+        /// Copy-constructor
+        AABB(const AABB& aabb);
+
+        /// Destructor
+        ~AABB();
+
+        /// Return the center point
+        Vector3 getCenter() const;
+
+        /// 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;
+
+        /// Set the maximum coordinates of the AABB
+        void setMax(const Vector3& max);
+
+        /// Return the size of the AABB in the three dimension x, y and z
+        Vector3 getExtent() const;
+
+        /// Inflate each side of the AABB by a given size
+        void inflate(decimal dx, decimal dy, decimal dz);
+
+        /// Return true if the current AABB is overlapping with the AABB in argument
+        bool testCollision(const AABB& aabb) const;
+
+        /// Return the volume of the AABB
+        decimal getVolume() const;
+
+        /// Merge the AABB in parameter with the current one
+        void mergeWithAABB(const AABB& aabb);
+
+        /// Replace the current AABB with a new AABB that is the union of two AABBs in parameters
+        void mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2);
+
+        /// Return true if the current AABB contains the AABB given in parameter
+        bool contains(const AABB& aabb) const;
+
+        /// Return true if a point is inside the AABB
+        bool contains(const Vector3& point) const;
+
+        /// Return true if the AABB of a triangle intersects the AABB
+        bool testCollisionTriangleAABB(const Vector3* trianglePoints) const;
+
+        /// Return true if the ray intersects the AABB
+        bool testRayIntersect(const Ray& ray) const;
+
+        /// Create and return an AABB for a triangle
+        static AABB createAABBForTriangle(const Vector3* trianglePoints);
+
+        /// Assignment operator
+        AABB& operator=(const AABB& aabb);
+
+        // -------------------- Friendship -------------------- //
+
+        friend class DynamicAABBTree;
+};
+
+// Return the center point of the AABB in world coordinates
+inline Vector3 AABB::getCenter() const {
+    return (mMinCoordinates + mMaxCoordinates) * decimal(0.5);
+}
+
+// Return the minimum coordinates of the AABB
+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;
+}
+
+// Set the maximum coordinates of the AABB
+inline void AABB::setMax(const Vector3& max) {
+    mMaxCoordinates = max;
+}
+
+// Return the size of the AABB in the three dimension x, y and z
+inline Vector3 AABB::getExtent() const {
+  return  mMaxCoordinates - mMinCoordinates;
+}
+
+// Inflate each side of the AABB by a given size
+inline void AABB::inflate(decimal dx, decimal dy, decimal dz) {
+    mMaxCoordinates += Vector3(dx, dy, dz);
+    mMinCoordinates -= Vector3(dx, dy, dz);
+}
+
+// Return true if the current AABB is overlapping with the AABB in argument.
+/// Two AABBs overlap if they overlap in the three x, y and z axis at the same time
+inline bool AABB::testCollision(const AABB& aabb) const {
+    if (mMaxCoordinates.x < aabb.mMinCoordinates.x ||
+        aabb.mMaxCoordinates.x < mMinCoordinates.x) return false;
+    if (mMaxCoordinates.y < aabb.mMinCoordinates.y ||
+        aabb.mMaxCoordinates.y < mMinCoordinates.y) return false;
+    if (mMaxCoordinates.z < aabb.mMinCoordinates.z||
+        aabb.mMaxCoordinates.z < mMinCoordinates.z) return false;
+    return true;
+}
+
+// Return the volume of the AABB
+inline decimal AABB::getVolume() const {
+    const Vector3 diff = mMaxCoordinates - mMinCoordinates;
+    return (diff.x * diff.y * diff.z);
+}
+
+// Return true if the AABB of a triangle intersects the AABB
+inline bool AABB::testCollisionTriangleAABB(const Vector3* trianglePoints) const {
+
+    if (min3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) > mMaxCoordinates.x) return false;
+    if (min3(trianglePoints[0].y, trianglePoints[1].y, trianglePoints[2].y) > mMaxCoordinates.y) return false;
+    if (min3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) > mMaxCoordinates.z) return false;
+
+    if (max3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) < mMinCoordinates.x) return false;
+    if (max3(trianglePoints[0].y, trianglePoints[1].y, trianglePoints[2].y) < mMinCoordinates.y) return false;
+    if (max3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) < mMinCoordinates.z) return false;
+
+    return true;
+}
+
+// Return true if a point is inside the AABB
+inline bool AABB::contains(const Vector3& point) const {
+
+    return (point.x >= mMinCoordinates.x - MACHINE_EPSILON && point.x <= mMaxCoordinates.x + MACHINE_EPSILON &&
+            point.y >= mMinCoordinates.y - MACHINE_EPSILON && point.y <= mMaxCoordinates.y + MACHINE_EPSILON &&
+            point.z >= mMinCoordinates.z - MACHINE_EPSILON && point.z <= mMaxCoordinates.z + MACHINE_EPSILON);
+}
+
+// Assignment operator
+inline AABB& AABB::operator=(const AABB& aabb) {
+    if (this != &aabb) {
+        mMinCoordinates = aabb.mMinCoordinates;
+        mMaxCoordinates = aabb.mMaxCoordinates;
+    }
+    return *this;
+}
+
+}
+
+#endif

src/collision/shapes/BoxShape.cpp

@@ -0,0 +1,132 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2015 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 "BoxShape.h"
+#include "collision/ProxyShape.h"
+#include "configuration.h"
+#include <vector>
+#include <cassert>
+
+using namespace reactphysics3d;
+
+// Constructor
+/**
+ * @param extent The vector with the three extents of the box (in meters)
+ * @param margin The collision margin (in meters) around the collision shape
+ */
+BoxShape::BoxShape(const Vector3& extent, decimal margin)
+         : ConvexShape(BOX, margin), mExtent(extent - Vector3(margin, margin, margin)) {
+    assert(extent.x > decimal(0.0) && extent.x > margin);
+    assert(extent.y > decimal(0.0) && extent.y > margin);
+    assert(extent.z > decimal(0.0) && extent.z > margin);
+}
+
+// Destructor
+BoxShape::~BoxShape() {
+
+}
+
+// Return the local inertia tensor of the collision shape
+/**
+ * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
+ *                    coordinates
+ * @param mass Mass to use to compute the inertia tensor of the collision shape
+ */
+void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
+    decimal factor = (decimal(1.0) / decimal(3.0)) * mass;
+    Vector3 realExtent = mExtent + Vector3(mMargin, mMargin, mMargin);
+    decimal xSquare = realExtent.x * realExtent.x;
+    decimal ySquare = realExtent.y * realExtent.y;
+    decimal zSquare = realExtent.z * realExtent.z;
+    tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0,
+                        0.0, factor * (xSquare + zSquare), 0.0,
+                        0.0, 0.0, factor * (xSquare + ySquare));
+}
+
+// Raycast method with feedback information
+bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
+
+    Vector3 rayDirection = ray.point2 - ray.point1;
+    decimal tMin = DECIMAL_SMALLEST;
+    decimal tMax = DECIMAL_LARGEST;
+    Vector3 normalDirection(decimal(0), decimal(0), decimal(0));
+    Vector3 currentNormal;
+
+    // For each of the three slabs
+    for (int i=0; i<3; i++) {
+
+        // If ray is parallel to the slab
+        if (std::abs(rayDirection[i]) < MACHINE_EPSILON) {
+
+            // If the ray's origin is not inside the slab, there is no hit
+            if (ray.point1[i] > mExtent[i] || ray.point1[i] < -mExtent[i]) return false;
+        }
+        else {
+
+            // Compute the intersection of the ray with the near and far plane of the slab
+            decimal oneOverD = decimal(1.0) / rayDirection[i];
+            decimal t1 = (-mExtent[i] - ray.point1[i]) * oneOverD;
+            decimal t2 = (mExtent[i] - ray.point1[i]) * oneOverD;
+            currentNormal[0] = (i == 0) ? -mExtent[i] : decimal(0.0);
+            currentNormal[1] = (i == 1) ? -mExtent[i] : decimal(0.0);
+            currentNormal[2] = (i == 2) ? -mExtent[i] : decimal(0.0);
+
+            // Swap t1 and t2 if need so that t1 is intersection with near plane and
+            // t2 with far plane
+            if (t1 > t2) {
+                std::swap(t1, t2);
+                currentNormal = -currentNormal;
+            }
+
+            // Compute the intersection of the of slab intersection interval with previous slabs
+            if (t1 > tMin) {
+                tMin = t1;
+                normalDirection = currentNormal;
+            }
+            tMax = std::min(tMax, t2);
+
+            // If tMin is larger than the maximum raycasting fraction, we return no hit
+            if (tMin > ray.maxFraction) return false;
+
+            // If the slabs intersection is empty, there is no hit
+            if (tMin > tMax) return false;
+        }
+    }
+
+    // If tMin is negative, we return no hit
+    if (tMin < decimal(0.0) || tMin > ray.maxFraction) return false;
+
+    // The ray intersects the three slabs, we compute the hit point
+    Vector3 localHitPoint = ray.point1 + tMin * rayDirection;
+
+    raycastInfo.body = proxyShape->getBody();
+    raycastInfo.proxyShape = proxyShape;
+    raycastInfo.hitFraction = tMin;
+    raycastInfo.worldPoint = localHitPoint;
+    raycastInfo.worldNormal = normalDirection;
+
+    return true;
+}

src/collision/shapes/BoxShape.h

@@ -0,0 +1,160 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_BOX_SHAPE_H
+#define REACTPHYSICS3D_BOX_SHAPE_H
+
+// Libraries
+#include <cfloat>
+#include "ConvexShape.h"
+#include "body/CollisionBody.h"
+#include "mathematics/mathematics.h"
+
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Class BoxShape
+/**
+ * This class represents a 3D box shape. Those axis are unit length.
+ * The three extents are half-widths of the box along the three
+ * axis x, y, z local axis. The "transform" of the corresponding
+ * rigid body will give an orientation and a position to the box. This
+ * collision shape uses an extra margin distance around it for collision
+ * detection purpose. The default margin is 4cm (if your units are meters,
+ * which is recommended). In case, you want to simulate small objects
+ * (smaller than the margin distance), you might want to reduce the margin by
+ * specifying your own margin distance using the "margin" parameter in the
+ * constructor of the box shape. Otherwise, it is recommended to use the
+ * default margin distance by not using the "margin" parameter in the constructor.
+ */
+class BoxShape : public ConvexShape {
+
+    protected :
+
+        // -------------------- Attributes -------------------- //
+
+        /// Extent sizes of the box in the x, y and z direction
+        Vector3 mExtent;
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        BoxShape(const BoxShape& shape);
+
+        /// Private assignment operator
+        BoxShape& operator=(const BoxShape& shape);
+
+        /// Return a local support point in a given direction without the object margin
+        virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
+                                                          void** cachedCollisionData) const;
+
+        /// Return true if a point is inside the collision shape
+        virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
+
+        /// Raycast method with feedback information
+        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
+
+        /// Return the number of bytes used by the collision shape
+        virtual size_t getSizeInBytes() const;
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        BoxShape(const Vector3& extent, decimal margin = OBJECT_MARGIN);
+
+        /// Destructor
+        virtual ~BoxShape();
+
+        /// Return the extents of the box
+        Vector3 getExtent() const;
+
+        /// Set the scaling vector of the collision shape
+        virtual void setLocalScaling(const Vector3& scaling);
+
+        /// Return the local bounds of the shape in x, y and z directions
+        virtual void getLocalBounds(Vector3& min, Vector3& max) const;
+
+        /// Return the local inertia tensor of the collision shape
+        virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
+};
+
+// Return the extents of the box
+/**
+ * @return The vector with the three extents of the box shape (in meters)
+ */
+inline Vector3 BoxShape::getExtent() const {
+    return mExtent + Vector3(mMargin, mMargin, mMargin);
+}
+
+// Set the scaling vector of the collision shape
+inline void BoxShape::setLocalScaling(const Vector3& scaling) {
+
+    mExtent = (mExtent / mScaling) * scaling;
+
+    CollisionShape::setLocalScaling(scaling);
+}
+
+// Return the local bounds of the shape in x, y and z directions
+/// This method is used to compute the AABB of the box
+/**
+ * @param min The minimum bounds of the shape in local-space coordinates
+ * @param max The maximum bounds of the shape in local-space coordinates
+ */
+inline void BoxShape::getLocalBounds(Vector3& min, Vector3& max) const {
+
+    // Maximum bounds
+    max = mExtent + Vector3(mMargin, mMargin, mMargin);
+
+    // Minimum bounds
+    min = -max;
+}
+
+// Return the number of bytes used by the collision shape
+inline size_t BoxShape::getSizeInBytes() const {
+    return sizeof(BoxShape);
+}
+
+// Return a local support point in a given direction without the objec margin
+inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
+                                                           void** cachedCollisionData) const {
+
+    return Vector3(direction.x < 0.0 ? -mExtent.x : mExtent.x,
+                   direction.y < 0.0 ? -mExtent.y : mExtent.y,
+                   direction.z < 0.0 ? -mExtent.z : mExtent.z);
+}
+
+// Return true if a point is inside the collision shape
+inline bool BoxShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
+    return (localPoint.x < mExtent[0] && localPoint.x > -mExtent[0] &&
+            localPoint.y < mExtent[1] && localPoint.y > -mExtent[1] &&
+            localPoint.z < mExtent[2] && localPoint.z > -mExtent[2]);
+}
+
+}
+
+#endif

src/collision/shapes/CapsuleShape.cpp

@@ -0,0 +1,281 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "CapsuleShape.h"
+#include "collision/ProxyShape.h"
+#include "configuration.h"
+#include <cassert>
+
+using namespace reactphysics3d;
+
+// Constructor
+/**
+ * @param radius The radius of the capsule (in meters)
+ * @param height The height of the capsule (in meters)
+ */
+CapsuleShape::CapsuleShape(decimal radius, decimal height)
+            : ConvexShape(CAPSULE, radius), mHalfHeight(height * decimal(0.5)) {
+    assert(radius > decimal(0.0));
+    assert(height > decimal(0.0));
+}
+
+// Destructor
+CapsuleShape::~CapsuleShape() {
+
+}
+
+// Return the local inertia tensor of the capsule
+/**
+ * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
+ *                    coordinates
+ * @param mass Mass to use to compute the inertia tensor of the collision shape
+ */
+void CapsuleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
+
+	// The inertia tensor formula for a capsule can be found in : Game Engine Gems, Volume 1
+	
+    decimal height = mHalfHeight + mHalfHeight;
+    decimal radiusSquare = mMargin * mMargin;
+	decimal heightSquare = height * height;
+	decimal radiusSquareDouble = radiusSquare + radiusSquare;
+    decimal factor1 = decimal(2.0) * mMargin / (decimal(4.0) * mMargin + decimal(3.0) * height);
+    decimal factor2 = decimal(3.0) * height / (decimal(4.0) * mMargin + decimal(3.0) * height);
+	decimal sum1 = decimal(0.4) * radiusSquareDouble;
+    decimal sum2 = decimal(0.75) * height * mMargin + decimal(0.5) * heightSquare;
+	decimal sum3 = decimal(0.25) * radiusSquare + decimal(1.0 / 12.0) * heightSquare;
+	decimal IxxAndzz = factor1 * mass * (sum1 + sum2) + factor2 * mass * sum3;
+	decimal Iyy = factor1 * mass * sum1 + factor2 * mass * decimal(0.25) * radiusSquareDouble;
+    tensor.setAllValues(IxxAndzz, 0.0, 0.0,
+                        0.0, Iyy, 0.0,
+                        0.0, 0.0, IxxAndzz);
+}
+
+// Return true if a point is inside the collision shape
+bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
+
+    const decimal diffYCenterSphere1 = localPoint.y - mHalfHeight;
+    const decimal diffYCenterSphere2 = localPoint.y + mHalfHeight;
+    const decimal xSquare = localPoint.x * localPoint.x;
+    const decimal zSquare = localPoint.z * localPoint.z;
+    const decimal squareRadius = mMargin * mMargin;
+
+    // Return true if the point is inside the cylinder or one of the two spheres of the capsule
+    return ((xSquare + zSquare) < squareRadius &&
+            localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) ||
+            (xSquare + zSquare + diffYCenterSphere1 * diffYCenterSphere1) < squareRadius ||
+            (xSquare + zSquare + diffYCenterSphere2 * diffYCenterSphere2) < squareRadius;
+}
+
+// Raycast method with feedback information
+bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
+
+    const Vector3 n = ray.point2 - ray.point1;
+
+    const decimal epsilon = decimal(0.01);
+    Vector3 p(decimal(0), -mHalfHeight, decimal(0));
+    Vector3 q(decimal(0), mHalfHeight, decimal(0));
+    Vector3 d = q - p;
+    Vector3 m = ray.point1 - p;
+    decimal t;
+
+    decimal mDotD = m.dot(d);
+    decimal nDotD = n.dot(d);
+    decimal dDotD = d.dot(d);
+
+    // Test if the segment is outside the cylinder
+    decimal vec1DotD = (ray.point1 - Vector3(decimal(0.0), -mHalfHeight - mMargin, decimal(0.0))).dot(d);
+    if (vec1DotD < decimal(0.0) && vec1DotD + nDotD < decimal(0.0)) return false;
+    decimal ddotDExtraCaps = decimal(2.0) * mMargin * d.y;
+    if (vec1DotD > dDotD + ddotDExtraCaps && vec1DotD + nDotD > dDotD + ddotDExtraCaps) return false;
+
+    decimal nDotN = n.dot(n);
+    decimal mDotN = m.dot(n);
+
+    decimal a = dDotD * nDotN - nDotD * nDotD;
+    decimal k = m.dot(m) - mMargin * mMargin;
+    decimal c = dDotD * k - mDotD * mDotD;
+
+    // If the ray is parallel to the capsule axis
+    if (std::abs(a) < epsilon) {
+
+        // If the origin is outside the surface of the capusle's cylinder, we return no hit
+        if (c > decimal(0.0)) return false;
+
+        // Here we know that the segment intersect an endcap of the capsule
+
+        // If the ray intersects with the "p" endcap of the capsule
+        if (mDotD < decimal(0.0)) {
+
+            // Check intersection between the ray and the "p" sphere endcap of the capsule
+            Vector3 hitLocalPoint;
+            decimal hitFraction;
+            if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
+                raycastInfo.body = proxyShape->getBody();
+                raycastInfo.proxyShape = proxyShape;
+                raycastInfo.hitFraction = hitFraction;
+                raycastInfo.worldPoint = hitLocalPoint;
+                Vector3 normalDirection = hitLocalPoint - p;
+                raycastInfo.worldNormal = normalDirection;
+
+                return true;
+            }
+
+            return false;
+        }
+        else if (mDotD > dDotD) {   // If the ray intersects with the "q" endcap of the cylinder
+
+            // Check intersection between the ray and the "q" sphere endcap of the capsule
+            Vector3 hitLocalPoint;
+            decimal hitFraction;
+            if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
+                raycastInfo.body = proxyShape->getBody();
+                raycastInfo.proxyShape = proxyShape;
+                raycastInfo.hitFraction = hitFraction;
+                raycastInfo.worldPoint = hitLocalPoint;
+                Vector3 normalDirection = hitLocalPoint - q;
+                raycastInfo.worldNormal = normalDirection;
+
+                return true;
+            }
+
+            return false;
+        }
+        else {  // If the origin is inside the cylinder, we return no hit
+            return false;
+        }
+    }
+    decimal b = dDotD * mDotN - nDotD * mDotD;
+    decimal discriminant = b * b - a * c;
+
+    // If the discriminant is negative, no real roots and therfore, no hit
+    if (discriminant < decimal(0.0)) return false;
+
+    // Compute the smallest root (first intersection along the ray)
+    decimal t0 = t = (-b - std::sqrt(discriminant)) / a;
+
+    // If the intersection is outside the finite cylinder of the capsule on "p" endcap side
+    decimal value = mDotD + t * nDotD;
+    if (value < decimal(0.0)) {
+
+        // Check intersection between the ray and the "p" sphere endcap of the capsule
+        Vector3 hitLocalPoint;
+        decimal hitFraction;
+        if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
+            raycastInfo.body = proxyShape->getBody();
+            raycastInfo.proxyShape = proxyShape;
+            raycastInfo.hitFraction = hitFraction;
+            raycastInfo.worldPoint = hitLocalPoint;
+            Vector3 normalDirection = hitLocalPoint - p;
+            raycastInfo.worldNormal = normalDirection;
+
+            return true;
+        }
+
+        return false;
+    }
+    else if (value > dDotD) {  // If the intersection is outside the finite cylinder on the "q" side
+
+        // Check intersection between the ray and the "q" sphere endcap of the capsule
+        Vector3 hitLocalPoint;
+        decimal hitFraction;
+        if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
+            raycastInfo.body = proxyShape->getBody();
+            raycastInfo.proxyShape = proxyShape;
+            raycastInfo.hitFraction = hitFraction;
+            raycastInfo.worldPoint = hitLocalPoint;
+            Vector3 normalDirection = hitLocalPoint - q;
+            raycastInfo.worldNormal = normalDirection;
+
+            return true;
+        }
+
+        return false;
+    }
+
+    t = t0;
+
+    // If the intersection is behind the origin of the ray or beyond the maximum
+    // raycasting distance, we return no hit
+    if (t < decimal(0.0) || t > ray.maxFraction) return false;
+
+    // Compute the hit information
+    Vector3 localHitPoint = ray.point1 + t * n;
+    raycastInfo.body = proxyShape->getBody();
+    raycastInfo.proxyShape = proxyShape;
+    raycastInfo.hitFraction = t;
+    raycastInfo.worldPoint = localHitPoint;
+    Vector3 v = localHitPoint - p;
+    Vector3 w = (v.dot(d) / d.lengthSquare()) * d;
+    Vector3 normalDirection = (localHitPoint - (p + w)).getUnit();
+    raycastInfo.worldNormal = normalDirection;
+
+    return true;
+}
+
+// Raycasting method between a ray one of the two spheres end cap of the capsule
+bool CapsuleShape::raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2,
+                                           const Vector3& sphereCenter, decimal maxFraction,
+                                           Vector3& hitLocalPoint, decimal& hitFraction) const {
+
+     const Vector3 m = point1 - sphereCenter;
+    decimal c = m.dot(m) - mMargin * mMargin;
+
+    // If the origin of the ray is inside the sphere, we return no intersection
+    if (c < decimal(0.0)) return false;
+
+    const Vector3 rayDirection = point2 - point1;
+    decimal b = m.dot(rayDirection);
+
+    // If the origin of the ray is outside the sphere and the ray
+    // is pointing away from the sphere, there is no intersection
+    if (b > decimal(0.0)) return false;
+
+    decimal raySquareLength = rayDirection.lengthSquare();
+
+    // Compute the discriminant of the quadratic equation
+    decimal discriminant = b * b - raySquareLength * c;
+
+    // If the discriminant is negative or the ray length is very small, there is no intersection
+    if (discriminant < decimal(0.0) || raySquareLength < MACHINE_EPSILON) return false;
+
+    // Compute the solution "t" closest to the origin
+    decimal t = -b - std::sqrt(discriminant);
+
+    assert(t >= decimal(0.0));
+
+    // If the hit point is withing the segment ray fraction
+    if (t < maxFraction * raySquareLength) {
+
+        // Compute the intersection information
+        t /= raySquareLength;
+        hitFraction = t;
+        hitLocalPoint = point1 + t * rayDirection;
+
+        return true;
+    }
+
+    return false;
+}

src/collision/shapes/CapsuleShape.h

@@ -0,0 +1,184 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_CAPSULE_SHAPE_H
+#define REACTPHYSICS3D_CAPSULE_SHAPE_H
+
+// Libraries
+#include "ConvexShape.h"
+#include "body/CollisionBody.h"
+#include "mathematics/mathematics.h"
+
+// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Class CapsuleShape
+/**
+ * This class represents a capsule collision shape that is defined around the Y axis.
+ * A capsule shape can be seen as the convex hull of two spheres.
+ * The capsule shape is defined by its radius (radius of the two spheres of the capsule)
+ * and its height (distance between the centers of the two spheres). This collision shape
+ * does not have an explicit object margin distance. The margin is implicitly the radius
+ * and height of the shape. Therefore, no need to specify an object margin for a
+ * capsule shape.
+ */
+class CapsuleShape : public ConvexShape {
+
+    protected :
+
+        // -------------------- Attributes -------------------- //
+
+        /// Half height of the capsule (height = distance between the centers of the two spheres)
+        decimal mHalfHeight;
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        CapsuleShape(const CapsuleShape& shape);
+
+        /// Private assignment operator
+        CapsuleShape& operator=(const CapsuleShape& shape);
+
+        /// Return a local support point in a given direction without the object margin
+        virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
+                                                          void** cachedCollisionData) const;
+
+        /// Return true if a point is inside the collision shape
+        virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
+
+        /// Raycast method with feedback information
+        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
+
+        /// Raycasting method between a ray one of the two spheres end cap of the capsule
+        bool raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2,
+                                     const Vector3& sphereCenter, decimal maxFraction,
+                                     Vector3& hitLocalPoint, decimal& hitFraction) const;
+
+        /// Return the number of bytes used by the collision shape
+        virtual size_t getSizeInBytes() const;
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        CapsuleShape(decimal radius, decimal height);
+
+        /// Destructor
+        virtual ~CapsuleShape();
+
+        /// Return the radius of the capsule
+        decimal getRadius() const;
+
+        /// Return the height of the capsule
+        decimal getHeight() const;
+
+        /// Set the scaling vector of the collision shape
+        virtual void setLocalScaling(const Vector3& scaling);
+
+        /// Return the local bounds of the shape in x, y and z directions
+        virtual void getLocalBounds(Vector3& min, Vector3& max) const;
+
+        /// Return the local inertia tensor of the collision shape
+        virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
+};
+
+// Get the radius of the capsule
+/**
+ * @return The radius of the capsule shape (in meters)
+ */
+inline decimal CapsuleShape::getRadius() const {
+    return mMargin;
+}
+
+// Return the height of the capsule
+/**
+ * @return The height of the capsule shape (in meters)
+ */
+inline decimal CapsuleShape::getHeight() const {
+    return mHalfHeight + mHalfHeight;
+}
+
+// Set the scaling vector of the collision shape
+inline void CapsuleShape::setLocalScaling(const Vector3& scaling) {
+
+    mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y;
+    mMargin = (mMargin / mScaling.x) * scaling.x;
+
+    CollisionShape::setLocalScaling(scaling);
+}
+
+// Return the number of bytes used by the collision shape
+inline size_t CapsuleShape::getSizeInBytes() const {
+    return sizeof(CapsuleShape);
+}
+
+// Return the local bounds of the shape in x, y and z directions
+// This method is used to compute the AABB of the box
+/**
+ * @param min The minimum bounds of the shape in local-space coordinates
+ * @param max The maximum bounds of the shape in local-space coordinates
+ */
+inline void CapsuleShape::getLocalBounds(Vector3& min, Vector3& max) const {
+
+    // Maximum bounds
+    max.x = mMargin;
+    max.y = mHalfHeight + mMargin;
+    max.z = mMargin;
+
+    // Minimum bounds
+    min.x = -mMargin;
+    min.y = -max.y;
+    min.z = min.x;
+}
+
+// Return a local support point in a given direction without the object margin.
+/// A capsule is the convex hull of two spheres S1 and S2. The support point in the direction "d"
+/// of the convex hull of a set of convex objects is the support point "p" in the set of all
+/// support points from all the convex objects with the maximum dot product with the direction "d".
+/// Therefore, in this method, we compute the support points of both top and bottom spheres of
+/// the capsule and return the point with the maximum dot product with the direction vector. Note
+/// that the object margin is implicitly the radius and height of the capsule.
+inline Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
+                                                        void** cachedCollisionData) const {
+
+    // Support point top sphere
+    decimal dotProductTop = mHalfHeight * direction.y;
+
+    // Support point bottom sphere
+    decimal dotProductBottom = -mHalfHeight * direction.y;
+
+    // Return the point with the maximum dot product
+    if (dotProductTop > dotProductBottom) {
+        return Vector3(0, mHalfHeight, 0);
+    }
+    else {
+        return Vector3(0, -mHalfHeight, 0);
+    }
+}
+
+}
+
+#endif

src/collision/shapes/CollisionShape.cpp

@@ -0,0 +1,75 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "CollisionShape.h"
+#include "engine/Profiler.h"
+#include "body/CollisionBody.h"
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Constructor
+CollisionShape::CollisionShape(CollisionShapeType type) : mType(type), mScaling(1.0, 1.0, 1.0) {
+    
+}
+
+// Destructor
+CollisionShape::~CollisionShape() {
+
+}
+
+// Compute the world-space AABB of the collision shape given a transform
+/**
+ * @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape
+ *                  computed in world-space coordinates
+ * @param transform Transform used to compute the AABB of the collision shape
+ */
+void CollisionShape::computeAABB(AABB& aabb, const Transform& transform) const {
+
+    PROFILE("CollisionShape::computeAABB()");
+
+    // Get the local bounds in x,y and z direction
+    Vector3 minBounds;
+    Vector3 maxBounds;
+    getLocalBounds(minBounds, maxBounds);
+
+    // Rotate the local bounds according to the orientation of the body
+    Matrix3x3 worldAxis = transform.getOrientation().getMatrix().getAbsoluteMatrix();
+    Vector3 worldMinBounds(worldAxis.getColumn(0).dot(minBounds),
+                           worldAxis.getColumn(1).dot(minBounds),
+                           worldAxis.getColumn(2).dot(minBounds));
+    Vector3 worldMaxBounds(worldAxis.getColumn(0).dot(maxBounds),
+                           worldAxis.getColumn(1).dot(maxBounds),
+                           worldAxis.getColumn(2).dot(maxBounds));
+
+    // Compute the minimum and maximum coordinates of the rotated extents
+    Vector3 minCoordinates = transform.getPosition() + worldMinBounds;
+    Vector3 maxCoordinates = transform.getPosition() + worldMaxBounds;
+
+    // Update the AABB with the new minimum and maximum coordinates
+    aabb.setMin(minCoordinates);
+    aabb.setMax(maxCoordinates);
+}

src/collision/shapes/CollisionShape.h

@@ -0,0 +1,167 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_COLLISION_SHAPE_H
+#define REACTPHYSICS3D_COLLISION_SHAPE_H
+
+// Libraries
+#include <cassert>
+#include <typeinfo>
+#include "mathematics/Vector3.h"
+#include "mathematics/Matrix3x3.h"
+#include "mathematics/Ray.h"
+#include "AABB.h"
+#include "collision/RaycastInfo.h"
+#include "memory/MemoryAllocator.h"
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+    
+/// Type of the collision shape
+enum CollisionShapeType {TRIANGLE, BOX, SPHERE, CONE, CYLINDER,
+                         CAPSULE, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD};
+const int NB_COLLISION_SHAPE_TYPES = 9;
+
+// Declarations
+class ProxyShape;
+class CollisionBody;
+
+// Class CollisionShape
+/**
+ * This abstract class represents the collision shape associated with a
+ * body that is used during the narrow-phase collision detection.
+ */
+class CollisionShape {
+        
+    protected :
+
+        // -------------------- Attributes -------------------- //
+
+        /// Type of the collision shape
+        CollisionShapeType mType;
+
+        /// Scaling vector of the collision shape
+        Vector3 mScaling;
+        
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        CollisionShape(const CollisionShape& shape);
+
+        /// Private assignment operator
+        CollisionShape& operator=(const CollisionShape& shape);
+
+        /// Return true if a point is inside the collision shape
+        virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0;
+
+        /// Raycast method with feedback information
+        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const=0;
+
+        /// Return the number of bytes used by the collision shape
+        virtual size_t getSizeInBytes() const = 0;
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        CollisionShape(CollisionShapeType type);
+
+        /// Destructor
+        virtual ~CollisionShape();
+
+        /// Return the type of the collision shapes
+        CollisionShapeType getType() const;
+
+        /// Return true if the collision shape is convex, false if it is concave
+        virtual bool isConvex() const=0;
+
+        /// Return the local bounds of the shape in x, y and z directions
+        virtual void getLocalBounds(Vector3& min, Vector3& max) const=0;
+
+        /// Return the scaling vector of the collision shape
+        Vector3 getScaling() const;
+
+        /// Set the local scaling vector of the collision shape
+        virtual void setLocalScaling(const Vector3& scaling);
+
+        /// Return the local inertia tensor of the collision shapes
+        virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0;
+
+        /// Compute the world-space AABB of the collision shape given a transform
+        virtual void computeAABB(AABB& aabb, const Transform& transform) const;
+
+        /// Return true if the collision shape type is a convex shape
+        static bool isConvex(CollisionShapeType shapeType);
+
+        /// Return the maximum number of contact manifolds in an overlapping pair given two shape types
+        static int computeNbMaxContactManifolds(CollisionShapeType shapeType1,
+                                                CollisionShapeType shapeType2);
+
+        // -------------------- Friendship -------------------- //
+
+        friend class ProxyShape;
+        friend class CollisionWorld;
+};
+
+// Return the type of the collision shape
+/**
+ * @return The type of the collision shape (box, sphere, cylinder, ...)
+ */
+inline CollisionShapeType CollisionShape::getType() const {
+    return mType;
+}
+
+// Return true if the collision shape type is a convex shape
+inline bool CollisionShape::isConvex(CollisionShapeType shapeType) {
+    return shapeType != CONCAVE_MESH && shapeType != HEIGHTFIELD;
+}
+
+// Return the scaling vector of the collision shape
+inline Vector3 CollisionShape::getScaling() const {
+    return mScaling;
+}
+
+// Set the scaling vector of the collision shape
+inline void CollisionShape::setLocalScaling(const Vector3& scaling) {
+    mScaling = scaling;
+}
+
+// Return the maximum number of contact manifolds allowed in an overlapping
+// pair wit the given two collision shape types
+inline int CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1,
+                                                        CollisionShapeType shapeType2) {
+    // If both shapes are convex
+    if (isConvex(shapeType1) && isConvex(shapeType2)) {
+        return NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE;
+    }   // If there is at least one concave shape
+    else {
+        return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE;
+    }
+}
+
+}
+
+#endif

src/collision/shapes/ConcaveMeshShape.cpp

@@ -0,0 +1,244 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "ConcaveMeshShape.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh)
+                 : ConcaveShape(CONCAVE_MESH) {
+    mTriangleMesh = triangleMesh;
+    mRaycastTestType = FRONT;
+
+    // Insert all the triangles into the dynamic AABB tree
+    initBVHTree();
+}
+
+// Destructor
+ConcaveMeshShape::~ConcaveMeshShape() {
+
+}
+
+// Insert all the triangles into the dynamic AABB tree
+void ConcaveMeshShape::initBVHTree() {
+
+    // TODO : Try to randomly add the triangles into the tree to obtain a better tree
+
+    // For each sub-part of the mesh
+    for (uint subPart=0; subPart<mTriangleMesh->getNbSubparts(); subPart++) {
+
+        // Get the triangle vertex array of the current sub-part
+        TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart);
+
+        TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType();
+        TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
+        unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
+        unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
+        int vertexStride = triangleVertexArray->getVerticesStride();
+        int indexStride = triangleVertexArray->getIndicesStride();
+
+        // For each triangle of the concave mesh
+        for (uint triangleIndex=0; triangleIndex<triangleVertexArray->getNbTriangles(); triangleIndex++) {
+
+            void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
+            Vector3 trianglePoints[3];
+
+            // For each vertex of the triangle
+            for (int k=0; k < 3; k++) {
+
+                // Get the index of the current vertex in the triangle
+                int vertexIndex = 0;
+                if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) {
+                    vertexIndex = ((uint*)vertexIndexPointer)[k];
+                }
+                else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
+                    vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
+                }
+                else {
+                    assert(false);
+                }
+
+                // Get the vertices components of the triangle
+                if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) {
+                    const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
+                    trianglePoints[k][0] = decimal(vertices[0]) * mScaling.x;
+                    trianglePoints[k][1] = decimal(vertices[1]) * mScaling.y;
+                    trianglePoints[k][2] = decimal(vertices[2]) * mScaling.z;
+                }
+                else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) {
+                    const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
+                    trianglePoints[k][0] = decimal(vertices[0]) * mScaling.x;
+                    trianglePoints[k][1] = decimal(vertices[1]) * mScaling.y;
+                    trianglePoints[k][2] = decimal(vertices[2]) * mScaling.z;
+                }
+                else {
+                    assert(false);
+                }
+            }
+
+            // Create the AABB for the triangle
+            AABB aabb = AABB::createAABBForTriangle(trianglePoints);
+            aabb.inflate(mTriangleMargin, mTriangleMargin, mTriangleMargin);
+
+            // Add the AABB with the index of the triangle into the dynamic AABB tree
+            mDynamicAABBTree.addObject(aabb, subPart, triangleIndex);
+        }
+    }
+}
+
+// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
+// given the start vertex index pointer of the triangle
+void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex,
+                                                           Vector3* outTriangleVertices) const {
+
+    // Get the triangle vertex array of the current sub-part
+    TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart);
+
+    TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType();
+    TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
+    unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
+    unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
+    int vertexStride = triangleVertexArray->getVerticesStride();
+    int indexStride = triangleVertexArray->getIndicesStride();
+
+    void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
+
+    // For each vertex of the triangle
+    for (int k=0; k < 3; k++) {
+
+        // Get the index of the current vertex in the triangle
+        int vertexIndex = 0;
+        if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) {
+            vertexIndex = ((uint*)vertexIndexPointer)[k];
+        }
+        else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
+            vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
+        }
+        else {
+            assert(false);
+        }
+
+        // Get the vertices components of the triangle
+        if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) {
+            const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
+            outTriangleVertices[k][0] = decimal(vertices[0]) * mScaling.x;
+            outTriangleVertices[k][1] = decimal(vertices[1]) * mScaling.y;
+            outTriangleVertices[k][2] = decimal(vertices[2]) * mScaling.z;
+        }
+        else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) {
+            const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
+            outTriangleVertices[k][0] = decimal(vertices[0]) * mScaling.x;
+            outTriangleVertices[k][1] = decimal(vertices[1]) * mScaling.y;
+            outTriangleVertices[k][2] = decimal(vertices[2]) * mScaling.z;
+        }
+        else {
+            assert(false);
+        }
+    }
+}
+
+// Use a callback method on all triangles of the concave shape inside a given AABB
+void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const {
+
+    ConvexTriangleAABBOverlapCallback overlapCallback(callback, *this, mDynamicAABBTree);
+
+    // Ask the Dynamic AABB Tree to report all the triangles that are overlapping
+    // with the AABB of the convex shape.
+    mDynamicAABBTree.reportAllShapesOverlappingWithAABB(localAABB, overlapCallback);
+}
+
+// Raycast method with feedback information
+/// Note that only the first triangle hit by the ray in the mesh will be returned, even if
+/// the ray hits many triangles.
+bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
+
+    PROFILE("ConcaveMeshShape::raycast()");
+
+    // Create the callback object that will compute ray casting against triangles
+    ConcaveMeshRaycastCallback raycastCallback(mDynamicAABBTree, *this, proxyShape, raycastInfo, ray);
+
+    // Ask the Dynamic AABB Tree to report all AABB nodes that are hit by the ray.
+    // The raycastCallback object will then compute ray casting against the triangles
+    // in the hit AABBs.
+    mDynamicAABBTree.raycast(ray, raycastCallback);
+
+    raycastCallback.raycastTriangles();
+
+    return raycastCallback.getIsHit();
+}
+
+// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree
+decimal ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) {
+
+    // Add the id of the hit AABB node into
+    mHitAABBNodes.push_back(nodeId);
+
+    return ray.maxFraction;
+}
+
+// Raycast all collision shapes that have been collected
+void ConcaveMeshRaycastCallback::raycastTriangles() {
+
+    std::vector<int>::const_iterator it;
+    decimal smallestHitFraction = mRay.maxFraction;
+
+    for (it = mHitAABBNodes.begin(); it != mHitAABBNodes.end(); ++it) {
+
+        // Get the node data (triangle index and mesh subpart index)
+        int32* data = mDynamicAABBTree.getNodeDataInt(*it);
+
+        // Get the triangle vertices for this node from the concave mesh shape
+        Vector3 trianglePoints[3];
+        mConcaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
+
+        // Create a triangle collision shape
+        decimal margin = mConcaveMeshShape.getTriangleMargin();
+        TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
+        triangleShape.setRaycastTestType(mConcaveMeshShape.getRaycastTestType());
+
+        // Ray casting test against the collision shape
+        RaycastInfo raycastInfo;
+        bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape);
+
+        // If the ray hit the collision shape
+        if (isTriangleHit && raycastInfo.hitFraction <= smallestHitFraction) {
+
+            assert(raycastInfo.hitFraction >= decimal(0.0));
+
+            mRaycastInfo.body = raycastInfo.body;
+            mRaycastInfo.proxyShape = raycastInfo.proxyShape;
+            mRaycastInfo.hitFraction = raycastInfo.hitFraction;
+            mRaycastInfo.worldPoint = raycastInfo.worldPoint;
+            mRaycastInfo.worldNormal = raycastInfo.worldNormal;
+            mRaycastInfo.meshSubpart = data[0];
+            mRaycastInfo.triangleIndex = data[1];
+
+            smallestHitFraction = raycastInfo.hitFraction;
+            mIsHit = true;
+        }
+    }
+}

src/collision/shapes/ConcaveMeshShape.h

@@ -0,0 +1,235 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_CONCAVE_MESH_SHAPE_H
+#define REACTPHYSICS3D_CONCAVE_MESH_SHAPE_H
+
+// Libraries
+#include "ConcaveShape.h"
+#include "collision/broadphase/DynamicAABBTree.h"
+#include "collision/TriangleMesh.h"
+#include "collision/shapes/TriangleShape.h"
+#include "engine/Profiler.h"
+
+namespace reactphysics3d {
+
+class ConcaveMeshShape;
+
+// class ConvexTriangleAABBOverlapCallback
+class ConvexTriangleAABBOverlapCallback : public DynamicAABBTreeOverlapCallback {
+
+    private:
+
+        TriangleCallback& mTriangleTestCallback;
+
+        // Reference to the concave mesh shape
+        const ConcaveMeshShape& mConcaveMeshShape;
+
+        // Reference to the Dynamic AABB tree
+        const DynamicAABBTree& mDynamicAABBTree;
+
+    public:
+
+        // Constructor
+        ConvexTriangleAABBOverlapCallback(TriangleCallback& triangleCallback, const ConcaveMeshShape& concaveShape,
+                                          const DynamicAABBTree& dynamicAABBTree)
+          : mTriangleTestCallback(triangleCallback), mConcaveMeshShape(concaveShape), mDynamicAABBTree(dynamicAABBTree) {
+
+        }
+
+        // Called when a overlapping node has been found during the call to
+        // DynamicAABBTree:reportAllShapesOverlappingWithAABB()
+        virtual void notifyOverlappingNode(int nodeId);
+
+};
+
+/// Class ConcaveMeshRaycastCallback
+class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
+
+    private :
+
+        std::vector<int32> mHitAABBNodes;
+        const DynamicAABBTree& mDynamicAABBTree;
+        const ConcaveMeshShape& mConcaveMeshShape;
+        ProxyShape* mProxyShape;
+        RaycastInfo& mRaycastInfo;
+        const Ray& mRay;
+        bool mIsHit;
+
+    public:
+
+        // Constructor
+        ConcaveMeshRaycastCallback(const DynamicAABBTree& dynamicAABBTree, const ConcaveMeshShape& concaveMeshShape,
+                                   ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray)
+            : mDynamicAABBTree(dynamicAABBTree), mConcaveMeshShape(concaveMeshShape), mProxyShape(proxyShape),
+              mRaycastInfo(raycastInfo), mRay(ray), mIsHit(false) {
+
+        }
+
+        /// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree
+        virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray);
+
+        /// Raycast all collision shapes that have been collected
+        void raycastTriangles();
+
+        /// Return true if a raycast hit has been found
+        bool getIsHit() const {
+            return mIsHit;
+        }
+};
+
+// Class ConcaveMeshShape
+/**
+ * This class represents a static concave mesh shape. Note that collision detection
+ * with a concave mesh shape can be very expensive. You should use only use
+ * this shape for a static mesh.
+ */
+class ConcaveMeshShape : public ConcaveShape {
+
+    protected:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Triangle mesh
+        TriangleMesh* mTriangleMesh;
+
+        /// Dynamic AABB tree to accelerate collision with the triangles
+        DynamicAABBTree mDynamicAABBTree;
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        ConcaveMeshShape(const ConcaveMeshShape& shape);
+
+        /// Private assignment operator
+        ConcaveMeshShape& operator=(const ConcaveMeshShape& shape);
+
+        /// Raycast method with feedback information
+        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
+
+        /// Return the number of bytes used by the collision shape
+        virtual size_t getSizeInBytes() const;
+
+        /// Insert all the triangles into the dynamic AABB tree
+        void initBVHTree();
+
+        /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
+        /// given the start vertex index pointer of the triangle.
+        void getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex,
+                                                 Vector3* outTriangleVertices) const;
+
+    public:
+
+        /// Constructor
+        ConcaveMeshShape(TriangleMesh* triangleMesh);
+
+        /// Destructor
+        ~ConcaveMeshShape();
+
+        /// Return the local bounds of the shape in x, y and z directions.
+        virtual void getLocalBounds(Vector3& min, Vector3& max) const;
+
+        /// Set the local scaling vector of the collision shape
+        virtual void setLocalScaling(const Vector3& scaling);
+
+        /// Return the local inertia tensor of the collision shape
+        virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
+
+        /// Use a callback method on all triangles of the concave shape inside a given AABB
+        virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const;
+
+        // ---------- Friendship ----------- //
+
+        friend class ConvexTriangleAABBOverlapCallback;
+        friend class ConcaveMeshRaycastCallback;
+};
+
+// Return the number of bytes used by the collision shape
+inline size_t ConcaveMeshShape::getSizeInBytes() const {
+    return sizeof(ConcaveMeshShape);
+}
+
+// Return the local bounds of the shape in x, y and z directions.
+// This method is used to compute the AABB of the box
+/**
+ * @param min The minimum bounds of the shape in local-space coordinates
+ * @param max The maximum bounds of the shape in local-space coordinates
+ */
+inline void ConcaveMeshShape::getLocalBounds(Vector3& min, Vector3& max) const {
+
+    // Get the AABB of the whole tree
+    AABB treeAABB = mDynamicAABBTree.getRootAABB();
+
+    min = treeAABB.getMin();
+    max = treeAABB.getMax();
+}
+
+// Set the local scaling vector of the collision shape
+inline void ConcaveMeshShape::setLocalScaling(const Vector3& scaling) {
+
+    CollisionShape::setLocalScaling(scaling);
+
+    // Reset the Dynamic AABB Tree
+    mDynamicAABBTree.reset();
+
+    // Rebuild Dynamic AABB Tree here
+    initBVHTree();
+}
+
+// Return the local inertia tensor of the shape
+/**
+ * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
+ *                    coordinates
+ * @param mass Mass to use to compute the inertia tensor of the collision shape
+ */
+inline void ConcaveMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
+
+    // Default inertia tensor
+    // Note that this is not very realistic for a concave triangle mesh.
+    // However, in most cases, it will only be used static bodies and therefore,
+    // the inertia tensor is not used.
+    tensor.setAllValues(mass, 0, 0,
+                        0, mass, 0,
+                        0, 0, mass);
+}
+
+// Called when a overlapping node has been found during the call to
+// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
+inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId) {
+
+    // Get the node data (triangle index and mesh subpart index)
+    int32* data = mDynamicAABBTree.getNodeDataInt(nodeId);
+
+    // Get the triangle vertices for this node from the concave mesh shape
+    Vector3 trianglePoints[3];
+    mConcaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
+
+    // Call the callback to test narrow-phase collision with this triangle
+    mTriangleTestCallback.testTriangle(trianglePoints);
+}
+
+}
+#endif
+

src/collision/shapes/ConcaveShape.cpp

@@ -0,0 +1,43 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "ConcaveShape.h"
+
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Constructor
+ConcaveShape::ConcaveShape(CollisionShapeType type)
+             : CollisionShape(type), mIsSmoothMeshCollisionEnabled(false),
+               mTriangleMargin(0), mRaycastTestType(FRONT) {
+
+}
+
+// Destructor
+ConcaveShape::~ConcaveShape() {
+
+}

src/collision/shapes/ConcaveShape.h

@@ -0,0 +1,158 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_CONCAVE_SHAPE_H
+#define REACTPHYSICS3D_CONCAVE_SHAPE_H
+
+// Libraries
+#include "CollisionShape.h"
+#include "TriangleShape.h"
+
+// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Class TriangleCallback
+/**
+ * This class is used to encapsulate a callback method for
+ * a single triangle of a ConcaveMesh.
+ */
+class TriangleCallback {
+
+    public:
+
+        /// Report a triangle
+        virtual void testTriangle(const Vector3* trianglePoints)=0;
+
+};
+
+
+// Class ConcaveShape
+/**
+ * This abstract class represents a concave collision shape associated with a
+ * body that is used during the narrow-phase collision detection.
+ */
+class ConcaveShape : public CollisionShape {
+
+    protected :
+
+        // -------------------- Attributes -------------------- //
+
+        /// True if the smooth mesh collision algorithm is enabled
+        bool mIsSmoothMeshCollisionEnabled;
+
+        // Margin use for collision detection for each triangle
+        decimal mTriangleMargin;
+
+        /// Raycast test type for the triangle (front, back, front-back)
+        TriangleRaycastSide mRaycastTestType;
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        ConcaveShape(const ConcaveShape& shape);
+
+        /// Private assignment operator
+        ConcaveShape& operator=(const ConcaveShape& shape);
+
+        /// Return true if a point is inside the collision shape
+        virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        ConcaveShape(CollisionShapeType type);
+
+        /// Destructor
+        virtual ~ConcaveShape();
+
+        /// Return the triangle margin
+        decimal getTriangleMargin() const;
+
+        /// Return the raycast test type (front, back, front-back)
+        TriangleRaycastSide getRaycastTestType() const;
+
+        // Set the raycast test type (front, back, front-back)
+        void setRaycastTestType(TriangleRaycastSide testType);
+
+        /// Return true if the collision shape is convex, false if it is concave
+        virtual bool isConvex() const;
+
+        /// Use a callback method on all triangles of the concave shape inside a given AABB
+        virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const=0;
+
+        /// Return true if the smooth mesh collision is enabled
+        bool getIsSmoothMeshCollisionEnabled() const;
+
+        /// Enable/disable the smooth mesh collision algorithm
+        void setIsSmoothMeshCollisionEnabled(bool isEnabled);
+};
+
+// Return the triangle margin
+inline decimal ConcaveShape::getTriangleMargin() const {
+    return mTriangleMargin;
+}
+
+/// Return true if the collision shape is convex, false if it is concave
+inline bool ConcaveShape::isConvex() const {
+    return false;
+}
+
+// Return true if a point is inside the collision shape
+inline bool ConcaveShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
+    return false;
+}
+
+// Return true if the smooth mesh collision is enabled
+inline bool ConcaveShape::getIsSmoothMeshCollisionEnabled() const {
+    return mIsSmoothMeshCollisionEnabled;
+}
+
+// Enable/disable the smooth mesh collision algorithm
+/// Smooth mesh collision is used to avoid collisions against some internal edges
+/// of the triangle mesh. If it is enabled, collsions with the mesh will be smoother
+/// but collisions computation is a bit more expensive.
+inline void ConcaveShape::setIsSmoothMeshCollisionEnabled(bool isEnabled) {
+    mIsSmoothMeshCollisionEnabled = isEnabled;
+}
+
+// Return the raycast test type (front, back, front-back)
+inline TriangleRaycastSide ConcaveShape::getRaycastTestType() const {
+    return mRaycastTestType;
+}
+
+// Set the raycast test type (front, back, front-back)
+/**
+ * @param testType Raycast test type for the triangle (front, back, front-back)
+ */
+inline void ConcaveShape::setRaycastTestType(TriangleRaycastSide testType) {
+    mRaycastTestType = testType;
+}
+
+}
+
+#endif
+

src/collision/shapes/ConeShape.cpp

@@ -0,0 +1,214 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include <complex>
+#include "configuration.h"
+#include "ConeShape.h"
+#include "collision/ProxyShape.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+/**
+ * @param radius Radius of the cone (in meters)
+ * @param height Height of the cone (in meters)
+ * @param margin Collision margin (in meters) around the collision shape
+ */
+ConeShape::ConeShape(decimal radius, decimal height, decimal margin)
+          : ConvexShape(CONE, margin), mRadius(radius), mHalfHeight(height * decimal(0.5)) {
+    assert(mRadius > decimal(0.0));
+    assert(mHalfHeight > decimal(0.0));
+    
+    // Compute the sine of the semi-angle at the apex point
+    mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height));
+}
+
+// Destructor
+ConeShape::~ConeShape() {
+
+}
+
+// Return a local support point in a given direction without the object margin
+Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
+                                                     void** cachedCollisionData) const {
+
+    const Vector3& v = direction;
+    decimal sinThetaTimesLengthV = mSinTheta * v.length();
+    Vector3 supportPoint;
+
+    if (v.y > sinThetaTimesLengthV) {
+        supportPoint = Vector3(0.0, mHalfHeight, 0.0);
+    }
+    else {
+        decimal projectedLength = sqrt(v.x * v.x + v.z * v.z);
+        if (projectedLength > MACHINE_EPSILON) {
+            decimal d = mRadius / projectedLength;
+            supportPoint = Vector3(v.x * d, -mHalfHeight, v.z * d);
+        }
+        else {
+            supportPoint = Vector3(0.0, -mHalfHeight, 0.0);
+        }
+    }
+
+    return supportPoint;
+}
+
+// Raycast method with feedback information
+// This implementation is based on the technique described by David Eberly in the article
+// "Intersection of a Line and a Cone" that can be found at
+// http://www.geometrictools.com/Documentation/IntersectionLineCone.pdf
+bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
+
+    const Vector3 r = ray.point2 - ray.point1;
+
+    const decimal epsilon = decimal(0.00001);
+    Vector3 V(0, mHalfHeight, 0);
+    Vector3 centerBase(0, -mHalfHeight, 0);
+    Vector3 axis(0, decimal(-1.0), 0);
+    decimal heightSquare = decimal(4.0) * mHalfHeight * mHalfHeight;
+    decimal cosThetaSquare = heightSquare / (heightSquare + mRadius * mRadius);
+    decimal factor = decimal(1.0) - cosThetaSquare;
+    Vector3 delta = ray.point1 - V;
+    decimal c0 = -cosThetaSquare * delta.x * delta.x  + factor * delta.y * delta.y -
+                  cosThetaSquare * delta.z * delta.z;
+    decimal c1 = -cosThetaSquare * delta.x * r.x + factor * delta.y * r.y - cosThetaSquare * delta.z * r.z;
+    decimal c2 = -cosThetaSquare * r.x * r.x  + factor * r.y * r.y - cosThetaSquare * r.z * r.z;
+    decimal tHit[] = {decimal(-1.0), decimal(-1.0), decimal(-1.0)};
+    Vector3 localHitPoint[3];
+    Vector3 localNormal[3];
+
+    // If c2 is different from zero
+    if (std::abs(c2) > MACHINE_EPSILON) {
+        decimal gamma = c1 * c1 - c0 * c2;
+
+        // If there is no real roots in the quadratic equation
+        if (gamma < decimal(0.0)) {
+            return false;
+        }
+        else if (gamma > decimal(0.0)) {    // The equation has two real roots
+
+            // Compute two intersections
+            decimal sqrRoot = std::sqrt(gamma);
+            tHit[0] = (-c1 - sqrRoot) / c2;
+            tHit[1] = (-c1 + sqrRoot) / c2;
+        }
+        else {  // If the equation has a single real root
+
+            // Compute the intersection
+            tHit[0] = -c1 / c2;
+        }
+    }
+    else {  // If c2 == 0
+
+        // If c2 = 0 and c1 != 0
+        if (std::abs(c1) > MACHINE_EPSILON) {
+            tHit[0] = -c0 / (decimal(2.0) * c1);
+        }
+        else {  // If c2 = c1 = 0
+
+            // If c0 is different from zero, no solution and if c0 = 0, we have a
+            // degenerate case, the whole ray is contained in the cone side
+            // but we return no hit in this case
+            return false;
+        }
+    }
+
+    // If the origin of the ray is inside the cone, we return no hit
+    if (testPointInside(ray.point1, NULL)) return false;
+
+    localHitPoint[0] = ray.point1 + tHit[0] * r;
+    localHitPoint[1] = ray.point1 + tHit[1] * r;
+
+    // Only keep hit points in one side of the double cone (the cone we are interested in)
+    if (axis.dot(localHitPoint[0] - V) < decimal(0.0)) {
+        tHit[0] = decimal(-1.0);
+    }
+    if (axis.dot(localHitPoint[1] - V) < decimal(0.0)) {
+        tHit[1] = decimal(-1.0);
+    }
+
+    // Only keep hit points that are within the correct height of the cone
+    if (localHitPoint[0].y < decimal(-mHalfHeight)) {
+        tHit[0] = decimal(-1.0);
+    }
+    if (localHitPoint[1].y < decimal(-mHalfHeight)) {
+        tHit[1] = decimal(-1.0);
+    }
+
+    // If the ray is in direction of the base plane of the cone
+    if (r.y > epsilon) {
+
+        // Compute the intersection with the base plane of the cone
+        tHit[2] = (-ray.point1.y - mHalfHeight) / (r.y);
+
+        // Only keep this intersection if it is inside the cone radius
+        localHitPoint[2] = ray.point1 + tHit[2] * r;
+
+        if ((localHitPoint[2] - centerBase).lengthSquare() > mRadius * mRadius) {
+            tHit[2] = decimal(-1.0);
+        }
+
+        // Compute the normal direction
+        localNormal[2] = axis;
+    }
+
+    // Find the smallest positive t value
+    int hitIndex = -1;
+    decimal t = DECIMAL_LARGEST;
+    for (int i=0; i<3; i++) {
+        if (tHit[i] < decimal(0.0)) continue;
+        if (tHit[i] < t) {
+            hitIndex = i;
+            t = tHit[hitIndex];
+        }
+    }
+
+    if (hitIndex < 0) return false;
+    if (t > ray.maxFraction) return false;
+
+    // Compute the normal direction for hit against side of the cone
+    if (hitIndex != 2) {
+        decimal h = decimal(2.0) * mHalfHeight;
+        decimal value1 = (localHitPoint[hitIndex].x * localHitPoint[hitIndex].x +
+                          localHitPoint[hitIndex].z * localHitPoint[hitIndex].z);
+        decimal rOverH = mRadius / h;
+        decimal value2 = decimal(1.0) + rOverH * rOverH;
+        decimal factor = decimal(1.0) / std::sqrt(value1 * value2);
+        decimal x = localHitPoint[hitIndex].x * factor;
+        decimal z = localHitPoint[hitIndex].z * factor;
+        localNormal[hitIndex].x = x;
+        localNormal[hitIndex].y = std::sqrt(x * x + z * z) * rOverH;
+        localNormal[hitIndex].z = z;
+    }
+
+    raycastInfo.body = proxyShape->getBody();
+    raycastInfo.proxyShape = proxyShape;
+    raycastInfo.hitFraction = t;
+    raycastInfo.worldPoint = localHitPoint[hitIndex];
+    raycastInfo.worldNormal = localNormal[hitIndex];
+
+    return true;
+}

src/collision/shapes/ConeShape.h

@@ -0,0 +1,186 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_CONE_SHAPE_H
+#define REACTPHYSICS3D_CONE_SHAPE_H
+
+// Libraries
+#include "ConvexShape.h"
+#include "body/CollisionBody.h"
+#include "mathematics/mathematics.h"
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Class ConeShape
+/**
+ * This class represents a cone collision shape centered at the
+ * origin and alligned with the Y axis. The cone is defined
+ * by its height and by the radius of its base. The center of the
+ * cone is at the half of the height. The "transform" of the
+ * corresponding rigid body gives an orientation and a position
+ * to the cone. This collision shape uses an extra margin distance around
+ * it for collision detection purpose. The default margin is 4cm (if your
+ * units are meters, which is recommended). In case, you want to simulate small
+ * objects (smaller than the margin distance), you might want to reduce the margin
+ * by specifying your own margin distance using the "margin" parameter in the
+ * constructor of the cone shape. Otherwise, it is recommended to use the
+ * default margin distance by not using the "margin" parameter in the constructor.
+ */
+class ConeShape : public ConvexShape {
+
+    protected :
+
+        // -------------------- Attributes -------------------- //
+
+        /// Radius of the base
+        decimal mRadius;
+
+        /// Half height of the cone
+        decimal mHalfHeight;
+
+        /// sine of the semi angle at the apex point
+        decimal mSinTheta;
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        ConeShape(const ConeShape& shape);
+
+        /// Private assignment operator
+        ConeShape& operator=(const ConeShape& shape);
+
+        /// Return a local support point in a given direction without the object margin
+        virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
+                                                          void** cachedCollisionData) const;
+
+        /// Return true if a point is inside the collision shape
+        virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
+
+        /// Raycast method with feedback information
+        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
+
+        /// Return the number of bytes used by the collision shape
+        virtual size_t getSizeInBytes() const;
+        
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        ConeShape(decimal mRadius, decimal height, decimal margin = OBJECT_MARGIN);
+
+        /// Destructor
+        virtual ~ConeShape();
+
+        /// Return the radius
+        decimal getRadius() const;
+
+        /// Return the height
+        decimal getHeight() const;
+
+        /// Set the scaling vector of the collision shape
+        virtual void setLocalScaling(const Vector3& scaling);
+
+        /// Return the local bounds of the shape in x, y and z directions
+        virtual void getLocalBounds(Vector3& min, Vector3& max) const;
+
+        /// Return the local inertia tensor of the collision shape
+        virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
+};
+
+// Return the radius
+/**
+ * @return Radius of the cone (in meters)
+ */
+inline decimal ConeShape::getRadius() const {
+    return mRadius;
+}
+
+// Return the height
+/**
+ * @return Height of the cone (in meters)
+ */
+inline decimal ConeShape::getHeight() const {
+    return decimal(2.0) * mHalfHeight;
+}
+
+// Set the scaling vector of the collision shape
+inline void ConeShape::setLocalScaling(const Vector3& scaling) {
+
+    mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y;
+    mRadius = (mRadius / mScaling.x) * scaling.x;
+
+    CollisionShape::setLocalScaling(scaling);
+}
+
+// Return the number of bytes used by the collision shape
+inline size_t ConeShape::getSizeInBytes() const {
+    return sizeof(ConeShape);
+}
+
+// Return the local bounds of the shape in x, y and z directions
+/**
+ * @param min The minimum bounds of the shape in local-space coordinates
+ * @param max The maximum bounds of the shape in local-space coordinates
+ */
+inline void ConeShape::getLocalBounds(Vector3& min, Vector3& max) const {
+
+    // Maximum bounds
+    max.x = mRadius + mMargin;
+    max.y = mHalfHeight + mMargin;
+    max.z = max.x;
+
+    // Minimum bounds
+    min.x = -max.x;
+    min.y = -max.y;
+    min.z = min.x;
+}
+
+// Return the local inertia tensor of the collision shape
+/**
+ * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
+ *                    coordinates
+ * @param mass Mass to use to compute the inertia tensor of the collision shape
+ */
+inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
+    decimal rSquare = mRadius * mRadius;
+    decimal diagXZ = decimal(0.15) * mass * (rSquare + mHalfHeight);
+    tensor.setAllValues(diagXZ, 0.0, 0.0,
+                        0.0, decimal(0.3) * mass * rSquare,
+                        0.0, 0.0, 0.0, diagXZ);
+}
+
+// Return true if a point is inside the collision shape
+inline bool ConeShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
+    const decimal radiusHeight = mRadius * (-localPoint.y + mHalfHeight) /
+                                          (mHalfHeight * decimal(2.0));
+    return (localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) &&
+           (localPoint.x * localPoint.x + localPoint.z * localPoint.z < radiusHeight *radiusHeight);
+}
+
+}
+
+#endif

src/collision/shapes/ConvexMeshShape.cpp

@@ -0,0 +1,268 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include <complex>
+#include "configuration.h"
+#include "ConvexMeshShape.h"
+
+using namespace reactphysics3d;
+
+// Constructor to initialize with an array of 3D vertices.
+/// This method creates an internal copy of the input vertices.
+/**
+ * @param arrayVertices Array with the vertices of the convex mesh
+ * @param nbVertices Number of vertices in the convex mesh
+ * @param stride Stride between the beginning of two elements in the vertices array
+ * @param margin Collision margin (in meters) around the collision shape
+ */
+ConvexMeshShape::ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride, decimal margin)
+                : ConvexShape(CONVEX_MESH, margin), mNbVertices(nbVertices), mMinBounds(0, 0, 0),
+                  mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false) {
+    assert(nbVertices > 0);
+    assert(stride > 0);
+
+    const unsigned char* vertexPointer = (const unsigned char*) arrayVertices;
+
+    // Copy all the vertices into the internal array
+    for (uint i=0; i<mNbVertices; i++) {
+        const decimal* newPoint = (const decimal*) vertexPointer;
+        mVertices.push_back(Vector3(newPoint[0], newPoint[1], newPoint[2]));
+        vertexPointer += stride;
+    }
+
+    // Recalculate the bounds of the mesh
+    recalculateBounds();
+}
+
+// Constructor to initialize with a triangle mesh
+/// This method creates an internal copy of the input vertices.
+/**
+ * @param triangleVertexArray Array with the vertices and indices of the vertices and triangles of the mesh
+ * @param isEdgesInformationUsed True if you want to use edges information for collision detection (faster but requires more memory)
+ * @param margin Collision margin (in meters) around the collision shape
+ */
+ConvexMeshShape::ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool isEdgesInformationUsed, decimal margin)
+                : ConvexShape(CONVEX_MESH, margin), mMinBounds(0, 0, 0),
+                  mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(isEdgesInformationUsed) {
+
+    TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType();
+    TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
+    unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
+    unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
+    int vertexStride = triangleVertexArray->getVerticesStride();
+    int indexStride = triangleVertexArray->getIndicesStride();
+
+    // For each vertex of the mesh
+    for (uint v = 0; v < triangleVertexArray->getNbVertices(); v++) {
+
+        // Get the vertices components of the triangle
+        if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) {
+            const float* vertices = (float*)(verticesStart + v * vertexStride);
+
+            Vector3 vertex(vertices[0], vertices[1], vertices[2] );
+            vertex = vertex * mScaling;
+            mVertices.push_back(vertex);
+        }
+        else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) {
+            const double* vertices = (double*)(verticesStart + v * vertexStride);
+
+            Vector3 vertex(vertices[0], vertices[1], vertices[2] );
+            vertex = vertex * mScaling;
+            mVertices.push_back(vertex);
+        }
+    }
+
+    // If we need to use the edges information of the mesh
+    if (mIsEdgesInformationUsed) {
+
+        // For each triangle of the mesh
+        for (uint triangleIndex=0; triangleIndex<triangleVertexArray->getNbTriangles(); triangleIndex++) {
+
+            void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
+
+            uint vertexIndex[3] = {0, 0, 0};
+
+            // For each vertex of the triangle
+            for (int k=0; k < 3; k++) {
+
+                // Get the index of the current vertex in the triangle
+                if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) {
+                    vertexIndex[k] = ((uint*)vertexIndexPointer)[k];
+                }
+                else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
+                    vertexIndex[k] = ((unsigned short*)vertexIndexPointer)[k];
+                }
+                else {
+                    assert(false);
+                }
+            }
+
+            // Add information about the edges
+            addEdge(vertexIndex[0], vertexIndex[1]);
+            addEdge(vertexIndex[0], vertexIndex[2]);
+            addEdge(vertexIndex[1], vertexIndex[2]);
+        }
+    }
+
+    mNbVertices = mVertices.size();
+    recalculateBounds();
+}
+
+// Constructor.
+/// If you use this constructor, you will need to set the vertices manually one by one using
+/// the addVertex() method.
+ConvexMeshShape::ConvexMeshShape(decimal margin)
+                : ConvexShape(CONVEX_MESH, margin), mNbVertices(0), mMinBounds(0, 0, 0),
+                  mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false) {
+
+}
+
+// Destructor
+ConvexMeshShape::~ConvexMeshShape() {
+
+}
+
+// Return a local support point in a given direction without the object margin.
+/// If the edges information is not used for collision detection, this method will go through
+/// the whole vertices list and pick up the vertex with the largest dot product in the support
+/// direction. This is an O(n) process with "n" being the number of vertices in the mesh.
+/// However, if the edges information is used, we can cache the previous support vertex and use
+/// it as a start in a hill-climbing (local search) process to find the new support vertex which
+/// will be in most of the cases very close to the previous one. Using hill-climbing, this method
+/// runs in almost constant time.
+Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
+                                                           void** cachedCollisionData) const {
+
+    assert(mNbVertices == mVertices.size());
+    assert(cachedCollisionData != NULL);
+
+    // Allocate memory for the cached collision data if not allocated yet
+    if ((*cachedCollisionData) == NULL) {
+        *cachedCollisionData = (int*) malloc(sizeof(int));
+        *((int*)(*cachedCollisionData)) = 0;
+    }
+
+    // If the edges information is used to speed up the collision detection
+    if (mIsEdgesInformationUsed) {
+
+        assert(mEdgesAdjacencyList.size() == mNbVertices);
+
+        uint maxVertex = *((int*)(*cachedCollisionData));
+        decimal maxDotProduct = direction.dot(mVertices[maxVertex]);
+        bool isOptimal;
+
+        // Perform hill-climbing (local search)
+        do {
+            isOptimal = true;
+
+            assert(mEdgesAdjacencyList.at(maxVertex).size() > 0);
+
+            // For all neighbors of the current vertex
+            std::set<uint>::const_iterator it;
+            std::set<uint>::const_iterator itBegin = mEdgesAdjacencyList.at(maxVertex).begin();
+            std::set<uint>::const_iterator itEnd = mEdgesAdjacencyList.at(maxVertex).end();
+            for (it = itBegin; it != itEnd; ++it) {
+
+                // Compute the dot product
+                decimal dotProduct = direction.dot(mVertices[*it]);
+
+                // If the current vertex is a better vertex (larger dot product)
+                if (dotProduct > maxDotProduct) {
+                    maxVertex = *it;
+                    maxDotProduct = dotProduct;
+                    isOptimal = false;
+                }
+            }
+
+        } while(!isOptimal);
+
+        // Cache the support vertex
+        *((int*)(*cachedCollisionData)) = maxVertex;
+
+        // Return the support vertex
+        return mVertices[maxVertex] * mScaling;
+    }
+    else {  // If the edges information is not used
+
+        double maxDotProduct = DECIMAL_SMALLEST;
+        uint indexMaxDotProduct = 0;
+
+        // For each vertex of the mesh
+        for (uint i=0; i<mNbVertices; i++) {
+
+            // Compute the dot product of the current vertex
+            double dotProduct = direction.dot(mVertices[i]);
+
+            // If the current dot product is larger than the maximum one
+            if (dotProduct > maxDotProduct) {
+                indexMaxDotProduct = i;
+                maxDotProduct = dotProduct;
+            }
+        }
+
+        assert(maxDotProduct >= decimal(0.0));
+
+        // Return the vertex with the largest dot product in the support direction
+        return mVertices[indexMaxDotProduct] * mScaling;
+    }
+}
+
+// Recompute the bounds of the mesh
+void ConvexMeshShape::recalculateBounds() {
+
+    // TODO : Only works if the local origin is inside the mesh
+    //        => Make it more robust (init with first vertex of mesh instead)
+
+    mMinBounds.setToZero();
+    mMaxBounds.setToZero();
+
+    // For each vertex of the mesh
+    for (uint i=0; i<mNbVertices; i++) {
+
+        if (mVertices[i].x > mMaxBounds.x) mMaxBounds.x = mVertices[i].x;
+        if (mVertices[i].x < mMinBounds.x) mMinBounds.x = mVertices[i].x;
+
+        if (mVertices[i].y > mMaxBounds.y) mMaxBounds.y = mVertices[i].y;
+        if (mVertices[i].y < mMinBounds.y) mMinBounds.y = mVertices[i].y;
+
+        if (mVertices[i].z > mMaxBounds.z) mMaxBounds.z = mVertices[i].z;
+        if (mVertices[i].z < mMinBounds.z) mMinBounds.z = mVertices[i].z;
+    }
+
+    // Apply the local scaling factor
+    mMaxBounds = mMaxBounds * mScaling;
+    mMinBounds = mMinBounds * mScaling;
+
+    // Add the object margin to the bounds
+    mMaxBounds += Vector3(mMargin, mMargin, mMargin);
+    mMinBounds -= Vector3(mMargin, mMargin, mMargin);
+}
+
+// Raycast method with feedback information
+bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
+    return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast(
+                                     ray, proxyShape, raycastInfo);
+}

src/collision/shapes/ConvexMeshShape.h

@@ -0,0 +1,265 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_CONVEX_MESH_SHAPE_H
+#define REACTPHYSICS3D_CONVEX_MESH_SHAPE_H
+
+// Libraries
+#include "ConvexShape.h"
+#include "engine/CollisionWorld.h"
+#include "mathematics/mathematics.h"
+#include "collision/TriangleMesh.h"
+#include "collision/narrowphase/GJK/GJKAlgorithm.h"
+#include <vector>
+#include <set>
+#include <map>
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Declaration
+class CollisionWorld;
+
+// Class ConvexMeshShape
+/**
+ * This class represents a convex mesh shape. In order to create a convex mesh shape, you
+ * need to indicate the local-space position of the mesh vertices. You do it either by
+ * passing a vertices array to the constructor or using the addVertex() method. Make sure
+ * that the set of vertices that you use to create the shape are indeed part of a convex
+ * mesh. The center of mass of the shape will be at the origin of the local-space geometry
+ * that you use to create the mesh. The method used for collision detection with a convex
+ * mesh shape has an O(n) running time with "n" beeing the number of vertices in the mesh.
+ * Therefore, you should try not to use too many vertices. However, it is possible to speed
+ * up the collision detection by using the edges information of your mesh. The running time
+ * of the collision detection that uses the edges is almost O(1) constant time at the cost
+ * of additional memory used to store the vertices. You can indicate edges information
+ * with the addEdge() method. Then, you must use the setIsEdgesInformationUsed(true) method
+ * in order to use the edges information for collision detection.
+ */
+class ConvexMeshShape : public ConvexShape {
+
+    protected :
+
+        // -------------------- Attributes -------------------- //
+
+        /// Array with the vertices of the mesh
+        std::vector<Vector3> mVertices;
+
+        /// Number of vertices in the mesh
+        uint mNbVertices;
+
+        /// Mesh minimum bounds in the three local x, y and z directions
+        Vector3 mMinBounds;
+
+        /// Mesh maximum bounds in the three local x, y and z directions
+        Vector3 mMaxBounds;
+
+        /// True if the shape contains the edges of the convex mesh in order to
+        /// make the collision detection faster
+        bool mIsEdgesInformationUsed;
+
+        /// Adjacency list representing the edges of the mesh
+        std::map<uint, std::set<uint> > mEdgesAdjacencyList;
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        ConvexMeshShape(const ConvexMeshShape& shape);
+
+        /// Private assignment operator
+        ConvexMeshShape& operator=(const ConvexMeshShape& shape);
+
+        /// Recompute the bounds of the mesh
+        void recalculateBounds();
+
+        /// Set the scaling vector of the collision shape
+        virtual void setLocalScaling(const Vector3& scaling);
+
+        /// Return a local support point in a given direction without the object margin.
+        virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
+                                                          void** cachedCollisionData) const;
+
+        /// Return true if a point is inside the collision shape
+        virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
+
+        /// Raycast method with feedback information
+        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
+
+        /// Return the number of bytes used by the collision shape
+        virtual size_t getSizeInBytes() const;
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor to initialize with an array of 3D vertices.
+        ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride,
+                        decimal margin = OBJECT_MARGIN);
+
+        /// Constructor to initialize with a triangle vertex array
+        ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool isEdgesInformationUsed = true,
+                        decimal margin = OBJECT_MARGIN);
+
+        /// Constructor.
+        ConvexMeshShape(decimal margin = OBJECT_MARGIN);
+
+        /// Destructor
+        virtual ~ConvexMeshShape();
+
+        /// Return the local bounds of the shape in x, y and z directions
+        virtual void getLocalBounds(Vector3& min, Vector3& max) const;
+
+        /// Return the local inertia tensor of the collision shape.
+        virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
+
+        /// Add a vertex into the convex mesh
+        void addVertex(const Vector3& vertex);
+
+        /// Add an edge into the convex mesh by specifying the two vertex indices of the edge.
+        void addEdge(uint v1, uint v2);
+
+        /// Return true if the edges information is used to speed up the collision detection
+        bool isEdgesInformationUsed() const;
+
+        /// Set the variable to know if the edges information is used to speed up the
+        /// collision detection
+        void setIsEdgesInformationUsed(bool isEdgesUsed);
+};
+
+/// Set the scaling vector of the collision shape
+inline void ConvexMeshShape::setLocalScaling(const Vector3& scaling) {
+    ConvexShape::setLocalScaling(scaling);
+    recalculateBounds();
+}
+
+// Return the number of bytes used by the collision shape
+inline size_t ConvexMeshShape::getSizeInBytes() const {
+    return sizeof(ConvexMeshShape);
+}
+
+// Return the local bounds of the shape in x, y and z directions
+/**
+ * @param min The minimum bounds of the shape in local-space coordinates
+ * @param max The maximum bounds of the shape in local-space coordinates
+ */
+inline void ConvexMeshShape::getLocalBounds(Vector3& min, Vector3& max) const {
+    min = mMinBounds;
+    max = mMaxBounds;
+}
+
+// Return the local inertia tensor of the collision shape.
+/// The local inertia tensor of the convex mesh is approximated using the inertia tensor
+/// of its bounding box.
+/**
+* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
+*                    coordinates
+* @param mass Mass to use to compute the inertia tensor of the collision shape
+*/
+inline void ConvexMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
+    decimal factor = (decimal(1.0) / decimal(3.0)) * mass;
+    Vector3 realExtent = decimal(0.5) * (mMaxBounds - mMinBounds);
+    assert(realExtent.x > 0 && realExtent.y > 0 && realExtent.z > 0);
+    decimal xSquare = realExtent.x * realExtent.x;
+    decimal ySquare = realExtent.y * realExtent.y;
+    decimal zSquare = realExtent.z * realExtent.z;
+    tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0,
+                        0.0, factor * (xSquare + zSquare), 0.0,
+                        0.0, 0.0, factor * (xSquare + ySquare));
+}
+
+// Add a vertex into the convex mesh
+/**
+ * @param vertex Vertex to be added
+ */
+inline void ConvexMeshShape::addVertex(const Vector3& vertex) {
+
+    // Add the vertex in to vertices array
+    mVertices.push_back(vertex);
+    mNbVertices++;
+
+    // Update the bounds of the mesh
+    if (vertex.x * mScaling.x > mMaxBounds.x) mMaxBounds.x = vertex.x * mScaling.x;
+    if (vertex.x * mScaling.x < mMinBounds.x) mMinBounds.x = vertex.x * mScaling.x;
+    if (vertex.y * mScaling.y > mMaxBounds.y) mMaxBounds.y = vertex.y * mScaling.y;
+    if (vertex.y * mScaling.y < mMinBounds.y) mMinBounds.y = vertex.y * mScaling.y;
+    if (vertex.z * mScaling.z > mMaxBounds.z) mMaxBounds.z = vertex.z * mScaling.z;
+    if (vertex.z * mScaling.z < mMinBounds.z) mMinBounds.z = vertex.z * mScaling.z;
+}
+
+// Add an edge into the convex mesh by specifying the two vertex indices of the edge.
+/// Note that the vertex indices start at zero and need to correspond to the order of
+/// the vertices in the vertices array in the constructor or the order of the calls
+/// of the addVertex() methods that you use to add vertices into the convex mesh.
+/**
+* @param v1 Index of the first vertex of the edge to add
+* @param v2 Index of the second vertex of the edge to add
+*/
+inline void ConvexMeshShape::addEdge(uint v1, uint v2) {
+
+    // If the entry for vertex v1 does not exist in the adjacency list
+    if (mEdgesAdjacencyList.count(v1) == 0) {
+        mEdgesAdjacencyList.insert(std::make_pair(v1, std::set<uint>()));
+    }
+
+    // If the entry for vertex v2 does not exist in the adjacency list
+    if (mEdgesAdjacencyList.count(v2) == 0) {
+        mEdgesAdjacencyList.insert(std::make_pair(v2, std::set<uint>()));
+    }
+
+    // Add the edge in the adjacency list
+    mEdgesAdjacencyList[v1].insert(v2);
+    mEdgesAdjacencyList[v2].insert(v1);
+}
+
+// Return true if the edges information is used to speed up the collision detection
+/**
+ * @return True if the edges information is used and false otherwise
+ */
+inline bool ConvexMeshShape::isEdgesInformationUsed() const {
+    return mIsEdgesInformationUsed;
+}
+
+// Set the variable to know if the edges information is used to speed up the
+// collision detection
+/**
+ * @param isEdgesUsed True if you want to use the edges information to speed up
+ *                    the collision detection with the convex mesh shape
+ */
+inline void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) {
+    mIsEdgesInformationUsed = isEdgesUsed;
+}
+
+// Return true if a point is inside the collision shape
+inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint,
+                                             ProxyShape* proxyShape) const {
+
+    // Use the GJK algorithm to test if the point is inside the convex mesh
+    return proxyShape->mBody->mWorld.mCollisionDetection.
+           mNarrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape);
+}
+
+}
+
+#endif

src/collision/shapes/ConvexShape.cpp

@@ -0,0 +1,62 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "ConvexShape.h"
+
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Constructor
+ConvexShape::ConvexShape(CollisionShapeType type, decimal margin)
+            : CollisionShape(type), mMargin(margin) {
+
+}
+
+// Destructor
+ConvexShape::~ConvexShape() {
+
+}
+
+// Return a local support point in a given direction with the object margin
+Vector3 ConvexShape::getLocalSupportPointWithMargin(const Vector3& direction,
+                                                    void** cachedCollisionData) const {
+
+    // Get the support point without margin
+    Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);
+
+    if (mMargin != decimal(0.0)) {
+
+        // Add the margin to the support point
+        Vector3 unitVec(0.0, -1.0, 0.0);
+        if (direction.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) {
+            unitVec = direction.getUnit();
+        }
+        supportPoint += unitVec * mMargin;
+    }
+
+    return supportPoint;
+}

src/collision/shapes/ConvexShape.h

@@ -0,0 +1,106 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_CONVEX_SHAPE_H
+#define REACTPHYSICS3D_CONVEX_SHAPE_H
+
+// Libraries
+#include "CollisionShape.h"
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Class ConvexShape
+/**
+ * This abstract class represents a convex collision shape associated with a
+ * body that is used during the narrow-phase collision detection.
+ */
+class ConvexShape : public CollisionShape {
+
+    protected :
+
+        // -------------------- Attributes -------------------- //
+
+        /// Margin used for the GJK collision detection algorithm
+        decimal mMargin;
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        ConvexShape(const ConvexShape& shape);
+
+        /// Private assignment operator
+        ConvexShape& operator=(const ConvexShape& shape);
+
+        // Return a local support point in a given direction with the object margin
+        Vector3 getLocalSupportPointWithMargin(const Vector3& direction,
+                                               void** cachedCollisionData) const;
+
+        /// Return a local support point in a given direction without the object margin
+        virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
+                                                          void** cachedCollisionData) const=0;
+
+        /// Return true if a point is inside the collision shape
+        virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0;
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        ConvexShape(CollisionShapeType type, decimal margin);
+
+        /// Destructor
+        virtual ~ConvexShape();
+
+        /// Return the current object margin
+        decimal getMargin() const;
+
+        /// Return true if the collision shape is convex, false if it is concave
+        virtual bool isConvex() const;
+
+        // -------------------- Friendship -------------------- //
+
+        friend class GJKAlgorithm;
+        friend class EPAAlgorithm;
+};
+
+/// Return true if the collision shape is convex, false if it is concave
+inline bool ConvexShape::isConvex() const {
+    return true;
+}
+
+// Return the current collision shape margin
+/**
+ * @return The margin (in meters) around the collision shape
+ */
+inline decimal ConvexShape::getMargin() const {
+    return mMargin;
+}
+
+}
+
+#endif
+

src/collision/shapes/CylinderShape.cpp

@@ -0,0 +1,236 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "CylinderShape.h"
+#include "collision/ProxyShape.h"
+#include "configuration.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+/**
+ * @param radius Radius of the cylinder (in meters)
+ * @param height Height of the cylinder (in meters)
+ * @param margin Collision margin (in meters) around the collision shape
+ */
+CylinderShape::CylinderShape(decimal radius, decimal height, decimal margin)
+              : ConvexShape(CYLINDER, margin), mRadius(radius),
+                mHalfHeight(height/decimal(2.0)) {
+    assert(radius > decimal(0.0));
+    assert(height > decimal(0.0));
+}
+
+// Destructor
+CylinderShape::~CylinderShape() {
+
+}
+
+// Return a local support point in a given direction without the object margin
+Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
+                                                         void** cachedCollisionData) const {
+
+    Vector3 supportPoint(0.0, 0.0, 0.0);
+    decimal uDotv = direction.y;
+    Vector3 w(direction.x, 0.0, direction.z);
+    decimal lengthW = sqrt(direction.x * direction.x + direction.z * direction.z);
+
+    if (lengthW > MACHINE_EPSILON) {
+        if (uDotv < 0.0) supportPoint.y = -mHalfHeight;
+        else supportPoint.y = mHalfHeight;
+        supportPoint += (mRadius / lengthW) * w;
+    }
+    else {
+         if (uDotv < 0.0) supportPoint.y = -mHalfHeight;
+         else supportPoint.y = mHalfHeight;
+    }
+
+    return supportPoint;
+}
+
+// Raycast method with feedback information
+/// Algorithm based on the one described at page 194 in Real-ime Collision Detection by
+/// Morgan Kaufmann.
+bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
+
+    const Vector3 n = ray.point2 - ray.point1;
+
+    const decimal epsilon = decimal(0.01);
+    Vector3 p(decimal(0), -mHalfHeight, decimal(0));
+    Vector3 q(decimal(0), mHalfHeight, decimal(0));
+    Vector3 d = q - p;
+    Vector3 m = ray.point1 - p;
+    decimal t;
+
+    decimal mDotD = m.dot(d);
+    decimal nDotD = n.dot(d);
+    decimal dDotD = d.dot(d);
+
+    // Test if the segment is outside the cylinder
+    if (mDotD < decimal(0.0) && mDotD + nDotD < decimal(0.0)) return false;
+    if (mDotD > dDotD && mDotD + nDotD > dDotD) return false;
+
+    decimal nDotN = n.dot(n);
+    decimal mDotN = m.dot(n);
+
+    decimal a = dDotD * nDotN - nDotD * nDotD;
+    decimal k = m.dot(m) - mRadius * mRadius;
+    decimal c = dDotD * k - mDotD * mDotD;
+
+    // If the ray is parallel to the cylinder axis
+    if (std::abs(a) < epsilon) {
+
+        // If the origin is outside the surface of the cylinder, we return no hit
+        if (c > decimal(0.0)) return false;
+
+        // Here we know that the segment intersect an endcap of the cylinder
+
+        // If the ray intersects with the "p" endcap of the cylinder
+        if (mDotD < decimal(0.0)) {
+
+            t = -mDotN / nDotN;
+
+            // If the intersection is behind the origin of the ray or beyond the maximum
+            // raycasting distance, we return no hit
+            if (t < decimal(0.0) || t > ray.maxFraction) return false;
+
+            // Compute the hit information
+            Vector3 localHitPoint = ray.point1 + t * n;
+            raycastInfo.body = proxyShape->getBody();
+            raycastInfo.proxyShape = proxyShape;
+            raycastInfo.hitFraction = t;
+            raycastInfo.worldPoint = localHitPoint;
+            Vector3 normalDirection(0, decimal(-1), 0);
+            raycastInfo.worldNormal = normalDirection;
+
+            return true;
+        }
+        else if (mDotD > dDotD) {   // If the ray intersects with the "q" endcap of the cylinder
+
+            t = (nDotD - mDotN) / nDotN;
+
+            // If the intersection is behind the origin of the ray or beyond the maximum
+            // raycasting distance, we return no hit
+            if (t < decimal(0.0) || t > ray.maxFraction) return false;
+
+            // Compute the hit information
+            Vector3 localHitPoint = ray.point1 + t * n;
+            raycastInfo.body = proxyShape->getBody();
+            raycastInfo.proxyShape = proxyShape;
+            raycastInfo.hitFraction = t;
+            raycastInfo.worldPoint = localHitPoint;
+            Vector3 normalDirection(0, decimal(1.0), 0);
+            raycastInfo.worldNormal = normalDirection;
+
+            return true;
+        }
+        else {  // If the origin is inside the cylinder, we return no hit
+            return false;
+        }
+    }
+    decimal b = dDotD * mDotN - nDotD * mDotD;
+    decimal discriminant = b * b - a * c;
+
+    // If the discriminant is negative, no real roots and therfore, no hit
+    if (discriminant < decimal(0.0)) return false;
+
+    // Compute the smallest root (first intersection along the ray)
+    decimal t0 = t = (-b - std::sqrt(discriminant)) / a;
+
+    // If the intersection is outside the cylinder on "p" endcap side
+    decimal value = mDotD + t * nDotD;
+    if (value < decimal(0.0)) {
+
+        // If the ray is pointing away from the "p" endcap, we return no hit
+        if (nDotD <= decimal(0.0)) return false;
+
+        // Compute the intersection against the "p" endcap (intersection agains whole plane)
+        t = -mDotD / nDotD;
+
+        // Keep the intersection if the it is inside the cylinder radius
+        if (k + t * (decimal(2.0) * mDotN + t) > decimal(0.0)) return false;
+
+        // If the intersection is behind the origin of the ray or beyond the maximum
+        // raycasting distance, we return no hit
+        if (t < decimal(0.0) || t > ray.maxFraction) return false;
+
+        // Compute the hit information
+        Vector3 localHitPoint = ray.point1 + t * n;
+        raycastInfo.body = proxyShape->getBody();
+        raycastInfo.proxyShape = proxyShape;
+        raycastInfo.hitFraction = t;
+        raycastInfo.worldPoint = localHitPoint;
+        Vector3 normalDirection(0, decimal(-1.0), 0);
+        raycastInfo.worldNormal = normalDirection;
+
+        return true;
+    }
+    else if (value > dDotD) {   // If the intersection is outside the cylinder on the "q" side
+
+        // If the ray is pointing away from the "q" endcap, we return no hit
+        if (nDotD >= decimal(0.0)) return false;
+
+        // Compute the intersection against the "q" endcap (intersection against whole plane)
+        t = (dDotD - mDotD) / nDotD;
+
+        // Keep the intersection if it is inside the cylinder radius
+        if (k + dDotD - decimal(2.0) * mDotD + t * (decimal(2.0) * (mDotN - nDotD) + t) >
+            decimal(0.0)) return false;
+
+        // If the intersection is behind the origin of the ray or beyond the maximum
+        // raycasting distance, we return no hit
+        if (t < decimal(0.0) || t > ray.maxFraction) return false;
+
+        // Compute the hit information
+        Vector3 localHitPoint = ray.point1 + t * n;
+        raycastInfo.body = proxyShape->getBody();
+        raycastInfo.proxyShape = proxyShape;
+        raycastInfo.hitFraction = t;
+        raycastInfo.worldPoint = localHitPoint;
+        Vector3 normalDirection(0, decimal(1.0), 0);
+        raycastInfo.worldNormal = normalDirection;
+
+        return true;
+    }
+
+    t = t0;
+
+    // If the intersection is behind the origin of the ray or beyond the maximum
+    // raycasting distance, we return no hit
+    if (t < decimal(0.0) || t > ray.maxFraction) return false;
+
+    // Compute the hit information
+    Vector3 localHitPoint = ray.point1 + t * n;
+    raycastInfo.body = proxyShape->getBody();
+    raycastInfo.proxyShape = proxyShape;
+    raycastInfo.hitFraction = t;
+    raycastInfo.worldPoint = localHitPoint;
+    Vector3 v = localHitPoint - p;
+    Vector3 w = (v.dot(d) / d.lengthSquare()) * d;
+    Vector3 normalDirection = (localHitPoint - (p + w));
+    raycastInfo.worldNormal = normalDirection;
+
+    return true;
+}

src/collision/shapes/CylinderShape.h

@@ -0,0 +1,182 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_CYLINDER_SHAPE_H
+#define REACTPHYSICS3D_CYLINDER_SHAPE_H
+
+// Libraries
+#include "ConvexShape.h"
+#include "body/CollisionBody.h"
+#include "mathematics/mathematics.h"
+
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Class CylinderShape
+/**
+ * This class represents a cylinder collision shape around the Y axis
+ * and centered at the origin. The cylinder is defined by its height
+ * and the radius of its base. The "transform" of the corresponding
+ * rigid body gives an orientation and a position to the cylinder.
+ * This collision shape uses an extra margin distance around it for collision
+ * detection purpose. The default margin is 4cm (if your units are meters,
+ * which is recommended). In case, you want to simulate small objects
+ * (smaller than the margin distance), you might want to reduce the margin by
+ * specifying your own margin distance using the "margin" parameter in the
+ * constructor of the cylinder shape. Otherwise, it is recommended to use the
+ * default margin distance by not using the "margin" parameter in the constructor.
+ */
+class CylinderShape : public ConvexShape {
+
+    protected :
+
+        // -------------------- Attributes -------------------- //
+
+        /// Radius of the base
+        decimal mRadius;
+
+        /// Half height of the cylinder
+        decimal mHalfHeight;
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        CylinderShape(const CylinderShape& shape);
+
+        /// Private assignment operator
+        CylinderShape& operator=(const CylinderShape& shape);
+
+        /// Return a local support point in a given direction without the object margin
+        virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
+                                                          void** cachedCollisionData) const;
+
+        /// Return true if a point is inside the collision shape
+        virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
+
+        /// Raycast method with feedback information
+        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
+
+        /// Return the number of bytes used by the collision shape
+        virtual size_t getSizeInBytes() const;
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        CylinderShape(decimal radius, decimal height, decimal margin = OBJECT_MARGIN);
+
+        /// Destructor
+        virtual ~CylinderShape();
+
+        /// Return the radius
+        decimal getRadius() const;
+
+        /// Return the height
+        decimal getHeight() const;
+
+        /// Set the scaling vector of the collision shape
+        virtual void setLocalScaling(const Vector3& scaling);
+
+        /// Return the local bounds of the shape in x, y and z directions
+        virtual void getLocalBounds(Vector3& min, Vector3& max) const;
+
+        /// Return the local inertia tensor of the collision shape
+        virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
+};
+
+// Return the radius
+/**
+ * @return Radius of the cylinder (in meters)
+ */
+inline decimal CylinderShape::getRadius() const {
+    return mRadius;
+}
+
+// Return the height
+/**
+ * @return Height of the cylinder (in meters)
+ */
+inline decimal CylinderShape::getHeight() const {
+    return mHalfHeight + mHalfHeight;
+}
+
+// Set the scaling vector of the collision shape
+inline void CylinderShape::setLocalScaling(const Vector3& scaling) {
+
+    mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y;
+    mRadius = (mRadius / mScaling.x) * scaling.x;
+
+    CollisionShape::setLocalScaling(scaling);
+}
+
+// Return the number of bytes used by the collision shape
+inline size_t CylinderShape::getSizeInBytes() const {
+    return sizeof(CylinderShape);
+}
+
+// Return the local bounds of the shape in x, y and z directions
+/**
+ * @param min The minimum bounds of the shape in local-space coordinates
+ * @param max The maximum bounds of the shape in local-space coordinates
+ */
+inline void CylinderShape::getLocalBounds(Vector3& min, Vector3& max) const {
+
+    // Maximum bounds
+    max.x = mRadius + mMargin;
+    max.y = mHalfHeight + mMargin;
+    max.z = max.x;
+
+    // Minimum bounds
+    min.x = -max.x;
+    min.y = -max.y;
+    min.z = min.x;
+}
+
+// Return the local inertia tensor of the cylinder
+/**
+ * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
+ *                    coordinates
+ * @param mass Mass to use to compute the inertia tensor of the collision shape
+ */
+inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
+    decimal height = decimal(2.0) * mHalfHeight;
+    decimal diag = (decimal(1.0) / decimal(12.0)) * mass * (3 * mRadius * mRadius + height * height);
+    tensor.setAllValues(diag, 0.0, 0.0, 0.0,
+                        decimal(0.5) * mass * mRadius * mRadius, 0.0,
+                        0.0, 0.0, diag);
+}
+
+// Return true if a point is inside the collision shape
+inline bool CylinderShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const{
+    return ((localPoint.x * localPoint.x + localPoint.z * localPoint.z) < mRadius * mRadius &&
+            localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight);
+}
+
+}
+
+#endif
+

src/collision/shapes/HeightFieldShape.cpp

@@ -0,0 +1,267 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "HeightFieldShape.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+/**
+ * @param nbGridColumns Number of columns in the grid of the height field
+ * @param nbGridRows Number of rows in the grid of the height field
+ * @param minHeight Minimum height value of the height field
+ * @param maxHeight Maximum height value of the height field
+ * @param heightFieldData Pointer to the first height value data (note that values are shared and not copied)
+ * @param dataType Data type for the height values (int, float, double)
+ * @param upAxis Integer representing the up axis direction (0 for x, 1 for y and 2 for z)
+ * @param integerHeightScale Scaling factor used to scale the height values (only when height values type is integer)
+ */
+HeightFieldShape::HeightFieldShape(int nbGridColumns, int nbGridRows, decimal minHeight, decimal maxHeight,
+                                   const void* heightFieldData, HeightDataType dataType, int upAxis,
+                                   decimal integerHeightScale)
+                 : ConcaveShape(HEIGHTFIELD), mNbColumns(nbGridColumns), mNbRows(nbGridRows),
+                   mWidth(nbGridColumns - 1), mLength(nbGridRows - 1), mMinHeight(minHeight),
+                   mMaxHeight(maxHeight), mUpAxis(upAxis), mIntegerHeightScale(integerHeightScale),
+                   mHeightDataType(dataType) {
+
+    assert(nbGridColumns >= 2);
+    assert(nbGridRows >= 2);
+    assert(mWidth >= 1);
+    assert(mLength >= 1);
+    assert(minHeight <= maxHeight);
+    assert(upAxis == 0 || upAxis == 1 || upAxis == 2);
+
+    mHeightFieldData = heightFieldData;
+
+    decimal halfHeight = (mMaxHeight - mMinHeight) * decimal(0.5);
+    assert(halfHeight >= 0);
+
+    // Compute the local AABB of the height field
+    if (mUpAxis == 0) {
+        mAABB.setMin(Vector3(-halfHeight, -mWidth * decimal(0.5), -mLength * decimal(0.5)));
+        mAABB.setMax(Vector3(halfHeight, mWidth * decimal(0.5), mLength* decimal(0.5)));
+    }
+    else if (mUpAxis == 1) {
+        mAABB.setMin(Vector3(-mWidth * decimal(0.5), -halfHeight, -mLength * decimal(0.5)));
+        mAABB.setMax(Vector3(mWidth * decimal(0.5), halfHeight, mLength * decimal(0.5)));
+    }
+    else if (mUpAxis == 2) {
+        mAABB.setMin(Vector3(-mWidth * decimal(0.5), -mLength * decimal(0.5), -halfHeight));
+        mAABB.setMax(Vector3(mWidth * decimal(0.5), mLength * decimal(0.5), halfHeight));
+    }
+}
+
+// Destructor
+HeightFieldShape::~HeightFieldShape() {
+
+}
+
+// Return the local bounds of the shape in x, y and z directions.
+// This method is used to compute the AABB of the box
+/**
+ * @param min The minimum bounds of the shape in local-space coordinates
+ * @param max The maximum bounds of the shape in local-space coordinates
+ */
+void HeightFieldShape::getLocalBounds(Vector3& min, Vector3& max) const {
+    min = mAABB.getMin() * mScaling;
+    max = mAABB.getMax() * mScaling;
+}
+
+// Test collision with the triangles of the height field shape. The idea is to use the AABB
+// of the body when need to test and see against which triangles of the height-field we need
+// to test for collision. We compute the sub-grid points that are inside the other body's AABB
+// and then for each rectangle in the sub-grid we generate two triangles that we use to test collision.
+void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const {
+
+   // Compute the non-scaled AABB
+   Vector3 inverseScaling(decimal(1.0) / mScaling.x, decimal(1.0) / mScaling.y, decimal(1.0) / mScaling.z);
+   AABB aabb(localAABB.getMin() * inverseScaling, localAABB.getMax() * inverseScaling);
+
+   // Compute the integer grid coordinates inside the area we need to test for collision
+   int minGridCoords[3];
+   int maxGridCoords[3];
+   computeMinMaxGridCoordinates(minGridCoords, maxGridCoords, aabb);
+
+   // Compute the starting and ending coords of the sub-grid according to the up axis
+   int iMin = 0;
+   int iMax = 0;
+   int jMin = 0;
+   int jMax = 0;
+   switch(mUpAxis) {
+        case 0 : iMin = clamp(minGridCoords[1], 0, mNbColumns - 1);
+                 iMax = clamp(maxGridCoords[1], 0, mNbColumns - 1);
+                 jMin = clamp(minGridCoords[2], 0, mNbRows - 1);
+                 jMax = clamp(maxGridCoords[2], 0, mNbRows - 1);
+                 break;
+        case 1 : iMin = clamp(minGridCoords[0], 0, mNbColumns - 1);
+                 iMax = clamp(maxGridCoords[0], 0, mNbColumns - 1);
+                 jMin = clamp(minGridCoords[2], 0, mNbRows - 1);
+                 jMax = clamp(maxGridCoords[2], 0, mNbRows - 1);
+                 break;
+        case 2 : iMin = clamp(minGridCoords[0], 0, mNbColumns - 1);
+                 iMax = clamp(maxGridCoords[0], 0, mNbColumns - 1);
+                 jMin = clamp(minGridCoords[1], 0, mNbRows - 1);
+                 jMax = clamp(maxGridCoords[1], 0, mNbRows - 1);
+                 break;
+   }
+
+   assert(iMin >= 0 && iMin < mNbColumns);
+   assert(iMax >= 0 && iMax < mNbColumns);
+   assert(jMin >= 0 && jMin < mNbRows);
+   assert(jMax >= 0 && jMax < mNbRows);
+
+   // For each sub-grid points (except the last ones one each dimension)
+   for (int i = iMin; i < iMax; i++) {
+       for (int j = jMin; j < jMax; j++) {
+
+           // Compute the four point of the current quad
+           Vector3 p1 = getVertexAt(i, j);
+           Vector3 p2 = getVertexAt(i, j + 1);
+           Vector3 p3 = getVertexAt(i + 1, j);
+           Vector3 p4 = getVertexAt(i + 1, j + 1);
+
+           // Generate the first triangle for the current grid rectangle
+           Vector3 trianglePoints[3] = {p1, p2, p3};
+
+           // Test collision against the first triangle
+           callback.testTriangle(trianglePoints);
+
+           // Generate the second triangle for the current grid rectangle
+           trianglePoints[0] = p3;
+           trianglePoints[1] = p2;
+           trianglePoints[2] = p4;
+
+           // Test collision against the second triangle
+           callback.testTriangle(trianglePoints);
+       }
+   }
+}
+
+// Compute the min/max grid coords corresponding to the intersection of the AABB of the height field and
+// the AABB to collide
+void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoords, const AABB& aabbToCollide) const {
+
+    // Clamp the min/max coords of the AABB to collide inside the height field AABB
+    Vector3 minPoint = Vector3::max(aabbToCollide.getMin(), mAABB.getMin());
+    minPoint = Vector3::min(minPoint, mAABB.getMax());
+
+    Vector3 maxPoint = Vector3::min(aabbToCollide.getMax(), mAABB.getMax());
+    maxPoint = Vector3::max(maxPoint, mAABB.getMin());
+
+    // Translate the min/max points such that the we compute grid points from [0 ... mNbWidthGridPoints]
+    // and from [0 ... mNbLengthGridPoints] because the AABB coordinates range are [-mWdith/2 ... mWidth/2]
+    // and [-mLength/2 ... mLength/2]
+    const Vector3 translateVec = mAABB.getExtent() * decimal(0.5);
+    minPoint += translateVec;
+    maxPoint += translateVec;
+
+    // Convert the floating min/max coords of the AABB into closest integer
+    // grid values (note that we use the closest grid coordinate that is out
+    // of the AABB)
+    minCoords[0] = computeIntegerGridValue(minPoint.x) - 1;
+    minCoords[1] = computeIntegerGridValue(minPoint.y) - 1;
+    minCoords[2] = computeIntegerGridValue(minPoint.z) - 1;
+
+    maxCoords[0] = computeIntegerGridValue(maxPoint.x) + 1;
+    maxCoords[1] = computeIntegerGridValue(maxPoint.y) + 1;
+    maxCoords[2] = computeIntegerGridValue(maxPoint.z) + 1;
+}
+
+// Raycast method with feedback information
+/// Note that only the first triangle hit by the ray in the mesh will be returned, even if
+/// the ray hits many triangles.
+bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
+
+    // TODO : Implement raycasting without using an AABB for the ray
+    //        but using a dynamic AABB tree or octree instead
+
+    PROFILE("HeightFieldShape::raycast()");
+
+    TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this);
+
+    // Compute the AABB for the ray
+    const Vector3 rayEnd = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
+    const AABB rayAABB(Vector3::min(ray.point1, rayEnd), Vector3::max(ray.point1, rayEnd));
+
+    testAllTriangles(triangleCallback, rayAABB);
+
+    return triangleCallback.getIsHit();
+}
+
+// Return the vertex (local-coordinates) of the height field at a given (x,y) position
+Vector3 HeightFieldShape::getVertexAt(int x, int y) const {
+
+    // Get the height value
+    const decimal height = getHeightAt(x, y);
+
+    // Height values origin
+    const decimal heightOrigin = -(mMaxHeight - mMinHeight) * decimal(0.5) - mMinHeight;
+
+    Vector3 vertex;
+    switch (mUpAxis) {
+        case 0: vertex = Vector3(heightOrigin + height, -mWidth * decimal(0.5) + x, -mLength * decimal(0.5) + y);
+                break;
+        case 1: vertex = Vector3(-mWidth * decimal(0.5) + x, heightOrigin + height, -mLength * decimal(0.5) + y);
+                break;
+        case 2: vertex = Vector3(-mWidth * decimal(0.5) + x, -mLength * decimal(0.5) + y, heightOrigin + height);
+                break;
+        default: assert(false);
+    }
+
+    assert(mAABB.contains(vertex));
+
+    return vertex * mScaling;
+}
+
+// Raycast test between a ray and a triangle of the heightfield
+void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints) {
+
+    // Create a triangle collision shape
+    decimal margin = mHeightFieldShape.getTriangleMargin();
+    TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
+    triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType());
+
+    // Ray casting test against the collision shape
+    RaycastInfo raycastInfo;
+    bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape);
+
+    // If the ray hit the collision shape
+    if (isTriangleHit && raycastInfo.hitFraction <= mSmallestHitFraction) {
+
+        assert(raycastInfo.hitFraction >= decimal(0.0));
+
+        mRaycastInfo.body = raycastInfo.body;
+        mRaycastInfo.proxyShape = raycastInfo.proxyShape;
+        mRaycastInfo.hitFraction = raycastInfo.hitFraction;
+        mRaycastInfo.worldPoint = raycastInfo.worldPoint;
+        mRaycastInfo.worldNormal = raycastInfo.worldNormal;
+        mRaycastInfo.meshSubpart = -1;
+        mRaycastInfo.triangleIndex = -1;
+
+        mSmallestHitFraction = raycastInfo.hitFraction;
+        mIsHit = true;
+    }
+}

src/collision/shapes/HeightFieldShape.h

@@ -0,0 +1,257 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_HEIGHTFIELD_SHAPE_H
+#define REACTPHYSICS3D_HEIGHTFIELD_SHAPE_H
+
+// Libraries
+#include "ConcaveShape.h"
+#include "collision/shapes/TriangleShape.h"
+#include "engine/Profiler.h"
+
+namespace reactphysics3d {
+
+class HeightFieldShape;
+
+// Class TriangleOverlapCallback
+/**
+ * This class is used for testing AABB and triangle overlap for raycasting
+ */
+class TriangleOverlapCallback : public TriangleCallback {
+
+    protected:
+
+        const Ray& mRay;
+        ProxyShape* mProxyShape;
+        RaycastInfo& mRaycastInfo;
+        bool mIsHit;
+        decimal mSmallestHitFraction;
+        const HeightFieldShape& mHeightFieldShape;
+
+    public:
+
+        // Constructor
+        TriangleOverlapCallback(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo,
+                                const HeightFieldShape& heightFieldShape)
+                               : mRay(ray), mProxyShape(proxyShape), mRaycastInfo(raycastInfo),
+                                 mHeightFieldShape (heightFieldShape) {
+            mIsHit = false;
+            mSmallestHitFraction = mRay.maxFraction;
+        }
+
+        bool getIsHit() const {return mIsHit;}
+
+        /// Raycast test between a ray and a triangle of the heightfield
+        virtual void testTriangle(const Vector3* trianglePoints);
+};
+
+
+// Class HeightFieldShape
+/**
+ * This class represents a static height field that can be used to represent
+ * a terrain. The height field is made of a grid with rows and columns with a
+ * height value at each grid point. Note that the height values are not copied into the shape
+ * but are shared instead. The height values can be of type integer, float or double.
+ * When creating a HeightFieldShape, you need to specify the minimum and maximum height value of
+ * your height field. Note that the HeightFieldShape will be re-centered based on its AABB. It means
+ * that for instance, if the minimum height value is -200 and the maximum value is 400, the final
+ * minimum height of the field in the simulation will be -300 and the maximum height will be 300.
+ */
+class HeightFieldShape : public ConcaveShape {
+
+    public:
+
+        /// Data type for the height data of the height field
+        enum HeightDataType {HEIGHT_FLOAT_TYPE, HEIGHT_DOUBLE_TYPE, HEIGHT_INT_TYPE};
+
+    protected:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Number of columns in the grid of the height field
+        int mNbColumns;
+
+        /// Number of rows in the grid of the height field
+        int mNbRows;
+
+        /// Height field width
+        decimal mWidth;
+
+        /// Height field length
+        decimal mLength;
+
+        /// Minimum height of the height field
+        decimal mMinHeight;
+
+        /// Maximum height of the height field
+        decimal mMaxHeight;
+
+        /// Up axis direction (0 => x, 1 => y, 2 => z)
+        int mUpAxis;
+
+        /// Height values scale for height field with integer height values
+        decimal mIntegerHeightScale;
+
+        /// Data type of the height values
+        HeightDataType mHeightDataType;
+
+        /// Array of data with all the height values of the height field
+        const void*	mHeightFieldData;
+
+        /// Local AABB of the height field (without scaling)
+        AABB mAABB;
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        HeightFieldShape(const HeightFieldShape& shape);
+
+        /// Private assignment operator
+        HeightFieldShape& operator=(const HeightFieldShape& shape);
+
+        /// Raycast method with feedback information
+        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
+
+        /// Return the number of bytes used by the collision shape
+        virtual size_t getSizeInBytes() const;
+
+        /// Insert all the triangles into the dynamic AABB tree
+        void initBVHTree();
+
+        /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
+        /// given the start vertex index pointer of the triangle.
+        void getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex,
+                                                 Vector3* outTriangleVertices) const;
+
+        /// Return the vertex (local-coordinates) of the height field at a given (x,y) position
+        Vector3 getVertexAt(int x, int y) const;
+
+        /// Return the height of a given (x,y) point in the height field
+        decimal getHeightAt(int x, int y) const;
+
+        /// Return the closest inside integer grid value of a given floating grid value
+        int computeIntegerGridValue(decimal value) const;
+
+        /// Compute the min/max grid coords corresponding to the intersection of the AABB of the height field and the AABB to collide
+        void computeMinMaxGridCoordinates(int* minCoords, int* maxCoords, const AABB& aabbToCollide) const;
+
+    public:
+
+        /// Constructor
+        HeightFieldShape(int nbGridColumns, int nbGridRows, decimal minHeight, decimal maxHeight,
+                         const void* heightFieldData, HeightDataType dataType,
+                         int upAxis = 1, decimal integerHeightScale = 1.0f);
+
+        /// Destructor
+        ~HeightFieldShape();
+
+        /// Return the number of rows in the height field
+        int getNbRows() const;
+
+        /// Return the number of columns in the height field
+        int getNbColumns() const;
+
+        /// Return the type of height value in the height field
+        HeightDataType getHeightDataType() const;
+
+        /// Return the local bounds of the shape in x, y and z directions.
+        virtual void getLocalBounds(Vector3& min, Vector3& max) const;
+
+        /// Set the local scaling vector of the collision shape
+        virtual void setLocalScaling(const Vector3& scaling);
+
+        /// Return the local inertia tensor of the collision shape
+        virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
+
+        /// Use a callback method on all triangles of the concave shape inside a given AABB
+        virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const;
+
+        // ---------- Friendship ----------- //
+
+        friend class ConvexTriangleAABBOverlapCallback;
+        friend class ConcaveMeshRaycastCallback;
+};
+
+// Return the number of rows in the height field
+inline int HeightFieldShape::getNbRows() const {
+    return mNbRows;
+}
+
+// Return the number of columns in the height field
+inline int HeightFieldShape::getNbColumns() const {
+    return mNbColumns;
+}
+
+// Return the type of height value in the height field
+inline HeightFieldShape::HeightDataType HeightFieldShape::getHeightDataType() const {
+    return mHeightDataType;
+}
+
+// Return the number of bytes used by the collision shape
+inline size_t HeightFieldShape::getSizeInBytes() const {
+    return sizeof(HeightFieldShape);
+}
+
+// Set the local scaling vector of the collision shape
+inline void HeightFieldShape::setLocalScaling(const Vector3& scaling) {
+    CollisionShape::setLocalScaling(scaling);
+}
+
+// Return the height of a given (x,y) point in the height field
+inline decimal HeightFieldShape::getHeightAt(int x, int y) const {
+
+    switch(mHeightDataType) {
+        case HEIGHT_FLOAT_TYPE : return ((float*)mHeightFieldData)[y * mNbColumns + x];
+        case HEIGHT_DOUBLE_TYPE : return ((double*)mHeightFieldData)[y * mNbColumns + x];
+        case HEIGHT_INT_TYPE : return ((int*)mHeightFieldData)[y * mNbColumns + x] * mIntegerHeightScale;
+        default: assert(false); return 0;
+    }
+}
+
+// Return the closest inside integer grid value of a given floating grid value
+inline int HeightFieldShape::computeIntegerGridValue(decimal value) const {
+    return (value < decimal(0.0)) ? value - decimal(0.5) : value + decimal(0.5);
+}
+
+// Return the local inertia tensor
+/**
+ * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
+ *                    coordinates
+ * @param mass Mass to use to compute the inertia tensor of the collision shape
+ */
+inline void HeightFieldShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
+
+    // Default inertia tensor
+    // Note that this is not very realistic for a concave triangle mesh.
+    // However, in most cases, it will only be used static bodies and therefore,
+    // the inertia tensor is not used.
+    tensor.setAllValues(mass, 0, 0,
+                        0, mass, 0,
+                        0, 0, mass);
+}
+
+}
+#endif
+

src/collision/shapes/SphereShape.cpp

@@ -0,0 +1,91 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "SphereShape.h"
+#include "collision/ProxyShape.h"
+#include "configuration.h"
+#include <cassert>
+
+using namespace reactphysics3d;
+
+// Constructor
+/**
+ * @param radius Radius of the sphere (in meters)
+ */
+SphereShape::SphereShape(decimal radius) : ConvexShape(SPHERE, radius) {
+    assert(radius > decimal(0.0));
+}
+
+// Destructor
+SphereShape::~SphereShape() {
+
+}
+
+// Raycast method with feedback information
+bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
+
+    const Vector3 m = ray.point1;
+    decimal c = m.dot(m) - mMargin * mMargin;
+
+    // If the origin of the ray is inside the sphere, we return no intersection
+    if (c < decimal(0.0)) return false;
+
+    const Vector3 rayDirection = ray.point2 - ray.point1;
+    decimal b = m.dot(rayDirection);
+
+    // If the origin of the ray is outside the sphere and the ray
+    // is pointing away from the sphere, there is no intersection
+    if (b > decimal(0.0)) return false;
+
+    decimal raySquareLength = rayDirection.lengthSquare();
+
+    // Compute the discriminant of the quadratic equation
+    decimal discriminant = b * b - raySquareLength * c;
+
+    // If the discriminant is negative or the ray length is very small, there is no intersection
+    if (discriminant < decimal(0.0) || raySquareLength < MACHINE_EPSILON) return false;
+
+    // Compute the solution "t" closest to the origin
+    decimal t = -b - std::sqrt(discriminant);
+
+    assert(t >= decimal(0.0));
+
+    // If the hit point is withing the segment ray fraction
+    if (t < ray.maxFraction * raySquareLength) {
+
+        // Compute the intersection information
+        t /= raySquareLength;
+        raycastInfo.body = proxyShape->getBody();
+        raycastInfo.proxyShape = proxyShape;
+        raycastInfo.hitFraction = t;
+        raycastInfo.worldPoint = ray.point1 + t * rayDirection;
+        raycastInfo.worldNormal = raycastInfo.worldPoint;
+
+        return true;
+    }
+
+    return false;
+}

src/collision/shapes/SphereShape.h

@@ -0,0 +1,183 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_SPHERE_SHAPE_H
+#define REACTPHYSICS3D_SPHERE_SHAPE_H
+
+// Libraries
+#include "ConvexShape.h"
+#include "body/CollisionBody.h"
+#include "mathematics/mathematics.h"
+
+// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Class SphereShape
+/**
+ * This class represents a sphere collision shape that is centered
+ * at the origin and defined by its radius. This collision shape does not
+ * have an explicit object margin distance. The margin is implicitly the
+ * radius of the sphere. Therefore, no need to specify an object margin
+ * for a sphere shape.
+ */
+class SphereShape : public ConvexShape {
+
+    protected :
+
+        // -------------------- Attributes -------------------- //
+
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        SphereShape(const SphereShape& shape);
+
+        /// Private assignment operator
+        SphereShape& operator=(const SphereShape& shape);
+
+        /// Return a local support point in a given direction without the object margin
+        virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
+                                                          void** cachedCollisionData) const;
+
+        /// Return true if a point is inside the collision shape
+        virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
+
+        /// Raycast method with feedback information
+        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
+
+        /// Return the number of bytes used by the collision shape
+        virtual size_t getSizeInBytes() const;
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        SphereShape(decimal radius);
+
+        /// Destructor
+        virtual ~SphereShape();
+
+        /// Return the radius of the sphere
+        decimal getRadius() const;
+
+        /// Set the scaling vector of the collision shape
+        virtual void setLocalScaling(const Vector3& scaling);
+
+        /// Return the local bounds of the shape in x, y and z directions.
+        virtual void getLocalBounds(Vector3& min, Vector3& max) const;
+
+        /// Return the local inertia tensor of the collision shape
+        virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
+
+        /// Update the AABB of a body using its collision shape
+        virtual void computeAABB(AABB& aabb, const Transform& transform) const;
+};
+
+// Get the radius of the sphere
+/**
+ * @return Radius of the sphere (in meters)
+ */
+inline decimal SphereShape::getRadius() const {
+    return mMargin;
+}
+
+// Set the scaling vector of the collision shape
+inline void SphereShape::setLocalScaling(const Vector3& scaling) {
+
+    mMargin = (mMargin / mScaling.x) * scaling.x;
+
+    CollisionShape::setLocalScaling(scaling);
+}
+
+// Return the number of bytes used by the collision shape
+inline size_t SphereShape::getSizeInBytes() const {
+    return sizeof(SphereShape);
+}
+
+// Return a local support point in a given direction without the object margin
+inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
+                                                              void** cachedCollisionData) const {
+
+    // Return the center of the sphere (the radius is taken into account in the object margin)
+    return Vector3(0.0, 0.0, 0.0);
+}
+
+// Return the local bounds of the shape in x, y and z directions.
+// This method is used to compute the AABB of the box
+/**
+ * @param min The minimum bounds of the shape in local-space coordinates
+ * @param max The maximum bounds of the shape in local-space coordinates
+ */
+inline void SphereShape::getLocalBounds(Vector3& min, Vector3& max) const {
+
+    // Maximum bounds
+    max.x = mMargin;
+    max.y = mMargin;
+    max.z = mMargin;
+
+    // Minimum bounds
+    min.x = -mMargin;
+    min.y = min.x;
+    min.z = min.x;
+}
+
+// Return the local inertia tensor of the sphere
+/**
+ * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
+ *                    coordinates
+ * @param mass Mass to use to compute the inertia tensor of the collision shape
+ */
+inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
+    decimal diag = decimal(0.4) * mass * mMargin * mMargin;
+    tensor.setAllValues(diag, 0.0, 0.0,
+                        0.0, diag, 0.0,
+                        0.0, 0.0, diag);
+}
+
+// Update the AABB of a body using its collision shape
+/**
+ * @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape
+ *                  computed in world-space coordinates
+ * @param transform Transform used to compute the AABB of the collision shape
+ */
+inline void SphereShape::computeAABB(AABB& aabb, const Transform& transform) const {
+
+    // Get the local extents in x,y and z direction
+    Vector3 extents(mMargin, mMargin, mMargin);
+
+    // Update the AABB with the new minimum and maximum coordinates
+    aabb.setMin(transform.getPosition() - extents);
+    aabb.setMax(transform.getPosition() + extents);
+}
+
+// Return true if a point is inside the collision shape
+inline bool SphereShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
+    return (localPoint.lengthSquare() < mMargin * mMargin);
+}
+
+}
+
+#endif

src/collision/shapes/TriangleShape.cpp

@@ -0,0 +1,127 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "TriangleShape.h"
+#include "collision/ProxyShape.h"
+#include "engine/Profiler.h"
+#include "configuration.h"
+#include <cassert>
+
+using namespace reactphysics3d;
+
+// Constructor
+/**
+ * @param point1 First point of the triangle
+ * @param point2 Second point of the triangle
+ * @param point3 Third point of the triangle
+ * @param margin The collision margin (in meters) around the collision shape
+ */
+TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, decimal margin)
+              : ConvexShape(TRIANGLE, margin) {
+    mPoints[0] = point1;
+    mPoints[1] = point2;
+    mPoints[2] = point3;
+    mRaycastTestType = FRONT;
+}
+
+// Destructor
+TriangleShape::~TriangleShape() {
+
+}
+
+// Raycast method with feedback information
+/// This method use the line vs triangle raycasting technique described in
+/// Real-time Collision Detection by Christer Ericson.
+bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
+
+    PROFILE("TriangleShape::raycast()");
+
+    const Vector3 pq = ray.point2 - ray.point1;
+    const Vector3 pa = mPoints[0] - ray.point1;
+    const Vector3 pb = mPoints[1] - ray.point1;
+    const Vector3 pc = mPoints[2] - ray.point1;
+
+    // Test if the line PQ is inside the eges BC, CA and AB. We use the triple
+    // product for this test.
+    const Vector3 m = pq.cross(pc);
+    decimal u = pb.dot(m);
+    if (mRaycastTestType == FRONT) {
+        if (u < decimal(0.0)) return false;
+    }
+    else if (mRaycastTestType == BACK) {
+        if (u > decimal(0.0)) return false;
+    }
+
+    decimal v = -pa.dot(m);
+    if (mRaycastTestType == FRONT) {
+        if (v < decimal(0.0)) return false;
+    }
+    else if (mRaycastTestType == BACK) {
+        if (v > decimal(0.0)) return false;
+    }
+    else if (mRaycastTestType == FRONT_AND_BACK) {
+        if (!sameSign(u, v)) return false;
+    }
+
+    decimal w = pa.dot(pq.cross(pb));
+    if (mRaycastTestType == FRONT) {
+        if (w < decimal(0.0)) return false;
+    }
+    else if (mRaycastTestType == BACK) {
+        if (w > decimal(0.0)) return false;
+    }
+    else if (mRaycastTestType == FRONT_AND_BACK) {
+        if (!sameSign(u, w)) return false;
+    }
+
+    // If the line PQ is in the triangle plane (case where u=v=w=0)
+    if (approxEqual(u, 0) && approxEqual(v, 0) && approxEqual(w, 0)) return false;
+
+    // Compute the barycentric coordinates (u, v, w) to determine the
+    // intersection point R, R = u * a + v * b + w * c
+    decimal denom = decimal(1.0) / (u + v + w);
+    u *= denom;
+    v *= denom;
+    w *= denom;
+
+    // Compute the local hit point using the barycentric coordinates
+    const Vector3 localHitPoint = u * mPoints[0] + v * mPoints[1] + w * mPoints[2];
+    const decimal hitFraction = (localHitPoint - ray.point1).length() / pq.length();
+
+    if (hitFraction < decimal(0.0) || hitFraction > ray.maxFraction) return false;
+
+    Vector3 localHitNormal = (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]);
+    if (localHitNormal.dot(pq) > decimal(0.0)) localHitNormal = -localHitNormal;
+
+    raycastInfo.body = proxyShape->getBody();
+    raycastInfo.proxyShape = proxyShape;
+    raycastInfo.worldPoint = localHitPoint;
+    raycastInfo.hitFraction = hitFraction;
+    raycastInfo.worldNormal = localHitNormal;
+
+    return true;
+}
+

src/collision/shapes/TriangleShape.h

@@ -0,0 +1,224 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_TRIANGLE_SHAPE_H
+#define REACTPHYSICS3D_TRIANGLE_SHAPE_H
+
+// Libraries
+#include "mathematics/mathematics.h"
+#include "ConvexShape.h"
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+/// Raycast test side for the triangle
+enum TriangleRaycastSide {
+
+    /// Raycast against front triangle
+    FRONT,
+
+    /// Raycast against back triangle
+    BACK,
+
+    /// Raycast against front and back triangle
+    FRONT_AND_BACK
+};
+
+// Class TriangleShape
+/**
+ * This class represents a triangle collision shape that is centered
+ * at the origin and defined three points.
+ */
+class TriangleShape : public ConvexShape {
+
+    protected:
+
+        // -------------------- Attribute -------------------- //
+
+        /// Three points of the triangle
+        Vector3 mPoints[3];
+
+        /// Raycast test type for the triangle (front, back, front-back)
+        TriangleRaycastSide mRaycastTestType;
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        TriangleShape(const TriangleShape& shape);
+
+        /// Private assignment operator
+        TriangleShape& operator=(const TriangleShape& shape);
+
+        /// Return a local support point in a given direction without the object margin
+        virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
+                                                          void** cachedCollisionData) const;
+
+        /// Return true if a point is inside the collision shape
+        virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
+
+        /// Raycast method with feedback information
+        virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
+
+        /// Return the number of bytes used by the collision shape
+        virtual size_t getSizeInBytes() const;
+
+    public:
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3,
+                      decimal margin = OBJECT_MARGIN);
+
+        /// Destructor
+        virtual ~TriangleShape();
+
+        /// Return the local bounds of the shape in x, y and z directions.
+        virtual void getLocalBounds(Vector3& min, Vector3& max) const;
+
+        /// Set the local scaling vector of the collision shape
+        virtual void setLocalScaling(const Vector3& scaling);
+
+        /// Return the local inertia tensor of the collision shape
+        virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
+
+        /// Update the AABB of a body using its collision shape
+        virtual void computeAABB(AABB& aabb, const Transform& transform) const;
+
+        /// Return the raycast test type (front, back, front-back)
+        TriangleRaycastSide getRaycastTestType() const;
+
+        // Set the raycast test type (front, back, front-back)
+        void setRaycastTestType(TriangleRaycastSide testType);
+
+        /// Return the coordinates of a given vertex of the triangle
+        Vector3 getVertex(int index) const;
+
+        // ---------- Friendship ---------- //
+
+        friend class ConcaveMeshRaycastCallback;
+        friend class TriangleOverlapCallback;
+};
+
+// Return the number of bytes used by the collision shape
+inline size_t TriangleShape::getSizeInBytes() const {
+    return sizeof(TriangleShape);
+}
+
+// Return a local support point in a given direction without the object margin
+inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
+                                                              void** cachedCollisionData) const {
+    Vector3 dotProducts(direction.dot(mPoints[0]), direction.dot(mPoints[1]), direction.dot(mPoints[2]));
+    return mPoints[dotProducts.getMaxAxis()];
+}
+
+// Return the local bounds of the shape in x, y and z directions.
+// This method is used to compute the AABB of the box
+/**
+ * @param min The minimum bounds of the shape in local-space coordinates
+ * @param max The maximum bounds of the shape in local-space coordinates
+ */
+inline void TriangleShape::getLocalBounds(Vector3& min, Vector3& max) const {
+
+    const Vector3 xAxis(mPoints[0].x, mPoints[1].x, mPoints[2].x);
+    const Vector3 yAxis(mPoints[0].y, mPoints[1].y, mPoints[2].y);
+    const Vector3 zAxis(mPoints[0].z, mPoints[1].z, mPoints[2].z);
+    min.setAllValues(xAxis.getMinValue(), yAxis.getMinValue(), zAxis.getMinValue());
+    max.setAllValues(xAxis.getMaxValue(), yAxis.getMaxValue(), zAxis.getMaxValue());
+
+    min -= Vector3(mMargin, mMargin, mMargin);
+    max += Vector3(mMargin, mMargin, mMargin);
+}
+
+// Set the local scaling vector of the collision shape
+inline void TriangleShape::setLocalScaling(const Vector3& scaling) {
+
+    mPoints[0] = (mPoints[0] / mScaling) * scaling;
+    mPoints[1] = (mPoints[1] / mScaling) * scaling;
+    mPoints[2] = (mPoints[2] / mScaling) * scaling;
+
+    CollisionShape::setLocalScaling(scaling);
+}
+
+// Return the local inertia tensor of the triangle shape
+/**
+ * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
+ *                    coordinates
+ * @param mass Mass to use to compute the inertia tensor of the collision shape
+ */
+inline void TriangleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
+    tensor.setToZero();
+}
+
+// Update the AABB of a body using its collision shape
+/**
+ * @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape
+ *                  computed in world-space coordinates
+ * @param transform Transform used to compute the AABB of the collision shape
+ */
+inline void TriangleShape::computeAABB(AABB& aabb, const Transform& transform) const {
+
+    const Vector3 worldPoint1 = transform * mPoints[0];
+    const Vector3 worldPoint2 = transform * mPoints[1];
+    const Vector3 worldPoint3 = transform * mPoints[2];
+
+    const Vector3 xAxis(worldPoint1.x, worldPoint2.x, worldPoint3.x);
+    const Vector3 yAxis(worldPoint1.y, worldPoint2.y, worldPoint3.y);
+    const Vector3 zAxis(worldPoint1.z, worldPoint2.z, worldPoint3.z);
+    aabb.setMin(Vector3(xAxis.getMinValue(), yAxis.getMinValue(), zAxis.getMinValue()));
+    aabb.setMax(Vector3(xAxis.getMaxValue(), yAxis.getMaxValue(), zAxis.getMaxValue()));
+}
+
+// Return true if a point is inside the collision shape
+inline bool TriangleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
+    return false;
+}
+
+// Return the raycast test type (front, back, front-back)
+inline TriangleRaycastSide TriangleShape::getRaycastTestType() const {
+    return mRaycastTestType;
+}
+
+// Set the raycast test type (front, back, front-back)
+/**
+ * @param testType Raycast test type for the triangle (front, back, front-back)
+ */
+inline void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) {
+    mRaycastTestType = testType;
+}
+
+// Return the coordinates of a given vertex of the triangle
+/**
+ * @param index Index (0 to 2) of a vertex of the triangle
+ */
+inline Vector3 TriangleShape::getVertex(int index) const {
+    assert(index >= 0 && index < 3);
+    return mPoints[index];
+}
+
+}
+
+#endif
+

src/configuration.h

@@ -0,0 +1,149 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_CONFIGURATION_H
+#define	REACTPHYSICS3D_CONFIGURATION_H
+
+// Libraries
+#include <limits>
+#include <cfloat>
+#include <utility>
+#include "decimal.h"
+
+// Windows platform
+#if defined(WIN32) ||defined(_WIN32) || defined(_WIN64) ||defined(__WIN32__) || defined(__WINDOWS__)
+    #define WINDOWS_OS
+#elif defined(__APPLE__)     // Apple platform
+    #define APPLE_OS
+#elif defined(__linux__) || defined(linux) || defined(__linux)    // Linux platform
+    #define LINUX_OS
+#endif
+
+/// Namespace reactphysics3d
+namespace reactphysics3d {
+
+// ------------------- Type definitions ------------------- //
+
+typedef unsigned int uint;
+typedef long unsigned int luint;
+typedef luint bodyindex;
+typedef std::pair<bodyindex, bodyindex> bodyindexpair;
+
+typedef signed short int16;
+typedef signed int int32;
+typedef unsigned short uint16;
+typedef unsigned int uint32;
+
+// ------------------- Enumerations ------------------- //
+
+/// Position correction technique used in the constraint solver (for joints).
+/// BAUMGARTE_JOINTS : Faster but can be innacurate in some situations.
+/// NON_LINEAR_GAUSS_SEIDEL : Slower but more precise. This is the option used by default.
+enum JointsPositionCorrectionTechnique {BAUMGARTE_JOINTS, NON_LINEAR_GAUSS_SEIDEL};
+
+/// Position correction technique used in the contact solver (for contacts)
+/// BAUMGARTE_CONTACTS : Faster but can be innacurate and can lead to unexpected bounciness
+///                      in some situations (due to error correction factor being added to
+///                      the bodies momentum).
+/// SPLIT_IMPULSES : A bit slower but the error correction factor is not added to the
+///                 bodies momentum. This is the option used by default.
+enum ContactsPositionCorrectionTechnique {BAUMGARTE_CONTACTS, SPLIT_IMPULSES};
+
+// ------------------- Constants ------------------- //
+
+/// Smallest decimal value (negative)
+const decimal DECIMAL_SMALLEST = - std::numeric_limits<decimal>::max();
+
+/// Maximum decimal value
+const decimal DECIMAL_LARGEST = std::numeric_limits<decimal>::max();
+
+/// Machine epsilon
+const decimal MACHINE_EPSILON = std::numeric_limits<decimal>::epsilon();
+
+/// Pi constant
+const decimal PI = decimal(3.14159265);
+
+/// 2*Pi constant
+const decimal PI_TIMES_2 = decimal(6.28318530);
+
+/// Default friction coefficient for a rigid body
+const decimal DEFAULT_FRICTION_COEFFICIENT = decimal(0.3);
+
+/// Default bounciness factor for a rigid body
+const decimal DEFAULT_BOUNCINESS = decimal(0.5);
+
+/// Default rolling resistance
+const decimal DEFAULT_ROLLING_RESISTANCE = decimal(0.0);
+
+/// True if the spleeping technique is enabled
+const bool SPLEEPING_ENABLED = true;
+
+/// Object margin for collision detection in meters (for the GJK-EPA Algorithm)
+const decimal OBJECT_MARGIN = decimal(0.04);
+
+/// Distance threshold for two contact points for a valid persistent contact (in meters)
+const decimal PERSISTENT_CONTACT_DIST_THRESHOLD = decimal(0.03);
+
+/// Velocity threshold for contact velocity restitution
+const decimal RESTITUTION_VELOCITY_THRESHOLD = decimal(1.0);
+
+/// Number of iterations when solving the velocity constraints of the Sequential Impulse technique
+const uint DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 10;
+
+/// Number of iterations when solving the position constraints of the Sequential Impulse technique
+const uint DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 5;
+
+/// Time (in seconds) that a body must stay still to be considered sleeping
+const float DEFAULT_TIME_BEFORE_SLEEP = 1.0f;
+
+/// A body with a linear velocity smaller than the sleep linear velocity (in m/s)
+/// might enter sleeping mode.
+const decimal DEFAULT_SLEEP_LINEAR_VELOCITY = decimal(0.02);
+
+/// A body with angular velocity smaller than the sleep angular velocity (in rad/s)
+/// might enter sleeping mode
+const decimal DEFAULT_SLEEP_ANGULAR_VELOCITY = decimal(3.0 * (PI / 180.0));
+
+/// In the broad-phase collision detection (dynamic AABB tree), the AABBs are
+/// inflated with a constant gap to allow the collision shape to move a little bit
+/// without triggering a large modification of the tree which can be costly
+const decimal DYNAMIC_TREE_AABB_GAP = decimal(0.1);
+
+/// In the broad-phase collision detection (dynamic AABB tree), the AABBs are
+/// also inflated in direction of the linear motion of the body by mutliplying the
+/// followin constant with the linear velocity and the elapsed time between two frames.
+const decimal DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER = decimal(1.7);
+
+/// Maximum number of contact manifolds in an overlapping pair that involves two
+/// convex collision shapes.
+const int NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE = 1;
+
+/// Maximum number of contact manifolds in an overlapping pair that involves at
+/// least one concave collision shape.
+const int NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3;
+
+}
+
+#endif

src/constraint/BallAndSocketJoint.cpp

@@ -0,0 +1,234 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "BallAndSocketJoint.h"
+#include "engine/ConstraintSolver.h"
+
+using namespace reactphysics3d;
+
+// Static variables definition
+const decimal BallAndSocketJoint::BETA = decimal(0.2);
+
+// Constructor
+BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo)
+                   : Joint(jointInfo), mImpulse(Vector3(0, 0, 0)) {
+
+    // Compute the local-space anchor point for each body
+    mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
+    mLocalAnchorPointBody2 = mBody2->getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
+}
+
+// Destructor
+BallAndSocketJoint::~BallAndSocketJoint() {
+
+}
+
+// Initialize before solving the constraint
+void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
+
+    // Initialize the bodies index in the velocity array
+    mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
+    mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
+
+    // Get the bodies center of mass and orientations
+    const Vector3& x1 = mBody1->mCenterOfMassWorld;
+    const Vector3& x2 = mBody2->mCenterOfMassWorld;
+    const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
+    const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
+
+    // Get the inertia tensor of bodies
+    mI1 = mBody1->getInertiaTensorInverseWorld();
+    mI2 = mBody2->getInertiaTensorInverseWorld();
+
+    // Compute the vector from body center to the anchor point in world-space
+    mR1World = orientationBody1 * mLocalAnchorPointBody1;
+    mR2World = orientationBody2 * mLocalAnchorPointBody2;
+
+    // Compute the corresponding skew-symmetric matrices
+    Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
+    Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
+
+    // Compute the matrix K=JM^-1J^t (3x3 matrix)
+    decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
+    Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
+                                    0, inverseMassBodies, 0,
+                                    0, 0, inverseMassBodies) +
+                           skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
+                           skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
+
+    // Compute the inverse mass matrix K^-1
+    mInverseMassMatrix.setToZero();
+    if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
+        mInverseMassMatrix = massMatrix.getInverse();
+    }
+
+    // Compute the bias "b" of the constraint
+    mBiasVector.setToZero();
+    if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
+        decimal biasFactor = (BETA / constraintSolverData.timeStep);
+        mBiasVector = biasFactor * (x2 + mR2World - x1 - mR1World);
+    }
+
+    // If warm-starting is not enabled
+    if (!constraintSolverData.isWarmStartingActive) {
+
+        // Reset the accumulated impulse
+        mImpulse.setToZero();
+    }
+}
+
+// Warm start the constraint (apply the previous impulse at the beginning of the step)
+void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
+
+    // Get the velocities
+    Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
+    Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
+    Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
+    Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+
+    // Compute the impulse P=J^T * lambda for the body 1
+    const Vector3 linearImpulseBody1 = -mImpulse;
+    const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World);
+
+    // Apply the impulse to the body 1
+    v1 += mBody1->mMassInverse * linearImpulseBody1;
+    w1 += mI1 * angularImpulseBody1;
+
+    // Compute the impulse P=J^T * lambda for the body 2
+    const Vector3 angularImpulseBody2 = -mImpulse.cross(mR2World);
+
+    // Apply the impulse to the body to the body 2
+    v2 += mBody2->mMassInverse * mImpulse;
+    w2 += mI2 * angularImpulseBody2;
+}
+
+// Solve the velocity constraint
+void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
+
+    // Get the velocities
+    Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
+    Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
+    Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
+    Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+
+    // Compute J*v
+    const Vector3 Jv = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World);
+
+    // Compute the Lagrange multiplier lambda
+    const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - mBiasVector);
+    mImpulse += deltaLambda;
+
+    // Compute the impulse P=J^T * lambda for the body 1
+    const Vector3 linearImpulseBody1 = -deltaLambda;
+    const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World);
+
+    // Apply the impulse to the body 1
+    v1 += mBody1->mMassInverse * linearImpulseBody1;
+    w1 += mI1 * angularImpulseBody1;
+
+    // Compute the impulse P=J^T * lambda for the body 2
+    const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World);
+
+    // Apply the impulse to the body 2
+    v2 += mBody2->mMassInverse * deltaLambda;
+    w2 += mI2 * angularImpulseBody2;
+}
+
+// Solve the position constraint (for position error correction)
+void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
+
+    // If the error position correction technique is not the non-linear-gauss-seidel, we do
+    // do not execute this method
+    if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return;
+
+    // Get the bodies center of mass and orientations
+    Vector3& x1 = constraintSolverData.positions[mIndexBody1];
+    Vector3& x2 = constraintSolverData.positions[mIndexBody2];
+    Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
+    Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
+
+    // Get the inverse mass and inverse inertia tensors of the bodies
+    decimal inverseMassBody1 = mBody1->mMassInverse;
+    decimal inverseMassBody2 = mBody2->mMassInverse;
+
+    // Recompute the inverse inertia tensors
+    mI1 = mBody1->getInertiaTensorInverseWorld();
+    mI2 = mBody2->getInertiaTensorInverseWorld();
+
+    // Compute the vector from body center to the anchor point in world-space
+    mR1World = q1 * mLocalAnchorPointBody1;
+    mR2World = q2 * mLocalAnchorPointBody2;
+
+    // Compute the corresponding skew-symmetric matrices
+    Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
+    Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
+
+    // Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation constraints
+    decimal inverseMassBodies = inverseMassBody1 + inverseMassBody2;
+    Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
+                                    0, inverseMassBodies, 0,
+                                    0, 0, inverseMassBodies) +
+                           skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
+                           skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
+    mInverseMassMatrix.setToZero();
+    if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
+        mInverseMassMatrix = massMatrix.getInverse();
+    }
+
+    // Compute the constraint error (value of the C(x) function)
+    const Vector3 constraintError = (x2 + mR2World - x1 - mR1World);
+
+    // Compute the Lagrange multiplier lambda
+    // TODO : Do not solve the system by computing the inverse each time and multiplying with the
+    //        right-hand side vector but instead use a method to directly solve the linear system.
+    const Vector3 lambda = mInverseMassMatrix * (-constraintError);
+
+    // Compute the impulse of body 1
+    const Vector3 linearImpulseBody1 = -lambda;
+    const Vector3 angularImpulseBody1 = lambda.cross(mR1World);
+
+    // Compute the pseudo velocity of body 1
+    const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
+    const Vector3 w1 = mI1 * angularImpulseBody1;
+
+    // Update the body center of mass and orientation of body 1
+    x1 += v1;
+    q1 += Quaternion(0, w1) * q1 * decimal(0.5);
+    q1.normalize();
+
+    // Compute the impulse of body 2
+    const Vector3 angularImpulseBody2 = -lambda.cross(mR2World);
+
+    // Compute the pseudo velocity of body 2
+    const Vector3 v2 = inverseMassBody2 * lambda;
+    const Vector3 w2 = mI2 * angularImpulseBody2;
+
+    // Update the body position/orientation of body 2
+    x2 += v2;
+    q2 += Quaternion(0, w2) * q2 * decimal(0.5);
+    q2.normalize();
+}
+

src/constraint/BallAndSocketJoint.h

@@ -0,0 +1,147 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_BALL_AND_SOCKET_JOINT_H
+#define REACTPHYSICS3D_BALL_AND_SOCKET_JOINT_H
+
+// Libraries
+#include "Joint.h"
+#include "mathematics/mathematics.h"
+
+namespace reactphysics3d {
+
+// Structure BallAndSocketJointInfo
+/**
+ * This structure is used to gather the information needed to create a ball-and-socket
+ * joint. This structure will be used to create the actual ball-and-socket joint.
+ */
+struct BallAndSocketJointInfo : public JointInfo {
+
+    public :
+
+        // -------------------- Attributes -------------------- //
+
+        /// Anchor point (in world-space coordinates)
+        Vector3 anchorPointWorldSpace;
+
+        /// Constructor
+        /**
+         * @param rigidBody1 Pointer to the first body of the joint
+         * @param rigidBody2 Pointer to the second body of the joint
+         * @param initAnchorPointWorldSpace The anchor point in world-space
+         *                                  coordinates
+         */
+        BallAndSocketJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
+                               const Vector3& initAnchorPointWorldSpace)
+                              : JointInfo(rigidBody1, rigidBody2, BALLSOCKETJOINT),
+                                anchorPointWorldSpace(initAnchorPointWorldSpace) {}
+};
+
+// Class BallAndSocketJoint
+/**
+ * This class represents a ball-and-socket joint that allows arbitrary rotation
+ * between two bodies. This joint has three degrees of freedom. It can be used to
+ * create a chain of bodies for instance.
+ */
+class BallAndSocketJoint : public Joint {
+
+    private :
+
+        // -------------------- Constants -------------------- //
+
+        // Beta value for the bias factor of position correction
+        static const decimal BETA;
+
+        // -------------------- Attributes -------------------- //
+
+        /// Anchor point of body 1 (in local-space coordinates of body 1)
+        Vector3 mLocalAnchorPointBody1;
+
+        /// Anchor point of body 2 (in local-space coordinates of body 2)
+        Vector3 mLocalAnchorPointBody2;
+
+        /// Vector from center of body 2 to anchor point in world-space
+        Vector3 mR1World;
+
+        /// Vector from center of body 2 to anchor point in world-space
+        Vector3 mR2World;
+
+        /// Inertia tensor of body 1 (in world-space coordinates)
+        Matrix3x3 mI1;
+
+        /// Inertia tensor of body 2 (in world-space coordinates)
+        Matrix3x3 mI2;
+
+        /// Bias vector for the constraint
+        Vector3 mBiasVector;
+
+        /// Inverse mass matrix K=JM^-1J^-t of the constraint
+        Matrix3x3 mInverseMassMatrix;
+
+        /// Accumulated impulse
+        Vector3 mImpulse;
+
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        BallAndSocketJoint(const BallAndSocketJoint& constraint);
+
+        /// Private assignment operator
+        BallAndSocketJoint& operator=(const BallAndSocketJoint& constraint);
+
+        /// Return the number of bytes used by the joint
+        virtual size_t getSizeInBytes() const;
+
+        /// Initialize before solving the constraint
+        virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
+
+        /// Warm start the constraint (apply the previous impulse at the beginning of the step)
+        virtual void warmstart(const ConstraintSolverData& constraintSolverData);
+
+        /// Solve the velocity constraint
+        virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
+
+        /// Solve the position constraint (for position error correction)
+        virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo);
+
+        /// Destructor
+        virtual ~BallAndSocketJoint();
+};
+
+// Return the number of bytes used by the joint
+inline size_t BallAndSocketJoint::getSizeInBytes() const {
+    return sizeof(BallAndSocketJoint);
+}
+
+}
+
+#endif

src/constraint/ContactPoint.cpp

@@ -0,0 +1,58 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "ContactPoint.h"
+#include "collision/ProxyShape.h"
+
+using namespace reactphysics3d;
+using namespace std;
+
+// Constructor
+ContactPoint::ContactPoint(const ContactPointInfo& contactInfo)
+             : mBody1(contactInfo.shape1->getBody()), mBody2(contactInfo.shape2->getBody()),
+               mNormal(contactInfo.normal),
+               mPenetrationDepth(contactInfo.penetrationDepth),
+               mLocalPointOnBody1(contactInfo.localPoint1),
+               mLocalPointOnBody2(contactInfo.localPoint2),
+               mWorldPointOnBody1(contactInfo.shape1->getBody()->getTransform() *
+                                  contactInfo.shape1->getLocalToBodyTransform() *
+                                  contactInfo.localPoint1),
+               mWorldPointOnBody2(contactInfo.shape2->getBody()->getTransform() *
+                                  contactInfo.shape2->getLocalToBodyTransform() *
+                                  contactInfo.localPoint2),
+               mIsRestingContact(false) {
+
+    mFrictionVectors[0] = Vector3(0, 0, 0);
+    mFrictionVectors[1] = Vector3(0, 0, 0);
+
+    assert(mPenetrationDepth > 0.0);
+
+}
+
+// Destructor
+ContactPoint::~ContactPoint() {
+
+}

src/constraint/ContactPoint.h

@@ -0,0 +1,375 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_CONTACT_POINT_H
+#define REACTPHYSICS3D_CONTACT_POINT_H
+
+// Libraries
+#include "body/CollisionBody.h"
+#include "collision/CollisionShapeInfo.h"
+#include "configuration.h"
+#include "mathematics/mathematics.h"
+#include "configuration.h"
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Structure ContactPointInfo
+/**
+ * This structure contains informations about a collision contact
+ * computed during the narrow-phase collision detection. Those
+ * informations are used to compute the contact set for a contact
+ * between two bodies.
+ */
+struct ContactPointInfo {
+
+    private:
+
+        // -------------------- Methods -------------------- //
+
+    public:
+
+        // -------------------- Attributes -------------------- //
+
+        /// First proxy shape of the contact
+        ProxyShape* shape1;
+
+        /// Second proxy shape of the contact
+        ProxyShape* shape2;
+
+        /// First collision shape
+        const CollisionShape* collisionShape1;
+
+        /// Second collision shape
+        const CollisionShape* collisionShape2;
+
+        /// Normalized normal vector of the collision contact in world space
+        Vector3 normal;
+
+        /// Penetration depth of the contact
+        decimal penetrationDepth;
+
+        /// Contact point of body 1 in local space of body 1
+        Vector3 localPoint1;
+
+        /// Contact point of body 2 in local space of body 2
+        Vector3 localPoint2;
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        ContactPointInfo(ProxyShape* proxyShape1, ProxyShape* proxyShape2, const CollisionShape* collShape1,
+                         const CollisionShape* collShape2, const Vector3& normal, decimal penetrationDepth,
+                         const Vector3& localPoint1, const Vector3& localPoint2)
+            : shape1(proxyShape1), shape2(proxyShape2), collisionShape1(collShape1), collisionShape2(collShape2),
+              normal(normal), penetrationDepth(penetrationDepth), localPoint1(localPoint1),
+              localPoint2(localPoint2) {
+
+        }
+};
+
+// Class ContactPoint
+/**
+ * This class represents a collision contact point between two
+ * bodies in the physics engine.
+ */
+class ContactPoint {
+
+    private :
+
+        // -------------------- Attributes -------------------- //
+
+        /// First rigid body of the contact
+        CollisionBody* mBody1;
+
+        /// Second rigid body of the contact
+        CollisionBody* mBody2;
+
+        /// Normalized normal vector of the contact (from body1 toward body2) in world space
+        const Vector3 mNormal;
+
+        /// Penetration depth
+        decimal mPenetrationDepth;
+
+        /// Contact point on body 1 in local space of body 1
+        const Vector3 mLocalPointOnBody1;
+
+        /// Contact point on body 2 in local space of body 2
+        const Vector3 mLocalPointOnBody2;
+
+        /// Contact point on body 1 in world space
+        Vector3 mWorldPointOnBody1;
+
+        /// Contact point on body 2 in world space
+        Vector3 mWorldPointOnBody2;
+
+        /// True if the contact is a resting contact (exists for more than one time step)
+        bool mIsRestingContact;
+
+        /// Two orthogonal vectors that span the tangential friction plane
+        Vector3 mFrictionVectors[2];
+
+        /// Cached penetration impulse
+        decimal mPenetrationImpulse;
+
+        /// Cached first friction impulse
+        decimal mFrictionImpulse1;
+
+        /// Cached second friction impulse
+        decimal mFrictionImpulse2;
+
+        /// Cached rolling resistance impulse
+        Vector3 mRollingResistanceImpulse;
+        
+        // -------------------- Methods -------------------- //
+
+        /// Private copy-constructor
+        ContactPoint(const ContactPoint& contact);
+
+        /// Private assignment operator
+        ContactPoint& operator=(const ContactPoint& contact);
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        ContactPoint(const ContactPointInfo& contactInfo);
+
+        /// Destructor
+        ~ContactPoint();
+
+        /// Return the reference to the body 1
+        CollisionBody* getBody1() const;
+
+        /// Return the reference to the body 2
+        CollisionBody* getBody2() const;
+
+        /// Return the normal vector of the contact
+        Vector3 getNormal() const;
+
+        /// Set the penetration depth of the contact
+        void setPenetrationDepth(decimal penetrationDepth);
+
+        /// Return the contact local point on body 1
+        Vector3 getLocalPointOnBody1() const;
+
+        /// Return the contact local point on body 2
+        Vector3 getLocalPointOnBody2() const;
+
+        /// Return the contact world point on body 1
+        Vector3 getWorldPointOnBody1() const;
+
+        /// Return the contact world point on body 2
+        Vector3 getWorldPointOnBody2() const;
+
+        /// Return the cached penetration impulse
+        decimal getPenetrationImpulse() const;
+
+        /// Return the cached first friction impulse
+        decimal getFrictionImpulse1() const;
+
+        /// Return the cached second friction impulse
+        decimal getFrictionImpulse2() const;
+
+        /// Return the cached rolling resistance impulse
+        Vector3 getRollingResistanceImpulse() const;
+
+        /// Set the cached penetration impulse
+        void setPenetrationImpulse(decimal impulse);
+
+        /// Set the first cached friction impulse
+        void setFrictionImpulse1(decimal impulse);
+
+        /// Set the second cached friction impulse
+        void setFrictionImpulse2(decimal impulse);
+
+        /// Set the cached rolling resistance impulse
+        void setRollingResistanceImpulse(const Vector3& impulse);
+
+        /// Set the contact world point on body 1
+        void setWorldPointOnBody1(const Vector3& worldPoint);
+
+        /// Set the contact world point on body 2
+        void setWorldPointOnBody2(const Vector3& worldPoint);
+
+        /// Return true if the contact is a resting contact
+        bool getIsRestingContact() const;
+
+        /// Set the mIsRestingContact variable
+        void setIsRestingContact(bool isRestingContact);
+
+        /// Get the first friction vector
+        Vector3 getFrictionVector1() const;
+
+        /// Set the first friction vector
+        void setFrictionVector1(const Vector3& frictionVector1);
+
+        /// Get the second friction vector
+        Vector3 getFrictionVector2() const;
+
+        /// Set the second friction vector
+        void setFrictionVector2(const Vector3& frictionVector2);
+
+        /// Return the penetration depth
+        decimal getPenetrationDepth() const;
+
+        /// Return the number of bytes used by the contact point
+        size_t getSizeInBytes() const;
+};
+
+// Return the reference to the body 1
+inline CollisionBody* ContactPoint::getBody1() const {
+    return mBody1;
+}
+
+// Return the reference to the body 2
+inline CollisionBody* ContactPoint::getBody2() const {
+    return mBody2;
+}
+
+// Return the normal vector of the contact
+inline Vector3 ContactPoint::getNormal() const {
+    return mNormal;
+}
+
+// Set the penetration depth of the contact
+inline void ContactPoint::setPenetrationDepth(decimal penetrationDepth) {
+    this->mPenetrationDepth = penetrationDepth;
+}
+
+// Return the contact point on body 1
+inline Vector3 ContactPoint::getLocalPointOnBody1() const {
+    return mLocalPointOnBody1;
+}
+
+// Return the contact point on body 2
+inline Vector3 ContactPoint::getLocalPointOnBody2() const {
+    return mLocalPointOnBody2;
+}
+
+// Return the contact world point on body 1
+inline Vector3 ContactPoint::getWorldPointOnBody1() const {
+    return mWorldPointOnBody1;
+}
+
+// Return the contact world point on body 2
+inline Vector3 ContactPoint::getWorldPointOnBody2() const {
+    return mWorldPointOnBody2;
+}
+
+// Return the cached penetration impulse
+inline decimal ContactPoint::getPenetrationImpulse() const {
+    return mPenetrationImpulse;
+}
+
+// Return the cached first friction impulse
+inline decimal ContactPoint::getFrictionImpulse1() const {
+    return mFrictionImpulse1;
+}
+
+// Return the cached second friction impulse
+inline decimal ContactPoint::getFrictionImpulse2() const {
+    return mFrictionImpulse2;
+}
+
+// Return the cached rolling resistance impulse
+inline Vector3 ContactPoint::getRollingResistanceImpulse() const {
+    return mRollingResistanceImpulse;
+}
+
+// Set the cached penetration impulse
+inline void ContactPoint::setPenetrationImpulse(decimal impulse) {
+    mPenetrationImpulse = impulse;
+}
+
+// Set the first cached friction impulse
+inline void ContactPoint::setFrictionImpulse1(decimal impulse) {
+    mFrictionImpulse1 = impulse;
+}
+
+// Set the second cached friction impulse
+inline void ContactPoint::setFrictionImpulse2(decimal impulse) {
+    mFrictionImpulse2 = impulse;
+}
+
+// Set the cached rolling resistance impulse
+inline void ContactPoint::setRollingResistanceImpulse(const Vector3& impulse) {
+    mRollingResistanceImpulse = impulse;
+}
+
+// Set the contact world point on body 1
+inline void ContactPoint::setWorldPointOnBody1(const Vector3& worldPoint) {
+    mWorldPointOnBody1 = worldPoint;
+}
+
+// Set the contact world point on body 2
+inline void ContactPoint::setWorldPointOnBody2(const Vector3& worldPoint) {
+    mWorldPointOnBody2 = worldPoint;
+}
+
+// Return true if the contact is a resting contact
+inline bool ContactPoint::getIsRestingContact() const {
+    return mIsRestingContact;
+}
+
+// Set the mIsRestingContact variable
+inline void ContactPoint::setIsRestingContact(bool isRestingContact) {
+    mIsRestingContact = isRestingContact;
+}
+
+// Get the first friction vector
+inline Vector3 ContactPoint::getFrictionVector1() const {
+    return mFrictionVectors[0];
+}
+
+// Set the first friction vector
+inline void ContactPoint::setFrictionVector1(const Vector3& frictionVector1) {
+    mFrictionVectors[0] = frictionVector1;
+}
+
+// Get the second friction vector
+inline Vector3 ContactPoint::getFrictionVector2() const {
+    return mFrictionVectors[1];
+}
+
+// Set the second friction vector
+inline void ContactPoint::setFrictionVector2(const Vector3& frictionVector2) {
+    mFrictionVectors[1] = frictionVector2;
+}
+
+// Return the penetration depth of the contact
+inline decimal ContactPoint::getPenetrationDepth() const {
+    return mPenetrationDepth;
+}
+
+// Return the number of bytes used by the contact point
+inline size_t ContactPoint::getSizeInBytes() const {
+    return sizeof(ContactPoint);
+}
+
+}
+
+#endif

src/constraint/FixedJoint.cpp

@@ -0,0 +1,329 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "FixedJoint.h"
+#include "engine/ConstraintSolver.h"
+
+using namespace reactphysics3d;
+
+// Static variables definition
+const decimal FixedJoint::BETA = decimal(0.2);
+
+// Constructor
+FixedJoint::FixedJoint(const FixedJointInfo& jointInfo)
+           : Joint(jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0, 0) {
+
+    // Compute the local-space anchor point for each body
+    const Transform& transform1 = mBody1->getTransform();
+    const Transform& transform2 = mBody2->getTransform();
+    mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace;
+    mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace;
+
+    // Compute the inverse of the initial orientation difference between the two bodies
+    mInitOrientationDifferenceInv = transform2.getOrientation() *
+                                    transform1.getOrientation().getInverse();
+    mInitOrientationDifferenceInv.normalize();
+    mInitOrientationDifferenceInv.inverse();
+}
+
+// Destructor
+FixedJoint::~FixedJoint() {
+
+}
+
+// Initialize before solving the constraint
+void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
+
+    // Initialize the bodies index in the velocity array
+    mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
+    mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
+
+    // Get the bodies positions and orientations
+    const Vector3& x1 = mBody1->mCenterOfMassWorld;
+    const Vector3& x2 = mBody2->mCenterOfMassWorld;
+    const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
+    const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
+
+    // Get the inertia tensor of bodies
+    mI1 = mBody1->getInertiaTensorInverseWorld();
+    mI2 = mBody2->getInertiaTensorInverseWorld();
+
+    // Compute the vector from body center to the anchor point in world-space
+    mR1World = orientationBody1 * mLocalAnchorPointBody1;
+    mR2World = orientationBody2 * mLocalAnchorPointBody2;
+
+    // Compute the corresponding skew-symmetric matrices
+    Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
+    Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
+
+    // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
+    decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
+    Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
+                                    0, inverseMassBodies, 0,
+                                    0, 0, inverseMassBodies) +
+                           skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
+                           skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
+
+    // Compute the inverse mass matrix K^-1 for the 3 translation constraints
+    mInverseMassMatrixTranslation.setToZero();
+    if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
+        mInverseMassMatrixTranslation = massMatrix.getInverse();
+    }
+
+    // Compute the bias "b" of the constraint for the 3 translation constraints
+    decimal biasFactor = (BETA / constraintSolverData.timeStep);
+    mBiasTranslation.setToZero();
+    if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
+        mBiasTranslation = biasFactor * (x2 + mR2World - x1 - mR1World);
+    }
+
+    // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
+    // contraints (3x3 matrix)
+    mInverseMassMatrixRotation = mI1 + mI2;
+    if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
+        mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse();
+    }
+
+    // Compute the bias "b" for the 3 rotation constraints
+    mBiasRotation.setToZero();
+    if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
+        Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
+        currentOrientationDifference.normalize();
+        const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
+        mBiasRotation = biasFactor * decimal(2.0) * qError.getVectorV();
+    }
+
+    // If warm-starting is not enabled
+    if (!constraintSolverData.isWarmStartingActive) {
+
+        // Reset the accumulated impulses
+        mImpulseTranslation.setToZero();
+        mImpulseRotation.setToZero();
+    }
+}
+
+// Warm start the constraint (apply the previous impulse at the beginning of the step)
+void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
+
+    // Get the velocities
+    Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
+    Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
+    Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
+    Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+
+    // Get the inverse mass of the bodies
+    const decimal inverseMassBody1 = mBody1->mMassInverse;
+    const decimal inverseMassBody2 = mBody2->mMassInverse;
+
+    // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1
+    Vector3 linearImpulseBody1 = -mImpulseTranslation;
+    Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World);
+
+    // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1
+    angularImpulseBody1 += -mImpulseRotation;
+
+    // Apply the impulse to the body 1
+    v1 += inverseMassBody1 * linearImpulseBody1;
+    w1 += mI1 * angularImpulseBody1;
+
+    // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 2
+    Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World);
+
+    // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 2
+    angularImpulseBody2 += mImpulseRotation;
+
+    // Apply the impulse to the body 2
+    v2 += inverseMassBody2 * mImpulseTranslation;
+    w2 += mI2 * angularImpulseBody2;
+}
+
+// Solve the velocity constraint
+void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
+
+    // Get the velocities
+    Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
+    Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
+    Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
+    Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+
+    // Get the inverse mass of the bodies
+    decimal inverseMassBody1 = mBody1->mMassInverse;
+    decimal inverseMassBody2 = mBody2->mMassInverse;
+
+    // --------------- Translation Constraints --------------- //
+
+    // Compute J*v for the 3 translation constraints
+    const Vector3 JvTranslation = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World);
+
+    // Compute the Lagrange multiplier lambda
+    const Vector3 deltaLambda = mInverseMassMatrixTranslation *
+                               (-JvTranslation - mBiasTranslation);
+    mImpulseTranslation += deltaLambda;
+
+    // Compute the impulse P=J^T * lambda for body 1
+    const Vector3 linearImpulseBody1 = -deltaLambda;
+    Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World);
+
+    // Apply the impulse to the body 1
+    v1 += inverseMassBody1 * linearImpulseBody1;
+    w1 += mI1 * angularImpulseBody1;
+
+    // Compute the impulse P=J^T * lambda  for body 2
+    const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World);
+
+    // Apply the impulse to the body 2
+    v2 += inverseMassBody2 * deltaLambda;
+    w2 += mI2 * angularImpulseBody2;
+
+    // --------------- Rotation Constraints --------------- //
+
+    // Compute J*v for the 3 rotation constraints
+    const Vector3 JvRotation = w2 - w1;
+
+    // Compute the Lagrange multiplier lambda for the 3 rotation constraints
+    Vector3 deltaLambda2 = mInverseMassMatrixRotation * (-JvRotation - mBiasRotation);
+    mImpulseRotation += deltaLambda2;
+
+    // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1
+    angularImpulseBody1 = -deltaLambda2;
+
+    // Apply the impulse to the body 1
+    w1 += mI1 * angularImpulseBody1;
+
+    // Apply the impulse to the body 2
+    w2 += mI2 * deltaLambda2;
+}
+
+// Solve the position constraint (for position error correction)
+void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
+
+    // If the error position correction technique is not the non-linear-gauss-seidel, we do
+    // do not execute this method
+    if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return;
+
+    // Get the bodies positions and orientations
+    Vector3& x1 = constraintSolverData.positions[mIndexBody1];
+    Vector3& x2 = constraintSolverData.positions[mIndexBody2];
+    Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
+    Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
+
+    // Get the inverse mass and inverse inertia tensors of the bodies
+    decimal inverseMassBody1 = mBody1->mMassInverse;
+    decimal inverseMassBody2 = mBody2->mMassInverse;
+
+    // Recompute the inverse inertia tensors
+    mI1 = mBody1->getInertiaTensorInverseWorld();
+    mI2 = mBody2->getInertiaTensorInverseWorld();
+
+    // Compute the vector from body center to the anchor point in world-space
+    mR1World = q1 * mLocalAnchorPointBody1;
+    mR2World = q2 * mLocalAnchorPointBody2;
+
+    // Compute the corresponding skew-symmetric matrices
+    Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
+    Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
+
+    // --------------- Translation Constraints --------------- //
+
+    // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
+    decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
+    Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
+                                    0, inverseMassBodies, 0,
+                                    0, 0, inverseMassBodies) +
+                           skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
+                           skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
+    mInverseMassMatrixTranslation.setToZero();
+    if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
+        mInverseMassMatrixTranslation = massMatrix.getInverse();
+    }
+
+    // Compute position error for the 3 translation constraints
+    const Vector3 errorTranslation = x2 + mR2World - x1 - mR1World;
+
+    // Compute the Lagrange multiplier lambda
+    const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation);
+
+    // Compute the impulse of body 1
+    Vector3 linearImpulseBody1 = -lambdaTranslation;
+    Vector3 angularImpulseBody1 = lambdaTranslation.cross(mR1World);
+
+    // Compute the pseudo velocity of body 1
+    const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
+    Vector3 w1 = mI1 * angularImpulseBody1;
+
+    // Update the body position/orientation of body 1
+    x1 += v1;
+    q1 += Quaternion(0, w1) * q1 * decimal(0.5);
+    q1.normalize();
+
+    // Compute the impulse of body 2
+    Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mR2World);
+
+    // Compute the pseudo velocity of body 2
+    const Vector3 v2 = inverseMassBody2 * lambdaTranslation;
+    Vector3 w2 = mI2 * angularImpulseBody2;
+
+    // Update the body position/orientation of body 2
+    x2 += v2;
+    q2 += Quaternion(0, w2) * q2 * decimal(0.5);
+    q2.normalize();
+
+    // --------------- Rotation Constraints --------------- //
+
+    // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
+    // contraints (3x3 matrix)
+    mInverseMassMatrixRotation = mI1 + mI2;
+    if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
+        mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse();
+    }
+
+    // Compute the position error for the 3 rotation constraints
+    Quaternion currentOrientationDifference = q2 * q1.getInverse();
+    currentOrientationDifference.normalize();
+    const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
+    const Vector3 errorRotation = decimal(2.0) * qError.getVectorV();
+
+    // Compute the Lagrange multiplier lambda for the 3 rotation constraints
+    Vector3 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation);
+
+    // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
+    angularImpulseBody1 = -lambdaRotation;
+
+    // Compute the pseudo velocity of body 1
+    w1 = mI1 * angularImpulseBody1;
+
+    // Update the body position/orientation of body 1
+    q1 += Quaternion(0, w1) * q1 * decimal(0.5);
+    q1.normalize();
+
+    // Compute the pseudo velocity of body 2
+    w2 = mI2 * lambdaRotation;
+
+    // Update the body position/orientation of body 2
+    q2 += Quaternion(0, w2) * q2 * decimal(0.5);
+    q2.normalize();
+}
+