[DEV] continue porting internal physics engine

.classpath

@@ -1,6 +1,11 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <classpath>
 	<classpathentry excluding="org/atriaSoft/ege/physics/|org/atriaSoft/ege/ia/|org/atriaSoft/ege/physics/shape/|org/atriaSoft/ege/particule/|org/atriaSoft/ege/camera/|org/atriaSoft/ege/|org/atriaSoft/ege/resource/tools/|org/atriaSoft/ege/render/|org/atriaSoft/ege/widget/|org/atriaSoft/ege/position/|org/atriaSoft/ege/elements/|org/atriaSoft/ege/resource/" kind="src" path="src"/>
+	<classpathentry kind="src" output="jege/bin/binTest" path="srcTest">
+		<attributes>
+			<attribute name="test" value="true"/>
+		</attributes>
+	</classpathentry>
 	<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER">
 		<attributes>
 			<attribute name="module" value="true"/>
@@ -71,5 +76,6 @@
 			<attribute name="module" value="true"/>
 		</attributes>
 	</classpathentry>
+	<classpathentry kind="con" path="org.eclipse.jdt.junit.JUNIT_CONTAINER/5"/>
 	<classpathentry kind="output" path="bin"/>
 </classpath>

src/org/atriaSoft/ephysics/ContactsPositionCorrectionTechnique.java

@@ -0,0 +1,12 @@
+package org.atriaSoft.ephysics;
+
+/// Position correction technique used in the contact solver (for contacts)
+/// BAUMGARTECONTACTS : 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).
+/// SPLITIMPULSES : A bit slower but the error correction factor is not added to the
+///                 bodies momentum. This is the option used by default.
+public enum ContactsPositionCorrectionTechnique {
+	BAUMGARTECONTACTS,
+	SPLITIMPULSES,
+}

src/org/atriaSoft/ephysics/JointsPositionCorrectionTechnique.java

@@ -0,0 +1,9 @@
+package org.atriaSoft.ephysics;
+
+/// Position correction technique used in the raint solver (for joints).
+/// BAUMGARTEJOINTS : Faster but can be innacurate in some situations.
+/// NONLINEARGAUSSSEIDEL : Slower but more precise. This is the option used by default.
+public enum JointsPositionCorrectionTechnique {
+	BAUMGARTEJOINTS,
+	NONLINEARGAUSSSEIDEL
+}

src/org/atriaSoft/ephysics/Log.java

@@ -0,0 +1,29 @@
+package org.atriaSoft.ephysics;
+
+public class Log {
+	private static String LIBNAME = "ephysic";
+	public static void print(String data) {
+		System.out.println(data);
+	}
+	public static void critical(String data) {
+		System.out.println("[C] " + LIBNAME + " | " + data);
+	}
+	public static void error(String data) {
+		System.out.println("[E] " + LIBNAME + " | " + data);
+	}
+	public static void warning(String data) {
+		System.out.println("[W] " + LIBNAME + " | " + data);
+	}
+	public static void info(String data) {
+		System.out.println("[I] " + LIBNAME + " | " + data);
+	}
+	public static void debug(String data) {
+		System.out.println("[D] " + LIBNAME + " | " + data);
+	}
+	public static void verbose(String data) {
+		System.out.println("[V] " + LIBNAME + " | " + data);
+	}
+	public static void todo(String data) {
+		System.out.println("[TODO] " + LIBNAME + " | " + data);
+	}
+}

src/org/atriaSoft/ephysics/Property.java

@@ -0,0 +1,70 @@
+package org.atriaSoft.ephysics;
+
+public class Property {
+	// ------------------- Type definitions ------------------- //
+	//typedef Pair<long, long> longpair;
+	// ------------------- Constants ------------------- //
+	public final static float FLTEPSILON = 0.00001f; // TODO: check this ...
+	/// Pi ant
+	public final static float PI = 3.14159265f;
+	
+	/// 2*Pi ant
+	public final static float PITIMES2 = 6.28318530f;
+	
+	/// Default friction coefficient for a rigid body
+	public final static float DEFAULTFRICTIONCOEFFICIENT = 0.3f;
+	
+	/// Default bounciness factor for a rigid body
+	public final static float DEFAULTBOUNCINESS = 0.5f;
+	
+	/// Default rolling resistance
+	public final static float DEFAULTROLLINGRESISTANCE = 0.0f;
+	
+	/// True if the spleeping technique is enabled
+	public final static boolean SPLEEPINGENABLED = true;
+	
+	/// Object margin for collision detection in meters (for the GJK-EPA Algorithm)
+	public final static float OBJECTMARGIN = 0.04f;
+	
+	/// Distance threshold for two contact points for a valid persistent contact (in meters)
+	public final static float PERSISTENTCONTACTDISTTHRESHOLD = 0.03f;
+	
+	/// Velocity threshold for contact velocity restitution
+	public final static float RESTITUTIONVELOCITYTHRESHOLD = 1.0f;
+	
+	/// Number of iterations when solving the velocity raints of the Sequential Impulse technique
+	public final static int DEFAULTVELOCITYSOLVERNBITERATIONS = 10;
+	
+	/// Number of iterations when solving the position raints of the Sequential Impulse technique
+	public final static int DEFAULTPOSITIONSOLVERNBITERATIONS = 5;
+	
+	/// Time (in seconds) that a body must stay still to be considered sleeping
+	public final static float DEFAULTTIMEBEFORESLEEP = 1.0f;
+	
+	/// A body with a linear velocity smaller than the sleep linear velocity (in m/s)
+	/// might enter sleeping mode.
+	public final static float DEFAULTSLEEPLINEARVELOCITY = 0.02f;
+	
+	/// A body with angular velocity smaller than the sleep angular velocity (in rad/s)
+	/// might enter sleeping mode
+	public final static float DEFAULTSLEEPANGULARVELOCITY = 3.0f * (PI / 180.0f);
+	
+	/// In the broad-phase collision detection (dynamic AABB tree), the AABBs are
+	/// inflated with a ant gap to allow the collision shape to move a little bit
+	/// without triggering a large modification of the tree which can be costly
+	public final static float DYNAMICTREEAABBGAP = 0.1f;
+	
+	/// 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 ant with the linear velocity and the elapsed time between two frames.
+	public final static float DYNAMICTREEAABBLINGAPMULTIPLIER = 1.7f;
+	
+	/// Maximum number of contact manifolds in an overlapping pair that involves two
+	/// convex collision shapes.
+	public final static int NBMAXCONTACTMANIFOLDSCONVEXSHAPE = 1;
+	
+	/// Maximum number of contact manifolds in an overlapping pair that involves at
+	/// least one concave collision shape.
+	public final static int NBMAXCONTACTMANIFOLDSCONCAVESHAPE = 3;
+}
+

src/org/atriaSoft/ephysics/body/Body.java

@@ -0,0 +1,141 @@
+package org.atriaSoft.ephysics.body;
+
+/**
+ * @brief Represent a body of the physics engine. You should not
+ * instantiante this class but instantiate the CollisionBody or RigidBody
+ * classes instead.
+ */
+class Body {
+	protected long id; //!< ID of the body
+	protected boolean isAlreadyInIsland; //!< True if the body has already been added in an island (for sleeping technique)
+	protected boolean isAllowedToSleep; //!< True if the body is allowed to go to sleep for better efficiency
+	/**
+	 * @brief 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.
+	 */
+	protected boolean isActive;
+	protected boolean isSleeping; //!< True if the body is sleeping (for sleeping technique)
+	protected float sleepTime; //!< Elapsed time since the body velocity was bellow the sleep velocity
+	protected Object userData; //!< Pointer that can be used to attach user data to the body
+	/**
+	 * @brief Constructor
+	 * @param[in] id ID of the new body
+	 */
+	public Body(long id) {
+		this.id = id;
+		this.isAlreadyInIsland = false;
+		this.isAllowedToSleep = true;
+		this.isActive = true;
+		this.isSleeping = false;
+		this.sleepTime = 0;
+		this.userData = null;
+	}
+	/**
+	 * @brief Return the id of the body
+	 * @return The ID of the body
+	 */
+	public long getID()  {
+		return this.id;
+	}
+	/**
+	 * @brief Return whether or not the body is allowed to sleep
+	 * @return True if the body is allowed to sleep and false otherwise
+	 */
+	public boolean isAllowedToSleep()  {
+		return this.isAllowedToSleep;
+	}
+	/**
+	 * @brief Set whether or not the body is allowed to go to sleep
+	 * @param[in] isAllowedToSleep True if the body is allowed to sleep
+	 */
+	public void setIsAllowedToSleep(boolean isAllowedToSleep) {
+		this.isAllowedToSleep = isAllowedToSleep;
+		if (!this.isAllowedToSleep) {
+			setIsSleeping(false);
+		}
+	}
+	/**
+	 * @brief Return whether or not the body is sleeping
+	 * @return True if the body is currently sleeping and false otherwise
+	 */
+	public boolean isSleeping()  {
+		return this.isSleeping;
+	}
+	/**
+	 * @brief Return true if the body is active
+	 * @return True if the body currently active and false otherwise
+	 */
+	public boolean isActive()  {
+		return this.isActive;
+	}
+	/**
+	 * @brief Set whether or not the body is active
+	 * @param[in] isActive True if you want to activate the body
+	 */
+	public void setIsActive(boolean isActive) {
+		this.isActive = isActive;
+	}
+	/**
+	 * @brief Set the variable to know whether or not the body is sleeping
+	 * @param[in] isSleeping Set the new status
+	 */
+	public void setIsSleeping(boolean isSleeping) {
+		if (isSleeping) {
+			this.sleepTime = 0.0f;
+		} else {
+			if (this.isSleeping) {
+				this.sleepTime = 0.0f;
+			}
+		}
+		this.isSleeping = isSleeping;
+	}
+	/**
+	 * @brief Return a pointer to the user data attached to this body
+	 * @return A pointer to the user data you have attached to the body
+	 */
+	public Object getUserData()  {
+		return this.userData;
+	}
+	/**
+	 * @brief Attach user data to this body
+	 * @param[in] userData A pointer to the user data you want to attach to the body
+	 */
+	public void setUserData(Object userData) {
+		this.userData = userData;
+	}
+	/**
+	 * @brief Smaller than operator
+	 * @param[in] obj Other object to compare
+	 * @return true if the current element is smaller
+	 */
+	public boolean isLess( Body obj)  {
+		return (this.id < obj.id);
+	}
+	/**
+	 * @brief Larger than operator
+	 * @param[in] obj Other object to compare
+	 * @return true if the current element is Bigger
+	 */
+	public boolean isUpper( Body obj)  {
+		return (this.id > obj.id);
+	}
+	/**
+	 * @brief Equal operator
+	 * @param[in] obj Other object to compare
+	 * @return true if the curretn element is equal
+	 */
+	public boolean isEqual( Body obj)  {
+		return (this.id == obj.id);
+	}
+	/**
+	 * @brief Not equal operator
+	 * @param[in] obj Other object to compare
+	 * @return true if the curretn element is NOT equal
+	 */
+	public boolean isDifferent( Body obj)  {
+		return (this.id != obj.id);
+	}
+}

src/org/atriaSoft/ephysics/body/BodyType.java

@@ -0,0 +1,7 @@
+package org.atriaSoft.ephysics.body;
+
+public enum BodyType {
+	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.
+}

src/org/atriaSoft/ephysics/body/CollisionBody.java

@@ -0,0 +1,345 @@
+package org.atriaSoft.ephysics.body;
+
+import org.atriaSoft.etk.math.Transform3D;
+
+/**
+ * @brief This class represents a body that is able to collide with others bodies. This class inherits from the Body class.
+ */
+class CollisionBody extends Body {
+	protected BodyType type; //!< Type of body (static, kinematic or dynamic)
+	protected Transform3D transform; //!< Position and orientation of the body
+	protected ProxyShape proxyCollisionShapes; //!< First element of the linked list of proxy collision shapes of this body
+	protected int numberCollisionShapes; //!< Number of collision shapes
+	protected ContactManifoldListElement* contactManifoldsList; //!< First element of the linked list of contact manifolds involving this body
+	protected CollisionWorld world; //!< Reference to the world the body belongs to
+	/**
+	 * @brief Reset the contact manifold lists
+	 */
+	protected void resetContactManifoldsList() {
+		// Delete the linked list of contact manifolds of that body
+		ContactManifoldListElement* currentElement = this.contactManifoldsList;
+		while (currentElement != null) {
+			ContactManifoldListElement* nextElement = currentElement.next;
+			// Delete the current element
+			ETKDELETE(ContactManifoldListElement, currentElement);
+			currentElement = nextElement;
+		}
+		this.contactManifoldsList = null;
+	}
+	/**
+	 * @brief Remove all the collision shapes
+	 */
+	protected void removeAllCollisionShapes() {
+		ProxyShape* current = this.proxyCollisionShapes;
+		// Look for the proxy shape that contains the collision shape in parameter
+		while(current != null) {
+			// Remove the proxy collision shape
+			ProxyShape* nextElement = current.this.next;
+			if (this.isActive) {
+				this.world.collisionDetection.removeProxyCollisionShape(current);
+			}
+			ETKDELETE(ProxyShape, current);
+			// Get the next element in the list
+			current = nextElement;
+		}
+		this.proxyCollisionShapes = null;
+	}
+	/**
+	 * @brief Update the broad-phase state for this body (because it has moved for instance)
+	 */
+	protected void updateBroadPhaseState() {
+		// For all the proxy collision shapes of the body
+		for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape.this.next) {
+			// Update the proxy
+			updateProxyShapeInBroadPhase(shape);
+		}
+	}
+	/**
+	 * @brief Update the broad-phase state of a proxy collision shape of the body
+	 */
+	protected void updateProxyShapeInBroadPhase(ProxyShape* proxyShape, boolean forceReinsert = false) {
+		AABB aabb;
+		proxyShape.getCollisionShape().computeAABB(aabb, this.transform * proxyShape.getLocalToBodyTransform());
+		this.world.collisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3f(0, 0, 0), forceReinsert);
+	}
+	/**
+	 * @brief Ask the broad-phase to test again the collision shapes of the body for collision (as if the body has moved).
+	 */
+	protected void askForBroadPhaseCollisionCheck() {
+		for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape.this.next) {
+			this.world.collisionDetection.askForBroadPhaseCollisionCheck(shape);  
+		}
+	}
+	/**
+	 * @brief Reset the this.isAlreadyInIsland variable of the body and contact manifolds.
+	 * This method also returns the number of contact manifolds of the body.
+	 */
+	protected int resetIsAlreadyInIslandAndCountManifolds() {
+		this.isAlreadyInIsland = false;
+		int nbManifolds = 0;
+		// Reset the this.isAlreadyInIsland variable of the contact manifolds for this body
+		ContactManifoldListElement* currentElement = this.contactManifoldsList;
+		while (currentElement != null) {
+			currentElement.contactManifold.this.isAlreadyInIsland = false;
+			currentElement = currentElement.next;
+			nbManifolds++;
+		}
+		return nbManifolds;
+	}
+	/**
+	 * @brief Constructor
+	 * @param[in] transform The transform of the body
+	 * @param[in] world The physics world where the body is created
+	 * @param[in] id ID of the body
+	 */
+	public CollisionBody( Transform3D transform, CollisionWorld world, long id) {
+		super(id);
+		this.type = DYNAMIC;
+		this.transform = transform;
+		this.proxyCollisionShapes = null;
+		this.numberCollisionShapes = 0;
+		this.contactManifoldsList = null;
+		this.world(world);
+		
+		Log.debug("         set transform: " + transform);
+		if (isnan(transform.getPosition().x()) == true) { // check NAN
+			Log.critical("         set transform: " + transform);
+		}
+		if (isinf(transform.getOrientation().z()) == true) {
+			Log.critical("         set transform: " + transform);
+		}
+	}
+	/**
+	 * @brief Return the type of the body
+	 * @return the type of the body (STATIC, KINEMATIC, DYNAMIC)
+	 */
+	public BodyType getType()  {
+		return this.type;
+	}
+	/**
+	 * @brief Set the type of the body
+	 * @param[in] type The type of the body (STATIC, KINEMATIC, DYNAMIC)
+	 */
+	public void setType(BodyType type) {
+		this.type = type;
+		if (this.type == STATIC) {
+			// Update the broad-phase state of the body
+			updateBroadPhaseState();
+		}
+	}
+	/**
+	 * @brief Set whether or not the body is active
+	 * @param[in] isActive True if you want to activate the body
+	 */
+	public void setIsActive(boolean isActive) {
+		// If the state does not change
+		if (this.isActive == isActive) {
+			return;
+		}
+		Body::setIsActive(isActive);
+		// If we have to activate the body
+		if (isActive == true) {
+			for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape.this.next) {
+				AABB aabb;
+				shape.getCollisionShape().computeAABB(aabb, this.transform * shape.this.localToBodyTransform);
+				this.world.collisionDetection.addProxyCollisionShape(shape, aabb);
+			}
+		} else {
+			for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape.this.next) {
+				this.world.collisionDetection.removeProxyCollisionShape(shape);
+			}
+			resetContactManifoldsList();
+		}
+	}
+	/**
+	 * @brief Return the current position and orientation
+	 * @return The current transformation of the body that transforms the local-space of the body into world-space
+	 */
+	public Transform3D getTransform()  {
+		return this.transform;
+	}
+	/**
+	 * @brief Set the current position and orientation
+	 * @param transform The transformation of the body that transforms the local-space of the body into world-space
+	 */
+	public void setTransform( Transform3D transform) {
+		Log.debug("         set transform: " + this.transform + " ==> " + transform);
+		if (isnan(transform.getPosition().x()) == true) { // check NAN
+			Log.critical("         set transform: " + this.transform + " ==> " + transform);
+		}
+		if (isinf(transform.getOrientation().z()) == true) {
+			Log.critical("         set transform: " + this.transform + " ==> " + transform);
+		}
+		this.transform = transform;
+		updateBroadPhaseState();
+	}
+	/**
+	 * @brief 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[in] collisionShape A pointer to the collision shape you want to add to the body
+	 * @param[in] 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.
+	 */
+	public ProxyShape addCollisionShape(CollisionShape collisionShape, Transform3D transform) {
+		// Create a proxy collision shape to attach the collision shape to the body
+		ProxyShape* proxyShape = ETKNEW(ProxyShape, this, collisionShape,transform, float(1));
+		// Add it to the list of proxy collision shapes of the body
+		if (this.proxyCollisionShapes == null) {
+			this.proxyCollisionShapes = proxyShape;
+		} else {
+			proxyShape.this.next = this.proxyCollisionShapes;
+			this.proxyCollisionShapes = proxyShape;
+		}
+		AABB aabb;
+		collisionShape.computeAABB(aabb, this.transform * transform);
+		this.world.collisionDetection.addProxyCollisionShape(proxyShape, aabb);
+		this.numberCollisionShapes++;
+		return proxyShape;
+	}
+	/**
+	 * @brief 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[in] proxyShape The pointer of the proxy shape you want to remove
+	 */
+	public void removeCollisionShape(ProxyShape proxyShape) {
+		ProxyShape* current = this.proxyCollisionShapes;
+		// If the the first proxy shape is the one to remove
+		if (current == proxyShape) {
+			this.proxyCollisionShapes = current.this.next;
+			if (this.isActive) {
+				this.world.collisionDetection.removeProxyCollisionShape(current);
+			}
+			ETKDELETE(ProxyShape, current);
+			current = null;
+			this.numberCollisionShapes--;
+			return;
+		}
+		// Look for the proxy shape that contains the collision shape in parameter
+		while(current.this.next != null) {
+			// If we have found the collision shape to remove
+			if (current.this.next == proxyShape) {
+				// Remove the proxy collision shape
+				ProxyShape* elementToRemove = current.this.next;
+				current.this.next = elementToRemove.this.next;
+				if (this.isActive) {
+					this.world.collisionDetection.removeProxyCollisionShape(elementToRemove);
+				}
+				ETKDELETE(ProxyShape, elementToRemove);
+				elementToRemove = null;
+				this.numberCollisionShapes--;
+				return;
+			}
+			// Get the next element in the list
+			current = current.this.next;
+		}
+	}
+	/**
+	 * @brief Get 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
+	 */
+	public ContactManifoldListElement getContactManifoldsList()  {
+		return this.contactManifoldsList;
+	}
+	/**
+	 * @brief 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[in] worldPoint The point to test (in world-space coordinates)
+	 * @return True if the point is inside the body
+	 */
+	public boolean testPointInside( Vector3f worldPoint) {
+		for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape.this.next) {
+			if (shape.testPointInside(worldPoint)) return true;
+		}
+		return false;
+	}
+	/**
+	 * @brief Raycast method with feedback information
+	 * The method returns the closest hit among all the collision shapes of the body
+	 * @param[in] 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
+	 */
+	public boolean raycast( Ray ray, RaycastInfo raycastInfo) {
+		if (this.isActive == false) {
+			return false;
+		}
+		boolean isHit = false;
+		Ray rayTemp(ray);
+		for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape.this.next) {
+			// Test if the ray hits the collision shape
+			if (shape.raycast(rayTemp, raycastInfo)) {
+				rayTemp.maxFraction = raycastInfo.hitFraction;
+				isHit = true;
+			}
+		}
+		return isHit;
+	}
+	/**
+	 * @brief 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
+	 */
+	public AABB getAABB() {
+		AABB bodyAABB;
+		if (this.proxyCollisionShapes == null) {
+			return bodyAABB;
+		}
+		this.proxyCollisionShapes.getCollisionShape().computeAABB(bodyAABB, this.transform * this.proxyCollisionShapes.getLocalToBodyTransform());
+		for (ProxyShape* shape = this.proxyCollisionShapes.this.next; shape != null; shape = shape.this.next) {
+			AABB aabb;
+			shape.getCollisionShape().computeAABB(aabb, this.transform * shape.getLocalToBodyTransform());
+			bodyAABB.mergeWithAABB(aabb);
+		}
+		return bodyAABB;
+	}
+	/**
+	 * @brief Get 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
+	 */
+	public ProxyShape getProxyShapesList() {
+		return this.proxyCollisionShapes;
+	}
+	/**
+	 * @brief Get 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
+	 */
+	public ProxyShape getProxyShapesList()  {
+		return this.proxyCollisionShapes;
+	}
+	/**
+	 * @brief Get the world-space coordinates of a point given the local-space coordinates of the body
+	 * @param[in] localPoint A point in the local-space coordinates of the body
+	 * @return The point in world-space coordinates
+	 */
+	public Vector3f getWorldPoint( Vector3f localPoint)  {
+		return this.transform * localPoint;
+	}
+	/**
+	 * @brief Get the world-space vector of a vector given in local-space coordinates of the body
+	 * @param[in] localVector A vector in the local-space coordinates of the body
+	 * @return The vector in world-space coordinates
+	 */
+	public Vector3f getWorldVector( Vector3f localVector)  {
+		return this.transform.getOrientation() * localVector;
+	}
+	/**
+	 * @brief Get the body local-space coordinates of a point given in the world-space coordinates
+	 * @param[in] worldPoint A point in world-space coordinates
+	 * @return The point in the local-space coordinates of the body
+	 */
+	public Vector3f getLocalPoint( Vector3f worldPoint)  {
+		return this.transform.getInverse() * worldPoint;
+	}
+	/**
+	 * @brief Get the body local-space coordinates of a vector given in the world-space coordinates
+	 * @param[in] worldVector A vector in world-space coordinates
+	 * @return The vector in the local-space coordinates of the body
+	 */
+	public Vector3f getLocalVector( Vector3f worldVector)  {
+		return this.transform.getOrientation().getInverse() * worldVector;
+	}
+}
+

src/org/atriaSoft/ephysics/body/RigidBody.java

@@ -0,0 +1,514 @@
+package org.atriaSoft.ephysics.body;
+
+import org.atriaSoft.etk.math.Matrix3f;
+import org.atriaSoft.etk.math.Transform3D;
+import org.atriaSoft.etk.math.Vector3f;
+
+/**
+	 * @brief This class represents a rigid body of the physics
+	 * engine. A rigid body is a non-deformable body that
+	 * has a ant mass. This class inherits from the
+	 * CollisionBody class.
+	 */
+class RigidBody extends CollisionBody {
+	protected float initMass; //!< Intial mass of the body
+	protected Vector3f centerOfMassLocal; //!< Center of mass of the body in local-space coordinates. The center of mass can therefore be different from the body origin
+	protected Vector3f centerOfMassWorld; //!< Center of mass of the body in world-space coordinates
+	protected Vector3f linearVelocity; //!< Linear velocity of the body
+	protected Vector3f angularVelocity; //!< Angular velocity of the body
+	protected Vector3f externalForce; //!< Current external force on the body
+	protected Vector3f externalTorque; //!< Current external torque on the body
+	protected Matrix3f inertiaTensorLocal; //!< Local inertia tensor of the body (in local-space) with respect to the center of mass of the body
+	protected Matrix3f inertiaTensorLocalInverse; //!< Inverse of the inertia tensor of the body
+	protected float massInverse; //!< Inverse of the mass of the body
+	protected boolean isGravityEnabled; //!< True if the gravity needs to be applied to this rigid body
+	protected Material material; //!< Material properties of the rigid body
+	protected float linearDamping; //!< Linear velocity damping factor
+	protected float angularDamping; //!< Angular velocity damping factor
+	protected JointListElement jointsList; //!< First element of the linked list of joints involving this body
+	/**
+	 * @brief Remove a joint from the joints list
+	 */
+	protected void removeJointFrom_jointsList( Joint joint) {
+		assert(joint != null);
+		assert(this.jointsList != null);
+		// Remove the joint from the linked list of the joints of the first body
+		if (this.jointsList.joint == joint) {   // If the first element is the one to remove
+			JointListElement* elementToRemove = this.jointsList;
+			this.jointsList = elementToRemove.next;
+			ETKDELETE(JointListElement, elementToRemove);
+			elementToRemove = null;
+		}
+		else {  // If the element to remove is not the first one in the list
+			JointListElement* currentElement = this.jointsList;
+			while (currentElement.next != null) {
+				if (currentElement.next.joint == joint) {
+					JointListElement* elementToRemove = currentElement.next;
+					currentElement.next = elementToRemove.next;
+					ETKDELETE(JointListElement, elementToRemove);
+					elementToRemove = null;
+					break;
+				}
+				currentElement = currentElement.next;
+			}
+		}
+	}
+	/**
+	 * @brief Update the transform of the body after a change of the center of mass
+	 */
+	protected void updateTransformWithCenterOfMass() {
+		// Translate the body according to the translation of the center of mass position
+		this.transform.setPosition(this.centerOfMassWorld - this.transform.getOrientation() * this.centerOfMassLocal);
+		if (isnan(this.transform.getPosition().x()) == true) {
+			Log.critical("updateTransformWithCenterOfMass: " + this.transform);
+		}
+		if (isinf(this.transform.getOrientation().z()) == true) {
+			Log.critical("         set transform: " + this.transform);
+		}
+	}
+	@Override
+	protected void updateBroadPhaseState() {
+		PROFILE("RigidBody::updateBroadPhaseState()");
+		DynamicsWorld world = staticcast<DynamicsWorld>(this.world);
+		 Vector3f displacement = world.timeStep * this.linearVelocity;
+		// For all the proxy collision shapes of the body
+		for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape.this.next) {
+			// Recompute the world-space AABB of the collision shape
+			AABB aabb;
+			Log.verbose("         : " + aabb.getMin() + " " + aabb.getMax());
+			Log.verbose("         this.transform: " + this.transform);
+			shape.getCollisionShape().computeAABB(aabb, this.transform *shape.getLocalToBodyTransform());
+			Log.verbose("         : " + aabb.getMin() + " " + aabb.getMax());
+			// Update the broad-phase state for the proxy collision shape
+			this.world.collisionDetection.updateProxyCollisionShape(shape, aabb, displacement);
+		}
+	}
+	/**
+	 * @brief 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
+	 */
+	public RigidBody( Transform3D transform, CollisionWorld world, long id) {
+		super(transform, world, id);
+		this.initMass = 1.0f;
+		this.centerOfMassLocal = new Vector3f(0, 0, 0;
+		this.centerOfMassWorld = transform.getPosition().clone();
+		this.isGravityEnabled = true;
+		this.linearDamping = 0.0f;
+		this.angularDamping = 0.0f;
+		this.jointsList = null;
+		// Compute the inverse mass
+		this.massInverse = 1.0f / this.initMass;
+	}
+	@Override
+	public void setType(BodyType type) {
+		if (this.type == type) {
+			return;
+		}
+		super.setType(type);
+		recomputeMassInformation();
+		if (this.type == STATIC) {
+			// Reset the velocity to zero
+			this.linearVelocity.setZero();
+			this.angularVelocity.setZero();
+		}
+		if (    this.type == STATIC
+		     || this.type == KINEMATIC) {
+			// Reset the inverse mass and inverse inertia tensor to zero
+			this.massInverse = 0.0f;
+			this.inertiaTensorLocal.setZero();
+			this.inertiaTensorLocalInverse.setZero();
+		} else {
+			this.massInverse = 1.0f / this.initMass;
+			this.inertiaTensorLocalInverse = this.inertiaTensorLocal.getInverse();
+		}
+		setIsSleeping(false);
+		resetContactManifoldsList();
+		// Ask the broad-phase to test again the collision shapes of the body for collision detection (as if the body has moved)
+		askForBroadPhaseCollisionCheck();
+		this.externalForce.setZero();
+		this.externalTorque.setZero();
+	}
+	/**
+	 * @brief Set the current position and orientation
+	 * @param[in] transform The transformation of the body that transforms the local-space of the body into world-space
+	 */
+	public void setTransform( Transform3D transform) {
+		Log.debug("         set transform: " + this.transform + " ==> " + transform);
+		if (isnan(transform.getPosition().x()) == true) {
+			Log.critical("         set transform: " + this.transform + " ==> " + transform);
+		}
+		if (isinf(transform.getOrientation().z()) == true) {
+			Log.critical("         set transform: " + this.transform + " ==> " + transform);
+		}
+		this.transform = transform;
+		 Vector3f oldCenterOfMass = this.centerOfMassWorld;
+		// Compute the new center of mass in world-space coordinates
+		this.centerOfMassWorld = this.transform * this.centerOfMassLocal;
+		// Update the linear velocity of the center of mass
+		this.linearVelocity += this.angularVelocity.cross(this.centerOfMassWorld - oldCenterOfMass);
+		updateBroadPhaseState();
+	}
+	/**
+	 * @brief Get the mass of the body
+	 * @return The mass (in kilograms) of the body
+	 */
+	public float getMass()  {
+		return this.initMass;
+	}
+	/**
+	 * @brief Get the linear velocity
+	 * @return The linear velocity vector of the body
+	 */
+	public Vector3f getLinearVelocity()  {
+		return this.linearVelocity;
+	}
+	/**
+	 * @brief Set the linear velocity of the rigid body.
+	 * @param[in] linearVelocity Linear velocity vector of the body
+	 */
+	public void setLinearVelocity( Vector3f linearVelocity) {
+		if (this.type == STATIC) {
+			return;
+		}
+		this.linearVelocity = linearVelocity;
+		if (this.linearVelocity.length2() > 0.0f) {
+			setIsSleeping(false);
+		}
+	}
+	/**
+	 * @brief Get the angular velocity of the body
+	 * @return The angular velocity vector of the body
+	 */
+	public Vector3f getAngularVelocity()  {
+		return this.angularVelocity;
+	}
+	/**
+	* @brief Set the angular velocity.
+	* @param[in] angularVelocity The angular velocity vector of the body
+	*/
+	public void setAngularVelocity( Vector3f angularVelocity) {
+		if (this.type == STATIC) {
+			return;
+		}
+		this.angularVelocity = angularVelocity;
+		if (this.angularVelocity.length2() > 0.0f) {
+			setIsSleeping(false);
+		}
+	}
+	/**
+	 * @brief Set the variable to know whether or not the body is sleeping
+	 * @param[in] isSleeping New sleeping state of the body
+	 */
+	public  void setIsSleeping(boolean isSleeping) {
+		if (isSleeping) {
+			this.linearVelocity.setZero();
+			this.angularVelocity.setZero();
+			this.externalForce.setZero();
+			this.externalTorque.setZero();
+		}
+		Body::setIsSleeping(isSleeping);
+	}
+	/**
+	 * @brief Get the local inertia tensor of the body (in local-space coordinates)
+	 * @return The 3x3 inertia tensor matrix of the body
+	 */
+	public Matrix3f getInertiaTensorLocal()  {
+		return this.inertiaTensorLocal;
+	}
+	/**
+	 * @brief Set the local inertia tensor of the body (in local-space coordinates)
+	 * @param[in] inertiaTensorLocal The 3x3 inertia tensor matrix of the body in local-space coordinates
+	 */
+	public void setInertiaTensorLocal( Matrix3f inertiaTensorLocal) {
+		if (this.type != DYNAMIC) {
+			return;
+		}
+		this.inertiaTensorLocal = inertiaTensorLocal;
+		this.inertiaTensorLocalInverse = this.inertiaTensorLocal.getInverse();
+	}
+	/**
+	 * @brief Set the local center of mass of the body (in local-space coordinates)
+	 * @param[in] centerOfMassLocal The center of mass of the body in local-space coordinates
+	 */
+	public void setCenterOfMassLocal( Vector3f centerOfMassLocal) {
+		if (this.type != DYNAMIC) {
+			return;
+		}
+		 Vector3f oldCenterOfMass = this.centerOfMassWorld;
+		this.centerOfMassLocal = centerOfMassLocal;
+		this.centerOfMassWorld = this.transform * this.centerOfMassLocal;
+		this.linearVelocity += this.angularVelocity.cross(this.centerOfMassWorld - oldCenterOfMass);
+	}
+	/**
+	 * @brief Set the mass of the rigid body
+	 * @param[in] mass The mass (in kilograms) of the body
+	 */
+	public void setMass(float mass) {
+		if (this.type != DYNAMIC) {
+			return;
+		}
+		this.initMass = mass;
+		if (this.initMass > 0.0f) {
+			this.massInverse = 1.0f / this.initMass;
+		} else {
+			this.initMass = 1.0f;
+			this.massInverse = 1.0f;
+		}
+	}
+	/**
+	 * @brief Get the inertia tensor in world coordinates.
+	 * The inertia tensor Iw in world coordinates is computed
+	 * with the local inertia tensor Ib in body coordinates
+	 * by Iw = R * Ib * 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
+	 */
+	public Matrix3f getInertiaTensorWorld()  {
+		// Compute and return the inertia tensor in world coordinates
+		return this.transform.getOrientation().getMatrix() * this.inertiaTensorLocal *
+		       this.transform.getOrientation().getMatrix().getTranspose();
+	}
+	/**
+	 * @brief Get the inverse of the inertia tensor in world coordinates.
+	 * The inertia tensor Iw in world coordinates is computed with the
+	 * local inverse inertia tensor Ib^-1 in body coordinates
+	 * by Iw = R * Ib^-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
+	 */
+	public Matrix3f getInertiaTensorInverseWorld()  {
+		// 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 this.transform.getOrientation().getMatrix() * this.inertiaTensorLocalInverse *
+		       this.transform.getOrientation().getMatrix().getTranspose();
+	}
+	/**
+	 * @brief get the need of gravity appling to this rigid body
+	 * @return True if the gravity is applied to the body
+	 */
+	public boolean isGravityEnabled()  {
+		return this.isGravityEnabled;
+	}
+	/**
+	 * @brief Set the variable to know if the gravity is applied to this rigid body
+	 * @param[in] isEnabled True if you want the gravity to be applied to this body
+	 */
+	public void enableGravity(boolean isEnabled) {
+		this.isGravityEnabled = isEnabled;
+	}
+	/**
+	 * @brief get a reference to the material properties of the rigid body
+	 * @return A reference to the material of the body
+	 */
+	public Material getMaterial() {
+		return this.material;
+	}
+	/**
+	 * @brief Set a new material for this rigid body
+	 * @param[in] material The material you want to set to the body
+	 */
+	public void setMaterial( Material material) {
+		this.material = material;
+	}
+	/**
+	 * @brief Get the linear velocity damping factor
+	 * @return The linear damping factor of this body
+	 */
+	public float getLinearDamping()  {
+		return this.linearDamping;
+	}
+	/**
+	 * @brief Set the linear damping factor. This is the ratio of the linear velocity that the body will lose every at seconds of simulation.
+	 * @param[in] linearDamping The linear damping factor of this body
+	 */
+	public void setLinearDamping(float linearDamping) {
+		assert(linearDamping >= 0.0f);
+		this.linearDamping = linearDamping;
+	}
+	/**
+	 * @brief Get the angular velocity damping factor
+	 * @return The angular damping factor of this body
+	 */
+	public float getAngularDamping()  {
+		return this.angularDamping;
+	}
+	/**
+	 * @brief Set the angular damping factor. This is the ratio of the angular velocity that the body will lose at every seconds of simulation.
+	 * @param[in] angularDamping The angular damping factor of this body
+	 */
+	public void setAngularDamping(float angularDamping) {
+		assert(angularDamping >= 0.0f);
+		this.angularDamping = angularDamping;
+	}
+	/**
+	 * @brief Get 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
+	 */
+	public JointListElement* getJointsList()  {
+		return this.jointsList;
+	}
+	/**
+	 * @brief Get 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
+	 */
+	public JointListElement* getJointsList() {
+		return this.jointsList;
+	}
+	/**
+	 * @brief 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[in] force The external force to apply on the center of mass of the body
+	 */
+	public void applyForceToCenterOfMass( Vector3f force) {
+		if (this.type != DYNAMIC) {
+			return;
+		}
+		if (this.isSleeping) {
+			setIsSleeping(false);
+		}
+		this.externalForce += force;
+	}
+	/**
+	 * @brief 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[in] force The force to apply on the body
+	 * @param[in] point The point where the force is applied (in world-space coordinates)
+	 */
+	public void applyForce( Vector3f force,  Vector3f point) {
+		if (this.type != DYNAMIC) {
+			return;
+		}
+		if (this.isSleeping) {
+			setIsSleeping(false);
+		}
+		this.externalForce += force;
+		this.externalTorque += (point - this.centerOfMassWorld).cross(force);
+	}
+	/**
+	 * @brief 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[in] torque The external torque to apply on the body
+	 */
+	public void applyTorque( Vector3f torque) {
+		if (this.type != DYNAMIC) {
+			return;
+		}
+		if (this.isSleeping) {
+			setIsSleeping(false);
+		}
+		this.externalTorque += torque;
+	}
+	/**
+	 * @brief Add a collision shape to the body.
+	 * When you add a collision shape to the body, an intternal 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[in] collisionShape The collision shape you want to add to the body
+	 * @param[in] transform The transformation of the collision shape that transforms the local-space of the collision shape into the local-space of the body
+	 * @param[in] 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.
+	 */
+	public ProxyShape addCollisionShape(CollisionShape collisionShape,
+	                               Transform3D transform,
+	                              float mass) {
+		assert(mass > 0.0f);
+		// Create a new proxy collision shape to attach the collision shape to the body
+		ProxyShape* proxyShape = ETKNEW(ProxyShape, this, collisionShape, transform, mass);
+		// Add it to the list of proxy collision shapes of the body
+		if (this.proxyCollisionShapes == null) {
+			this.proxyCollisionShapes = proxyShape;
+		} else {
+			proxyShape.this.next = this.proxyCollisionShapes;
+			this.proxyCollisionShapes = proxyShape;
+		}
+		// Compute the world-space AABB of the new collision shape
+		AABB aabb;
+		collisionShape.computeAABB(aabb, this.transform * transform);
+		// Notify the collision detection about this new collision shape
+		this.world.collisionDetection.addProxyCollisionShape(proxyShape, aabb);
+		this.numberCollisionShapes++;
+		recomputeMassInformation();
+		return proxyShape;
+	}
+	public void removeCollisionShape(ProxyShape proxyShape) {
+		CollisionBody::removeCollisionShape(proxyShape);
+		recomputeMassInformation();
+	}
+
+	/**
+	 * @brief Recompute the center of mass, total mass and inertia tensor of the body using all the collision shapes attached to the body.
+	 */
+	public void recomputeMassInformation() {
+		this.initMass = 0.0f;
+		this.massInverse = 0.0f;
+		this.inertiaTensorLocal.setZero();
+		this.inertiaTensorLocalInverse.setZero();
+		this.centerOfMassLocal.setZero();
+		// If it is STATIC or KINEMATIC body
+		if (this.type == STATIC || this.type == KINEMATIC) {
+			this.centerOfMassWorld = this.transform.getPosition();
+			return;
+		}
+		assert(this.type == DYNAMIC);
+		// Compute the total mass of the body
+		for (ProxyShape* shape = this.proxyCollisionShapes; shape != NULL; shape = shape.this.next) {
+			this.initMass += shape.getMass();
+			this.centerOfMassLocal += shape.getLocalToBodyTransform().getPosition() * shape.getMass();
+		}
+		if (this.initMass > 0.0f) {
+			this.massInverse = 1.0f / this.initMass;
+		} else {
+			this.initMass = 1.0f;
+			this.massInverse = 1.0f;
+		}
+		// Compute the center of mass
+		 Vector3f oldCenterOfMass = this.centerOfMassWorld;
+		this.centerOfMassLocal *= this.massInverse;
+		this.centerOfMassWorld = this.transform * this.centerOfMassLocal;
+		// Compute the total mass and inertia tensor using all the collision shapes
+		for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape.this.next) {
+			// Get the inertia tensor of the collision shape in its local-space
+			Matrix3f inertiaTensor;
+			shape.getCollisionShape().computeLocalInertiaTensor(inertiaTensor, shape.getMass());
+			// Convert the collision shape inertia tensor into the local-space of the body
+			 Transform3D shapeTransform = shape.getLocalToBodyTransform();
+			Matrix3f 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.
+			Vector3f offset = shapeTransform.getPosition() - this.centerOfMassLocal;
+			float offsetSquare = offset.length2();
+			Vector3f off1 = offset * (-offset.x());
+			Vector3f off2 = offset * (-offset.y());
+			Vector3f off3 = offset * (-offset.z());
+			Matrix3f offsetMatrix(off1.x()+offsetSquare, off1.y(),              off1.z(),
+			                            off2.x(),              off2.y()+offsetSquare, off2.z(),
+			                            off3.x(),              off3.y(),              off3.z()+offsetSquare);
+			offsetMatrix *= shape.getMass();
+			this.inertiaTensorLocal += inertiaTensor + offsetMatrix;
+		}
+		// Compute the local inverse inertia tensor
+		this.inertiaTensorLocalInverse = this.inertiaTensorLocal.getInverse();
+		// Update the linear velocity of the center of mass
+		this.linearVelocity += this.angularVelocity.cross(this.centerOfMassWorld - oldCenterOfMass);
+	}
+
+}
+

src/org/atriaSoft/ephysics/collision/CollisionDetection.cpp

@@ -0,0 +1,458 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/CollisionDetection.hpp>
+#include <ephysics/engine/CollisionWorld.hpp>
+#include <ephysics/body/Body.hpp>
+#include <ephysics/collision/shapes/BoxShape.hpp>
+#include <ephysics/body/RigidBody.hpp>
+#include <ephysics/configuration.hpp>
+
+// We want to use the ReactPhysics3D namespace
+using namespace ephysics;
+using namespace std;
+
+// Constructor
+CollisionDetection::CollisionDetection(CollisionWorld* world):
+  this.world(world),
+  this.broadPhaseAlgorithm(*this),
+  this.isCollisionShapesAdded(false) {
+	// Set the default collision dispatch configuration
+	setCollisionDispatch(this.defaultCollisionDispatch);
+	// Fill-in the collision detection matrix with algorithms
+	fillInCollisionMatrix();
+}
+
+CollisionDetection::~CollisionDetection() {
+	
+}
+
+void CollisionDetection::computeCollisionDetection() {
+	PROFILE("CollisionDetection::computeCollisionDetection()");
+	// Compute the broad-phase collision detection
+	computeBroadPhase();
+	// Compute the narrow-phase collision detection
+	computeNarrowPhase();
+}
+
+void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback,  Set<int> shapes1,  Set<int> 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);
+}
+
+void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback,  Set<int> shapes1,  Set<int> shapes2) {
+	// For each possible collision pair of bodies
+	Map<overlappingpairid, OverlappingPair*>::Iterator it;
+	for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ++it) {
+		OverlappingPair* pair = it.second;
+		 ProxyShape* shape1 = pair.getShape1();
+		 ProxyShape* shape2 = pair.getShape2();
+		assert(shape1.this.broadPhaseID != shape2.this.broadPhaseID);
+		// 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.this.broadPhaseID) == 0
+		          || shapes2.count(shape2.this.broadPhaseID) == 0 )
+		     && (    shapes1.count(shape2.this.broadPhaseID) == 0
+		          || shapes2.count(shape1.this.broadPhaseID) == 0 ) ) {
+			continue;
+		}
+		if (    !shapes1.empty()
+		     && shapes2.empty()
+		     && shapes1.count(shape1.this.broadPhaseID) == 0
+		     && shapes1.count(shape2.this.broadPhaseID) == 0) {
+			continue;
+		}
+		if (    !shapes2.empty()
+		     && shapes1.empty()
+		     && shapes2.count(shape1.this.broadPhaseID) == 0
+		     && shapes2.count(shape2.this.broadPhaseID) == 0) {
+			continue;
+		}
+		// For each contact manifold set of the overlapping pair
+		 ContactManifoldSet manifoldSet = pair.getContactManifoldSet();
+		for (int j=0; j<manifoldSet.getNbContactManifolds(); j++) {
+			 ContactManifold* manifold = manifoldSet.getContactManifold(j);
+			// For each contact manifold of the manifold set
+			for (int 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);
+				}
+			}
+		}
+	}
+}
+
+void CollisionDetection::computeBroadPhase() {
+	PROFILE("CollisionDetection::computeBroadPhase()");
+	// If new collision shapes have been added to bodies
+	if (this.isCollisionShapesAdded) {
+		// Ask the broad-phase to recompute the overlapping pairs of collision
+		// shapes. This call can only add new overlapping pairs in the collision
+		// detection.
+		this.broadPhaseAlgorithm.computeOverlappingPairs();
+	}
+}
+
+void CollisionDetection::computeNarrowPhase() {
+	PROFILE("CollisionDetection::computeNarrowPhase()");
+	// Clear the set of overlapping pairs in narrow-phase contact
+	this.contactOverlappingPairs.clear();
+	// For each possible collision pair of bodies
+	Map<overlappingpairid, OverlappingPair*>::Iterator it;
+	for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ) {
+		OverlappingPair* pair = it.second;
+		ProxyShape* shape1 = pair.getShape1();
+		ProxyShape* shape2 = pair.getShape2();
+		assert(shape1.this.broadPhaseID != shape2.this.broadPhaseID);
+		// 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) ||
+			 !this.broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
+			// TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
+			// Destroy the overlapping pair
+			ETKDELETE(OverlappingPair, it.second);
+			it.second = null;
+			it = this.overlappingPairs.erase(it);
+			continue;
+		} else {
+			++it;
+		}
+		CollisionBody*  body1 = shape1.getBody();
+		CollisionBody*  body2 = shape2.getBody();
+		// Update the contact cache of the overlapping pair
+		pair.update();
+		// Check that at least one body is awake and not static
+		boolean isBody1Active = !body1.isSleeping() && body1.getType() != STATIC;
+		boolean 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
+		longpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
+		if (this.noCollisionPairs.count(bodiesIndex) > 0) {
+			continue;
+		}
+		// Select the narrow phase algorithm to use according to the two collision shapes
+		 CollisionShapeType shape1Type = shape1.getCollisionShape().getType();
+		 CollisionShapeType shape2Type = shape2.getCollisionShape().getType();
+		NarrowPhaseAlgorithm* narrowPhaseAlgorithm = this.collisionMatrix[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();
+}
+
+void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback,  Set<int> shapes1,  Set<int> shapes2) {
+	this.contactOverlappingPairs.clear();
+	// For each possible collision pair of bodies
+	Map<overlappingpairid, OverlappingPair*>::Iterator it;
+	for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ) {
+		OverlappingPair* pair = it.second;
+		ProxyShape* shape1 = pair.getShape1();
+		ProxyShape* shape2 = pair.getShape2();
+		assert(shape1.this.broadPhaseID != shape2.this.broadPhaseID);
+		// 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.this.broadPhaseID) == 0
+		          || shapes2.count(shape2.this.broadPhaseID) == 0 )
+		     && (    shapes1.count(shape2.this.broadPhaseID) == 0
+		          || shapes2.count(shape1.this.broadPhaseID) == 0 ) ) {
+			++it;
+			continue;
+		}
+		if (    !shapes1.empty()
+		     && shapes2.empty()
+		     && shapes1.count(shape1.this.broadPhaseID) == 0
+		     && shapes1.count(shape2.this.broadPhaseID) == 0) {
+			++it;
+			continue;
+		}
+		if (    !shapes2.empty()
+		     && shapes1.empty()
+		     && shapes2.count(shape1.this.broadPhaseID) == 0
+		     && shapes2.count(shape2.this.broadPhaseID) == 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 )
+		     || !this.broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2) ) {
+			// TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
+			// Destroy the overlapping pair
+			ETKDELETE(OverlappingPair, it.second);
+			it.second = null;
+			it = this.overlappingPairs.erase(it);
+			continue;
+		} else {
+			++it;
+		}
+		CollisionBody*  body1 = shape1.getBody();
+		CollisionBody*  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;
+		}
+		longpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
+		if (this.noCollisionPairs.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
+		 CollisionShapeType shape1Type = shape1.getCollisionShape().getType();
+		 CollisionShapeType shape2Type = shape2.getCollisionShape().getType();
+		NarrowPhaseAlgorithm* narrowPhaseAlgorithm = this.collisionMatrix[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();
+}
+
+void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) {
+	assert(shape1.this.broadPhaseID != shape2.this.broadPhaseID);
+	// 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 (this.overlappingPairs.find(pairID) != this.overlappingPairs.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 = ETKNEW(OverlappingPair, shape1, shape2, nbMaxManifolds);
+	assert(newPair != null);
+	this.overlappingPairs.set(pairID, newPair);
+	// Wake up the two bodies
+	shape1.getBody().setIsSleeping(false);
+	shape2.getBody().setIsSleeping(false);
+}
+
+void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
+	// Remove all the overlapping pairs involving this proxy shape
+	Map<overlappingpairid, OverlappingPair*>::Iterator it;
+	for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ) {
+		if (it.second.getShape1().this.broadPhaseID == proxyShape.this.broadPhaseID||
+			it.second.getShape2().this.broadPhaseID == proxyShape.this.broadPhaseID) {
+			// TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
+			// Destroy the overlapping pair
+			ETKDELETE(OverlappingPair, it.second);
+			it.second = null;
+			it = this.overlappingPairs.erase(it);
+		} else {
+			++it;
+		}
+	}
+	// Remove the body from the broad-phase
+	this.broadPhaseAlgorithm.removeProxyCollisionShape(proxyShape);
+}
+
+void CollisionDetection::notifyContact(OverlappingPair* overlappingPair,  ContactPointInfo contactInfo) {
+	// If it is the first contact since the pairs are overlapping
+	if (overlappingPair.getNbContactPoints() == 0) {
+		// Trigger a callback event
+		if (this.world.this.eventListener != NULL) {
+			this.world.this.eventListener.beginContact(contactInfo);
+		}
+	}
+	// Create a new contact
+	createContact(overlappingPair, contactInfo);
+	// Trigger a callback event for the new contact
+	if (this.world.this.eventListener != NULL) {
+		this.world.this.eventListener.newContact(contactInfo);
+	}
+}
+
+void CollisionDetection::createContact(OverlappingPair* overlappingPair,  ContactPointInfo contactInfo) {
+	// Create a new contact
+	ContactPoint* contact = ETKNEW(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());
+	this.contactOverlappingPairs.set(pairId, overlappingPair);
+}
+
+void CollisionDetection::addAllContactManifoldsToBodies() {
+	// For each overlapping pairs in contact during the narrow-phase
+	Map<overlappingpairid, OverlappingPair*>::Iterator it;
+	for (it = this.contactOverlappingPairs.begin(); it != this.contactOverlappingPairs.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);
+	}
+}
+
+void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
+	assert(pair != null);
+	CollisionBody* body1 = pair.getShape1().getBody();
+	CollisionBody* body2 = pair.getShape2().getBody();
+	 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
+		body1.this.contactManifoldsList = ETKNEW(ContactManifoldListElement, contactManifold, body1.this.contactManifoldsList);;
+		// Add the contact manifold at the beginning of the linked
+		// list of the contact manifolds of the second body
+		body2.this.contactManifoldsList = ETKNEW(ContactManifoldListElement, contactManifold, body2.this.contactManifoldsList);;
+	}
+}
+
+void CollisionDetection::clearContactPoints() {
+	// For each overlapping pair
+	Map<overlappingpairid, OverlappingPair*>::Iterator it;
+	for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ++it) {
+		it.second.clearContactPoints();
+	}
+}
+
+void CollisionDetection::fillInCollisionMatrix() {
+	// For each possible type of collision shape
+	for (int i=0; i<NBCOLLISIONSHAPETYPES; i++) {
+		for (int j=0; j<NBCOLLISIONSHAPETYPES; j++) {
+			this.collisionMatrix[i][j] = this.collisionDispatch.selectAlgorithm(i, j);
+		}
+	}
+}
+
+EventListener* CollisionDetection::getWorldEventListener() {
+   return this.world.this.eventListener;
+}
+
+void TestCollisionBetweenShapesCallback::notifyContact(OverlappingPair* overlappingPair,  ContactPointInfo contactInfo) {
+	this.collisionCallback.notifyContact(contactInfo);
+}
+
+NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(CollisionShapeType shape1Type, CollisionShapeType shape2Type)  {
+	return this.collisionMatrix[shape1Type][shape2Type];
+}
+
+void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisionDispatch) {
+	this.collisionDispatch = collisionDispatch;
+	this.collisionDispatch.init(this);
+	// Fill-in the collision matrix with the new algorithms to use
+	fillInCollisionMatrix();
+}
+
+void CollisionDetection::addProxyCollisionShape(ProxyShape* proxyShape,  AABB aabb) {
+	// Add the body to the broad-phase
+	this.broadPhaseAlgorithm.addProxyCollisionShape(proxyShape, aabb);
+	this.isCollisionShapesAdded = true;
+}
+
+void CollisionDetection::addNoCollisionPair(CollisionBody* body1, CollisionBody* body2) {
+	this.noCollisionPairs.set(OverlappingPair::computeBodiesIndexPair(body1, body2));
+}
+
+void CollisionDetection::removeNoCollisionPair(CollisionBody* body1, CollisionBody* body2) {
+	this.noCollisionPairs.erase(this.noCollisionPairs.find(OverlappingPair::computeBodiesIndexPair(body1, body2)));
+}
+
+void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape) {
+	this.broadPhaseAlgorithm.addMovedCollisionShape(shape.this.broadPhaseID);
+}
+
+void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape,  AABB aabb,  Vector3f displacement, boolean forceReinsert) {
+	this.broadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement);
+}
+
+void CollisionDetection::raycast(RaycastCallback* raycastCallback,  Ray ray, int raycastWithCategoryMaskBits)  {
+	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
+	this.broadPhaseAlgorithm.raycast(ray, rayCastTest, raycastWithCategoryMaskBits);
+}
+
+boolean CollisionDetection::testAABBOverlap( ProxyShape* shape1,  ProxyShape* shape2)  {
+	// If one of the shape's body is not active, we return no overlap
+	if (    !shape1.getBody().isActive()
+	     || !shape2.getBody().isActive()) {
+		return false;
+	}
+	return this.broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2);
+}
+
+CollisionWorld* CollisionDetection::getWorld() {
+	return this.world;
+}
+
+

src/org/atriaSoft/ephysics/collision/CollisionDetection.java

@@ -0,0 +1,114 @@
+package org.atriaSoft.ephysics.collision;
+
+	
+	class TestCollisionBetweenShapesCallback extends  NarrowPhaseCallback {
+		private:
+			CollisionCallback* collisionCallback; //!<
+		public:
+			// Constructor
+			TestCollisionBetweenShapesCallback(CollisionCallback* callback):
+			  this.collisionCallback(callback) {
+				
+			}
+			// Called by a narrow-phase collision algorithm when a new contact has been found
+			 void notifyContact(OverlappingPair* overlappingPair,
+			                            ContactPointInfo contactInfo);
+	};
+	
+	/**
+	 * @brief It 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 extends  NarrowPhaseCallback {
+		private :
+			CollisionDispatch* collisionDispatch; //!< Collision Detection Dispatch configuration
+			DefaultCollisionDispatch defaultCollisionDispatch; //!< Default collision dispatch configuration
+			NarrowPhaseAlgorithm* collisionMatrix[NBCOLLISIONSHAPETYPES][NBCOLLISIONSHAPETYPES]; //!< Collision detection matrix (algorithms to use)
+			CollisionWorld* world; //!< Pointer to the physics world
+			Map<overlappingpairid, OverlappingPair*> overlappingPairs; //!< Broad-phase overlapping pairs
+			Map<overlappingpairid, OverlappingPair*> contactOverlappingPairs; //!< Overlapping pairs in contact (during the current Narrow-phase collision detection)
+			BroadPhaseAlgorithm broadPhaseAlgorithm; //!< Broad-phase algorithm
+			// TODO : Delete this
+			GJKAlgorithm narrowPhaseGJKAlgorithm; //!< Narrow-phase GJK algorithm
+			Set<longpair> noCollisionPairs; //!< Set of pair of bodies that cannot collide between each other
+			boolean isCollisionShapesAdded; //!< True if some collision shapes have been added previously
+			/// 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 :
+			/// Constructor
+			CollisionDetection(CollisionWorld* world);
+			/// 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) ;
+			/// Add a proxy collision shape to the collision detection
+			void addProxyCollisionShape(ProxyShape* proxyShape,  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,
+			                                AABB aabb,
+			                                Vector3f displacement = Vector3f(0, 0, 0),
+			                               boolean 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.
+			/// 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.
+			void askForBroadPhaseCollisionCheck(ProxyShape* shape);
+			/// Compute the collision detection
+			void computeCollisionDetection();
+			/// Compute the collision detection
+			void testCollisionBetweenShapes(CollisionCallback* callback,
+			                                 Set<int> shapes1,
+			                                 Set<int> shapes2);
+			/// Report collision between two sets of shapes
+			void reportCollisionBetweenShapes(CollisionCallback* callback,
+			                                   Set<int> shapes1,
+			                                   Set<int> shapes2) ;
+			/// Ray casting method
+			void raycast(RaycastCallback* raycastCallback,
+			              Ray ray,
+			             int raycastWithCategoryMaskBits) ;
+			/// Test if the AABBs of two bodies overlap
+			boolean testAABBOverlap( CollisionBody* body1,
+			                      CollisionBody* body2) ;
+			/// Test if the AABBs of two proxy shapes overlap
+			boolean testAABBOverlap( ProxyShape* shape1,
+			                      ProxyShape* shape2) ;
+			/// Allow the broadphase to notify the collision detection about an overlapping pair.
+			/// This method is called by the broad-phase collision detection algorithm
+			void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
+			/// Compute the narrow-phase collision detection
+			void computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
+			                                      Set<int> shapes1,
+			                                      Set<int> shapes2);
+			/// Return a pointer to the world
+			CollisionWorld* getWorld();
+			/// Return the world event listener
+			EventListener* getWorldEventListener();
+			/// Called by a narrow-phase collision algorithm when a new contact has been found
+			 void notifyContact(OverlappingPair* overlappingPair,  ContactPointInfo contactInfo) ;
+			/// Create a new contact
+			void createContact(OverlappingPair* overlappingPair,  ContactPointInfo contactInfo);
+			friend class DynamicsWorld;
+			friend class ConvexMeshShape;
+	};
+
+}

src/org/atriaSoft/ephysics/collision/CollisionShapeInfo.hpp

@@ -0,0 +1,41 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#pragma once
+
+#include <ephysics/collision/shapes/CollisionShape.hpp>
+
+namespace ephysics {
+	/**
+	 * @brief It regroups different things about a collision shape. This is
+	 * used to pass information about a collision shape to a collision algorithm.
+	 */
+	struct CollisionShapeInfo {
+		public:
+			OverlappingPair* overlappingPair; //!< Broadphase overlapping pair
+			ProxyShape* proxyShape; //!< Proxy shape
+			 CollisionShape* collisionShape; //!< Pointer to the collision shape
+			Transform3D shapeToWorldTransform; //!< Transform3D that maps from collision shape local-space to world-space
+			void** cachedCollisionData; //!< Cached collision data of the proxy shape
+			/// Constructor
+			CollisionShapeInfo(ProxyShape* proxyCollisionShape,
+			                    CollisionShape* shape,
+			                    Transform3D shapeLocalToWorldTransform,
+			                   OverlappingPair* pair,
+			                   void** cachedData):
+			  overlappingPair(pair),
+			  proxyShape(proxyCollisionShape),
+			  collisionShape(shape),
+			  shapeToWorldTransform(shapeLocalToWorldTransform),
+			  cachedCollisionData(cachedData) {
+				
+			}
+	};
+}
+
+

src/org/atriaSoft/ephysics/collision/ContactManifold.hpp

@@ -0,0 +1,154 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#pragma once
+
+#include <etk/Vector.hpp>
+#include <ephysics/body/CollisionBody.hpp>
+#include <ephysics/collision/ProxyShape.hpp>
+#include <ephysics/raint/ContactPoint.hpp>
+
+namespace ephysics {
+
+	 int MAXCONTACTPOINTSINMANIFOLD = 4; //!< Maximum number of contacts in the manifold
+
+	/**
+	 * @brief This structure represents a single element of a linked list of contact manifolds
+	 */
+	struct ContactManifoldListElement {
+		public:
+			ContactManifold* contactManifold; //!< Pointer to the actual contact manifold
+			ContactManifoldListElement* next; //!< Next element of the list
+			ContactManifoldListElement(ContactManifold* initContactManifold,
+			                           ContactManifoldListElement* initNext):
+			  contactManifold(initContactManifold),
+			  next(initNext) {
+				
+			}
+	};
+	
+	/**
+	 * @brief 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/GDC10CoumansErwinContact.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 {
+		public:
+			/// Constructor
+			ContactManifold(ProxyShape* shape1,
+			                ProxyShape* shape2,
+			                int16t normalDirectionId);
+		private:
+			ProxyShape* shape1; //!< Pointer to the first proxy shape of the contact
+			ProxyShape* shape2; //!< Pointer to the second proxy shape of the contact
+			ContactPoint* contactPoints[MAXCONTACTPOINTSINMANIFOLD]; //!< Contact points in the manifold
+			int16t normalDirectionId; //!< Normal direction Id (Unique Id representing the normal direction)
+			int nbContactPoints; //!< Number of contacts in the cache
+			Vector3f frictionVector1; //!< First friction vector of the contact manifold
+			Vector3f frictionvec2; //!< Second friction vector of the contact manifold
+			float frictionImpulse1; //!< First friction raint accumulated impulse
+			float frictionImpulse2; //!< Second friction raint accumulated impulse
+			float frictionTwistImpulse; //!< Twist friction raint accumulated impulse
+			Vector3f rollingResistanceImpulse; //!< Accumulated rolling resistance impulse
+			boolean isAlreadyInIsland; //!< True if the contact manifold has already been added into an island
+			/// Return the index of maximum area
+			int getMaxArea(float area0, float area1, float area2, float area3) ;
+			/**
+			 * @brief Return the index of the contact 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 getIndexOfDeepestPenetration(ContactPoint* newContact) ;
+			/**
+			 * @brief 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 getIndexToRemove(int indexMaxPenetration,  Vector3f newPoint) ;
+			/// Remove a contact point from the manifold
+			void removeContactPoint(int index);
+			/// Return true if the contact manifold has already been added into an island
+			boolean isAlreadyInIsland() ;
+		public:
+			/// Return a pointer to the first proxy shape of the contact
+			ProxyShape* getShape1() ;
+			/// Return a pointer to the second proxy shape of the contact
+			ProxyShape* getShape2() ;
+			/// Return a pointer to the first body of the contact manifold
+			CollisionBody* getBody1() ;
+			/// Return a pointer to the second body of the contact manifold
+			CollisionBody* getBody2() ;
+			/// Return the normal direction Id
+			int16t getNormalDirectionId() ;
+			/// Add a contact point to the manifold
+			void addContactPoint(ContactPoint* contact);
+			/**
+			 * @brief 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 update( Transform3D transform1,
+			             Transform3D transform2);
+			/// Clear the contact manifold
+			void clear();
+			/// Return the number of contact points in the manifold
+			int getNbContactPoints() ;
+			/// Return the first friction vector at the center of the contact manifold
+			 Vector3f getFrictionVector1() ;
+			/// set the first friction vector at the center of the contact manifold
+			void setFrictionVector1( Vector3f frictionVector1);
+			/// Return the second friction vector at the center of the contact manifold
+			 Vector3f getFrictionvec2() ;
+			/// set the second friction vector at the center of the contact manifold
+			void setFrictionvec2( Vector3f frictionvec2);
+			/// Return the first friction accumulated impulse
+			float getFrictionImpulse1() ;
+			/// Set the first friction accumulated impulse
+			void setFrictionImpulse1(float frictionImpulse1);
+			/// Return the second friction accumulated impulse
+			float getFrictionImpulse2() ;
+			/// Set the second friction accumulated impulse
+			void setFrictionImpulse2(float frictionImpulse2);
+			/// Return the friction twist accumulated impulse
+			float getFrictionTwistImpulse() ;
+			/// Set the friction twist accumulated impulse
+			void setFrictionTwistImpulse(float frictionTwistImpulse);
+			/// Set the accumulated rolling resistance impulse
+			void setRollingResistanceImpulse( Vector3f rollingResistanceImpulse);
+			/// Return a contact point of the manifold
+			ContactPoint* getContactPoint(int index) ;
+			/// Return the normalized averaged normal vector
+			Vector3f getAverageContactNormal() ;
+			/// Return the largest depth of all the contact points
+			float getLargestContactDepth();
+	};
+
+}
+
+

src/org/atriaSoft/ephysics/collision/ContactManifold.java

@@ -0,0 +1,300 @@
+package org.atriaSoft.ephysics.collision;
+
+ContactManifold::ContactManifold(ProxyShape* shape1,
+                                 ProxyShape* shape2,
+                                 int normalDirectionId):
+  this.shape1(shape1),
+  this.shape2(shape2),
+  this.normalDirectionId(normalDirectionId),
+  this.nbContactPoints(0),
+  this.frictionImpulse1(0.0),
+  this.frictionImpulse2(0.0),
+  this.frictionTwistImpulse(0.0),
+  this.isAlreadyInIsland(false) {
+	
+}
+
+ContactManifold::~ContactManifold() {
+	clear();
+}
+
+void ContactManifold::addContactPoint(ContactPoint* contact) {
+	// For contact already in the manifold
+	for (int i=0; i<this.nbContactPoints; i++) {
+		// Check if the new point point does not correspond to a same contact point
+		// already in the manifold.
+		float distance = (this.contactPoints[i].getWorldPointOnBody1() - contact.getWorldPointOnBody1()).length2();
+		if (distance <= PERSISTENTCONTACTDISTTHRESHOLD*PERSISTENTCONTACTDISTTHRESHOLD) {
+			// Delete the new contact
+			ETKDELETE(ContactPoint, contact);
+			assert(this.nbContactPoints > 0);
+			return;
+		}
+	}
+	// If the contact manifold is full
+	if (this.nbContactPoints == MAXCONTACTPOINTSINMANIFOLD) {
+		int indexMaxPenetration = getIndexOfDeepestPenetration(contact);
+		int indexToRemove = getIndexToRemove(indexMaxPenetration, contact.getLocalPointOnBody1());
+		removeContactPoint(indexToRemove);
+	}
+	// Add the new contact point in the manifold
+	this.contactPoints[this.nbContactPoints] = contact;
+	this.nbContactPoints++;
+	assert(this.nbContactPoints > 0);
+}
+
+void ContactManifold::removeContactPoint(int index) {
+	assert(index < this.nbContactPoints);
+	assert(this.nbContactPoints > 0);
+	// Call the destructor explicitly and tell the memory allocator that
+	// the corresponding memory block is now free
+	ETKDELETE(ContactPoint, this.contactPoints[index]);
+	this.contactPoints[index] = null;
+	// If we don't remove the last index
+	if (index < this.nbContactPoints - 1) {
+		this.contactPoints[index] = this.contactPoints[this.nbContactPoints - 1];
+	}
+	this.nbContactPoints--;
+}
+
+void ContactManifold::update( Transform3D transform1,  Transform3D transform2) {
+	if (this.nbContactPoints == 0) {
+		return;
+	}
+	// Update the world coordinates and penetration depth of the contact points in the manifold
+	for (int i=0; i<this.nbContactPoints; i++) {
+		this.contactPoints[i].setWorldPointOnBody1(transform1 * this.contactPoints[i].getLocalPointOnBody1());
+		this.contactPoints[i].setWorldPointOnBody2(transform2 * this.contactPoints[i].getLocalPointOnBody2());
+		this.contactPoints[i].setPenetrationDepth((this.contactPoints[i].getWorldPointOnBody1() - this.contactPoints[i].getWorldPointOnBody2()).dot(this.contactPoints[i].getNormal()));
+	}
+	 float squarePersistentContactThreshold = PERSISTENTCONTACTDISTTHRESHOLD * PERSISTENTCONTACTDISTTHRESHOLD;
+	// Remove the contact points that don't represent very well the contact manifold
+	for (int i=staticcast<int>(this.nbContactPoints)-1; i>=0; i--) {
+		assert(i < staticcast<int>(this.nbContactPoints));
+		// Compute the distance between contact points in the normal direction
+		float distanceNormal = -this.contactPoints[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
+			Vector3f projOfPoint1 = this.contactPoints[i].getWorldPointOnBody1() + this.contactPoints[i].getNormal() * distanceNormal;
+			Vector3f projDifference = this.contactPoints[i].getWorldPointOnBody2() - projOfPoint1;
+			// If the orthogonal distance is larger than the valid distance
+			// threshold, we remove the contact
+			if (projDifference.length2() > squarePersistentContactThreshold) {
+				removeContactPoint(i);
+			}
+		}
+	}
+}
+
+int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact)  {
+	assert(this.nbContactPoints == MAXCONTACTPOINTSINMANIFOLD);
+	int indexMaxPenetrationDepth = -1;
+	float maxPenetrationDepth = newContact.getPenetrationDepth();
+	// For each contact in the cache
+	for (int i=0; i<this.nbContactPoints; i++) {
+		// If the current contact has a larger penetration depth
+		if (this.contactPoints[i].getPenetrationDepth() > maxPenetrationDepth) {
+			maxPenetrationDepth = this.contactPoints[i].getPenetrationDepth();
+			indexMaxPenetrationDepth = i;
+		}
+	}
+	// Return the index of largest penetration depth
+	return indexMaxPenetrationDepth;
+}
+
+int ContactManifold::getIndexToRemove(int indexMaxPenetration,  Vector3f newPoint)  {
+	assert(this.nbContactPoints == MAXCONTACTPOINTSINMANIFOLD);
+	float area0 = 0.0f; // Area with contact 1,2,3 and newPoint
+	float area1 = 0.0f; // Area with contact 0,2,3 and newPoint
+	float area2 = 0.0f; // Area with contact 0,1,3 and newPoint
+	float area3 = 0.0f; // Area with contact 0,1,2 and newPoint
+	if (indexMaxPenetration != 0) {
+		// Compute the area
+		Vector3f vector1 = newPoint - this.contactPoints[1].getLocalPointOnBody1();
+		Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1() - this.contactPoints[2].getLocalPointOnBody1();
+		Vector3f crossProduct = vector1.cross(vector2);
+		area0 = crossProduct.length2();
+	}
+	if (indexMaxPenetration != 1) {
+		// Compute the area
+		Vector3f vector1 = newPoint - this.contactPoints[0].getLocalPointOnBody1();
+		Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1() - this.contactPoints[2].getLocalPointOnBody1();
+		Vector3f crossProduct = vector1.cross(vector2);
+		area1 = crossProduct.length2();
+	}
+	if (indexMaxPenetration != 2) {
+		// Compute the area
+		Vector3f vector1 = newPoint - this.contactPoints[0].getLocalPointOnBody1();
+		Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1() - this.contactPoints[1].getLocalPointOnBody1();
+		Vector3f crossProduct = vector1.cross(vector2);
+		area2 = crossProduct.length2();
+	}
+	if (indexMaxPenetration != 3) {
+		// Compute the area
+		Vector3f vector1 = newPoint - this.contactPoints[0].getLocalPointOnBody1();
+		Vector3f vector2 = this.contactPoints[2].getLocalPointOnBody1() - this.contactPoints[1].getLocalPointOnBody1();
+		Vector3f crossProduct = vector1.cross(vector2);
+		area3 = crossProduct.length2();
+	}
+	// Return the index of the contact to remove
+	return getMaxArea(area0, area1, area2, area3);
+}
+
+int ContactManifold::getMaxArea(float area0, float area1, float area2, float area3)  {
+	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 (int iii=0; iii<this.nbContactPoints; ++iii) {
+		
+		// Call the destructor explicitly and tell the memory allocator that
+		// the corresponding memory block is now free
+		ETKDELETE(ContactPoint, this.contactPoints[iii]);
+		this.contactPoints[iii] = null;
+	}
+	this.nbContactPoints = 0;
+}
+
+// Return a pointer to the first proxy shape of the contact
+ProxyShape* ContactManifold::getShape1()  {
+	return this.shape1;
+}
+
+// Return a pointer to the second proxy shape of the contact
+ProxyShape* ContactManifold::getShape2()  {
+	return this.shape2;
+}
+
+// Return a pointer to the first body of the contact manifold
+CollisionBody* ContactManifold::getBody1()  {
+	return this.shape1.getBody();
+}
+
+// Return a pointer to the second body of the contact manifold
+CollisionBody* ContactManifold::getBody2()  {
+	return this.shape2.getBody();
+}
+
+// Return the normal direction Id
+int16t ContactManifold::getNormalDirectionId()  {
+	return this.normalDirectionId;
+}
+
+// Return the number of contact points in the manifold
+int ContactManifold::getNbContactPoints()  {
+	return this.nbContactPoints;
+}
+
+// Return the first friction vector at the center of the contact manifold
+ Vector3f ContactManifold::getFrictionVector1()  {
+	return this.frictionVector1;
+}
+
+// set the first friction vector at the center of the contact manifold
+void ContactManifold::setFrictionVector1( Vector3f frictionVector1) {
+	this.frictionVector1 = frictionVector1;
+}
+
+// Return the second friction vector at the center of the contact manifold
+ Vector3f ContactManifold::getFrictionvec2()  {
+	return this.frictionvec2;
+}
+
+// set the second friction vector at the center of the contact manifold
+void ContactManifold::setFrictionvec2( Vector3f frictionvec2) {
+	this.frictionvec2 = frictionvec2;
+}
+
+// Return the first friction accumulated impulse
+float ContactManifold::getFrictionImpulse1()  {
+	return this.frictionImpulse1;
+}
+
+// Set the first friction accumulated impulse
+void ContactManifold::setFrictionImpulse1(float frictionImpulse1) {
+	this.frictionImpulse1 = frictionImpulse1;
+}
+
+// Return the second friction accumulated impulse
+float ContactManifold::getFrictionImpulse2()  {
+	return this.frictionImpulse2;
+}
+
+// Set the second friction accumulated impulse
+void ContactManifold::setFrictionImpulse2(float frictionImpulse2) {
+	this.frictionImpulse2 = frictionImpulse2;
+}
+
+// Return the friction twist accumulated impulse
+float ContactManifold::getFrictionTwistImpulse()  {
+	return this.frictionTwistImpulse;
+}
+
+// Set the friction twist accumulated impulse
+void ContactManifold::setFrictionTwistImpulse(float frictionTwistImpulse) {
+	this.frictionTwistImpulse = frictionTwistImpulse;
+}
+
+// Set the accumulated rolling resistance impulse
+void ContactManifold::setRollingResistanceImpulse( Vector3f rollingResistanceImpulse) {
+	this.rollingResistanceImpulse = rollingResistanceImpulse;
+}
+
+// Return a contact point of the manifold
+ContactPoint* ContactManifold::getContactPoint(int index)  {
+	assert(index < this.nbContactPoints);
+	return this.contactPoints[index];
+}
+
+// Return true if the contact manifold has already been added into an island
+boolean ContactManifold::isAlreadyInIsland()  {
+	return this.isAlreadyInIsland;
+}
+
+// Return the normalized averaged normal vector
+Vector3f ContactManifold::getAverageContactNormal()  {
+	Vector3f averageNormal;
+	for (int i=0; i<this.nbContactPoints; i++) {
+		averageNormal += this.contactPoints[i].getNormal();
+	}
+	return averageNormal.safeNormalized();
+}
+
+// Return the largest depth of all the contact points
+float ContactManifold::getLargestContactDepth()  {
+	float largestDepth = 0.0f;
+	for (int i=0; i<this.nbContactPoints; i++) {
+		float depth = this.contactPoints[i].getPenetrationDepth();
+		if (depth > largestDepth) {
+			largestDepth = depth;
+		}
+	}
+	return largestDepth;
+}

src/org/atriaSoft/ephysics/collision/ContactManifoldSet.cpp

@@ -0,0 +1,197 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/ContactManifoldSet.hpp>
+
+using namespace ephysics;
+
+ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1,
+                                       ProxyShape* shape2,
+                                       int nbMaxManifolds):
+  this.nbMaxManifolds(nbMaxManifolds),
+  this.nbManifolds(0),
+  this.shape1(shape1),
+  this.shape2(shape2) {
+	assert(nbMaxManifolds >= 1);
+}
+
+ContactManifoldSet::~ContactManifoldSet() {
+	clear();
+}
+
+void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
+	// Compute an Id corresponding to the normal direction (using a cubemap)
+	int16t normalDirectionId = computeCubemapNormalId(contact.getNormal());
+	// If there is no contact manifold yet
+	if (this.nbManifolds == 0) {
+		createManifold(normalDirectionId);
+		this.manifolds[0].addContactPoint(contact);
+		assert(this.manifolds[this.nbManifolds-1].getNbContactPoints() > 0);
+		for (int i=0; i<this.nbManifolds; i++) {
+			assert(this.manifolds[i].getNbContactPoints() > 0);
+		}
+		return;
+	}
+	// Select the manifold with the most similar normal (if exists)
+	int similarManifoldIndex = 0;
+	if (this.nbMaxManifolds > 1) {
+		similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId);
+	}
+	// If a similar manifold has been found
+	if (similarManifoldIndex != -1) {
+		// Add the contact point to that similar manifold
+		this.manifolds[similarManifoldIndex].addContactPoint(contact);
+		assert(this.manifolds[similarManifoldIndex].getNbContactPoints() > 0);
+		return;
+	}
+	// If the maximum number of manifold has not been reached yet
+	if (this.nbManifolds < this.nbMaxManifolds) {
+		// Create a new manifold for the contact point
+		createManifold(normalDirectionId);
+		this.manifolds[this.nbManifolds-1].addContactPoint(contact);
+		for (int i=0; i<this.nbManifolds; i++) {
+			assert(this.manifolds[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;
+	float minDepth = contact.getPenetrationDepth();
+	assert(this.nbManifolds == this.nbMaxManifolds);
+	for (int i=0; i<this.nbManifolds; i++) {
+		float depth = this.manifolds[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
+		ETKDELETE(ContactPoint, contact);
+		contact = null;
+		return;
+	}
+	assert(smallestDepthIndex >= 0 && smallestDepthIndex < this.nbManifolds);
+	// Here we need to replace an existing manifold with a new one (that contains
+	// the new contact point)
+	removeManifold(smallestDepthIndex);
+	createManifold(normalDirectionId);
+	this.manifolds[this.nbManifolds-1].addContactPoint(contact);
+	assert(this.manifolds[this.nbManifolds-1].getNbContactPoints() > 0);
+	for (int i=0; i<this.nbManifolds; i++) {
+		assert(this.manifolds[i].getNbContactPoints() > 0);
+	}
+	return;
+}
+
+int ContactManifoldSet::selectManifoldWithSimilarNormal(int16t normalDirectionId)  {
+	// Return the Id of the manifold with the same normal direction id (if exists)
+	for (int i=0; i<this.nbManifolds; i++) {
+		if (normalDirectionId == this.manifolds[i].getNormalDirectionId()) {
+			return i;
+		}
+	}
+	return -1;
+}
+
+int16t ContactManifoldSet::computeCubemapNormalId( Vector3f normal)  {
+	assert(normal.length2() > FLTEPSILON);
+	int faceNo;
+	float u, v;
+	float max = max3(fabs(normal.x()), fabs(normal.y()), fabs(normal.z()));
+	Vector3f 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) * CONTACTCUBEMAPFACENBSUBDIVISIONS);
+	int indexV = floor(((v + 1)/2) * CONTACTCUBEMAPFACENBSUBDIVISIONS);
+	if (indexU == CONTACTCUBEMAPFACENBSUBDIVISIONS) {
+		indexU--;
+	}
+	if (indexV == CONTACTCUBEMAPFACENBSUBDIVISIONS) {
+		indexV--;
+	}
+	 int nbSubDivInFace = CONTACTCUBEMAPFACENBSUBDIVISIONS * CONTACTCUBEMAPFACENB_SUBDIVISIONS;
+	return faceNo * 200 + indexU * nbSubDivInFace + indexV;
+}
+
+void ContactManifoldSet::update() {
+	for (int i=this.nbManifolds-1; i>=0; i--) {
+		// Update the contact manifold
+		this.manifolds[i].update(this.shape1.getBody().getTransform() * this.shape1.getLocalToBodyTransform(),
+							  this.shape2.getBody().getTransform() * this.shape2.getLocalToBodyTransform());
+		// Remove the contact manifold if has no contact points anymore
+		if (this.manifolds[i].getNbContactPoints() == 0) {
+			removeManifold(i);
+		}
+	}
+}
+
+void ContactManifoldSet::clear() {
+	for (int i=this.nbManifolds-1; i>=0; i--) {
+		removeManifold(i);
+	}
+	assert(this.nbManifolds == 0);
+}
+
+void ContactManifoldSet::createManifold(int16t normalDirectionId) {
+	assert(this.nbManifolds < this.nbMaxManifolds);
+	this.manifolds[this.nbManifolds] = ETKNEW(ContactManifold, this.shape1, this.shape2, normalDirectionId);
+	this.nbManifolds++;
+}
+
+void ContactManifoldSet::removeManifold(int index) {
+	assert(this.nbManifolds > 0);
+	assert(index >= 0 && index < this.nbManifolds);
+	// Delete the new contact
+	ETKDELETE(ContactManifold, this.manifolds[index]);
+	this.manifolds[index] = null;
+	for (int i=index; (i+1) < this.nbManifolds; i++) {
+		this.manifolds[i] = this.manifolds[i+1];
+	}
+	this.nbManifolds--;
+}
+
+ProxyShape* ContactManifoldSet::getShape1()  {
+	return this.shape1;
+}
+
+ProxyShape* ContactManifoldSet::getShape2()  {
+	return this.shape2;
+}
+
+int ContactManifoldSet::getNbContactManifolds()  {
+	return this.nbManifolds;
+}
+
+ContactManifold* ContactManifoldSet::getContactManifold(int index)  {
+	assert(index >= 0 && index < this.nbManifolds);
+	return this.manifolds[index];
+}
+
+int ContactManifoldSet::getTotalNbContactPoints()  {
+	int nbPoints = 0;
+	for (int i=0; i<this.nbManifolds; i++) {
+		nbPoints += this.manifolds[i].getNbContactPoints();
+	}
+	return nbPoints;
+}

src/org/atriaSoft/ephysics/collision/ContactManifoldSet.java

@@ -0,0 +1,52 @@
+package org.atriaSoft.ephysics.collision;
+
+	 int MAXMANIFOLDSINCONTACTMANIFOLDSET = 3;   // Maximum number of contact manifolds in the set
+	 int CONTACTCUBEMAPFACENBSUBDIVISIONS = 3;	// N Number for the N x N subdivisions of the cubemap
+	/**
+	 * @brief 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:
+			int nbMaxManifolds; //!< Maximum number of contact manifolds in the set
+			int nbManifolds; //!< Current number of contact manifolds in the set
+			ProxyShape* shape1; //!< Pointer to the first proxy shape of the contact
+			ProxyShape* shape2; //!< Pointer to the second proxy shape of the contact
+			ContactManifold* manifolds[MAXMANIFOLDSINCONTACTMANIFOLDSET]; //!< Contact manifolds of the set
+			/// Create a new contact manifold and add it to the set
+			void createManifold(int 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(int16t normalDirectionId) ;
+			// 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
+			int16t computeCubemapNormalId( Vector3f normal) ;
+		public:
+			/// Constructor
+			ContactManifoldSet(ProxyShape* shape1,
+			                   ProxyShape* shape2,
+			                   int nbMaxManifolds);
+			/// Return the first proxy shape
+			ProxyShape* getShape1() ;
+			/// Return the second proxy shape
+			ProxyShape* getShape2() ;
+			/// 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() ;
+			/// Return a given contact manifold
+			ContactManifold* getContactManifold(int index) ;
+			/// Return the total number of contact points in the set of manifolds
+			int getTotalNbContactPoints() ;
+	};
+
+}
+

src/org/atriaSoft/ephysics/collision/ProxyShape.cpp

@@ -0,0 +1,218 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/ProxyShape.hpp>
+
+using namespace ephysics;
+
+// Constructor
+/**
+ * @param body Pointer to the parent body
+ * @param shape Pointer to the collision shape
+ * @param transform Transform3Dation from collision shape local-space to body local-space
+ * @param mass Mass of the collision shape (in kilograms)
+ */
+ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape,  Transform3D transform, float mass)
+		   :this.body(body), this.collisionShape(shape), this.localToBodyTransform(transform), this.mass(mass),
+			this.next(NULL), this.broadPhaseID(-1), this.cachedCollisionData(NULL), this.userData(NULL),
+			this.collisionCategoryBits(0x0001), this.collideWithMaskBits(0xFFFF) {
+
+}
+
+// Destructor
+ProxyShape::~ProxyShape() {
+	// Release the cached collision data memory
+	if (this.cachedCollisionData != NULL) {
+		free(this.cachedCollisionData);
+	}
+}
+
+// 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
+ */
+boolean ProxyShape::testPointInside( Vector3f worldPoint) {
+	 Transform3D localToWorld = this.body.getTransform() * this.localToBodyTransform;
+	 Vector3f localPoint = localToWorld.getInverse() * worldPoint;
+	return this.collisionShape.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
+ */
+boolean ProxyShape::raycast( Ray ray, RaycastInfo raycastInfo) {
+
+	// If the corresponding body is not active, it cannot be hit by rays
+	if (!this.body.isActive()) return false;
+
+	// Convert the ray into the local-space of the collision shape
+	 Transform3D localToWorldTransform = getLocalToWorldTransform();
+	 Transform3D worldToLocalTransform = localToWorldTransform.getInverse();
+	Ray rayLocal(worldToLocalTransform * ray.point1,
+				 worldToLocalTransform * ray.point2,
+				 ray.maxFraction);
+
+	boolean isHit = this.collisionShape.raycast(rayLocal, raycastInfo, this);
+	if (isHit == true) {
+		// Convert the raycast info into world-space
+		raycastInfo.worldPoint = localToWorldTransform * raycastInfo.worldPoint;
+		raycastInfo.worldNormal = localToWorldTransform.getOrientation() * raycastInfo.worldNormal;
+		raycastInfo.worldNormal.normalize();
+	}
+	return isHit;
+}
+
+// Return the pointer to the cached collision data
+void** ProxyShape::getCachedCollisionData()  {
+	return this.cachedCollisionData;
+}
+
+// Return the collision shape
+/**
+ * @return Pointer to the internal collision shape
+ */
+ CollisionShape* ProxyShape::getCollisionShape()  {
+	return this.collisionShape;
+}
+
+// Return the parent body
+/**
+ * @return Pointer to the parent body
+ */
+CollisionBody* ProxyShape::getBody()  {
+	return this.body;
+}
+
+// Return the mass of the collision shape
+/**
+ * @return Mass of the collision shape (in kilograms)
+ */
+float ProxyShape::getMass()  {
+	return this.mass;
+}
+
+// Return a pointer to the user data attached to this body
+/**
+ * @return A pointer to the user data stored into the proxy shape
+ */
+void* ProxyShape::getUserData()  {
+	return this.userData;
+}
+
+// Attach user data to this body
+/**
+ * @param userData Pointer to the user data you want to store within the proxy shape
+ */
+void ProxyShape::setUserData(void* userData) {
+	this.userData = 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
+ */
+ Transform3D ProxyShape::getLocalToBodyTransform()  {
+	return this.localToBodyTransform;
+}
+
+// Set the local to parent body transform
+void ProxyShape::setLocalToBodyTransform( Transform3D transform) {
+
+	this.localToBodyTransform = transform;
+
+	this.body.setIsSleeping(false);
+
+	// Notify the body that the proxy shape has to be updated in the broad-phase
+	this.body.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
+ */
+ Transform3D ProxyShape::getLocalToWorldTransform()  {
+	return this.body.this.transform * this.localToBodyTransform;
+}
+
+// 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
+ */
+ProxyShape* ProxyShape::getNext() {
+	return this.next;
+}
+
+// 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
+ */
+ ProxyShape* ProxyShape::getNext()  {
+	return this.next;
+}
+
+// Return the collision category bits
+/**
+ * @return The collision category bits mask of the proxy shape
+ */
+int ProxyShape::getCollisionCategoryBits()  {
+	return this.collisionCategoryBits;
+}
+
+// Set the collision category bits
+/**
+ * @param collisionCategoryBits The collision category bits mask of the proxy shape
+ */
+void ProxyShape::setCollisionCategoryBits(int collisionCategoryBits) {
+	this.collisionCategoryBits = collisionCategoryBits;
+}
+
+// Return the collision bits mask
+/**
+ * @return The bits mask that specifies with which collision category this shape will collide
+ */
+int ProxyShape::getCollideWithMaskBits()  {
+	return this.collideWithMaskBits;
+}
+
+// Set the collision bits mask
+/**
+ * @param collideWithMaskBits The bits mask that specifies with which collision category this shape will collide
+ */
+void ProxyShape::setCollideWithMaskBits(int collideWithMaskBits) {
+	this.collideWithMaskBits = collideWithMaskBits;
+}
+
+// Return the local scaling vector of the collision shape
+/**
+ * @return The local scaling vector
+ */
+Vector3f ProxyShape::getLocalScaling()  {
+	return this.collisionShape.getScaling();
+}
+
+// Set the local scaling vector of the collision shape
+/**
+ * @param scaling The new local scaling vector
+ */
+void ProxyShape::setLocalScaling( Vector3f scaling) {
+
+	// Set the local scaling of the collision shape
+	this.collisionShape.setLocalScaling(scaling);
+
+	this.body.setIsSleeping(false);
+
+	// Notify the body that the proxy shape has to be updated in the broad-phase
+	this.body.updateProxyShapeInBroadPhase(this, true);
+}

src/org/atriaSoft/ephysics/collision/ProxyShape.java

@@ -0,0 +1,115 @@
+package org.atriaSoft.ephysics.collision;
+
+	/**
+	 * @breif 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:
+			CollisionBody* body; //!< Pointer to the parent body
+			CollisionShape* collisionShape; //!< Internal collision shape
+			Transform3D localToBodyTransform; //!< Local-space to parent body-space transform (does not change over time)
+			float mass; //!< Mass (in kilogramms) of the corresponding collision shape
+			ProxyShape* next; //!< Pointer to the next proxy shape of the body (linked list)
+			int broadPhaseID; //!< Broad-phase ID (node ID in the dynamic AABB tree)
+			void* cachedCollisionData; //!< Cached collision data
+			void* userData; //!< Pointer to user data
+			/**
+			 * @brief 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 this.collideWithMaskBits variable so that given
+			 * categories of shapes collide with each other and do not collide with
+			 * other categories.
+			 */
+			int collisionCategoryBits;
+			/**
+			 * @brief 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.
+			 */
+			int collideWithMaskBits;
+		public:
+			/// Constructor
+			ProxyShape(CollisionBody* body,
+			           CollisionShape* shape,
+			            Transform3D transform,
+			           float mass);
+			/// Return the collision shape
+			CollisionShape* getCollisionShape() ;
+	
+			/// Return the parent body
+			CollisionBody* getBody() ;
+	
+			/// Return the mass of the collision shape
+			float getMass() ;
+	
+			/// Return a pointer to the user data attached to this body
+			void* getUserData() ;
+	
+			/// Attach user data to this body
+			void setUserData(void* userData);
+	
+			/// Return the local to parent body transform
+			Transform3D getLocalToBodyTransform() ;
+	
+			/// Set the local to parent body transform
+			void setLocalToBodyTransform( Transform3D transform);
+	
+			/// Return the local to world transform
+			 Transform3D getLocalToWorldTransform() ;
+	
+			/// Return true if a point is inside the collision shape
+			boolean testPointInside( Vector3f worldPoint);
+	
+			/// Raycast method with feedback information
+			boolean raycast( Ray ray, RaycastInfo raycastInfo);
+	
+			/// Return the collision bits mask
+			int getCollideWithMaskBits() ;
+	
+			/// Set the collision bits mask
+			void setCollideWithMaskBits(int collideWithMaskBits);
+	
+			/// Return the collision category bits
+			int getCollisionCategoryBits() ;
+	
+			/// Set the collision category bits
+			void setCollisionCategoryBits(int 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
+			 ProxyShape* getNext() ;
+	
+			/// Return the pointer to the cached collision data
+			void** getCachedCollisionData();
+	
+			/// Return the local scaling vector of the collision shape
+			Vector3f getLocalScaling() ;
+	
+			/// Set the local scaling vector of the collision shape
+			 void setLocalScaling( Vector3f scaling);
+	
+			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;
+	
+	};
+
+}
+

src/org/atriaSoft/ephysics/collision/RaycastInfo.cpp

@@ -0,0 +1,30 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/RaycastInfo.hpp>
+#include <ephysics/collision/ProxyShape.hpp>
+
+using namespace ephysics;
+
+// Ray cast test against a proxy shape
+float RaycastTest::raycastAgainstShape(ProxyShape* shape,  Ray ray) {
+
+	// Ray casting test against the collision shape
+	RaycastInfo raycastInfo;
+	boolean 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/org/atriaSoft/ephysics/collision/RaycastInfo.java

@@ -0,0 +1,68 @@
+package org.atriaSoft.ephysics.collision;
+
+	/**
+	 * @brief It contains the information about a raycast hit.
+	 */
+	struct RaycastInfo {
+		private:
+			/// Private copy ructor
+			RaycastInfo( RaycastInfo) = delete;
+			/// Private assignment operator
+			RaycastInfo operator=( RaycastInfo) = delete;
+		public:
+			Vector3f worldPoint; //!< Hit point in world-space coordinates
+			Vector3f worldNormal; //!< Surface normal at hit point in world-space coordinates
+			float hitFraction; //!< 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)
+			int meshSubpart; //!< Mesh subpart index that has been hit (only used for triangles mesh and -1 otherwise)
+			int triangleIndex; //!< Hit triangle index (only used for triangles mesh and -1 otherwise)
+			CollisionBody* body; //!< Pointer to the hit collision body
+			ProxyShape* proxyShape; //!< Pointer to the hit proxy collision shape
+			/// Constructor
+			RaycastInfo() :
+			  meshSubpart(-1),
+			  triangleIndex(-1),
+			  body(null),
+			  proxyShape(null) {
+				
+			}
+	};
+	
+	/**
+	 * @brief It 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:
+			/**
+			 * @brief 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[in] raycastInfo Information about the raycast hit
+			 * @return Value that controls the continuation of the ray after a hit
+			 */
+			 float notifyRaycastHit( RaycastInfo raycastInfo)=0;
+	};
+	
+	struct RaycastTest {
+		public:
+			RaycastCallback* userCallback; //!< User callback class
+			/// Constructor
+			RaycastTest(RaycastCallback* callback) {
+				userCallback = callback;
+			}
+			/// Ray cast test against a proxy shape
+			float raycastAgainstShape(ProxyShape* shape,  Ray ray);
+	};
+
+}
+

src/org/atriaSoft/ephysics/collision/TriangleMesh.cpp

@@ -0,0 +1,14 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/TriangleMesh.hpp>
+
+ephysics::TriangleMesh::TriangleMesh() {
+	
+}
+

src/org/atriaSoft/ephysics/collision/TriangleMesh.java

@@ -0,0 +1,40 @@
+package org.atriaSoft.ephysics.collision;
+
+	/**
+	 * @brief 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:
+			Vector<TriangleVertexArray*> triangleArrays; //!< All the triangle arrays of the mesh (one triangle array per part)
+		public:
+			/**
+			 * @brief Constructor
+			 */
+			TriangleMesh();
+			/**
+			 * @brief Add a subpart of the mesh
+			 */
+			void addSubpart(TriangleVertexArray* triangleVertexArray) {
+				this.triangleArrays.pushBack(triangleVertexArray );
+			}
+			/**
+			 * @brief Get a pointer to a given subpart (triangle vertex array) of the mesh
+			 */
+			TriangleVertexArray* getSubpart(int indexSubpart)  {
+				assert(indexSubpart < this.triangleArrays.size());
+				return this.triangleArrays[indexSubpart];
+			}
+			/**
+			 * @brief Get the number of subparts of the mesh
+			 */
+			int getNbSubparts()  {
+				return this.triangleArrays.size();
+			}
+	};
+}
+
+

src/org/atriaSoft/ephysics/collision/TriangleVertexArray.cpp

@@ -0,0 +1,40 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/TriangleVertexArray.hpp>
+
+
+ephysics::TriangleVertexArray::TriangleVertexArray( Vector<Vector3f> vertices,  Vector<int> triangles):
+  this.vertices(vertices),
+  this.triangles(triangles) {
+	
+}
+
+long ephysics::TriangleVertexArray::getNbVertices()  {
+	return this.vertices.size();
+}
+
+long ephysics::TriangleVertexArray::getNbTriangles()  {
+	return this.triangles.size()/3;
+}
+
+ Vector<Vector3f> ephysics::TriangleVertexArray::getVertices()  {
+	return this.vertices;
+}
+
+ Vector<int> ephysics::TriangleVertexArray::getIndices() {
+	return this.triangles;
+}
+
+ephysics::Triangle ephysics::TriangleVertexArray::getTriangle(int id)  {
+	ephysics::Triangle out;
+	out[0] = this.vertices[this.triangles[id*3]];
+	out[1] = this.vertices[this.triangles[id*3+1]];
+	out[2] = this.vertices[this.triangles[id*3+2]];
+	return out;
+}

src/org/atriaSoft/ephysics/collision/TriangleVertexArray.java

@@ -0,0 +1,61 @@
+package org.atriaSoft.ephysics.collision;
+
+	class Triangle {
+		public:
+			Vector3f value[3];
+			Vector3f operator[] (long id) {
+				return value[id];
+			}
+	};
+	/**
+	 * 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 {
+		protected:
+			Vector<Vector3f> vertices; //!< Vertice list
+			Vector<int> triangles; //!< List of triangle (3 pos for each triangle)
+		public:
+			/**
+			 * @brief Constructor
+			 * @param[in] vertices List Of all vertices
+			 * @param[in] triangles List of all linked points
+			 */
+			TriangleVertexArray( Vector<Vector3f> vertices,
+			                     Vector<int> triangles);
+			/**
+			 * @brief Get the number of vertices
+			 * @return Number of vertices
+			 */
+			long getNbVertices() ;
+			/**
+			 * @brief Get the number of triangle
+			 * @return Number of triangles
+			 */
+			long getNbTriangles() ;
+			/**
+			 * @brief Get The table of the vertices
+			 * @return reference on the vertices
+			 */
+			 Vector<Vector3f> getVertices() ;
+			/**
+			 * @brief Get The table of the triangle indice
+			 * @return reference on the triangle indice
+			 */
+			 Vector<int> getIndices() ;
+			/**
+			 * @brief Get a triangle at the specific ID
+			 * @return Buffer of 3 points
+			 */
+			ephysics::Triangle getTriangle(int id) ;
+	};
+
+
+}
+
+

src/org/atriaSoft/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp

@@ -0,0 +1,203 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/broadphase/BroadPhaseAlgorithm.hpp>
+#include <ephysics/collision/CollisionDetection.hpp>
+#include <ephysics/engine/Profiler.hpp>
+
+using namespace ephysics;
+
+BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection collisionDetection):
+  this.dynamicAABBTree(DYNAMICTREEAABBGAP),
+  this.collisionDetection(collisionDetection) {
+	this.movedShapes.reserve(8);
+	this.potentialPairs.reserve(8);
+}
+
+BroadPhaseAlgorithm::~BroadPhaseAlgorithm() {
+	
+}
+
+void BroadPhaseAlgorithm::addMovedCollisionShape(int broadPhaseID) {
+	this.movedShapes.pushBack(broadPhaseID);
+}
+
+void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
+	auto it = this.movedShapes.begin();
+	while (it != this.movedShapes.end()) {
+		if (*it == broadPhaseID) {
+			it = this.movedShapes.erase(it);
+		} else {
+			++it;
+		}
+	}
+	/*
+	assert(this.numberNonUsedMovedShapes <= this.numberMovedShapes);
+
+	// If less than the quarter of allocated elements of the non-static shapes IDs array
+	// are used, we release some allocated memory
+	if ((this.numberMovedShapes - this.numberNonUsedMovedShapes) < this.numberAllocatedMovedShapes / 4 &&
+			this.numberAllocatedMovedShapes > 8) {
+
+		this.numberAllocatedMovedShapes /= 2;
+		int* oldArray = this.movedShapes;
+		this.movedShapes = (int*) malloc(this.numberAllocatedMovedShapes * sizeof(int));
+		assert(this.movedShapes != NULL);
+		int nbElements = 0;
+		for (int i=0; i<this.numberMovedShapes; i++) {
+			if (oldArray[i] != -1) {
+				this.movedShapes[nbElements] = oldArray[i];
+				nbElements++;
+			}
+		}
+		this.numberMovedShapes = nbElements;
+		this.numberNonUsedMovedShapes = 0;
+		free(oldArray);
+	}
+
+	// Remove the broad-phase ID from the array
+	for (int i=0; i<this.numberMovedShapes; i++) {
+		if (this.movedShapes[i] == broadPhaseID) {
+			this.movedShapes[i] = -1;
+			this.numberNonUsedMovedShapes++;
+			break;
+		}
+	}
+	*/
+}
+
+void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape,  AABB aabb) {
+	// Add the collision shape into the dynamic AABB tree and get its broad-phase ID
+	int nodeId = this.dynamicAABBTree.addObject(aabb, proxyShape);
+	// Set the broad-phase ID of the proxy shape
+	proxyShape.this.broadPhaseID = nodeId;
+	// Add the collision shape into the array of bodies that have moved (or have been created)
+	// during the last simulation step
+	addMovedCollisionShape(proxyShape.this.broadPhaseID);
+}
+
+void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) {
+	int broadPhaseID = proxyShape.this.broadPhaseID;
+	// Remove the collision shape from the dynamic AABB tree
+	this.dynamicAABBTree.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);
+}
+
+void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape,
+                                                     AABB aabb,
+                                                     Vector3f displacement,
+                                                    boolean forceReinsert) {
+	int broadPhaseID = proxyShape.this.broadPhaseID;
+	assert(broadPhaseID >= 0);
+	// Update the dynamic AABB tree according to the movement of the collision shape
+	boolean hasBeenReInserted = this.dynamicAABBTree.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);
+	}
+}
+
+static boolean sortFunction( Pair<int,int> pair1,  Pair<int,int> pair2) {
+	if (pair1.first < pair2.first) {
+		return true;
+	}
+	if (pair1.first == pair2.first) {
+		return pair1.second < pair2.second;
+	}
+	return false;
+}
+
+void BroadPhaseAlgorithm::computeOverlappingPairs() {
+	this.potentialPairs.clear();
+	// For all collision shapes that have moved (or have been created) during the
+	// last simulation step
+	for (auto it: this.movedShapes) {
+		if (it == -1) {
+			// impossible case ...
+			continue;
+		}
+		// Get the AABB of the shape
+		 AABB shapeAABB = this.dynamicAABBTree.getFatAABB(it);
+		// 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.
+		this.dynamicAABBTree.reportAllShapesOverlappingWithAABB(shapeAABB, [](int nodeId) mutable {
+		                                                                	// If both the nodes are the same, we do not create store the overlapping pair
+		                                                                	if (it == nodeId) {
+		                                                                		return;
+		                                                                	}
+		                                                                	// Add the new potential pair into the array of potential overlapping pairs
+		                                                                	this.potentialPairs.pushBack(makePair(min(it, nodeId), max(it, nodeId) ));
+		                                                                });
+	}
+	// Reset the array of collision shapes that have move (or have been created) during the last simulation step
+	this.movedShapes.clear();
+	// Sort the array of potential overlapping pairs in order to remove duplicate pairs
+	algorithm::quickSort(this.potentialPairs, sortFunction);
+	// Check all the potential overlapping pairs avoiding duplicates to report unique
+	// overlapping pairs
+	int iii=0;
+	while (iii < this.potentialPairs.size()) {
+		// Get a potential overlapping pair
+		 Pair<int,int> pair = (this.potentialPairs[iii]);
+		++iii;
+		// Get the two collision shapes of the pair
+		ProxyShape* shape1 = staticcast<ProxyShape*>(this.dynamicAABBTree.getNodeDataPointer(pair.first));
+		ProxyShape* shape2 = staticcast<ProxyShape*>(this.dynamicAABBTree.getNodeDataPointer(pair.second));
+		// Notify the collision detection about the overlapping pair
+		this.collisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2);
+		// Skip the duplicate overlapping pairs
+		while (iii < this.potentialPairs.size()) {
+			// Get the next pair
+			 Pair<int,int> nextPair = this.potentialPairs[iii];
+			// If the next pair is different from the previous one, we stop skipping pairs
+			if (    nextPair.first != pair.first
+			     || nextPair.second != pair.second) {
+				break;
+			}
+			++iii;
+		}
+	}
+}
+
+float BroadPhaseRaycastCallback::operator()(int nodeId,  Ray ray) {
+	float hitFraction = float(-1.0);
+	// Get the proxy shape from the node
+	ProxyShape* proxyShape = staticcast<ProxyShape*>(this.dynamicAABBTree.getNodeDataPointer(nodeId));
+	// Check if the raycast filtering mask allows raycast against this shape
+	if ((this.raycastWithCategoryMaskBits  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 = this.raycastTest.raycastAgainstShape(proxyShape, ray);
+	}
+	return hitFraction;
+}
+
+boolean BroadPhaseAlgorithm::testOverlappingShapes( ProxyShape* shape1,
+                                                 ProxyShape* shape2)  {
+	// Get the two AABBs of the collision shapes
+	 AABB aabb1 = this.dynamicAABBTree.getFatAABB(shape1.this.broadPhaseID);
+	 AABB aabb2 = this.dynamicAABBTree.getFatAABB(shape2.this.broadPhaseID);
+	// Check if the two AABBs are overlapping
+	return aabb1.testCollision(aabb2);
+}
+
+void BroadPhaseAlgorithm::raycast( Ray ray,
+                                  RaycastTest raycastTest,
+                                  int raycastWithCategoryMaskBits)  {
+	PROFILE("BroadPhaseAlgorithm::raycast()");
+	BroadPhaseRaycastCallback broadPhaseRaycastCallback(this.dynamicAABBTree, raycastWithCategoryMaskBits, raycastTest);
+	this.dynamicAABBTree.raycast(ray, broadPhaseRaycastCallback);
+}
+

src/org/atriaSoft/ephysics/collision/broadphase/BroadPhaseAlgorithm.java

@@ -0,0 +1,76 @@
+package org.atriaSoft.ephysics.collision.broadphase;
+
+	
+	// TODO : remove this as callback ... DynamicAABBTreeOverlapCallback {
+	/**
+	 * Callback called when the AABB of a leaf node is hit by a ray the
+	 * broad-phase Dynamic AABB Tree.
+	 */
+	class BroadPhaseRaycastCallback {
+		private :
+			 DynamicAABBTree this.dynamicAABBTree;
+			int this.raycastWithCategoryMaskBits;
+			RaycastTest this.raycastTest;
+		public:
+			// Constructor
+			BroadPhaseRaycastCallback( DynamicAABBTree dynamicAABBTree,
+			                          int raycastWithCategoryMaskBits,
+			                          RaycastTest raycastTest):
+			  this.dynamicAABBTree(dynamicAABBTree),
+			  this.raycastWithCategoryMaskBits(raycastWithCategoryMaskBits),
+			  this.raycastTest(raycastTest) {
+				
+			}
+			// Called for a broad-phase shape that has to be tested for raycast
+			float operator()(int nodeId,  Ray ray);
+	};
+	
+	/**
+	 * @brief It 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 :
+			DynamicAABBTree this.dynamicAABBTree; //!< Dynamic AABB tree
+			Vector<int> this.movedShapes; //!< 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.
+			Vector<Pair<int,int>> this.potentialPairs; //!< Temporary array of potential overlapping pairs (with potential duplicates)
+			CollisionDetection this.collisionDetection; //!< Reference to the collision detection object
+			/// Private copy-ructor
+			BroadPhaseAlgorithm( BroadPhaseAlgorithm obj);
+			/// Private assignment operator
+			BroadPhaseAlgorithm operator=( BroadPhaseAlgorithm obj);
+		public :
+			/// Constructor
+			BroadPhaseAlgorithm(CollisionDetection collisionDetection);
+			/// Destructor
+			 ~BroadPhaseAlgorithm();
+			/// Add a proxy collision shape into the broad-phase collision detection
+			void addProxyCollisionShape(ProxyShape* proxyShape,  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,
+			                                AABB aabb,
+			                                Vector3f displacement,
+			                               boolean 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);
+			/// Compute all the overlapping pairs of collision shapes
+			void computeOverlappingPairs();
+			/// Return true if the two broad-phase collision shapes are overlapping
+			boolean testOverlappingShapes( ProxyShape* shape1,  ProxyShape* shape2) ;
+			/// Ray casting method
+			void raycast( Ray ray,
+			             RaycastTest raycastTest,
+			             int raycastWithCategoryMaskBits) ;
+	};
+
+}
+

src/org/atriaSoft/ephysics/collision/broadphase/DynamicAABBTree.cpp

@@ -0,0 +1,688 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/broadphase/DynamicAABBTree.hpp>
+#include <ephysics/collision/broadphase/BroadPhaseAlgorithm.hpp>
+#include <ephysics/memory/Stack.hpp>
+#include <ephysics/engine/Profiler.hpp>
+#include <ephysics/debug.hpp>
+
+using namespace ephysics;
+
+ int TreeNode::NULLTREENODE = -1;
+
+DynamicAABBTree::DynamicAABBTree(float extraAABBGap):
+  this.extraAABBGap(extraAABBGap) {
+	init();
+}
+
+DynamicAABBTree::~DynamicAABBTree() {
+	free(this.nodes);
+}
+
+// Initialize the tree
+void DynamicAABBTree::init() {
+	this.rootNodeID = TreeNode::NULLTREENODE;
+	this.numberNodes = 0;
+	this.numberAllocatedNodes = 8;
+	// Allocate memory for the nodes of the tree
+	this.nodes = (TreeNode*) malloc(this.numberAllocatedNodes * sizeof(TreeNode));
+	assert(this.nodes);
+	memset(this.nodes, 0, this.numberAllocatedNodes * sizeof(TreeNode));
+	// Initialize the allocated nodes
+	for (int i=0; i<this.numberAllocatedNodes - 1; i++) {
+		this.nodes[i].nextNodeID = i + 1;
+		this.nodes[i].height = -1;
+	}
+	this.nodes[this.numberAllocatedNodes - 1].nextNodeID = TreeNode::NULLTREENODE;
+	this.nodes[this.numberAllocatedNodes - 1].height = -1;
+	this.freeNodeID = 0;
+}
+
+// Clear all the nodes and reset the tree
+void DynamicAABBTree::reset() {
+	// Free the allocated memory for the nodes
+	free(this.nodes);
+	// 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 (this.freeNodeID == TreeNode::NULLTREENODE) {
+		assert(this.numberNodes == this.numberAllocatedNodes);
+		// Allocate more nodes in the tree
+		this.numberAllocatedNodes *= 2;
+		TreeNode* oldNodes = this.nodes;
+		this.nodes = (TreeNode*) malloc(this.numberAllocatedNodes * sizeof(TreeNode));
+		assert(this.nodes);
+		memcpy(this.nodes, oldNodes, this.numberNodes * sizeof(TreeNode));
+		free(oldNodes);
+		// Initialize the allocated nodes
+		for (int i=this.numberNodes; i<this.numberAllocatedNodes - 1; i++) {
+			this.nodes[i].nextNodeID = i + 1;
+			this.nodes[i].height = -1;
+		}
+		this.nodes[this.numberAllocatedNodes - 1].nextNodeID = TreeNode::NULLTREENODE;
+		this.nodes[this.numberAllocatedNodes - 1].height = -1;
+		this.freeNodeID = this.numberNodes;
+	}
+	// Get the next free node
+	int freeNodeID = this.freeNodeID;
+	this.freeNodeID = this.nodes[freeNodeID].nextNodeID;
+	this.nodes[freeNodeID].parentID = TreeNode::NULLTREENODE;
+	this.nodes[freeNodeID].height = 0;
+	this.numberNodes++;
+	return freeNodeID;
+}
+
+// Release a node
+void DynamicAABBTree::releaseNode(int nodeID) {
+	assert(this.numberNodes > 0);
+	assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes);
+	assert(this.nodes[nodeID].height >= 0);
+	this.nodes[nodeID].nextNodeID = this.freeNodeID;
+	this.nodes[nodeID].height = -1;
+	this.freeNodeID = nodeID;
+	this.numberNodes--;
+}
+
+// Internally add an object into the tree
+int DynamicAABBTree::addObjectInternal( 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
+	 Vector3f gap(this.extraAABBGap, this.extraAABBGap, this.extraAABBGap);
+	this.nodes[nodeID].aabb.setMin(aabb.getMin() - gap);
+	this.nodes[nodeID].aabb.setMax(aabb.getMax() + gap);
+	// Set the height of the node in the tree
+	this.nodes[nodeID].height = 0;
+	// Insert the new leaf node in the tree
+	insertLeafNode(nodeID);
+	assert(this.nodes[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 < this.numberAllocatedNodes);
+	assert(this.nodes[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).
+boolean DynamicAABBTree::updateObject(int nodeID,  AABB newAABB,  Vector3f displacement, bool forceReinsert) {
+	PROFILE("DynamicAABBTree::updateObject()");
+	assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes);
+	assert(this.nodes[nodeID].isLeaf());
+	assert(this.nodes[nodeID].height >= 0);
+	Log.verbose(" compare : " + this.nodes[nodeID].aabb.minCoordinates + " " + this.nodes[nodeID].aabb.maxCoordinates);
+	Log.verbose("         : " + newAABB.minCoordinates + " " + newAABB.maxCoordinates);
+	// If the new AABB is still inside the fat AABB of the node
+	if (    forceReinsert == false
+	     && this.nodes[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 ant gap
+	this.nodes[nodeID].aabb = newAABB;
+	 Vector3f gap(this.extraAABBGap, this.extraAABBGap, this.extraAABBGap);
+	this.nodes[nodeID].aabb.minCoordinates -= gap;
+	this.nodes[nodeID].aabb.maxCoordinates += gap;
+	// Inflate the fat AABB in direction of the linear motion of the AABB
+	if (displacement.x() < 0.0f) {
+		this.nodes[nodeID].aabb.minCoordinates.setX(this.nodes[nodeID].aabb.minCoordinates.x() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.x());
+	} else {
+		this.nodes[nodeID].aabb.maxCoordinates.setX(this.nodes[nodeID].aabb.maxCoordinates.x() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.x());
+	}
+	if (displacement.y() < 0.0f) {
+		this.nodes[nodeID].aabb.minCoordinates.setY(this.nodes[nodeID].aabb.minCoordinates.y() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.y());
+	} else {
+		this.nodes[nodeID].aabb.maxCoordinates.setY(this.nodes[nodeID].aabb.maxCoordinates.y() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.y());
+	}
+	if (displacement.z() < 0.0f) {
+		this.nodes[nodeID].aabb.minCoordinates.setZ(this.nodes[nodeID].aabb.minCoordinates.z() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.z());
+	} else {
+		this.nodes[nodeID].aabb.maxCoordinates.setZ(this.nodes[nodeID].aabb.maxCoordinates.z() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.z());
+	}
+	Log.error(" compare : " + this.nodes[nodeID].aabb.minCoordinates + " " + this.nodes[nodeID].aabb.maxCoordinates);
+	Log.error("         : " + newAABB.minCoordinates + " " + newAABB.maxCoordinates);
+	if (this.nodes[nodeID].aabb.contains(newAABB) == false) {
+		//Log.critical("ERROR");
+	}
+	assert(this.nodes[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 (this.rootNodeID == TreeNode::NULLTREENODE) {
+		this.rootNodeID = nodeID;
+		this.nodes[this.rootNodeID].parentID = TreeNode::NULLTREENODE;
+		return;
+	}
+	assert(this.rootNodeID != TreeNode::NULLTREENODE);
+	// Find the best sibling node for the new node
+	AABB newNodeAABB = this.nodes[nodeID].aabb;
+	int currentNodeID = this.rootNodeID;
+	while (!this.nodes[currentNodeID].isLeaf()) {
+		int leftChild = this.nodes[currentNodeID].children[0];
+		int rightChild = this.nodes[currentNodeID].children[1];
+		// Compute the merged AABB
+		float volumeAABB = this.nodes[currentNodeID].aabb.getVolume();
+		AABB mergedAABBs;
+		mergedAABBs.mergeTwoAABBs(this.nodes[currentNodeID].aabb, newNodeAABB);
+		float mergedVolume = mergedAABBs.getVolume();
+		// Compute the cost of making the current node the sibbling of the new node
+		float costS = float(2.0) * mergedVolume;
+		// Compute the minimum cost of pushing the new node further down the tree (inheritance cost)
+		float costI = float(2.0) * (mergedVolume - volumeAABB);
+		// Compute the cost of descending into the left child
+		float costLeft;
+		AABB currentAndLeftAABB;
+		currentAndLeftAABB.mergeTwoAABBs(newNodeAABB, this.nodes[leftChild].aabb);
+		if (this.nodes[leftChild].isLeaf()) {   // If the left child is a leaf
+			costLeft = currentAndLeftAABB.getVolume() + costI;
+		} else {
+			float leftChildVolume = this.nodes[leftChild].aabb.getVolume();
+			costLeft = costI + currentAndLeftAABB.getVolume() - leftChildVolume;
+		}
+		// Compute the cost of descending into the right child
+		float costRight;
+		AABB currentAndRightAABB;
+		currentAndRightAABB.mergeTwoAABBs(newNodeAABB, this.nodes[rightChild].aabb);
+		if (this.nodes[rightChild].isLeaf()) {   // If the right child is a leaf
+			costRight = currentAndRightAABB.getVolume() + costI;
+		} else {
+			float rightChildVolume = this.nodes[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 = this.nodes[siblingNode].parentID;
+	int newParentNode = allocateNode();
+	this.nodes[newParentNode].parentID = oldParentNode;
+	this.nodes[newParentNode].aabb.mergeTwoAABBs(this.nodes[siblingNode].aabb, newNodeAABB);
+	this.nodes[newParentNode].height = this.nodes[siblingNode].height + 1;
+	assert(this.nodes[newParentNode].height > 0);
+	// If the sibling node was not the root node
+	if (oldParentNode != TreeNode::NULLTREENODE) {
+		assert(!this.nodes[oldParentNode].isLeaf());
+		if (this.nodes[oldParentNode].children[0] == siblingNode) {
+			this.nodes[oldParentNode].children[0] = newParentNode;
+		} else {
+			this.nodes[oldParentNode].children[1] = newParentNode;
+		}
+		this.nodes[newParentNode].children[0] = siblingNode;
+		this.nodes[newParentNode].children[1] = nodeID;
+		this.nodes[siblingNode].parentID = newParentNode;
+		this.nodes[nodeID].parentID = newParentNode;
+	} else {
+		// If the sibling node was the root node
+		this.nodes[newParentNode].children[0] = siblingNode;
+		this.nodes[newParentNode].children[1] = nodeID;
+		this.nodes[siblingNode].parentID = newParentNode;
+		this.nodes[nodeID].parentID = newParentNode;
+		this.rootNodeID = newParentNode;
+	}
+	// Move up in the tree to change the AABBs that have changed
+	currentNodeID = this.nodes[nodeID].parentID;
+	assert(!this.nodes[currentNodeID].isLeaf());
+	while (currentNodeID != TreeNode::NULLTREENODE) {
+		// Balance the sub-tree of the current node if it is not balanced
+		currentNodeID = balanceSubTreeAtNode(currentNodeID);
+		assert(this.nodes[nodeID].isLeaf());
+		assert(!this.nodes[currentNodeID].isLeaf());
+		int leftChild = this.nodes[currentNodeID].children[0];
+		int rightChild = this.nodes[currentNodeID].children[1];
+		assert(leftChild != TreeNode::NULLTREENODE);
+		assert(rightChild != TreeNode::NULLTREENODE);
+		// Recompute the height of the node in the tree
+		this.nodes[currentNodeID].height = max(this.nodes[leftChild].height,
+												this.nodes[rightChild].height) + 1;
+		assert(this.nodes[currentNodeID].height > 0);
+		// Recompute the AABB of the node
+		this.nodes[currentNodeID].aabb.mergeTwoAABBs(this.nodes[leftChild].aabb, this.nodes[rightChild].aabb);
+		currentNodeID = this.nodes[currentNodeID].parentID;
+	}
+	assert(this.nodes[nodeID].isLeaf());
+}
+
+// Remove a leaf node from the tree
+void DynamicAABBTree::removeLeafNode(int nodeID) {
+	assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes);
+	assert(this.nodes[nodeID].isLeaf());
+	// If we are removing the root node (root node is a leaf in this case)
+	if (this.rootNodeID == nodeID) {
+		this.rootNodeID = TreeNode::NULLTREENODE;
+		return;
+	}
+	int parentNodeID = this.nodes[nodeID].parentID;
+	int grandParentNodeID = this.nodes[parentNodeID].parentID;
+	int siblingNodeID;
+	if (this.nodes[parentNodeID].children[0] == nodeID) {
+		siblingNodeID = this.nodes[parentNodeID].children[1];
+	} else {
+		siblingNodeID = this.nodes[parentNodeID].children[0];
+	}
+	// If the parent of the node to remove is not the root node
+	if (grandParentNodeID != TreeNode::NULLTREENODE) {
+		// Destroy the parent node
+		if (this.nodes[grandParentNodeID].children[0] == parentNodeID) {
+			this.nodes[grandParentNodeID].children[0] = siblingNodeID;
+		} else {
+			assert(this.nodes[grandParentNodeID].children[1] == parentNodeID);
+			this.nodes[grandParentNodeID].children[1] = siblingNodeID;
+		}
+		this.nodes[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::NULLTREENODE) {
+			// Balance the current sub-tree if necessary
+			currentNodeID = balanceSubTreeAtNode(currentNodeID);
+			assert(!this.nodes[currentNodeID].isLeaf());
+			// Get the two children of the current node
+			int leftChildID = this.nodes[currentNodeID].children[0];
+			int rightChildID = this.nodes[currentNodeID].children[1];
+			// Recompute the AABB and the height of the current node
+			this.nodes[currentNodeID].aabb.mergeTwoAABBs(this.nodes[leftChildID].aabb,
+													 this.nodes[rightChildID].aabb);
+			this.nodes[currentNodeID].height = max(this.nodes[leftChildID].height,
+													this.nodes[rightChildID].height) + 1;
+			assert(this.nodes[currentNodeID].height > 0);
+			currentNodeID = this.nodes[currentNodeID].parentID;
+		}
+	} else { // If the parent of the node to remove is the root node
+		// The sibling node becomes the new root node
+		this.rootNodeID = siblingNodeID;
+		this.nodes[siblingNodeID].parentID = TreeNode::NULLTREENODE;
+		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::NULLTREENODE);
+	TreeNode* nodeA = this.nodes + 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 < this.numberAllocatedNodes);
+	assert(nodeCID >= 0 && nodeCID < this.numberAllocatedNodes);
+	TreeNode* nodeB = this.nodes + nodeBID;
+	TreeNode* nodeC = this.nodes + 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 < this.numberAllocatedNodes);
+		assert(nodeGID >= 0 && nodeGID < this.numberAllocatedNodes);
+		TreeNode* nodeF = this.nodes + nodeFID;
+		TreeNode* nodeG = this.nodes + nodeGID;
+		nodeC.children[0] = nodeID;
+		nodeC.parentID = nodeA.parentID;
+		nodeA.parentID = nodeCID;
+		if (nodeC.parentID != TreeNode::NULLTREENODE) {
+			if (this.nodes[nodeC.parentID].children[0] == nodeID) {
+				this.nodes[nodeC.parentID].children[0] = nodeCID;
+			} else {
+				assert(this.nodes[nodeC.parentID].children[1] == nodeID);
+				this.nodes[nodeC.parentID].children[1] = nodeCID;
+			}
+		} else {
+			this.rootNodeID = 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 = max(nodeB.height, nodeG.height) + 1;
+			nodeC.height = 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 = max(nodeB.height, nodeF.height) + 1;
+			nodeC.height = 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 < this.numberAllocatedNodes);
+		assert(nodeGID >= 0 && nodeGID < this.numberAllocatedNodes);
+		TreeNode* nodeF = this.nodes + nodeFID;
+		TreeNode* nodeG = this.nodes + nodeGID;
+		nodeB.children[0] = nodeID;
+		nodeB.parentID = nodeA.parentID;
+		nodeA.parentID = nodeBID;
+		if (nodeB.parentID != TreeNode::NULLTREENODE) {
+			if (this.nodes[nodeB.parentID].children[0] == nodeID) {
+				this.nodes[nodeB.parentID].children[0] = nodeBID;
+			} else {
+				assert(this.nodes[nodeB.parentID].children[1] == nodeID);
+				this.nodes[nodeB.parentID].children[1] = nodeBID;
+			}
+		} else {
+			this.rootNodeID = 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 = max(nodeC.height, nodeG.height) + 1;
+			nodeB.height = 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 = max(nodeC.height, nodeF.height) + 1;
+			nodeB.height = 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( AABB aabb, Function<void(int nodeId)> callback)  {
+	if (callback == null) {
+		Log.error("call with null callback");
+		return;
+	}
+	// Create a stack with the nodes to visit
+	Stack<int, 64> stack;
+	stack.push(this.rootNodeID);
+	// 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::NULLTREENODE) {
+			continue;
+		}
+		// Get the corresponding node
+		 TreeNode* nodeToVisit = this.nodes + 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(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( ephysics::Ray ray, Function<float(int nodeId,  ephysics::Ray ray)> callback)  {
+	PROFILE("DynamicAABBTree::raycast()");
+	if (callback == null) {
+		Log.error("call with null callback");
+		return;
+	}
+	float maxFraction = ray.maxFraction;
+	Stack<int, 128> stack;
+	stack.push(this.rootNodeID);
+	// 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::NULLTREENODE) {
+			continue;
+		}
+		// Get the corresponding node
+		 TreeNode* node = this.nodes + nodeID;
+		Ray rayTemp(ray.point1, ray.point2, maxFraction);
+		// Test if the ray intersects with the current node AABB
+		if (node.aabb.testRayIntersect(rayTemp) == false) {
+			continue;
+		}
+		// If the node is a leaf of the tree
+		if (node.isLeaf()) {
+			// Call the callback that will raycast again the broad-phase shape
+			float hitFraction = callback(nodeID, rayTemp);
+			// If the user returned a hitFraction of zero, it means that
+			// the raycasting should stop here
+			if (hitFraction == 0.0f) {
+				return;
+			}
+			// If the user returned a positive fraction
+			if (hitFraction > 0.0f) {
+				// 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]);
+		}
+	}
+}
+
+// Return true if the node is a leaf of the tree
+boolean TreeNode::isLeaf()  {
+	return (height == 0);
+}
+
+// Return the fat AABB corresponding to a given node ID
+ AABB DynamicAABBTree::getFatAABB(int nodeID)  {
+	assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes);
+	return this.nodes[nodeID].aabb;
+}
+
+// Return the pointer to the data array of a given leaf node of the tree
+int* DynamicAABBTree::getNodeDataInt(int nodeID)  {
+	assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes);
+	assert(this.nodes[nodeID].isLeaf());
+	return this.nodes[nodeID].dataInt;
+}
+
+// Return the pointer to the data pointer of a given leaf node of the tree
+void* DynamicAABBTree::getNodeDataPointer(int nodeID)  {
+	assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes);
+	assert(this.nodes[nodeID].isLeaf());
+	return this.nodes[nodeID].dataPointer;
+}
+
+// Return the root AABB of the tree
+AABB DynamicAABBTree::getRootAABB()  {
+	return getFatAABB(this.rootNodeID);
+}
+
+// Add an object into the tree. This method creates a new leaf node in the tree and
+// returns the ID of the corresponding node.
+int DynamicAABBTree::addObject( AABB aabb, int data1, int data2) {
+	int nodeId = addObjectInternal(aabb);
+	this.nodes[nodeId].dataInt[0] = data1;
+	this.nodes[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.
+int DynamicAABBTree::addObject( AABB aabb, void* data) {
+	int nodeId = addObjectInternal(aabb);
+	this.nodes[nodeId].dataPointer = data;
+	return nodeId;
+}
+
+
+#ifdef DEBUG
+
+// Check if the tree structure is valid (for debugging purpose)
+void DynamicAABBTree::check()  {
+	// Recursively check each node
+	checkNode(this.rootNodeID);
+	int nbFreeNodes = 0;
+	int freeNodeID = this.freeNodeID;
+	// Check the free nodes
+	while(freeNodeID != TreeNode::NULLTREENODE) {
+		assert(0 <= freeNodeID && freeNodeID < this.numberAllocatedNodes);
+		freeNodeID = this.nodes[freeNodeID].nextNodeID;
+		nbFreeNodes++;
+	}
+	assert(this.numberNodes + nbFreeNodes == this.numberAllocatedNodes);
+}
+
+// Check if the node structure is valid (for debugging purpose)
+void DynamicAABBTree::checkNode(int nodeID)  {
+	if (nodeID == TreeNode::NULLTREENODE) {
+		return;
+	}
+	// If it is the root
+	if (nodeID == this.rootNodeID) {
+		assert(this.nodes[nodeID].parentID == TreeNode::NULLTREENODE);
+	}
+	// Get the children nodes
+	TreeNode* pNode = this.nodes + 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::NULLTREENODE);
+		assert(rightChild == TreeNode::NULLTREENODE);
+		assert(pNode.height == 0);
+	} else {
+		// Check that the children node IDs are valid
+		assert(0 <= leftChild && leftChild < this.numberAllocatedNodes);
+		assert(0 <= rightChild && rightChild < this.numberAllocatedNodes);
+		// Check that the children nodes have the correct parent node
+		assert(this.nodes[leftChild].parentID == nodeID);
+		assert(this.nodes[rightChild].parentID == nodeID);
+		// Check the height of node
+		int height = 1 + max(this.nodes[leftChild].height, this.nodes[rightChild].height);
+		assert(this.nodes[nodeID].height == height);
+		// Check the AABB of the node
+		AABB aabb;
+		aabb.mergeTwoAABBs(this.nodes[leftChild].aabb, this.nodes[rightChild].aabb);
+		assert(aabb.getMin() == this.nodes[nodeID].aabb.getMin());
+		assert(aabb.getMax() == this.nodes[nodeID].aabb.getMax());
+		// Recursively check the children nodes
+		checkNode(leftChild);
+		checkNode(rightChild);
+	}
+}
+
+// Compute the height of the tree
+int DynamicAABBTree::computeHeight() {
+	return computeHeight(this.rootNodeID);
+}
+
+// Compute the height of a given node in the tree
+int DynamicAABBTree::computeHeight(int nodeID) {
+	assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes);
+	TreeNode* node = this.nodes + 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 + max(leftHeight, rightHeight);
+}
+
+#endif

src/org/atriaSoft/ephysics/collision/broadphase/DynamicAABBTree.java

@@ -0,0 +1,102 @@
+package org.atriaSoft.ephysics.collision.broadphase;
+
+	// TODO: to replace this, create a Tree<T> template (multiple child) or TreeRedBlack<T>
+	/**
+	 * @brief It represents a node of the dynamic AABB tree.
+	 */
+	struct TreeNode {
+		 static int NULLTREENODE; //!< Null tree node ant
+		/**
+		 * @brief A node is either in the tree (has a parent) or in the free nodes list (has a next node)
+		 */
+		union {
+			int parentID; //!< Parent node ID
+			int nextNodeID; //!< Next allocated node ID
+		};
+		/**
+		 * @brief A node is either a leaf (has data) or is an internal node (has children)
+		 */
+		union {
+			int children[2]; //!< Left and right child of the node (children[0] = left child)
+			//! Two pieces of data stored at that node (in case the node is a leaf)
+			union {
+				int dataInt[2];
+				void* dataPointer;
+			};
+		};
+		int16t height; //!< Height of the node in the tree
+		AABB aabb; //!< Fat axis aligned bounding box (AABB) corresponding to the node
+		/// Return true if the node is a leaf of the tree
+		boolean isLeaf() ;
+	};
+	
+	/**
+	 * @brief It 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:
+			TreeNode* this.nodes; //!< Pointer to the memory location of the nodes of the tree
+			int this.rootNodeID; //!< ID of the root node of the tree
+			int this.freeNodeID; //!< ID of the first node of the list of free (allocated) nodes in the tree that we can use
+			int this.numberAllocatedNodes; //!< Number of allocated nodes in the tree
+			int this.numberNodes; //!< Number of nodes in the tree
+			float this.extraAABBGap; //!< 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
+			/// 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( AABB aabb);
+			/// Initialize the tree
+			void init();
+			#ifndef NDEBUG
+				/// Check if the tree structure is valid (for debugging purpose)
+				void check() ;
+				/// Check if the node structure is valid (for debugging purpose)
+				void checkNode(int nodeID) ;
+			#endif
+		public:
+			/// Constructor
+			DynamicAABBTree(float extraAABBGap = 0.0f);
+			/// Destructor
+			 ~DynamicAABBTree();
+			/// Add an object into the tree (where node data are two integers)
+			int addObject( AABB aabb, int data1, int data2);
+			/// Add an object into the tree (where node data is a pointer)
+			int addObject( AABB aabb, void* data);
+			/// Remove an object from the tree
+			void removeObject(int nodeID);
+			/// Update the dynamic tree after an object has moved.
+			boolean updateObject(int nodeID,  AABB newAABB,  Vector3f displacement, bool forceReinsert = false);
+			/// Return the fat AABB corresponding to a given node ID
+			 AABB getFatAABB(int nodeID) ;
+			/// Return the pointer to the data array of a given leaf node of the tree
+			int* getNodeDataInt(int nodeID) ;
+			/// Return the data pointer of a given leaf node of the tree
+			void* getNodeDataPointer(int nodeID) ;
+			/// Report all shapes overlapping with the AABB given in parameter.
+			void reportAllShapesOverlappingWithAABB( AABB aabb, Function<void(int nodeId)> callback) ;
+			/// Ray casting method
+			void raycast( Ray ray, Function<float(int nodeId,  ephysics::Ray ray)> callback) ;
+			/// Compute the height of the tree
+			int computeHeight();
+			/// Return the root AABB of the tree
+			AABB getRootAABB() ;
+			/// Clear all the nodes and reset the tree
+			void reset();
+	};
+
+
+}

src/org/atriaSoft/ephysics/collision/narrowphase/CollisionDispatch.java

@@ -0,0 +1,21 @@
+package org.atriaSoft.ephysics.collision.narrowphase;
+
+	/**
+	 * @biref 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 {
+		public:
+			/// Initialize the collision dispatch configuration
+			 void init(CollisionDetection* collisionDetection) {
+				// Nothing to do ...
+			}
+			/// Select and return the narrow-phase collision detection algorithm to
+			/// use between two types of collision shapes.
+			NarrowPhaseAlgorithm* selectAlgorithm(int shape1Type, int shape2Type) = 0;
+	};
+
+}
+
+

src/org/atriaSoft/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp

@@ -0,0 +1,241 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/shapes/ConcaveShape.hpp>
+#include <ephysics/collision/shapes/TriangleShape.hpp>
+#include <ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.hpp>
+#include <ephysics/collision/CollisionDetection.hpp>
+#include <ephysics/engine/CollisionWorld.hpp>
+#include <etk/algorithm.hpp>
+
+using namespace ephysics;
+
+ConcaveVsConvexAlgorithm::ConcaveVsConvexAlgorithm() {
+	
+}
+
+void ConcaveVsConvexAlgorithm::testCollision( CollisionShapeInfo shape1Info,
+                                              CollisionShapeInfo shape2Info,
+                                             NarrowPhaseCallback* callback) {
+	ProxyShape* convexProxyShape;
+	ProxyShape* concaveProxyShape;
+	 ConvexShape* convexShape;
+	 ConcaveShape* concaveShape;
+	// Collision shape 1 is convex, collision shape 2 is concave
+	if (shape1Info.collisionShape.isConvex()) {
+		convexProxyShape = shape1Info.proxyShape;
+		convexShape = staticcast< ConvexShape*>(shape1Info.collisionShape);
+		concaveProxyShape = shape2Info.proxyShape;
+		concaveShape = staticcast< ConcaveShape*>(shape2Info.collisionShape);
+	} else {
+		// Collision shape 2 is convex, collision shape 1 is concave
+		convexProxyShape = shape2Info.proxyShape;
+		convexShape = staticcast< ConvexShape*>(shape2Info.collisionShape);
+		concaveProxyShape = shape1Info.proxyShape;
+		concaveShape = staticcast< ConcaveShape*>(shape1Info.collisionShape);
+	}
+	// Set the parameters of the callback object
+	ConvexVsTriangleCallback convexVsTriangleCallback;
+	convexVsTriangleCallback.setCollisionDetection(this.collisionDetection);
+	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()) {
+		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, callback);
+	} else {
+		convexVsTriangleCallback.setNarrowPhaseCallback(callback);
+		// Call the convex vs triangle callback for each triangle of the concave shape
+		concaveShape.testAllTriangles(convexVsTriangleCallback, aabb);
+	}
+}
+
+void ConvexVsTriangleCallback::testTriangle( Vector3f* trianglePoints) {
+	// Create a triangle collision shape
+	float margin = this.concaveShape.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 = this.collisionDetection.getCollisionAlgorithm(triangleShape.getType(), this.convexShape.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(this.overlappingPair);
+	// Create the CollisionShapeInfo objects
+	CollisionShapeInfo shapeConvexInfo(this.convexProxyShape,
+	                                   this.convexShape,
+	                                   this.convexProxyShape.getLocalToWorldTransform(),
+	                                   this.overlappingPair,
+	                                   this.convexProxyShape.getCachedCollisionData());
+	CollisionShapeInfo shapeConcaveInfo(this.concaveProxyShape,
+	                                    triangleShape,
+	                                    this.concaveProxyShape.getLocalToWorldTransform(),
+	                                    this.overlappingPair,
+	                                    this.concaveProxyShape.getCachedCollisionData());
+	// Use the collision algorithm to test collision between the triangle and the other convex shape
+	algo.testCollision(shapeConvexInfo, shapeConcaveInfo, this.narrowPhaseCallback);
+}
+
+static boolean sortFunction( SmoothMeshContactInfo contact1,  SmoothMeshContactInfo contact2) {
+	return contact1.contactInfo.penetrationDepth <= contact2.contactInfo.penetrationDepth;
+}
+
+void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair,
+                                                          Vector<SmoothMeshContactInfo> contactPoints,
+                                                          NarrowPhaseCallback* callback) {
+	// Set with the triangle vertices already processed to void further contacts with same triangle
+	Vector<Pair<int, Vector3f>> processTriangleVertices;
+	// Sort the list of narrow-phase contacts according to their penetration depth
+	algorithm::quickSort(contactPoints, sortFunction);
+	// For each contact point (from smaller penetration depth to larger)
+	Vector<SmoothMeshContactInfo>::Iterator it;
+	for (it = contactPoints.begin(); it != contactPoints.end(); ++it) {
+		 SmoothMeshContactInfo info = *it;
+		 Vector3f contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
+		// Compute the barycentric coordinates of the point in the triangle
+		float u, v, w;
+		computeBarycentricCoordinatesInTriangle(info.triangleVertices[0],
+												info.triangleVertices[1],
+												info.triangleVertices[2],
+												contactPoint, u, v, w);
+		int nbZeros = 0;
+		boolean isUZero = approxEqual(u, 0, 0.0001);
+		boolean isVZero = approxEqual(v, 0, 0.0001);
+		boolean isWZero = approxEqual(w, 0, 0.0001);
+		if (isUZero) {
+			nbZeros++;
+		}
+		if (isVZero) {
+			nbZeros++;
+		}
+		if (isWZero) {
+			nbZeros++;
+		}
+		// If it is a vertex contact
+		if (nbZeros == 2) {
+			Vector3f 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
+				callback.notifyContact(overlappingPair, info.contactInfo);
+			}
+		} else if (nbZeros == 1) {
+			// If it is an edge contact
+			Vector3f contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]);
+			Vector3f 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
+				callback.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
+			Vector3f a = info.triangleVertices[1] - info.triangleVertices[0];
+			Vector3f b = info.triangleVertices[2] - info.triangleVertices[0];
+			Vector3f localNormal = a.cross(b);
+			newContactInfo.normal = firstShape.getLocalToWorldTransform().getOrientation() * localNormal;
+			Vector3f firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
+			Vector3f 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
+			Transform3D worldToLocalSecondPoint = secondShape.getLocalToWorldTransform().getInverse();
+			if (info.isFirstShapeTriangle) {
+				Vector3f newSecondWorldPoint = firstWorldPoint + newContactInfo.normal;
+				newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint;
+			} else {
+				Vector3f newSecondWorldPoint = firstWorldPoint - newContactInfo.normal;
+				newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint;
+			}
+			// Report the contact
+			callback.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]);
+	}
+}
+
+boolean ConcaveVsConvexAlgorithm::hasVertexBeenProcessed( Vector<Pair<int, Vector3f>> processTriangleVertices,  Vector3f vertex)  {
+	/* TODO : Vector<Pair<int, Vector3f>> was an unordered map ... ==> stupid idee... I replace code because I do not have enouth time to do something good...
+	int key = int(vertex.x() * vertex.y() * vertex.z());
+	auto range = processTriangleVertices.equalrange(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;
+	*/
+	// TODO : This is not really the same ...
+	for (auto it: processTriangleVertices) {
+		if (    vertex.x() == it.second.x()
+		     && vertex.y() == it.second.y()
+		     && vertex.z() == it.second.z()) {
+			return true;
+		}
+	}
+	return false;
+}
+
+void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair,
+                                                        ContactPointInfo contactInfo) {
+	Vector3f triangleVertices[3];
+	boolean isFirstShapeTriangle;
+	// If the collision shape 1 is the triangle
+	if (contactInfo.collisionShape1.getType() == TRIANGLE) {
+		assert(contactInfo.collisionShape2.getType() != TRIANGLE);
+		 TriangleShape* triangleShape = staticcast< 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);
+		 TriangleShape* triangleShape = staticcast< 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
+	this.contactPoints.pushBack(smoothContactInfo);
+}

src/org/atriaSoft/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.java

@@ -0,0 +1,135 @@
+package org.atriaSoft.ephysics.collision.narrowphase;
+
+
+	/**
+	 * @brief 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 extends  TriangleCallback {
+		protected:
+			CollisionDetection* this.collisionDetection; //!< Pointer to the collision detection object
+			NarrowPhaseCallback* this.narrowPhaseCallback; //!< Narrow-phase collision callback
+			 ConvexShape* this.convexShape; //!< Convex collision shape to test collision with
+			 ConcaveShape* this.concaveShape; //!< Concave collision shape
+			ProxyShape* this.convexProxyShape; //!< Proxy shape of the convex collision shape
+			ProxyShape* this.concaveProxyShape; //!< Proxy shape of the concave collision shape
+			OverlappingPair* this.overlappingPair; //!< Broadphase overlapping pair
+			static boolean contactsDepthCompare( ContactPointInfo contact1,
+			                                  ContactPointInfo contact2);
+		public:
+			/// Set the collision detection pointer
+			void setCollisionDetection(CollisionDetection* collisionDetection) {
+				this.collisionDetection = collisionDetection;
+			}
+			/// Set the narrow-phase collision callback
+			void setNarrowPhaseCallback(NarrowPhaseCallback* callback) {
+				this.narrowPhaseCallback = callback;
+			}
+			/// Set the convex collision shape to test collision with
+			void setConvexShape( ConvexShape* convexShape) {
+				this.convexShape = convexShape;
+			}
+			/// Set the concave collision shape
+			void setConcaveShape( ConcaveShape* concaveShape) {
+				this.concaveShape = concaveShape;
+			}
+			/// Set the broadphase overlapping pair
+			void setOverlappingPair(OverlappingPair* overlappingPair) {
+				this.overlappingPair = overlappingPair;
+			}
+			/// Set the proxy shapes of the two collision shapes
+			void setProxyShapes(ProxyShape* convexProxyShape, ProxyShape* concaveProxyShape) {
+				this.convexProxyShape = convexProxyShape;
+				this.concaveProxyShape = concaveProxyShape;
+			}
+			/// Test collision between a triangle and the convex mesh shape
+			 void testTriangle( Vector3f* trianglePoints);
+	};
+
+	/**
+	 * @brief This class is used to store data about a contact with a triangle for the smooth
+	 * mesh algorithm.
+	 */
+	class SmoothMeshContactInfo {
+		public:
+			ContactPointInfo contactInfo;
+			boolean isFirstShapeTriangle;
+			Vector3f triangleVertices[3];
+			/// Constructor
+			SmoothMeshContactInfo( ContactPointInfo contact,
+			                      boolean firstShapeTriangle,
+			                       Vector3f trianglePoint1,
+			                       Vector3f trianglePoint2,
+			                       Vector3f trianglePoint3):
+			  contactInfo(contact) {
+				isFirstShapeTriangle = firstShapeTriangle;
+				triangleVertices[0] = trianglePoint1;
+				triangleVertices[1] = trianglePoint2;
+				triangleVertices[2] = trianglePoint3;
+			}
+			SmoothMeshContactInfo() {
+				// TODO: add it for Vector
+			}
+	};
+
+	/*
+	struct ContactsDepthCompare {
+		boolean operator()( SmoothMeshContactInfo contact1,  SmoothMeshContactInfo contact2) {
+			return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth;
+		}
+	};
+	*/
+
+	/**
+	 * @brief 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 extends  NarrowPhaseCallback {
+		private:
+			Vector<SmoothMeshContactInfo> this.contactPoints;
+		public:
+			// Constructor
+			SmoothCollisionNarrowPhaseCallback(Vector<SmoothMeshContactInfo> contactPoints):
+			  this.contactPoints(contactPoints) {
+				
+			}
+			/// Called by a narrow-phase collision algorithm when a new contact has been found
+			 void notifyContact(OverlappingPair* overlappingPair,  ContactPointInfo contactInfo);
+	};
+
+	/**
+	 * @brief 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 extends  NarrowPhaseAlgorithm {
+		protected :
+			/// Private copy-ructor
+			ConcaveVsConvexAlgorithm( ConcaveVsConvexAlgorithm algorithm);
+			/// Private assignment operator
+			ConcaveVsConvexAlgorithm operator=( ConcaveVsConvexAlgorithm algorithm);
+			/// Process the concave triangle mesh collision using the smooth mesh collision algorithm
+			void processSmoothMeshCollision(OverlappingPair* overlappingPair,
+			                                Vector<SmoothMeshContactInfo> contactPoints,
+			                                NarrowPhaseCallback* narrowPhaseCallback);
+			/// Add a triangle vertex into the set of processed triangles
+			void addProcessedVertex(Vector<Pair<int, Vector3f>> processTriangleVertices,  Vector3f vertex) {
+				processTriangleVertices.pushBack(makePair(int(vertex.x() * vertex.y() * vertex.z()), vertex));
+			}
+			/// Return true if the vertex is in the set of already processed vertices
+			boolean hasVertexBeenProcessed( Vector<Pair<int, Vector3f>> processTriangleVertices,
+			                             Vector3f vertex) ;
+		public :
+			/// Constructor
+			ConcaveVsConvexAlgorithm();
+			/// Compute a contact info if the two bounding volume collide
+			 void testCollision( CollisionShapeInfo shape1Info,
+			                            CollisionShapeInfo shape2Info,
+			                           NarrowPhaseCallback* narrowPhaseCallback);
+	};
+
+}
+

src/org/atriaSoft/ephysics/collision/narrowphase/DefaultCollisionDispatch.cpp

@@ -0,0 +1,47 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+// Libraries
+#include <ephysics/collision/narrowphase/DefaultCollisionDispatch.hpp>
+#include <ephysics/collision/shapes/CollisionShape.hpp>
+
+using namespace ephysics;
+
+
+DefaultCollisionDispatch::DefaultCollisionDispatch() {
+	
+}
+
+
+void DefaultCollisionDispatch::init(CollisionDetection* collisionDetection) {
+	// Initialize the collision algorithms
+	this.sphereVsSphereAlgorithm.init(collisionDetection);
+	this.GJKAlgorithm.init(collisionDetection);
+	this.concaveVsConvexAlgorithm.init(collisionDetection);
+}
+
+
+NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int type2) {
+	CollisionShapeType shape1Type = staticcast<CollisionShapeType>(type1);
+	CollisionShapeType shape2Type = staticcast<CollisionShapeType>(type2);
+	// Sphere vs Sphere algorithm
+	if (shape1Type == SPHERE && shape2Type == SPHERE) {
+		return this.sphereVsSphereAlgorithm;
+	} else if (    (    !CollisionShape::isConvex(shape1Type)
+	                 && CollisionShape::isConvex(shape2Type) )
+	            || (    !CollisionShape::isConvex(shape2Type)
+	                 && CollisionShape::isConvex(shape1Type) ) ) {
+		// Concave vs Convex algorithm
+		return this.concaveVsConvexAlgorithm;
+	} else if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) {
+		// Convex vs Convex algorithm (GJK algorithm)
+		return this.GJKAlgorithm;
+	} else {
+		return null;
+	}
+}

src/org/atriaSoft/ephysics/collision/narrowphase/DefaultCollisionDispatch.java

@@ -0,0 +1,23 @@
+package org.atriaSoft.ephysics.collision.narrowphase;
+
+	/**
+	 * @brief This is the default collision dispatch configuration use in ephysics.
+	 * Collision dispatching decides which collision
+	 * algorithm to use given two types of proxy collision shapes.
+	 */
+	class DefaultCollisionDispatch extends  CollisionDispatch {
+		protected:
+			SphereVsSphereAlgorithm this.sphereVsSphereAlgorithm; //!< Sphere vs Sphere collision algorithm
+			ConcaveVsConvexAlgorithm this.concaveVsConvexAlgorithm; //!< Concave vs Convex collision algorithm
+			GJKAlgorithm this.GJKAlgorithm; //!< GJK Algorithm
+		public:
+			/**
+			 * @brief Constructor
+			 */
+			DefaultCollisionDispatch();
+			void init(CollisionDetection* collisionDetection) ;
+			NarrowPhaseAlgorithm* selectAlgorithm(int type1, int type2) ;
+	};
+}
+
+

src/org/atriaSoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp

@@ -0,0 +1,353 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/narrowphase/EPA/EPAAlgorithm.hpp>
+#include <ephysics/engine/Profiler.hpp>
+#include <ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp>
+#include <ephysics/collision/narrowphase/EPA/TrianglesStore.hpp>
+
+using namespace ephysics;
+
+EPAAlgorithm::EPAAlgorithm() {
+	
+}
+
+EPAAlgorithm::~EPAAlgorithm() {
+	
+}
+
+int EPAAlgorithm::isOriginInTetrahedron( Vector3f p1,  Vector3f p2,  Vector3f p3,  Vector3f p4)  {
+	// Check vertex 1
+	Vector3f normal1 = (p2-p1).cross(p3-p1);
+	if ((normal1.dot(p1) > 0.0) == (normal1.dot(p4) > 0.0)) {
+		return 4;
+	}
+	// Check vertex 2
+	Vector3f normal2 = (p4-p2).cross(p3-p2);
+	if ((normal2.dot(p2) > 0.0) == (normal2.dot(p1) > 0.0)) {
+		return 1;
+	}
+	// Check vertex 3
+	Vector3f normal3 = (p4-p3).cross(p1-p3);
+	if ((normal3.dot(p3) > 0.0) == (normal3.dot(p2) > 0.0)) {
+		return 2;
+	}
+	// Check vertex 4
+	Vector3f 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;
+}
+
+void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex,
+                                                           CollisionShapeInfo shape1Info,
+                                                            Transform3D transform1,
+                                                           CollisionShapeInfo shape2Info,
+                                                            Transform3D transform2,
+                                                           Vector3f vector,
+                                                           NarrowPhaseCallback* narrowPhaseCallback) {
+	PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()");
+	assert(shape1Info.collisionShape.isConvex());
+	assert(shape2Info.collisionShape.isConvex());
+	 ConvexShape* shape1 = staticcast< ConvexShape*>(shape1Info.collisionShape);
+	 ConvexShape* shape2 = staticcast< ConvexShape*>(shape2Info.collisionShape);
+	void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
+	void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
+	Vector3f suppPointsA[MAXSUPPORTPOINTS];  // Support points of object A in local coordinates
+	Vector3f suppPointsB[MAXSUPPORTPOINTS];  // Support points of object B in local coordinates
+	Vector3f points[MAXSUPPORTPOINTS];	   // Current points
+	TrianglesStore triangleStore;			 // Store the triangles
+	Set<TriangleEPA*> triangleHeap; // list of face candidate of the EPA algorithm sorted lower square dist to upper square dist
+	triangleHeap.setComparator([](TriangleEPA *   face1, TriangleEPA *   face2) {
+	                           		return (face1.getDistSquare() < face2.getDistSquare());
+	                           });
+	// Transform3D 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)
+	Transform3D 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
+	int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
+	// Compute the tolerance
+	float tolerance = FLTEPSILON * simplex.getMaxLengthSquareOfAPoint();
+	// 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 ruct 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
+			// ruct the polytope are the three support points in those three directions
+			// v1, v2 and v3.
+			// Direction of the segment
+			Vector3f d = (points[1] - points[0]).safeNormalized();
+			// Choose the coordinate axis from the minimal absolute component of the vector d
+			int minAxis = d.absolute().getMinAxis();
+			// Compute sin(60)
+			 float sin60 = float(sqrt(3.0)) * 0.5f;
+			// 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
+			Vector3f v1 = d.cross(Vector3f(minAxis == 0, minAxis == 1, minAxis == 2));
+			Vector3f v2 = rotationQuat * v1;
+			Vector3f 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 ruct 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 ructed tetrahedron is not correct
+				if (!((face0 != NULL) && (face1 != NULL) hjkhjkhjkhkj (face2 != NULL) hjkhjkhjkhkj (face3 != NULL)
+				   && face0.getDistSquare() > 0.0 hjkhjkhjkhkj face1.getDistSquare() > 0.0
+				   && face2.getDistSquare() > 0.0 hjkhjkhjkhkj 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, FLTMAX);
+				addFaceCandidate(face1, triangleHeap, FLTMAX);
+				addFaceCandidate(face2, triangleHeap, FLTMAX);
+				addFaceCandidate(face3, triangleHeap, FLTMAX);
+				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
+			Vector3f v1 = points[1] - points[0];
+			Vector3f v2 = points[2] - points[0];
+			Vector3f 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 ruct 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 ruct 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 ructed 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, FLTMAX);
+			addFaceCandidate(face1, triangleHeap, FLTMAX);
+			addFaceCandidate(face2, triangleHeap, FLTMAX);
+			addFaceCandidate(face3, triangleHeap, FLTMAX);
+			nbVertices = 4;
+		}
+		break;
+	}
+	// At this point, we have a polytope that contains the origin. Therefore, we
+	// can run the EPA algorithm.
+	if (triangleHeap.size() == 0) {
+		return;
+	}
+	TriangleEPA* triangle = 0;
+	float upperBoundSquarePenDepth = FLTMAX;
+	do {
+		triangle = triangleHeap[0];
+		triangleHeap.popFront();
+		Log.info("rm from heap:");
+		for (long iii=0; iii<triangleHeap.size(); ++iii) {
+			Log.info("    [" + iii + "] " + triangleHeap[iii].getDistSquare());
+		}
+		// 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 == MAXSUPPORTPOINTS) {
+				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
+			float wDotv = points[indexNewVertex].dot(triangle.getClosestPoint());
+			Log.info("      point=" + points[indexNewVertex]);
+			Log.info("close point=" + triangle.getClosestPoint());
+			Log.info("         ==>" + wDotv);
+			if (wDotv < 0.0) {
+				Log.error("depth penetration error " + wDotv);
+				continue;
+			}
+			EPHYASSERT(wDotv >= 0.0, "depth penetration error " + wDotv);
+			float wDotVSquare = wDotv * wDotv / triangle.getDistSquare();
+			if (wDotVSquare < upperBoundSquarePenDepth) {
+				upperBoundSquarePenDepth = wDotVSquare;
+			}
+			// Compute the error
+			float error = wDotv - triangle.getDistSquare();
+			if (error <= max(tolerance, RELERRORSQUARE * 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.
+			long 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, upperBoundSquarePenDepth);
+				i++;
+			}
+		}
+	} while(    triangleHeap.size() > 0
+	         && triangleHeap[0].getDistSquare() <= upperBoundSquarePenDepth);
+	// Compute the contact info
+	vector = transform1.getOrientation() * triangle.getClosestPoint();
+	Vector3f pALocal = triangle.computeClosestPointOfObject(suppPointsA);
+	Vector3f pBLocal = body2Tobody1.getInverse() * triangle.computeClosestPointOfObject(suppPointsB);
+	Vector3f normal = vector.safeNormalized();
+	float penetrationDepth = vector.length();
+	EPHYASSERT(penetrationDepth >= 0.0, "penetration depth <0");
+	if (normal.length2() < FLTEPSILON) {
+		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/org/atriaSoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.java

@@ -0,0 +1,71 @@
+package org.atriaSoft.ephysics.collision.narrowphase.EPA;
+
+	/// Maximum number of support points of the polytope
+	 int MAXSUPPORTPOINTS = 100;
+	/// Maximum number of facets of the polytope
+	 int MAXFACETS = 200;
+	/**
+	 * @brief 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:
+			/// Private copy-ructor
+			EPAAlgorithm( EPAAlgorithm algorithm);
+			/// Private assignment operator
+			EPAAlgorithm operator=( EPAAlgorithm algorithm);
+			/// Add a triangle face in the candidate triangle heap
+			void addFaceCandidate(TriangleEPA* triangle,
+			                      Set<TriangleEPA*> heap,
+			                      float 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.add(triangle);
+					Log.info("add in heap:");
+					for (long iii=0; iii<heap.size(); ++iii) {
+						Log.info("    [" + iii + "] " + heap[iii].getDistSquare());
+					}
+				}
+			}
+			// 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 isOriginInTetrahedron( Vector3f p1,  Vector3f p2,  Vector3f p3,  Vector3f p4) ;
+		public:
+			/// Constructor
+			EPAAlgorithm();
+			/// Initalize the algorithm
+			void init() {
+				
+			}
+			// 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 computePenetrationDepthAndContactPoints( Simplex simplex,
+			                                             CollisionShapeInfo shape1Info,
+			                                              Transform3D transform1,
+			                                             CollisionShapeInfo shape2Info,
+			                                              Transform3D transform2,
+			                                             Vector3f v,
+			                                             NarrowPhaseCallback* narrowPhaseCallback);
+	};
+}
+

src/org/atriaSoft/ephysics/collision/narrowphase/EPA/EdgeEPA.cpp

@@ -0,0 +1,97 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/narrowphase/EPA/EdgeEPA.hpp>
+#include <ephysics/collision/narrowphase/EPA/TriangleEPA.hpp>
+#include <ephysics/collision/narrowphase/EPA/TrianglesStore.hpp>
+#include <etk/types.hpp>
+
+using namespace ephysics;
+
+
+EdgeEPA::EdgeEPA() {
+	
+}
+
+EdgeEPA::EdgeEPA(TriangleEPA* ownerTriangle, int index):
+  this.ownerTriangle(ownerTriangle),
+  this.index(index) {
+	assert(index >= 0 && index < 3);
+}
+
+EdgeEPA::EdgeEPA( EdgeEPA obj):
+  this.ownerTriangle(obj.ownerTriangle),
+  this.index(obj.index) {
+	
+}
+
+EdgeEPA::EdgeEPA(EdgeEPA&& obj):
+  this.ownerTriangle(null) {
+	swap(this.ownerTriangle, obj.ownerTriangle);
+	swap(this.index, obj.index);
+}
+
+int EdgeEPA::getSourceVertexIndex()  {
+	return (*this.ownerTriangle)[this.index];
+}
+
+int EdgeEPA::getTargetVertexIndex()  {
+	return (*this.ownerTriangle)[indexOfNextCounterClockwiseEdge(this.index)];
+}
+
+boolean EdgeEPA::computeSilhouette( Vector3f* vertices, int indexNewVertex,
+								TrianglesStore triangleStore) {
+	// If the edge has not already been visited
+	if (!this.ownerTriangle.getIsObsolete()) {
+		// If the triangle of this edge is not visible from the given point
+		if (!this.ownerTriangle.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
+			this.ownerTriangle.setIsObsolete(true);
+			int backup = triangleStore.getNbTriangles();
+			if(!this.ownerTriangle.getAdjacentEdge(indexOfNextCounterClockwiseEdge(this.this.index)).computeSilhouette(vertices,
+				                                                                                                   indexNewVertex,
+				                                                                                                   triangleStore)) {
+				this.ownerTriangle.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 (!this.ownerTriangle.getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(this.this.index)).computeSilhouette(vertices,
+				                                                                                                               indexNewVertex,
+				                                                                                                               triangleStore)) {
+				this.ownerTriangle.setIsObsolete(false);
+				triangleStore.resize(backup);
+				TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
+																  getTargetVertexIndex(),
+																  getSourceVertexIndex());
+				if (triangle != NULL) {
+					halfLink(EdgeEPA(triangle, 1), *this);
+					return true;
+				}
+				return false;
+			}
+		}
+	}
+	return true;
+}
+

src/org/atriaSoft/ephysics/collision/narrowphase/EPA/EdgeEPA.java

@@ -0,0 +1,63 @@
+package org.atriaSoft.ephysics.collision.narrowphase.EPA;
+
+/** 
+ * @brief Class EdgeEPA
+ * This class represents an edge of the current polytope in the EPA algorithm.
+ */
+class EdgeEPA {
+	private:
+		/// Pointer to the triangle that contains this edge
+		TriangleEPA* this.ownerTriangle;
+		/// 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 this.index;
+	public:
+		/// Constructor
+		EdgeEPA();
+		/// Constructor
+		EdgeEPA(TriangleEPA* ownerTriangle, int index);
+		/// Copy-ructor
+		EdgeEPA( EdgeEPA obj);
+		/// Move-ructor
+		EdgeEPA(EdgeEPA obj);
+		/// Return the pointer to the owner triangle
+		TriangleEPA* getOwnerTriangle()  {
+			return this.ownerTriangle;
+		}
+		/// Return the index of the edge in the triangle
+		int getIndex()  {
+			return this.index;
+		}
+		/// Return index of the source vertex of the edge
+		int getSourceVertexIndex() ;
+		/// Return the index of the target vertex of the edge
+		int getTargetVertexIndex() ;
+		/// Execute the recursive silhouette algorithm from this edge
+		boolean computeSilhouette( Vector3f* vertices, int index, TrianglesStore triangleStore);
+		/// Assignment operator
+		EdgeEPA operator=( EdgeEPA obj) {
+			this.ownerTriangle = obj.ownerTriangle;
+			this.index = obj.index;
+			return *this;
+		}
+		/// Move operator
+		EdgeEPA operator=(EdgeEPA obj) {
+			swap(this.ownerTriangle, obj.ownerTriangle);
+			swap(this.index, obj.index);
+			return *this;
+		}
+};
+
+// Return the index of the next counter-clockwise edge of the ownver triangle
+inline int indexOfNextCounterClockwiseEdge(int iii) {
+	return (iii + 1) % 3;
+}
+
+// Return the index of the previous counter-clockwise edge of the ownver triangle
+inline int indexOfPreviousCounterClockwiseEdge(int iii) {
+	return (iii + 2) % 3;
+}
+
+}
+
+

src/org/atriaSoft/ephysics/collision/narrowphase/EPA/TriangleEPA.cpp

@@ -0,0 +1,102 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/narrowphase/EPA/TriangleEPA.hpp>
+#include <ephysics/collision/narrowphase/EPA/EdgeEPA.hpp>
+#include <ephysics/collision/narrowphase/EPA/TrianglesStore.hpp>
+
+using namespace ephysics;
+
+TriangleEPA::TriangleEPA() {
+	
+}
+
+TriangleEPA::TriangleEPA(int indexVertex1, int indexVertex2, int indexVertex3):
+  this.isObsolete(false) {
+	this.indicesVertices[0] = indexVertex1;
+	this.indicesVertices[1] = indexVertex2;
+	this.indicesVertices[2] = indexVertex3;
+}
+
+void TriangleEPA::set(int indexVertex1, int indexVertex2, int indexVertex3) {
+	this.isObsolete = false;
+	this.indicesVertices[0] = indexVertex1;
+	this.indicesVertices[1] = indexVertex2;
+	this.indicesVertices[2] = indexVertex3;
+}
+
+TriangleEPA::~TriangleEPA() {
+	
+}
+
+boolean TriangleEPA::computeClosestPoint( Vector3f* vertices) {
+	 Vector3f p0 = vertices[this.indicesVertices[0]];
+	Vector3f v1 = vertices[this.indicesVertices[1]] - p0;
+	Vector3f v2 = vertices[this.indicesVertices[2]] - p0;
+	float v1Dotv1 = v1.dot(v1);
+	float v1Dotv2 = v1.dot(v2);
+	float v2Dotv2 = v2.dot(v2);
+	float p0Dotv1 = p0.dot(v1);
+	float p0Dotv2 = p0.dot(v2);
+	// Compute determinant
+	this.determinant = v1Dotv1 * v2Dotv2 - v1Dotv2 * v1Dotv2;
+	// Compute lambda values
+	this.lambda1 = p0Dotv2 * v1Dotv2 - p0Dotv1 * v2Dotv2;
+	this.lambda2 = p0Dotv1 * v1Dotv2 - p0Dotv2 * v1Dotv1;
+	// If the determinant is positive
+	if (this.determinant > 0.0) {
+		// Compute the closest point v
+		this.closestPoint = p0 + 1.0f / this.determinant * (this.lambda1 * v1 + this.lambda2 * v2);
+		// Compute the square distance of closest point to the origin
+		this.distSquare = this.closestPoint.dot(this.closestPoint);
+		return true;
+	}
+	return false;
+}
+
+boolean ephysics::link( EdgeEPA edge0,  EdgeEPA edge1) {
+	if (    edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex()
+	     && edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex() ) {
+		edge0.getOwnerTriangle().this.adjacentEdges[edge0.getIndex()] = edge1;
+		edge1.getOwnerTriangle().this.adjacentEdges[edge1.getIndex()] = edge0;
+		return true;
+	}
+	return false;
+}
+
+void ephysics::halfLink( EdgeEPA edge0,  EdgeEPA edge1) {
+	assert(    edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex()
+	        && edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex());
+	edge0.getOwnerTriangle().this.adjacentEdges[edge0.getIndex()] = edge1;
+}
+
+
+boolean TriangleEPA::computeSilhouette( Vector3f* vertices, int indexNewVertex,
+									TrianglesStore triangleStore) {
+	int 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
+	boolean result = this.adjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) &&
+				  this.adjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore) &&
+				  this.adjacentEdges[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/org/atriaSoft/ephysics/collision/narrowphase/EPA/TriangleEPA.java

@@ -0,0 +1,128 @@
+package org.atriaSoft.ephysics.collision.narrowphase.EPA;
+
+	boolean link( EdgeEPA edge0,  EdgeEPA edge1);
+	void halfLink( EdgeEPA edge0,  EdgeEPA edge1);
+	/**
+	 * @brief Class TriangleEPA
+	 * This class represents a triangle face of the current polytope in the EPA algorithm.
+	 */
+	class TriangleEPA {
+		private:
+			int this.indicesVertices[3]; //!< Indices of the vertices yi of the triangle
+			EdgeEPA this.adjacentEdges[3]; //!< Three adjacent edges of the triangle (edges of other triangles)
+			boolean this.isObsolete; //!< True if the triangle face is visible from the new support point
+			float this.determinant; //!< Determinant
+			Vector3f this.closestPoint; //!< Point v closest to the origin on the affine hull of the triangle
+			float this.lambda1; //!< Lambda1 value such that v = lambda0 * y0 + lambda1 * y1 + lambda2 * y2
+			float this.lambda2; //!< Lambda1 value such that v = lambda0 * y0 + lambda1 * y1 + lambda2 * y2
+			float this.distSquare; //!< Square distance of the point closest point v to the origin
+		public:
+			/// Private copy-ructor
+			TriangleEPA( TriangleEPA triangle) {
+				this.indicesVertices[0] = triangle.indicesVertices[0];
+				this.indicesVertices[1] = triangle.indicesVertices[1];
+				this.indicesVertices[2] = triangle.indicesVertices[2];
+				this.adjacentEdges[0] = triangle.adjacentEdges[0];
+				this.adjacentEdges[1] = triangle.adjacentEdges[1];
+				this.adjacentEdges[2] = triangle.adjacentEdges[2];
+				this.isObsolete = triangle.isObsolete;
+				this.determinant = triangle.determinant;
+				this.closestPoint = triangle.closestPoint;
+				this.lambda1 = triangle.lambda1;
+				this.lambda2 = triangle.lambda2;
+				this.distSquare = triangle.distSquare;
+			}
+			/// Private assignment operator
+			TriangleEPA operator=( TriangleEPA triangle) {
+				this.indicesVertices[0] = triangle.indicesVertices[0];
+				this.indicesVertices[1] = triangle.indicesVertices[1];
+				this.indicesVertices[2] = triangle.indicesVertices[2];
+				this.adjacentEdges[0] = triangle.adjacentEdges[0];
+				this.adjacentEdges[1] = triangle.adjacentEdges[1];
+				this.adjacentEdges[2] = triangle.adjacentEdges[2];
+				this.isObsolete = triangle.isObsolete;
+				this.determinant = triangle.determinant;
+				this.closestPoint = triangle.closestPoint;
+				this.lambda1 = triangle.lambda1;
+				this.lambda2 = triangle.lambda2;
+				this.distSquare = triangle.distSquare;
+				return *this;
+			}
+			/// Constructor
+			TriangleEPA();
+			/// Constructor
+			TriangleEPA(int v1, int v2, int v3);
+			/// Constructor
+			void set(int v1, int v2, int v3);
+			/// Return an adjacent edge of the triangle
+			EdgeEPA getAdjacentEdge(int index) {
+				assert(index >= 0 && index < 3);
+				return this.adjacentEdges[index];
+			}
+			/// Set an adjacent edge of the triangle
+			void setAdjacentEdge(int index, EdgeEPA edge) {
+				assert(index >=0 && index < 3);
+				this.adjacentEdges[index] = edge;
+			}
+			/// Return the square distance of the closest point to origin
+			float getDistSquare()  {
+				return this.distSquare;
+			}
+			/// Set the isObsolete value
+			void setIsObsolete(boolean isObsolete) {
+				this.isObsolete = isObsolete;
+			}
+			/// Return true if the triangle face is obsolete
+			boolean getIsObsolete()  {
+				return this.isObsolete;
+			}
+			/// Return the point closest to the origin
+			 Vector3f getClosestPoint()  {
+				return this.closestPoint;
+			}
+			// Return true if the closest point on affine hull is inside the triangle
+			boolean isClosestPointInternalToTriangle()  {
+				return (this.lambda1 >= 0.0 && this.lambda2 >= 0.0 hjkhjkhjkhkj (this.lambda1 + this.lambda2) <= this.determinant);
+			}
+			/// Return true if the triangle is visible from a given vertex
+			boolean isVisibleFromVertex( Vector3f* vertices, int index)  {
+				Vector3f closestToVert = vertices[index] - this.closestPoint;
+				return (this.closestPoint.dot(closestToVert) > 0.0);
+			}
+			/// Compute the point v closest to the origin of this triangle
+			boolean computeClosestPoint( Vector3f* vertices);
+			/// Compute the point of an object closest to the origin
+			Vector3f computeClosestPointOfObject( Vector3f* supportPointsOfObject) {
+				 Vector3f p0 = supportPointsOfObject[this.indicesVertices[0]];
+				return p0 + 1.0f/this.determinant * (this.lambda1 * (supportPointsOfObject[this.indicesVertices[1]] - p0) +
+									   this.lambda2 * (supportPointsOfObject[this.indicesVertices[2]] - p0));
+			}
+			// 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.
+			boolean computeSilhouette( Vector3f* vertices, int index, TrianglesStore triangleStore);
+			/// Access operator
+			int operator[](int pos)  {
+				assert(pos >= 0 && pos <3);
+				return this.indicesVertices[pos];
+			}
+			/// 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).
+			friend boolean link( EdgeEPA edge0,  EdgeEPA edge1);
+			/// 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.
+			friend void halfLink( EdgeEPA edge0,  EdgeEPA edge1);
+	};
+
+}

src/org/atriaSoft/ephysics/collision/narrowphase/EPA/TrianglesStore.cpp

@@ -0,0 +1,10 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/narrowphase/EPA/TrianglesStore.hpp>
+

src/org/atriaSoft/ephysics/collision/narrowphase/EPA/TrianglesStore.java

@@ -0,0 +1,52 @@
+package org.atriaSoft.ephysics.collision.narrowphase.EPA;
+
+	 int MAXTRIANGLES = 200;	 // Maximum number of triangles
+	/**
+	 * @brief This class stores several triangles of the polytope in the EPA algorithm.
+	 */
+	class TrianglesStore {
+		private:
+			Array<TriangleEPA, MAXTRIANGLES> this.triangles; //!< Triangles
+		public:
+			/// Constructor
+			TrianglesStore() = default;
+			/// Clear all the storage
+			void clear() {
+				this.triangles.clear();
+			}
+			/// Return the number of triangles
+			long getNbTriangles()  {
+				return this.triangles.size();
+			}
+			/// Set the number of triangles
+			void resize(int backup) {
+				if (backup > this.triangles.size()) {
+					Log.error("RESIZE BIGGER : " +  backup + " > " + this.triangles.size());
+				}
+				this.triangles.resize(backup);
+			}
+			/// Return the last triangle
+			TriangleEPA last() {
+				return this.triangles.back();
+			}
+			/// Create a new triangle
+			TriangleEPA* newTriangle( Vector3f* vertices, int v0, int v1, int v2) {
+				// If we have not reached the maximum number of triangles
+				if (this.triangles.size() < MAXTRIANGLES) {
+					TriangleEPA tmp(v0, v1, v2);
+					if (!tmp.computeClosestPoint(vertices)) {
+						return null;
+					}
+					this.triangles.pushBack(move(tmp));
+					return this.triangles.back();
+				}
+				// We are at the limit (internal)
+				return null;
+			}
+			/// Access operator
+			TriangleEPA operator[](int id) {
+				return this.triangles[id];
+			}
+	};
+}
+

src/org/atriaSoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp

@@ -0,0 +1,370 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp>
+#include <ephysics/collision/narrowphase/GJK/Simplex.hpp>
+#include <ephysics/raint/ContactPoint.hpp>
+#include <ephysics/configuration.hpp>
+#include <ephysics/engine/Profiler.hpp>
+
+using namespace ephysics;
+
+GJKAlgorithm::GJKAlgorithm() : NarrowPhaseAlgorithm() {
+	
+}
+
+GJKAlgorithm::~GJKAlgorithm() {
+	
+}
+
+void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info,
+								  CollisionShapeInfo shape2Info,
+								 NarrowPhaseCallback* narrowPhaseCallback) {
+	PROFILE("GJKAlgorithm::testCollision()");
+	Vector3f suppA; // Support point of object A
+	Vector3f suppB; // Support point of object B
+	Vector3f w; // Support point of Minkowski difference A-B
+	Vector3f pA; // Closest point of object A
+	Vector3f pB; // Closest point of object B
+	float vDotw;
+	float prevDistSquare;
+	assert(shape1Info.collisionShape.isConvex());
+	assert(shape2Info.collisionShape.isConvex());
+	 ConvexShape* shape1 = staticcast< ConvexShape*>(shape1Info.collisionShape);
+	 ConvexShape* shape2 = staticcast< ConvexShape*>(shape2Info.collisionShape);
+	void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
+	void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
+	// Get the local-space to world-space transforms
+	 Transform3D transform1 = shape1Info.shapeToWorldTransform;
+	 Transform3D transform2 = shape2Info.shapeToWorldTransform;
+	// Transform3D 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)
+	Transform3D body2Tobody1 = transform1.getInverse() * transform2;
+	// Matrix that transform a direction from local
+	// space of body 1 into local space of body 2
+	Matrix3f rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *
+							  transform1.getOrientation().getMatrix();
+	// Initialize the margin (sum of margins of both objects)
+	float margin = shape1.getMargin() + shape2.getMargin();
+	float marginSquare = margin * margin;
+	assert(margin > 0.0);
+	// Create a simplex set
+	Simplex simplex;
+	// Get the previous point V (last cached separating axis)
+	Vector3f v = this.currentOverlappingPair.getCachedSeparatingAxis();
+	// Initialize the upper bound for the square distance
+	float distSquare = FLTMAX;
+	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
+			this.currentOverlappingPair.setCachedSeparatingAxis(v);
+			// No intersection, we return
+			return;
+		}
+		// If the objects intersect only in the margins
+		if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * RELERRORSQUARE) {
+			// 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
+			float 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
+			Vector3f normal = transform1.getOrientation() * (-v.safeNormalized());
+			float 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
+			float 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
+			Vector3f normal = transform1.getOrientation() * (-v.safeNormalized());
+			float 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
+			float 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
+			Vector3f normal = transform1.getOrientation() * (-v.safeNormalized());
+			float 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.length2();
+		// If the distance to the closest point doesn't improve a lot
+		if (prevDistSquare - distSquare <= FLTEPSILON * prevDistSquare) {
+			simplex.backupClosestPointInSimplex(v);
+			
+			// Get the new squared distance
+			distSquare = v.length2();
+			// 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
+			float 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
+			Vector3f normal = transform1.getOrientation() * (-v.safeNormalized());
+			float 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 > FLTEPSILON *
+								 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);
+}
+
+void GJKAlgorithm::computePenetrationDepthForEnlargedObjects( CollisionShapeInfo shape1Info,
+															  Transform3D transform1,
+															  CollisionShapeInfo shape2Info,
+															  Transform3D transform2,
+															 NarrowPhaseCallback* narrowPhaseCallback,
+															 Vector3f v) {
+	PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()");
+	Simplex simplex;
+	Vector3f suppA;
+	Vector3f suppB;
+	Vector3f w;
+	float vDotw;
+	float distSquare = FLTMAX;
+	float prevDistSquare;
+	assert(shape1Info.collisionShape.isConvex());
+	assert(shape2Info.collisionShape.isConvex());
+	 ConvexShape* shape1 = staticcast< ConvexShape*>(shape1Info.collisionShape);
+	 ConvexShape* shape2 = staticcast< ConvexShape*>(shape2Info.collisionShape);
+	void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
+	void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
+	// Transform3D 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)
+	Transform3D body2ToBody1 = transform1.getInverse() * transform2;
+	// Matrix that transform a direction from local space of body 1 into local space of body 2
+	Matrix3f 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.length2();
+		if (prevDistSquare - distSquare <= FLTEPSILON * prevDistSquare) {
+			return;
+		}
+	} while(!simplex.isFull() && distSquare > FLTEPSILON *
+								 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 this.algoEPA.computePenetrationDepthAndContactPoints(simplex, shape1Info,
+															transform1, shape2Info, transform2,
+															v, narrowPhaseCallback);
+}
+
+boolean GJKAlgorithm::testPointInside( Vector3f localPoint, ProxyShape* proxyShape) {
+	Vector3f suppA;			 // Support point of object A
+	Vector3f w;				 // Support point of Minkowski difference A-B
+	float prevDistSquare;
+	assert(proxyShape.getCollisionShape().isConvex());
+	 ConvexShape* shape = staticcast< ConvexShape*>(proxyShape.getCollisionShape());
+	void** shapeCachedCollisionData = proxyShape.getCachedCollisionData();
+	// Support point of object B (object B is a single point)
+	 Vector3f suppB(localPoint);
+	// Create a simplex set
+	Simplex simplex;
+	// Initial supporting direction
+	Vector3f v(1, 1, 1);
+	// Initialize the upper bound for the square distance
+	float distSquare = FLTMAX;
+	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.length2();
+		// If the distance to the closest point doesn't improve a lot
+		if (prevDistSquare - distSquare <= FLTEPSILON * prevDistSquare) {
+			return false;
+		}
+	} while(    !simplex.isFull()
+	         && distSquare > FLTEPSILON * simplex.getMaxLengthSquareOfAPoint());
+	// The point is inside the collision shape
+	return true;
+}
+
+boolean GJKAlgorithm::raycast( Ray ray, ProxyShape* proxyShape, RaycastInfo raycastInfo) {
+	assert(proxyShape.getCollisionShape().isConvex());
+	 ConvexShape* shape = staticcast< ConvexShape*>(proxyShape.getCollisionShape());
+	void** shapeCachedCollisionData = proxyShape.getCachedCollisionData();
+	Vector3f suppA;	  // Current lower bound point on the ray (starting at ray's origin)
+	Vector3f suppB;	  // Support point on the collision shape
+	 float machineEpsilonSquare = FLTEPSILON * FLTEPSILON;
+	 float epsilon = float(0.0001);
+	// Convert the ray origin and direction into the local-space of the collision shape
+	Vector3f rayDirection = ray.point2 - ray.point1;
+	// If the points of the segment are two close, return no hit
+	if (rayDirection.length2() < machineEpsilonSquare) return false;
+	Vector3f w;
+	// Create a simplex set
+	Simplex simplex;
+	Vector3f n(0.0f, float(0.0), float(0.0));
+	float lambda = 0.0f;
+	suppA = ray.point1;	// Current lower bound point on the ray (starting at ray's origin)
+	suppB = shape.getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData);
+	Vector3f v = suppA - suppB;
+	float vDotW, vDotR;
+	float distSquare = v.length2();
+	int nbIterations = 0;
+	// GJK Algorithm loop
+	while (distSquare > epsilon && nbIterations < MAXITERATIONSGJKRAYCAST) {
+		// Compute the support points
+		suppB = shape.getLocalSupportPointWithoutMargin(v, shapeCachedCollisionData);
+		w = suppA - suppB;
+		vDotW = v.dot(w);
+		if (vDotW > float(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.length2();
+		} else {
+			distSquare = 0.0f;
+		}
+		// 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 < FLTEPSILON) {
+		return false;
+	}
+	// Compute the closet points of both objects (without the margins)
+	Vector3f pointA;
+	Vector3f 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.length2() >= machineEpsilonSquare) { // The normal vector is valid
+		raycastInfo.worldNormal = n;
+	} else {  // Degenerated normal vector, we return a zero normal vector
+		raycastInfo.worldNormal = Vector3f(float(0), float(0), float(0));
+	}
+	return true;
+}

src/org/atriaSoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.java

@@ -0,0 +1,67 @@
+package org.atriaSoft.ephysics.collision.narrowphase.GJK;
+
+	 float RELERROR = float(1.0e-3);
+	 float RELERRORSQUARE = RELERROR * RELERROR;
+	 int MAXITERATIONSGJKRAYCAST = 32;
+	/**
+	 * @brief 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 extends  NarrowPhaseAlgorithm {
+		private :
+			EPAAlgorithm this.algoEPA; //!< EPA Algorithm
+			/// Private copy-ructor
+			GJKAlgorithm( GJKAlgorithm algorithm);
+			/// Private assignment operator
+			GJKAlgorithm operator=( GJKAlgorithm algorithm);
+			/// 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 computePenetrationDepthForEnlargedObjects( CollisionShapeInfo shape1Info,
+			                                                Transform3D transform1,
+			                                                CollisionShapeInfo shape2Info,
+			                                                Transform3D transform2,
+			                                               NarrowPhaseCallback* narrowPhaseCallback,
+			                                               Vector3f v);
+		public :
+			/// Constructor
+			GJKAlgorithm();
+			/// Initalize the algorithm
+			 void init(CollisionDetection* collisionDetection) {
+				NarrowPhaseAlgorithm::init(collisionDetection);
+				this.algoEPA.init();
+			};
+			// 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 testCollision( CollisionShapeInfo shape1Info,
+			                            CollisionShapeInfo shape2Info,
+			                           NarrowPhaseCallback* narrowPhaseCallback);
+			/// Use the GJK Algorithm to find if a point is inside a convex collision shape
+			boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape);
+			/// 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".
+			boolean raycast( Ray ray, ProxyShape* proxyShape, RaycastInfo raycastInfo);
+	};
+}
+

src/org/atriaSoft/ephysics/collision/narrowphase/GJK/Simplex.cpp

@@ -0,0 +1,368 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/narrowphase/GJK/Simplex.hpp>
+
+using namespace ephysics;
+
+Simplex::Simplex() : mBitsCurrentSimplex(0x0), mAllBits(0x0) {
+
+}
+
+// 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( Vector3f point,  Vector3f suppPointA,  Vector3f 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
+boolean Simplex::isPointInSimplex( Vector3f point)  {
+	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
+int Simplex::getSimplex(Vector3f* suppPointsA, Vector3f* suppPointsB,
+								 Vector3f* points)  {
+	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 "yi" in X, we have
+/// detXi value bigger than zero
+boolean Simplex::isProperSubset(Bits subset)  {
+	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
+boolean Simplex::isAffinelyDependent()  {
+	float 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 Ix and
+///	2. delta(X U {yj})j <= 0 for each "j" not in Ix
+boolean Simplex::isValidSubset(Bits subset)  {
+	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 {yj})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(lambdai * ai)	where "ai" are the support points of object A
+///	  pB = sum(lambdai * bi)	where "bi" are the support points of object B
+///	  with lambdai = deltaXi / deltaX
+void Simplex::computeClosestPointsOfAandB(Vector3f pA, Vector3f pB)  {
+	float deltaX = 0.0;
+	pA.setValue(0.0, 0.0, 0.0);
+	pB.setValue(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);
+	float factor = 1.0f / 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.
+boolean Simplex::computeClosestPoint(Vector3f 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(Vector3f v) {
+	float minDistSquare = FLTMAX;
+	Bits bit;
+
+	for (bit = mAllBits; bit != 0x0; bit--) {
+		if (isSubset(bit, mAllBits) && isProperSubset(bit)) {
+			Vector3f u = computeClosestPointForSubset(bit);
+			float 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"
+Vector3f Simplex::computeClosestPointForSubset(Bits subset) {
+	Vector3f v(0.0, 0.0, 0.0); // Closet point v = sum(lambdai * points[i])
+	mMaxLengthSquare = 0.0;
+	float 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(lambdai * 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 (1.0f / deltaX) * v;
+}

src/org/atriaSoft/ephysics/collision/narrowphase/GJK/Simplex.java

@@ -0,0 +1,149 @@
+package org.atriaSoft.ephysics.collision.narrowphase.GJK;
+
+// 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
+		Vector3f mPoints[4];
+
+		/// pointsLengthSquare[i] = (points[i].length)^2
+		float mPointsLengthSquare[4];
+
+		/// Maximum length of pointsLengthSquare[i]
+		float mMaxLengthSquare;
+
+		/// Support points of object A in local coordinates
+		Vector3f mSuppPointsA[4];
+
+		/// Support points of object B in local coordinates
+		Vector3f mSuppPointsB[4];
+
+		/// diff[i][j] contains points[i] - points[j]
+		Vector3f mDiffLength[4][4];
+
+		/// Cached determinant values
+		float mDet[16][4];
+
+		/// norm[i][j] = (diff[i][j].length())^2
+		float 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
+		int mBitsCurrentSimplex;
+
+		/// Number between 1 and 4 that identify the last found support point
+		int mLastFound;
+
+		/// Position of the last found support point (lastFoundBit = 0x1 + lastFound)
+		int mLastFoundBit;
+
+		/// allBits = bitsCurrentSimplex | lastFoundBit;
+		int mAllBits;
+
+		// -------------------- Methods -------------------- //
+
+		/// Private copy-ructor
+		Simplex( Simplex simplex);
+
+		/// Private assignment operator
+		Simplex operator=( Simplex simplex);
+
+		/// Return true if some bits of "a" overlap with bits of "b"
+		boolean overlap(Bits a, Bits b) ;
+
+		/// Return true if the bits of "b" is a subset of the bits of "a"
+		boolean isSubset(Bits a, Bits b) ;
+
+		/// Return true if the subset is a valid one for the closest point computation.
+		boolean isValidSubset(Bits subset) ;
+
+		/// Return true if the subset is a proper subset.
+		boolean isProperSubset(Bits subset) ;
+
+		/// 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
+		Vector3f computeClosestPointForSubset(Bits subset);
+
+	public:
+
+		// -------------------- Methods -------------------- //
+
+		/// Constructor
+		Simplex();
+
+		/// Return true if the simplex contains 4 points
+		boolean isFull() ;
+
+		/// Return true if the simplex is empty
+		boolean isEmpty() ;
+
+		/// Return the points of the simplex
+		int getSimplex(Vector3f* mSuppPointsA, Vector3f* mSuppPointsB,
+								Vector3f* mPoints) ;
+
+		/// Return the maximum squared length of a point
+		float getMaxLengthSquareOfAPoint() ;
+
+		/// Add a new support point of (A-B) into the simplex.
+		void addPoint( Vector3f point,  Vector3f suppPointA,  Vector3f suppPointB);
+
+		/// Return true if the point is in the simplex
+		boolean isPointInSimplex( Vector3f point) ;
+
+		/// Return true if the set is affinely dependent
+		boolean isAffinelyDependent() ;
+
+		/// Backup the closest point
+		void backupClosestPointInSimplex(Vector3f point);
+
+		/// Compute the closest points "pA" and "pB" of object A and B.
+		void computeClosestPointsOfAandB(Vector3f pA, Vector3f pB) ;
+
+		/// Compute the closest point to the origin of the current simplex.
+		boolean computeClosestPoint(Vector3f v);
+};
+
+// Return true if some bits of "a" overlap with bits of "b"
+inline boolean Simplex::overlap(Bits a, Bits b)  {
+	return ((a  b) != 0x0);
+}
+
+// Return true if the bits of "b" is a subset of the bits of "a"
+inline boolean Simplex::isSubset(Bits a, Bits b)  {
+	return ((a  b) == a);
+}
+
+// Return true if the simplex contains 4 points
+inline boolean Simplex::isFull()  {
+	return (mBitsCurrentSimplex == 0xf);
+}
+
+// Return true if the simple is empty
+inline boolean Simplex::isEmpty()  {
+	return (mBitsCurrentSimplex == 0x0);
+}
+
+// Return the maximum squared length of a point
+inline float Simplex::getMaxLengthSquareOfAPoint()  {
+	return mMaxLengthSquare;
+}
+
+}
+

src/org/atriaSoft/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.cpp

@@ -0,0 +1,24 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp>
+
+using namespace ephysics;
+
+NarrowPhaseAlgorithm::NarrowPhaseAlgorithm():
+  this.currentOverlappingPair(null) {
+	
+}
+
+void NarrowPhaseAlgorithm::init(CollisionDetection* collisionDetection) {
+	this.collisionDetection = collisionDetection;
+}
+
+void NarrowPhaseAlgorithm::setCurrentOverlappingPair(OverlappingPair* overlappingPair) {
+	this.currentOverlappingPair = overlappingPair;
+}

src/org/atriaSoft/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.java

@@ -0,0 +1,39 @@
+package org.atriaSoft.ephysics.collision.narrowphase;
+
+	
+	/**
+	 * @brief It 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
+			 void notifyContact(OverlappingPair* overlappingPair,
+			                            ContactPointInfo contactInfo) = 0;
+	
+	};
+	
+	/**
+	 * @breif It 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 :
+			CollisionDetection* this.collisionDetection; //!< Pointer to the collision detection object
+			OverlappingPair* this.currentOverlappingPair; //!< Overlapping pair of the bodies currently tested for collision
+		public :
+			/// Constructor
+			NarrowPhaseAlgorithm();
+			/// Initalize the algorithm
+			 void init(CollisionDetection* collisionDetection);
+			/// Set the current overlapping pair of bodies
+			void setCurrentOverlappingPair(OverlappingPair* overlappingPair);
+			/// Compute a contact info if the two bounding volume collide
+			 void testCollision( CollisionShapeInfo shape1Info,
+			                            CollisionShapeInfo shape2Info,
+			                           NarrowPhaseCallback* narrowPhaseCallback) = 0;
+	};
+}
+
+

src/org/atriaSoft/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp

@@ -0,0 +1,51 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/narrowphase/SphereVsSphereAlgorithm.hpp>
+#include <ephysics/collision/shapes/SphereShape.hpp>
+
+ephysics::SphereVsSphereAlgorithm::SphereVsSphereAlgorithm() :
+  NarrowPhaseAlgorithm() {
+	
+}
+
+void ephysics::SphereVsSphereAlgorithm::testCollision( ephysics::CollisionShapeInfo shape1Info,
+                                                       ephysics::CollisionShapeInfo shape2Info,
+                                                      ephysics::NarrowPhaseCallback* narrowPhaseCallback) {
+	// Get the sphere collision shapes
+	 ephysics::SphereShape* sphereShape1 = staticcast< ephysics::SphereShape*>(shape1Info.collisionShape);
+	 ephysics::SphereShape* sphereShape2 = staticcast< ephysics::SphereShape*>(shape2Info.collisionShape);
+	// Get the local-space to world-space transforms
+	 Transform3D transform1 = shape1Info.shapeToWorldTransform;
+	 Transform3D transform2 = shape2Info.shapeToWorldTransform;
+	// Compute the distance between the centers
+	Vector3f vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
+	float squaredDistanceBetweenCenters = vectorBetweenCenters.length2();
+	// Compute the sum of the radius
+	float sumRadius = sphereShape1.getRadius() + sphereShape2.getRadius();
+	// If the sphere collision shapes intersect
+	if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) {
+		Vector3f centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
+		Vector3f centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
+		Vector3f intersectionOnBody1 = sphereShape1.getRadius() * centerSphere2InBody1LocalSpace.safeNormalized();
+		Vector3f intersectionOnBody2 = sphereShape2.getRadius() * centerSphere1InBody2LocalSpace.safeNormalized();
+		float penetrationDepth = sumRadius - sqrt(squaredDistanceBetweenCenters);
+		
+		// Create the contact info object
+		ephysics::ContactPointInfo contactInfo(shape1Info.proxyShape,
+		                                       shape2Info.proxyShape,
+		                                       shape1Info.collisionShape,
+		                                       shape2Info.collisionShape,
+		                                       vectorBetweenCenters.safeNormalized(),
+		                                       penetrationDepth,
+		                                       intersectionOnBody1,
+		                                       intersectionOnBody2);
+		// Notify about the new contact
+		narrowPhaseCallback.notifyContact(shape1Info.overlappingPair, contactInfo);
+	}
+}

src/org/atriaSoft/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.java

@@ -0,0 +1,29 @@
+package org.atriaSoft.ephysics.collision.narrowphase;
+
+	/**
+	 * @brief It is used to compute the narrow-phase collision detection
+	 * between two sphere collision shapes.
+	 */
+	class SphereVsSphereAlgorithm extends  NarrowPhaseAlgorithm {
+		protected :
+			SphereVsSphereAlgorithm( SphereVsSphereAlgorithm) = delete;
+			SphereVsSphereAlgorithm operator=( SphereVsSphereAlgorithm) = delete;
+		public :
+			/**
+			 * @brief Constructor
+			 */
+			SphereVsSphereAlgorithm();
+			/**
+			 * @brief Destructor
+			 */
+			 ~SphereVsSphereAlgorithm() = default;
+			/**
+			 * @brief Compute a contact info if the two bounding volume collide
+			 */
+			 void testCollision( CollisionShapeInfo shape1Info,
+			                            CollisionShapeInfo shape2Info,
+			                           NarrowPhaseCallback* narrowPhaseCallback);
+	};
+}
+
+

src/org/atriaSoft/ephysics/collision/shapes/AABB.cpp

@@ -0,0 +1,210 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+
+// Libraries
+#include <ephysics/collision/shapes/AABB.hpp>
+#include <ephysics/configuration.hpp>
+
+using namespace ephysics;
+using namespace std;
+
+AABB::AABB():
+  this.minCoordinates(0,0,0),
+  this.maxCoordinates(0,0,0) {
+	
+}
+
+AABB::AABB( Vector3f minCoordinates,  Vector3f maxCoordinates):
+  this.minCoordinates(minCoordinates),
+  this.maxCoordinates(maxCoordinates) {
+	
+}
+
+AABB::AABB( AABB aabb):
+  this.minCoordinates(aabb.minCoordinates),
+  this.maxCoordinates(aabb.maxCoordinates) {
+	
+}
+
+void AABB::mergeWithAABB( AABB aabb) {
+	this.minCoordinates.setX(min(this.minCoordinates.x(), aabb.minCoordinates.x()));
+	this.minCoordinates.setY(min(this.minCoordinates.y(), aabb.minCoordinates.y()));
+	this.minCoordinates.setZ(min(this.minCoordinates.z(), aabb.minCoordinates.z()));
+	this.maxCoordinates.setX(max(this.maxCoordinates.x(), aabb.maxCoordinates.x()));
+	this.maxCoordinates.setY(max(this.maxCoordinates.y(), aabb.maxCoordinates.y()));
+	this.maxCoordinates.setZ(max(this.maxCoordinates.z(), aabb.maxCoordinates.z()));
+}
+
+void AABB::mergeTwoAABBs( AABB aabb1,  AABB aabb2) {
+	this.minCoordinates.setX(min(aabb1.minCoordinates.x(), aabb2.minCoordinates.x()));
+	this.minCoordinates.setY(min(aabb1.minCoordinates.y(), aabb2.minCoordinates.y()));
+	this.minCoordinates.setZ(min(aabb1.minCoordinates.z(), aabb2.minCoordinates.z()));
+	this.maxCoordinates.setX(max(aabb1.maxCoordinates.x(), aabb2.maxCoordinates.x()));
+	this.maxCoordinates.setY(max(aabb1.maxCoordinates.y(), aabb2.maxCoordinates.y()));
+	this.maxCoordinates.setZ(max(aabb1.maxCoordinates.z(), aabb2.maxCoordinates.z()));
+}
+
+boolean AABB::contains( AABB aabb)  {
+	boolean isInside = true;
+	isInside = isInside && this.minCoordinates.x() <= aabb.minCoordinates.x();
+	isInside = isInside && this.minCoordinates.y() <= aabb.minCoordinates.y();
+	isInside = isInside && this.minCoordinates.z() <= aabb.minCoordinates.z();
+	isInside = isInside && this.maxCoordinates.x() >= aabb.maxCoordinates.x();
+	isInside = isInside && this.maxCoordinates.y() >= aabb.maxCoordinates.y();
+	isInside = isInside && this.maxCoordinates.z() >= aabb.maxCoordinates.z();
+	return isInside;
+}
+
+AABB AABB::createAABBForTriangle( Vector3f* trianglePoints) {
+	Vector3f minCoords(trianglePoints[0].x(), trianglePoints[0].y(), trianglePoints[0].z());
+	Vector3f maxCoords(trianglePoints[0].x(), trianglePoints[0].y(), trianglePoints[0].z());
+	if (trianglePoints[1].x() < minCoords.x()) {
+		minCoords.setX(trianglePoints[1].x());
+	}
+	if (trianglePoints[1].y() < minCoords.y()) {
+		minCoords.setY(trianglePoints[1].y());
+	}
+	if (trianglePoints[1].z() < minCoords.z()) {
+		minCoords.setZ(trianglePoints[1].z());
+	}
+	if (trianglePoints[2].x() < minCoords.x()) {
+		minCoords.setX(trianglePoints[2].x());
+	}
+	if (trianglePoints[2].y() < minCoords.y()) {
+		minCoords.setY(trianglePoints[2].y());
+	}
+	if (trianglePoints[2].z() < minCoords.z()) {
+		minCoords.setZ(trianglePoints[2].z());
+	}
+	if (trianglePoints[1].x() > maxCoords.x()) {
+		maxCoords.setX(trianglePoints[1].x());
+	}
+	if (trianglePoints[1].y() > maxCoords.y()) {
+		maxCoords.setY(trianglePoints[1].y());
+	}
+	if (trianglePoints[1].z() > maxCoords.z()) {
+		maxCoords.setZ(trianglePoints[1].z());
+	}
+	if (trianglePoints[2].x() > maxCoords.x()) {
+		maxCoords.setX(trianglePoints[2].x());
+	}
+	if (trianglePoints[2].y() > maxCoords.y()) {
+		maxCoords.setY(trianglePoints[2].y());
+	}
+	if (trianglePoints[2].z() > maxCoords.z()) {
+		maxCoords.setZ(trianglePoints[2].z());
+	}
+	return AABB(minCoords, maxCoords);
+}
+
+boolean AABB::testRayIntersect( Ray ray)  {
+	 Vector3f point2 = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
+	 Vector3f e = this.maxCoordinates - this.minCoordinates;
+	 Vector3f d = point2 - ray.point1;
+	 Vector3f m = ray.point1 + point2 - this.minCoordinates - this.maxCoordinates;
+	// Test if the AABB face normals are separating axis
+	float adx = abs(d.x());
+	if (abs(m.x()) > e.x() + adx) {
+		return false;
+	}
+	float ady = abs(d.y());
+	if (abs(m.y()) > e.y() + ady) {
+		return false;
+	}
+	float adz = abs(d.z());
+	if (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)
+	 float epsilon = 0.00001;
+	adx += epsilon;
+	ady += epsilon;
+	adz += epsilon;
+	// Test if the cross products between face normals and ray direction are
+	// separating axis
+	if (abs(m.y() * d.z() - m.z() * d.y()) > e.y() * adz + e.z() * ady) {
+		return false;
+	}
+	if (abs(m.z() * d.x() - m.x() * d.z()) > e.x() * adz + e.z() * adx) {
+		return false;
+	}
+	if (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;
+}
+
+Vector3f AABB::getExtent()  {
+  return this.maxCoordinates - this.minCoordinates;
+}
+
+void AABB::inflate(float dx, float dy, float dz) {
+	this.maxCoordinates += Vector3f(dx, dy, dz);
+	this.minCoordinates -= Vector3f(dx, dy, dz);
+}
+
+boolean AABB::testCollision( AABB aabb)  {
+	if (    this.maxCoordinates.x() < aabb.minCoordinates.x()
+	     || aabb.maxCoordinates.x() < this.minCoordinates.x()) {
+		return false;
+	}
+	if (    this.maxCoordinates.y() < aabb.minCoordinates.y()
+	     || aabb.maxCoordinates.y() < this.minCoordinates.y()) {
+		return false;
+	}
+	if (    this.maxCoordinates.z() < aabb.minCoordinates.z()
+	     || aabb.maxCoordinates.z() < this.minCoordinates.z()) {
+		return false;
+	}
+	return true;
+}
+
+float AABB::getVolume()  {
+	 Vector3f diff = this.maxCoordinates - this.minCoordinates;
+	return (diff.x() * diff.y() * diff.z());
+}
+
+boolean AABB::testCollisionTriangleAABB( Vector3f* trianglePoints)  {
+	if (min3(trianglePoints[0].x(), trianglePoints[1].x(), trianglePoints[2].x()) > this.maxCoordinates.x()) {
+		return false;
+	}
+	if (min3(trianglePoints[0].y(), trianglePoints[1].y(), trianglePoints[2].y()) > this.maxCoordinates.y()) {
+		return false;
+	}
+	if (min3(trianglePoints[0].z(), trianglePoints[1].z(), trianglePoints[2].z()) > this.maxCoordinates.z()) {
+		return false;
+	}
+	if (max3(trianglePoints[0].x(), trianglePoints[1].x(), trianglePoints[2].x()) < this.minCoordinates.x()) {
+		return false;
+	}
+	if (max3(trianglePoints[0].y(), trianglePoints[1].y(), trianglePoints[2].y()) < this.minCoordinates.y()) {
+		return false;
+	}
+	if (max3(trianglePoints[0].z(), trianglePoints[1].z(), trianglePoints[2].z()) < this.minCoordinates.z()) {
+		return false;
+	}
+	return true;
+}
+
+boolean AABB::contains( Vector3f point)  {
+	return    point.x() >= this.minCoordinates.x() - FLTEPSILON && point.x() <= this.maxCoordinates.x() + FLTEPSILON
+	       && point.y() >= this.minCoordinates.y() - FLTEPSILON hjkhjkhjkhkj point.y() <= this.maxCoordinates.y() + FLTEPSILON
+	       && point.z() >= this.minCoordinates.z() - FLTEPSILON hjkhjkhjkhkj point.z() <= this.maxCoordinates.z() + FLTEPSILON;
+}
+
+AABB AABB::operator=( AABB aabb) {
+	if (this != aabb) {
+		this.minCoordinates = aabb.minCoordinates;
+		this.maxCoordinates = aabb.maxCoordinates;
+	}
+	return *this;
+}
+

src/org/atriaSoft/ephysics/collision/shapes/AABB.java

@@ -0,0 +1,143 @@
+package org.atriaSoft.ephysics.collision.shapes;
+
+	/**
+	 * @brief 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 :
+			/// Minimum world coordinates of the AABB on the x,y and z axis
+			Vector3f minCoordinates;
+			/// Maximum world coordinates of the AABB on the x,y and z axis
+			Vector3f maxCoordinates;
+		public :
+			/**
+			 * @brief default contructor
+			 */
+			AABB();
+			/**
+			 * @brief contructor Whit sizes
+			 * @param[in] minCoordinates Minimum coordinates
+			 * @param[in] maxCoordinates Maximum coordinates
+			 */
+			AABB( Vector3f minCoordinates,  Vector3f maxCoordinates);
+			/**
+			 * @brief Copy-contructor
+			 * @param[in] aabb the object to copy
+			 */
+			AABB( AABB aabb);
+			/**
+			 * @brief Get the center point of the AABB box
+			 * @return The 3D position of the center
+			 */
+			Vector3f getCenter()  {
+				return (this.minCoordinates + this.maxCoordinates) * 0.5f;
+			}
+			/**
+			 * @brief Get the minimum coordinates of the AABB
+			 * @return The 3d minimum coordonates
+			 */
+			 Vector3f getMin()  {
+				return this.minCoordinates;
+			}
+			/**
+			 * @brief Set the minimum coordinates of the AABB
+			 * @param[in] min The 3d minimum coordonates
+			 */
+			void setMin( Vector3f min) {
+				this.minCoordinates = min;
+			}
+			/**
+			 * @brief Return the maximum coordinates of the AABB
+			 * @return The 3d maximum coordonates
+			 */
+			 Vector3f getMax()  {
+				return this.maxCoordinates;
+			}
+			/**
+			 * @brief Set the maximum coordinates of the AABB
+			 * @param[in] max The 3d maximum coordonates
+			 */
+			void setMax( Vector3f max) {
+				this.maxCoordinates = max;
+			}
+			/**
+			 * @brief Get the size of the AABB in the three dimension x, y and z
+			 * @return the AABB 3D size
+			 */
+			Vector3f getExtent() ;
+			/**
+			 * @brief Inflate each side of the AABB by a given size
+			 * @param[in] dx Inflate X size
+			 * @param[in] dy Inflate Y size
+			 * @param[in] dz Inflate Z size
+			 */
+			void inflate(float dx, float dy, float dz);
+			/**
+			 * @brief 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
+			 * @param[in] aabb Other AABB box to check.
+			 * @return true Collision detected
+			 * @return false Not collide
+			 */
+			boolean testCollision( AABB aabb) ;
+			/**
+			 * @brief Get the volume of the AABB
+			 * @return The 3D volume.
+			 */
+			float getVolume() ;
+			/**
+			 * @brief Merge the AABB in parameter with the current one
+			 * @param[in] aabb Other AABB box to merge.
+			 */
+			void mergeWithAABB( AABB aabb);
+			/**
+			 * @brief Replace the current AABB with a new AABB that is the union of two AABBs in parameters
+			 * @param[in] aabb1 first AABB box to merge with aabb2.
+			 * @param[in] aabb2 second AABB box to merge with aabb1.
+			 */
+			void mergeTwoAABBs( AABB aabb1,  AABB aabb2);
+			/**
+			 * @brief Return true if the current AABB contains the AABB given in parameter
+			 * @param[in] aabb AABB box that is contains in the current.
+			 * @return true The parameter in contained inside
+			 */
+			boolean contains( AABB aabb) ;
+			/**
+			 * @brief Return true if a point is inside the AABB
+			 * @param[in] point Point to check.
+			 * @return true The point in contained inside
+			 */
+			boolean contains( Vector3f point) ;
+			/**
+			 * @brief check if the AABB of a triangle intersects the AABB
+			 * @param[in] trianglePoints List of 3 point od a triangle
+			 * @return true The triangle is contained in the Box
+			 */
+			boolean testCollisionTriangleAABB( Vector3f* trianglePoints) ;
+			/**
+			 * @brief check if the ray intersects the AABB
+			 * This method use the line vs AABB raycasting technique described in
+			 * Real-time Collision Detection by Christer Ericson.
+			 * @param[in] ray Ray to test
+			 * @return true The raytest intersect the AABB box
+			 */
+			boolean testRayIntersect( Ray ray) ;
+			/**
+			 * @brief Create and return an AABB for a triangle
+			 * @param[in] trianglePoints List of 3 point od a triangle
+			 * @return An AABB box
+			 */
+			static AABB createAABBForTriangle( Vector3f* trianglePoints);
+			/**
+			 * @brief Assignment operator
+			 * @param[in] aabb The other box to compare
+			 * @return reference on this
+			 */
+			AABB operator=( AABB aabb);
+	};
+
+
+}

src/org/atriaSoft/ephysics/collision/shapes/BoxShape.cpp

@@ -0,0 +1,133 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+// Libraries
+#include <ephysics/collision/shapes/BoxShape.hpp>
+#include <ephysics/collision/ProxyShape.hpp>
+#include <ephysics/configuration.hpp>
+#include <etk/Vector.hpp>
+
+using namespace ephysics;
+
+BoxShape::BoxShape( Vector3f extent, float margin):
+  ConvexShape(BOX, margin),
+  this.extent(extent - Vector3f(margin, margin, margin)) {
+	assert(extent.x() > 0.0f && extent.x() > margin);
+	assert(extent.y() > 0.0f && extent.y() > margin);
+	assert(extent.z() > 0.0f && extent.z() > margin);
+}
+
+void BoxShape::computeLocalInertiaTensor(Matrix3f tensor, float mass)  {
+	float factor = (1.0f / float(3.0)) * mass;
+	Vector3f realExtent = this.extent + Vector3f(this.margin, this.margin, this.margin);
+	float xSquare = realExtent.x() * realExtent.x();
+	float ySquare = realExtent.y() * realExtent.y();
+	float zSquare = realExtent.z() * realExtent.z();
+	tensor.setValue(factor * (ySquare + zSquare), 0.0, 0.0,
+	                 0.0, factor * (xSquare + zSquare), 0.0,
+	                 0.0, 0.0, factor * (xSquare + ySquare));
+}
+
+boolean BoxShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape)  {
+	Vector3f rayDirection = ray.point2 - ray.point1;
+	float tMin = FLTMIN;
+	float tMax = FLTMAX;
+	Vector3f normalDirection(0,0,0);
+	Vector3f currentNormal(0,0,0);
+	// For each of the three slabs
+	for (int iii=0; iii<3; ++iii) {
+		// If ray is parallel to the slab
+		if (abs(rayDirection[iii]) < FLTEPSILON) {
+			// If the ray's origin is not inside the slab, there is no hit
+			if (ray.point1[iii] > this.extent[iii] || ray.point1[iii] < -this.extent[iii]) {
+				return false;
+			}
+		} else {
+			// Compute the intersection of the ray with the near and far plane of the slab
+			float oneOverD = 1.0f / rayDirection[iii];
+			float t1 = (-this.extent[iii] - ray.point1[iii]) * oneOverD;
+			float t2 = (this.extent[iii] - ray.point1[iii]) * oneOverD;
+			currentNormal[0] = (iii == 0) ? -this.extent[iii] : 0.0f;
+			currentNormal[1] = (iii == 1) ? -this.extent[iii] : 0.0f;
+			currentNormal[2] = (iii == 2) ? -this.extent[iii] : 0.0f;
+			// Swap t1 and t2 if need so that t1 is intersection with near plane and
+			// t2 with far plane
+			if (t1 > t2) {
+				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 = 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 < 0.0f
+	     || tMin > ray.maxFraction) {
+		return false;
+	}
+	if (normalDirection == Vector3f(0,0,0)) {
+		return false;
+	}
+	// The ray intersects the three slabs, we compute the hit point
+	Vector3f localHitPoint = ray.point1 + tMin * rayDirection;
+	raycastInfo.body = proxyShape.getBody();
+	raycastInfo.proxyShape = proxyShape;
+	raycastInfo.hitFraction = tMin;
+	raycastInfo.worldPoint = localHitPoint;
+	raycastInfo.worldNormal = normalDirection;
+	return true;
+}
+
+Vector3f BoxShape::getExtent()  {
+	return this.extent + Vector3f(this.margin, this.margin, this.margin);
+}
+
+void BoxShape::setLocalScaling( Vector3f scaling) {
+	this.extent = (this.extent / this.scaling) * scaling;
+	CollisionShape::setLocalScaling(scaling);
+}
+
+void BoxShape::getLocalBounds(Vector3f min, Vector3f max)  {
+	// Maximum bounds
+	max = this.extent + Vector3f(this.margin, this.margin, this.margin);
+	// Minimum bounds
+	min = -max;
+}
+
+long BoxShape::getSizeInBytes()  {
+	return sizeof(BoxShape);
+}
+
+Vector3f BoxShape::getLocalSupportPointWithoutMargin( Vector3f direction,
+                                                 void** cachedCollisionData)  {
+	return Vector3f(direction.x() < 0.0 ? -this.extent.x() : this.extent.x(),
+	            direction.y() < 0.0 ? -this.extent.y() : this.extent.y(),
+	            direction.z() < 0.0 ? -this.extent.z() : this.extent.z());
+}
+
+boolean BoxShape::testPointInside( Vector3f localPoint, ProxyShape* proxyShape)  {
+	return (    localPoint.x() < this.extent[0]
+	         && localPoint.x() > -this.extent[0]
+	         && localPoint.y() < this.extent[1]
+	         && localPoint.y() > -this.extent[1]
+	         && localPoint.z() < this.extent[2]
+	         && localPoint.z() > -this.extent[2]);
+}
+

src/org/atriaSoft/ephysics/collision/shapes/BoxShape.java

@@ -0,0 +1,41 @@
+package org.atriaSoft.ephysics.collision.shapes;
+
+
+/**
+ * @brief It 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
+ * ructor of the box shape. Otherwise, it is recommended to use the
+ * default margin distance by not using the "margin" parameter in the ructor.
+ */
+class BoxShape extends  ConvexShape {
+	public:
+		/**
+		 * @brief 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( Vector3f extent, float margin = OBJECTMARGIN);
+		/**
+		 * @brief Return the extents of the box
+		 * @return The vector with the three extents of the box shape (in meters)
+		 */
+		Vector3f getExtent();
+		void setLocalScaling( Vector3f scaling);
+		void getLocalBounds(Vector3f min, Vector3f max);
+		void computeLocalInertiaTensor(Matrix3f tensor, float mass);
+	protected:
+		Vector3f extent; //!< Extent sizes of the box in the x, y and z direction
+		Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData);
+		boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape);
+		boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape);
+		long getSizeInBytes();
+};
+
+}

src/org/atriaSoft/ephysics/collision/shapes/CapsuleShape.cpp

@@ -0,0 +1,260 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/shapes/CapsuleShape.hpp>
+#include <ephysics/collision/ProxyShape.hpp>
+#include <ephysics/configuration.hpp>
+
+using namespace ephysics;
+
+CapsuleShape::CapsuleShape(float radius, float height):
+  ConvexShape(CAPSULE, radius),
+  this.halfHeight(height * 0.5f) {
+	assert(radius > 0.0f);
+	assert(height > 0.0f);
+}
+
+void CapsuleShape::computeLocalInertiaTensor(Matrix3f tensor, float mass)  {
+	// The inertia tensor formula for a capsule can be found in : Game Engine Gems, Volume 1
+	float height = this.halfHeight + this.halfHeight;
+	float radiusSquare = this.margin * this.margin;
+	float heightSquare = height * height;
+	float radiusSquareDouble = radiusSquare + radiusSquare;
+	float factor1 = float(2.0) * this.margin / (float(4.0) * this.margin + float(3.0) * height);
+	float factor2 = float(3.0) * height / (float(4.0) * this.margin + float(3.0) * height);
+	float sum1 = float(0.4) * radiusSquareDouble;
+	float sum2 = float(0.75) * height * this.margin + 0.5f * heightSquare;
+	float sum3 = float(0.25) * radiusSquare + float(1.0 / 12.0) * heightSquare;
+	float IxxAndzz = factor1 * mass * (sum1 + sum2) + factor2 * mass * sum3;
+	float Iyy = factor1 * mass * sum1 + factor2 * mass * float(0.25) * radiusSquareDouble;
+	tensor.setValue(IxxAndzz, 0.0, 0.0,
+	                 0.0, Iyy, 0.0,
+	                 0.0, 0.0, IxxAndzz);
+}
+
+boolean CapsuleShape::testPointInside( Vector3f localPoint, ProxyShape* proxyShape)  {
+	 float diffYCenterSphere1 = localPoint.y() - this.halfHeight;
+	 float diffYCenterSphere2 = localPoint.y() + this.halfHeight;
+	 float xSquare = localPoint.x() * localPoint.x();
+	 float zSquare = localPoint.z() * localPoint.z();
+	 float squareRadius = this.margin * this.margin;
+	// Return true if the point is inside the cylinder or one of the two spheres of the capsule
+	return ((xSquare + zSquare) < squareRadius &&
+			localPoint.y() < this.halfHeight && localPoint.y() > -this.halfHeight) ||
+			(xSquare + zSquare + diffYCenterSphere1 * diffYCenterSphere1) < squareRadius ||
+			(xSquare + zSquare + diffYCenterSphere2 * diffYCenterSphere2) < squareRadius;
+}
+
+boolean CapsuleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape)  {
+	 Vector3f n = ray.point2 - ray.point1;
+	 float epsilon = float(0.01);
+	Vector3f p(float(0), -this.halfHeight, float(0));
+	Vector3f q(float(0), this.halfHeight, float(0));
+	Vector3f d = q - p;
+	Vector3f m = ray.point1 - p;
+	float t;
+	float mDotD = m.dot(d);
+	float nDotD = n.dot(d);
+	float dDotD = d.dot(d);
+	// Test if the segment is outside the cylinder
+	float vec1DotD = (ray.point1 - Vector3f(0.0f, -this.halfHeight - this.margin, float(0.0))).dot(d);
+	if (    vec1DotD < 0.0f
+	     && vec1DotD + nDotD < float(0.0)) {
+		return false;
+	}
+	float ddotDExtraCaps = float(2.0) * this.margin * d.y();
+	if (    vec1DotD > dDotD + ddotDExtraCaps
+	     && vec1DotD + nDotD > dDotD + ddotDExtraCaps) {
+		return false;
+	}
+	float nDotN = n.dot(n);
+	float mDotN = m.dot(n);
+	float a = dDotD * nDotN - nDotD * nDotD;
+	float k = m.dot(m) - this.margin * this.margin;
+	float c = dDotD * k - mDotD * mDotD;
+	// If the ray is parallel to the capsule axis
+	if (abs(a) < epsilon) {
+		// If the origin is outside the surface of the capusle's cylinder, we return no hit
+		if (c > 0.0f) {
+			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 < 0.0f) {
+			// Check intersection between the ray and the "p" sphere endcap of the capsule
+			Vector3f hitLocalPoint;
+			float 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;
+				Vector3f 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
+			Vector3f hitLocalPoint;
+			float 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;
+				Vector3f normalDirection = hitLocalPoint - q;
+				raycastInfo.worldNormal = normalDirection;
+				return true;
+			}
+			return false;
+		} else {
+			// If the origin is inside the cylinder, we return no hit
+			return false;
+		}
+	}
+	float b = dDotD * mDotN - nDotD * mDotD;
+	float discriminant = b * b - a * c;
+	// If the discriminant is negative, no real roots and therfore, no hit
+	if (discriminant < 0.0f) {
+		return false;
+	}
+	// Compute the smallest root (first intersection along the ray)
+	float t0 = t = (-b - sqrt(discriminant)) / a;
+	// If the intersection is outside the finite cylinder of the capsule on "p" endcap side
+	float value = mDotD + t * nDotD;
+	if (value < 0.0f) {
+		// Check intersection between the ray and the "p" sphere endcap of the capsule
+		Vector3f hitLocalPoint;
+		float 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;
+			Vector3f 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
+		Vector3f hitLocalPoint;
+		float 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;
+			Vector3f 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 < 0.0f || t > ray.maxFraction) {
+		return false;
+	}
+	// Compute the hit information
+	Vector3f localHitPoint = ray.point1 + t * n;
+	raycastInfo.body = proxyShape.getBody();
+	raycastInfo.proxyShape = proxyShape;
+	raycastInfo.hitFraction = t;
+	raycastInfo.worldPoint = localHitPoint;
+	Vector3f v = localHitPoint - p;
+	Vector3f w = (v.dot(d) / d.length2()) * d;
+	Vector3f normalDirection = (localHitPoint - (p + w)).safeNormalized();
+	raycastInfo.worldNormal = normalDirection;
+	return true;
+}
+
+boolean CapsuleShape::raycastWithSphereEndCap( Vector3f point1,
+                                            Vector3f point2,
+                                            Vector3f sphereCenter,
+                                           float maxFraction,
+                                           Vector3f hitLocalPoint,
+                                           float hitFraction)  {
+	  Vector3f m = point1 - sphereCenter;
+	float c = m.dot(m) - this.margin * this.margin;
+	// If the origin of the ray is inside the sphere, we return no intersection
+	if (c < 0.0f) {
+		return false;
+	}
+	 Vector3f rayDirection = point2 - point1;
+	float 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 > 0.0f) {
+		return false;
+	}
+	float raySquareLength = rayDirection.length2();
+	// Compute the discriminant of the quadratic equation
+	float discriminant = b * b - raySquareLength * c;
+	// If the discriminant is negative or the ray length is very small, there is no intersection
+	if (    discriminant < 0.0f
+	     || raySquareLength < FLTEPSILON) {
+		return false;
+	}
+	// Compute the solution "t" closest to the origin
+	float t = -b - sqrt(discriminant);
+	assert(t >= 0.0f);
+	// 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;
+}
+
+float CapsuleShape::getRadius()  {
+	return this.margin;
+}
+
+float CapsuleShape::getHeight()  {
+	return this.halfHeight + this.halfHeight;
+}
+
+void CapsuleShape::setLocalScaling( Vector3f scaling) {
+	this.halfHeight = (this.halfHeight / this.scaling.y()) * scaling.y();
+	this.margin = (this.margin / this.scaling.x()) * scaling.x();
+	CollisionShape::setLocalScaling(scaling);
+}
+
+long CapsuleShape::getSizeInBytes()  {
+	return sizeof(CapsuleShape);
+}
+
+void CapsuleShape::getLocalBounds(Vector3f min, Vector3f max)  {
+	// Maximum bounds
+	max.setX(this.margin);
+	max.setY(this.halfHeight + this.margin);
+	max.setZ(this.margin);
+	// Minimum bounds
+	min.setX(-this.margin);
+	min.setY(-max.y());
+	min.setZ(min.x());
+}
+
+Vector3f CapsuleShape::getLocalSupportPointWithoutMargin( Vector3f direction,
+                                                     void** cachedCollisionData)  {
+	// Support point top sphere
+	float dotProductTop = this.halfHeight * direction.y();
+	// Support point bottom sphere
+	float dotProductBottom = -this.halfHeight * direction.y();
+	// Return the point with the maximum dot product
+	if (dotProductTop > dotProductBottom) {
+		return Vector3f(0, this.halfHeight, 0);
+	}
+	return Vector3f(0, -this.halfHeight, 0);
+}

src/org/atriaSoft/ephysics/collision/shapes/CapsuleShape.java

@@ -0,0 +1,53 @@
+package org.atriaSoft.ephysics.collision.shapes;
+
+	/**
+	 * @brief It 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 extends  ConvexShape {
+		public :
+			/**
+			 * @brief Constructor
+			 * @param radius The radius of the capsule (in meters)
+			 * @param height The height of the capsule (in meters)
+			 */
+			CapsuleShape(float radius, float height);
+			/// DELETE copy-ructor
+			CapsuleShape( CapsuleShape shape) = delete;
+			/// DELETE assignment operator
+			CapsuleShape operator=( CapsuleShape shape) = delete;
+			/**
+			 * Get the radius of the capsule
+			 * @return The radius of the capsule shape (in meters)
+			 */
+			float getRadius() ;
+			/**
+			 * @brief Return the height of the capsule
+			 * @return The height of the capsule shape (in meters)
+			 */
+			float getHeight() ;
+			void setLocalScaling( Vector3f scaling) ;
+			void getLocalBounds(Vector3f min, Vector3f max)  ;
+			void computeLocalInertiaTensor(Matrix3f tensor, float mass)  ;
+		protected:
+			float halfHeight; //!< Half height of the capsule (height = distance between the centers of the two spheres)
+			Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData)  ;
+			boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape)  ;
+			boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape)  ;
+			/**
+			 * @brief Raycasting method between a ray one of the two spheres end cap of the capsule
+			 */
+			boolean raycastWithSphereEndCap( Vector3f point1,
+			                              Vector3f point2,
+			                              Vector3f sphereCenter,
+			                             float maxFraction,
+			                             Vector3f hitLocalPoint,
+			                             float hitFraction) ;
+			long getSizeInBytes()  ;
+	};
+}

src/org/atriaSoft/ephysics/collision/shapes/CollisionShape.cpp

@@ -0,0 +1,53 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/shapes/CollisionShape.hpp>
+#include <ephysics/engine/Profiler.hpp>
+#include <ephysics/body/CollisionBody.hpp>
+
+// We want to use the ReactPhysics3D namespace
+using namespace ephysics;
+
+CollisionShape::CollisionShape(CollisionShapeType type) :
+  this.type(type),
+  this.scaling(1.0f, 1.0f, 1.0f) {
+	
+}
+
+void CollisionShape::computeAABB(AABB aabb,  Transform3D transform)  {
+	PROFILE("CollisionShape::computeAABB()");
+	// Get the local bounds in x,y and z direction
+	Vector3f minBounds(0,0,0);
+	Vector3f maxBounds(0,0,0);
+	getLocalBounds(minBounds, maxBounds);
+	// Rotate the local bounds according to the orientation of the body
+	Matrix3f worldAxis = transform.getOrientation().getMatrix().getAbsolute();
+	Vector3f worldMinBounds(worldAxis.getColumn(0).dot(minBounds),
+	                    worldAxis.getColumn(1).dot(minBounds),
+	                    worldAxis.getColumn(2).dot(minBounds));
+	Vector3f 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
+	Vector3f minCoordinates = transform.getPosition() + worldMinBounds;
+	Vector3f maxCoordinates = transform.getPosition() + worldMaxBounds;
+	// Update the AABB with the new minimum and maximum coordinates
+	aabb.setMin(minCoordinates);
+	aabb.setMax(maxCoordinates);
+}
+
+int CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1,
+                                                     CollisionShapeType shapeType2) {
+	// If both shapes are convex
+	if (isConvex(shapeType1) && isConvex(shapeType2)) {
+		return NBMAXCONTACTMANIFOLDSCONVEXSHAPE;
+	}
+	// If there is at least one concave shape
+	return NBMAXCONTACTMANIFOLDSCONCAVESHAPE;
+}
+

src/org/atriaSoft/ephysics/collision/shapes/CollisionShape.java

@@ -0,0 +1,90 @@
+package org.atriaSoft.ephysics.collision.shapes;
+
+enum CollisionShapeType {TRIANGLE, BOX, SPHERE, CONE, CYLINDER,
+						 CAPSULE, CONVEXMESH, CONCAVEMESH, HEIGHTFIELD};
+ int NBCOLLISIONSHAPETYPES = 9;
+
+class ProxyShape;
+class CollisionBody;
+
+/**
+ * This abstract class represents the collision shape associated with a
+ * body that is used during the narrow-phase collision detection.
+ */
+class CollisionShape {
+	public :
+		/// Constructor
+		CollisionShape(CollisionShapeType type);
+		/**
+		 * @brief Get the type of the collision shapes
+		 * @return The type of the collision shape (box, sphere, cylinder, ...)
+		 */
+		CollisionShapeType getType()  {
+			return this.type;
+		}
+		/**
+		 * @brief Check if the shape is convex
+		 * @return true If the collision shape is convex
+		 * @return false If it is concave
+		 */
+		 boolean isConvex() = 0;
+		/**
+		 * @brief Get 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 getLocalBounds(Vector3f min, Vector3f max) =0;
+		/// Return the scaling vector of the collision shape
+		Vector3f getScaling()  {
+			return this.scaling;
+		}
+		/**
+		 * @brief Set the scaling vector of the collision shape
+		 */
+		 void setLocalScaling( Vector3f scaling) {
+			this.scaling = scaling;
+		}
+		/**
+		 * @brief Compute the local inertia tensor of the sphere
+		 * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space coordinates
+		 * @param[in] mass Mass to use to compute the inertia tensor of the collision shape
+		 */
+		 void computeLocalInertiaTensor(Matrix3f tensor, float mass) =0;
+		/**
+		 * @brief 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[in] transform Transform3D used to compute the AABB of the collision shape
+		 */
+		 void computeAABB(AABB aabb,  Transform3D transform) ;
+		/**
+		 * @brief Check if the shape is convex
+		 * @param[in] shapeType shape type
+		 * @return true If the collision shape is convex
+		 * @return false If it is concave
+		 */
+		static boolean isConvex(CollisionShapeType shapeType) {
+			return    shapeType != CONCAVEMESH
+			       && shapeType != HEIGHTFIELD;
+		}
+		/**
+		 * @brief Get the maximum number of contact
+		 * @return The maximum number of contact manifolds in an overlapping pair given two shape types
+		 */
+		static int computeNbMaxContactManifolds(CollisionShapeType shapeType1,
+		                                            CollisionShapeType shapeType2);
+	protected :
+		CollisionShapeType type; //!< Type of the collision shape
+		Vector3f scaling; //!< Scaling vector of the collision shape
+		/// Return true if a point is inside the collision shape
+		 boolean testPointInside( Vector3f worldPoint, ProxyShape* proxyShape)  = 0;
+		/// Raycast method with feedback information
+		 boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape)  = 0;
+		/// Return the number of bytes used by the collision shape
+		 long getSizeInBytes()  = 0;
+};
+
+
+
+}
+

src/org/atriaSoft/ephysics/collision/shapes/ConcaveMeshShape.cpp

@@ -0,0 +1,147 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/shapes/ConcaveMeshShape.hpp>
+#include <ephysics/debug.hpp>
+
+using namespace ephysics;
+
+ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh):
+  ConcaveShape(CONCAVEMESH) {
+	this.triangleMesh = triangleMesh;
+	this.raycastTestType = FRONT;
+	initBVHTree();
+}
+
+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 (int subPart=0; subPart<this.triangleMesh.getNbSubparts(); subPart++) {
+		// Get the triangle vertex array of the current sub-part
+		TriangleVertexArray* triangleVertexArray = this.triangleMesh.getSubpart(subPart);
+		// For each triangle of the concave mesh
+		for (long iii=0; iii<triangleVertexArray.getNbTriangles(); ++iii) {
+			ephysics::Triangle trianglePoints = triangleVertexArray.getTriangle(iii);
+			Vector3f trianglePoints2[3];
+			trianglePoints2[0] = trianglePoints[0];
+			trianglePoints2[1] = trianglePoints[1];
+			trianglePoints2[2] = trianglePoints[2];
+			// Create the AABB for the triangle
+			AABB aabb = AABB::createAABBForTriangle(trianglePoints2);
+			aabb.inflate(this.triangleMargin, this.triangleMargin, this.triangleMargin);
+			// Add the AABB with the index of the triangle into the dynamic AABB tree
+			this.dynamicAABBTree.addObject(aabb, subPart, iii);
+		}
+	}
+}
+
+void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int subPart, int triangleIndex, Vector3f* outTriangleVertices)  {
+	EPHYASSERT(outTriangleVertices != null, "Input check error");
+	// Get the triangle vertex array of the current sub-part
+	TriangleVertexArray* triangleVertexArray = this.triangleMesh.getSubpart(subPart);
+	if (triangleVertexArray == null) {
+		Log.error("get null ...");
+	}
+	ephysics::Triangle trianglePoints = triangleVertexArray.getTriangle(triangleIndex);
+	outTriangleVertices[0] = trianglePoints[0] * this.scaling;
+	outTriangleVertices[1] = trianglePoints[1] * this.scaling;
+	outTriangleVertices[2] = trianglePoints[2] * this.scaling;
+}
+
+void ConcaveMeshShape::testAllTriangles(TriangleCallback callback,  AABB localAABB)  {
+	// Ask the Dynamic AABB Tree to report all the triangles that are overlapping
+	// with the AABB of the convex shape.
+	this.dynamicAABBTree.reportAllShapesOverlappingWithAABB(localAABB, [](int nodeId) {
+	                                                     	// Get the node data (triangle index and mesh subpart index)
+	                                                     	int* data = this.dynamicAABBTree.getNodeDataInt(nodeId);
+	                                                     	// Get the triangle vertices for this node from the concave mesh shape
+	                                                     	Vector3f trianglePoints[3];
+	                                                     	getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
+	                                                     	// Call the callback to test narrow-phase collision with this triangle
+	                                                     	callback.testTriangle(trianglePoints);
+	                                                     });
+}
+
+boolean ConcaveMeshShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape)  {
+	PROFILE("ConcaveMeshShape::raycast()");
+	// Create the callback object that will compute ray casting against triangles
+	ConcaveMeshRaycastCallback raycastCallback(this.dynamicAABBTree, *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.
+	this.dynamicAABBTree.raycast(ray, [](int nodeId,  ephysics::Ray ray) mutable { return raycastCallback(nodeId, ray);});
+	raycastCallback.raycastTriangles();
+	return raycastCallback.getIsHit();
+}
+
+float ConcaveMeshRaycastCallback::operator()(int nodeId,  Ray ray) {
+	// Add the id of the hit AABB node into
+	this.hitAABBNodes.pushBack(nodeId);
+	return ray.maxFraction;
+}
+
+void ConcaveMeshRaycastCallback::raycastTriangles() {
+	Vector<int>::Iterator it;
+	float smallestHitFraction = this.ray.maxFraction;
+	for (it = this.hitAABBNodes.begin(); it != this.hitAABBNodes.end(); ++it) {
+		// Get the node data (triangle index and mesh subpart index)
+		int* data = this.dynamicAABBTree.getNodeDataInt(*it);
+		// Get the triangle vertices for this node from the concave mesh shape
+		Vector3f trianglePoints[3];
+		this.concaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
+		// Create a triangle collision shape
+		float margin = this.concaveMeshShape.getTriangleMargin();
+		TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
+		triangleShape.setRaycastTestType(this.concaveMeshShape.getRaycastTestType());
+		// Ray casting test against the collision shape
+		RaycastInfo raycastInfo;
+		boolean isTriangleHit = triangleShape.raycast(this.ray, raycastInfo, this.proxyShape);
+		// If the ray hit the collision shape
+		if (isTriangleHit && raycastInfo.hitFraction <= smallestHitFraction) {
+			assert(raycastInfo.hitFraction >= 0.0f);
+			this.raycastInfo.body = raycastInfo.body;
+			this.raycastInfo.proxyShape = raycastInfo.proxyShape;
+			this.raycastInfo.hitFraction = raycastInfo.hitFraction;
+			this.raycastInfo.worldPoint = raycastInfo.worldPoint;
+			this.raycastInfo.worldNormal = raycastInfo.worldNormal;
+			this.raycastInfo.meshSubpart = data[0];
+			this.raycastInfo.triangleIndex = data[1];
+			smallestHitFraction = raycastInfo.hitFraction;
+			this.isHit = true;
+		}
+	}
+}
+
+long ConcaveMeshShape::getSizeInBytes()  {
+	return sizeof(ConcaveMeshShape);
+}
+
+void ConcaveMeshShape::getLocalBounds(Vector3f min, Vector3f max)  {
+	// Get the AABB of the whole tree
+	AABB treeAABB = this.dynamicAABBTree.getRootAABB();
+	min = treeAABB.getMin();
+	max = treeAABB.getMax();
+}
+
+void ConcaveMeshShape::setLocalScaling( Vector3f scaling) {
+	CollisionShape::setLocalScaling(scaling);
+	this.dynamicAABBTree.reset();
+	initBVHTree();
+}
+
+void ConcaveMeshShape::computeLocalInertiaTensor(Matrix3f tensor, float mass)  {
+	// 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.setValue(mass, 0,     0,
+	                 0,     mass, 0,
+	                 0,     0,     mass);
+}
+
+

src/org/atriaSoft/ephysics/collision/shapes/ConcaveMeshShape.java

@@ -0,0 +1,63 @@
+package org.atriaSoft.ephysics.collision.shapes;
+
+	class ConcaveMeshRaycastCallback {
+		private:
+			Vector<int> hitAABBNodes;
+			DynamicAABBTree dynamicAABBTree;
+			ConcaveMeshShape concaveMeshShape;
+			ProxyShape* proxyShape;
+			RaycastInfo raycastInfo;
+			Ray ray;
+			boolean isHit;
+		public:
+			// Constructor
+			ConcaveMeshRaycastCallback( DynamicAABBTree dynamicAABBTree,
+			                            ConcaveMeshShape concaveMeshShape,
+			                           ProxyShape* proxyShape,
+			                           RaycastInfo raycastInfo,
+			                            Ray ray):
+			  this.dynamicAABBTree(dynamicAABBTree),
+			  this.concaveMeshShape(concaveMeshShape),
+			  this.proxyShape(proxyShape),
+			  this.raycastInfo(raycastInfo),
+			  this.ray(ray),
+			  this.isHit(false) {
+				
+			}
+			/// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree
+			float operator()(int nodeId,  ephysics::Ray ray);
+			/// Raycast all collision shapes that have been collected
+			void raycastTriangles();
+			/// Return true if a raycast hit has been found
+			boolean getIsHit()  {
+				return this.isHit;
+			}
+	};
+	/**
+	 * @brief 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 extends  ConcaveShape {
+		public:
+			/// Constructor
+			ConcaveMeshShape(TriangleMesh* triangleMesh);
+			void getLocalBounds(Vector3f min, Vector3f max);
+			void setLocalScaling( Vector3f scaling);
+			void computeLocalInertiaTensor(Matrix3f tensor, float mass);
+			void testAllTriangles(TriangleCallback callback,  AABB localAABB);
+		protected:
+			TriangleMesh* triangleMesh; //!< Triangle mesh
+			DynamicAABBTree dynamicAABBTree; //!< Dynamic AABB tree to accelerate collision with the triangles
+			boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape);
+			long getSizeInBytes();
+			/// 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(int subPart,
+			                                         int triangleIndex,
+			                                         Vector3f* outTriangleVertices) ;
+	};
+
+}

src/org/atriaSoft/ephysics/collision/shapes/ConcaveShape.cpp

@@ -0,0 +1,50 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/shapes/ConcaveShape.hpp>
+
+
+// We want to use the ReactPhysics3D namespace
+using namespace ephysics;
+
+// Constructor
+ConcaveShape::ConcaveShape(CollisionShapeType type):
+  CollisionShape(type),
+  this.isSmoothMeshCollisionEnabled(false),
+  this.triangleMargin(0),
+  this.raycastTestType(FRONT) {
+	
+}
+
+float ConcaveShape::getTriangleMargin()  {
+	return this.triangleMargin;
+}
+
+boolean ConcaveShape::isConvex()  {
+	return false;
+}
+
+boolean ConcaveShape::testPointInside( Vector3f localPoint, ProxyShape* proxyShape)  {
+	return false;
+}
+
+boolean ConcaveShape::getIsSmoothMeshCollisionEnabled()  {
+	return this.isSmoothMeshCollisionEnabled;
+}
+
+void ConcaveShape::setIsSmoothMeshCollisionEnabled(boolean isEnabled) {
+	this.isSmoothMeshCollisionEnabled = isEnabled;
+}
+
+TriangleRaycastSide ConcaveShape::getRaycastTestType()  {
+	return this.raycastTestType;
+}
+
+void ConcaveShape::setRaycastTestType(TriangleRaycastSide testType) {
+	this.raycastTestType = testType;
+}

src/org/atriaSoft/ephysics/collision/shapes/ConcaveShape.java

@@ -0,0 +1,56 @@
+package org.atriaSoft.ephysics.collision.shapes;
+
+	/**
+	 * @brief It is used to encapsulate a callback method for
+	 * a single triangle of a ConcaveMesh.
+	 */
+	class TriangleCallback {
+		/// Report a triangle
+		public void testTriangle( Vector3f* trianglePoints)=0;
+	};
+	
+	/**
+	 * @brief This abstract class represents a concave collision shape associated with a
+	 * body that is used during the narrow-phase collision detection.
+	 */
+	class ConcaveShape extends  CollisionShape {
+		public :
+			/// Constructor
+			ConcaveShape(CollisionShapeType type);
+			/// DELETE copy-ructor
+			ConcaveShape( ConcaveShape shape) = delete;
+			/// DELETE assignment operator
+			ConcaveShape operator=( ConcaveShape shape) = delete;
+		protected :
+			boolean isSmoothMeshCollisionEnabled; //!< True if the smooth mesh collision algorithm is enabled
+			float triangleMargin; //!< Margin use for collision detection for each triangle
+			TriangleRaycastSide raycastTestType; //!< Raycast test type for the triangle (front, back, front-back)
+			boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape)  ;
+		public:
+			/// Return the triangle margin
+			float getTriangleMargin() ;
+			/// Return the raycast test type (front, back, front-back)
+			TriangleRaycastSide getRaycastTestType() ;
+			/**
+			 * @brief Set the raycast test type (front, back, front-back)
+			 * @param testType Raycast test type for the triangle (front, back, front-back)
+			 */
+			void setRaycastTestType(TriangleRaycastSide testType);
+			/// Return true if the collision shape is convex, false if it is concave
+			 boolean isConvex()  ;
+			/// Use a callback method on all triangles of the concave shape inside a given AABB
+			 void testAllTriangles(TriangleCallback callback,  AABB localAABB) =0;
+			/// Return true if the smooth mesh collision is enabled
+			boolean getIsSmoothMeshCollisionEnabled() ;
+			/**
+			 * @brief 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.
+			 */
+			void setIsSmoothMeshCollisionEnabled(boolean isEnabled);
+	};
+
+}
+
+

src/org/atriaSoft/ephysics/collision/shapes/ConeShape.cpp

@@ -0,0 +1,202 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/configuration.hpp>
+#include <ephysics/collision/shapes/ConeShape.hpp>
+#include <ephysics/collision/ProxyShape.hpp>
+
+using namespace ephysics;
+
+ConeShape::ConeShape(float radius, float height, float margin):
+  ConvexShape(CONE, margin),
+  this.radius(radius),
+  this.halfHeight(height * 0.5f) {
+	assert(this.radius > 0.0f);
+	assert(this.halfHeight > 0.0f);
+	// Compute the sine of the semi-angle at the apex point
+	this.sinTheta = this.radius / (sqrt(this.radius * this.radius + height * height));
+}
+
+Vector3f ConeShape::getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData)  {
+	 Vector3f v = direction;
+	float sinThetaTimesLengthV = this.sinTheta * v.length();
+	Vector3f supportPoint;
+	if (v.y() > sinThetaTimesLengthV) {
+		supportPoint = Vector3f(0.0, this.halfHeight, 0.0);
+	} else {
+		float projectedLength = sqrt(v.x() * v.x() + v.z() * v.z());
+		if (projectedLength > FLTEPSILON) {
+			float d = this.radius / projectedLength;
+			supportPoint = Vector3f(v.x() * d, -this.halfHeight, v.z() * d);
+		} else {
+			supportPoint = Vector3f(0.0, -this.halfHeight, 0.0);
+		}
+	}
+	return supportPoint;
+}
+
+boolean ConeShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape)  {
+	 Vector3f r = ray.point2 - ray.point1;
+	 float epsilon = float(0.00001);
+	Vector3f V(0, this.halfHeight, 0);
+	Vector3f centerBase(0, -this.halfHeight, 0);
+	Vector3f axis(0, float(-1.0), 0);
+	float heightSquare = float(4.0) * this.halfHeight * this.halfHeight;
+	float cosThetaSquare = heightSquare / (heightSquare + this.radius * this.radius);
+	float factor = 1.0f - cosThetaSquare;
+	Vector3f delta = ray.point1 - V;
+	float c0 = -cosThetaSquare * delta.x() * delta.x()  + factor * delta.y() * delta.y() - cosThetaSquare * delta.z() * delta.z();
+	float c1 = -cosThetaSquare * delta.x() * r.x() + factor * delta.y() * r.y() - cosThetaSquare * delta.z() * r.z();
+	float c2 = -cosThetaSquare * r.x() * r.x()  + factor * r.y() * r.y() - cosThetaSquare * r.z() * r.z();
+	float tHit[] = {float(-1.0), float(-1.0), float(-1.0)};
+	Vector3f localHitPoint[3];
+	Vector3f localNormal[3];
+	// If c2 is different from zero
+	if (abs(c2) > FLTEPSILON) {
+		float gamma = c1 * c1 - c0 * c2;
+		// If there is no real roots in the quadratic equation
+		if (gamma < 0.0f) {
+			return false;
+		} else if (gamma > 0.0f) {	// The equation has two real roots
+			// Compute two intersections
+			float sqrRoot = 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 (abs(c1) > FLTEPSILON) {
+			// If c2 = 0 and c1 != 0
+			tHit[0] = -c0 / (float(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) < 0.0f) {
+		tHit[0] = float(-1.0);
+	}
+	if (axis.dot(localHitPoint[1] - V) < 0.0f) {
+		tHit[1] = float(-1.0);
+	}
+	// Only keep hit points that are within the correct height of the cone
+	if (localHitPoint[0].y() < float(-this.halfHeight)) {
+		tHit[0] = float(-1.0);
+	}
+	if (localHitPoint[1].y() < float(-this.halfHeight)) {
+		tHit[1] = float(-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() - this.halfHeight) / (r.y());
+		// Only keep this intersection if it is inside the cone radius
+		localHitPoint[2] = ray.point1 + tHit[2] * r;
+		if ((localHitPoint[2] - centerBase).length2() > this.radius * this.radius) {
+			tHit[2] = float(-1.0);
+		}
+		// Compute the normal direction
+		localNormal[2] = axis;
+	}
+	// Find the smallest positive t value
+	int hitIndex = -1;
+	float t = FLTMAX;
+	for (int i=0; i<3; i++) {
+		if (tHit[i] < 0.0f) {
+			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) {
+		float h = float(2.0) * this.halfHeight;
+		float value1 = (localHitPoint[hitIndex].x() * localHitPoint[hitIndex].x() + localHitPoint[hitIndex].z() * localHitPoint[hitIndex].z());
+		float rOverH = this.radius / h;
+		float value2 = 1.0f + rOverH * rOverH;
+		float factor = 1.0f / sqrt(value1 * value2);
+		float x = localHitPoint[hitIndex].x() * factor;
+		float z = localHitPoint[hitIndex].z() * factor;
+		localNormal[hitIndex].setX(x);
+		localNormal[hitIndex].setY(sqrt(x * x + z * z) * rOverH);
+		localNormal[hitIndex].setZ(z);
+	}
+	raycastInfo.body = proxyShape.getBody();
+	raycastInfo.proxyShape = proxyShape;
+	raycastInfo.hitFraction = t;
+	raycastInfo.worldPoint = localHitPoint[hitIndex];
+	raycastInfo.worldNormal = localNormal[hitIndex];
+	return true;
+}
+
+float ConeShape::getRadius()  {
+	return this.radius;
+}
+
+float ConeShape::getHeight()  {
+	return float(2.0) * this.halfHeight;
+}
+
+void ConeShape::setLocalScaling( Vector3f scaling) {
+	this.halfHeight = (this.halfHeight / this.scaling.y()) * scaling.y();
+	this.radius = (this.radius / this.scaling.x()) * scaling.x();
+	CollisionShape::setLocalScaling(scaling);
+}
+
+long ConeShape::getSizeInBytes()  {
+	return sizeof(ConeShape);
+}
+
+void ConeShape::getLocalBounds(Vector3f min, Vector3f max)  {
+	// Maximum bounds
+	max.setX(this.radius + this.margin);
+	max.setY(this.halfHeight + this.margin);
+	max.setZ(max.x());
+	// Minimum bounds
+	min.setX(-max.x());
+	min.setY(-max.y());
+	min.setZ(min.x());
+}
+
+void ConeShape::computeLocalInertiaTensor(Matrix3f tensor, float mass)  {
+	float rSquare = this.radius * this.radius;
+	float diagXZ = float(0.15) * mass * (rSquare + this.halfHeight);
+	tensor.setValue(diagXZ, 0.0, 0.0,
+	                 0.0, float(0.3) * mass * rSquare,
+	                 0.0, 0.0, 0.0, diagXZ);
+}
+
+boolean ConeShape::testPointInside( Vector3f localPoint, ProxyShape* proxyShape)  {
+	 float radiusHeight =   this.radius
+	                           * (-localPoint.y() + this.halfHeight)
+	                           / (this.halfHeight * float(2.0));
+	return (    localPoint.y() < this.halfHeight
+	         && localPoint.y() > -this.halfHeight)
+	         && (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z() < radiusHeight *radiusHeight);
+}

src/org/atriaSoft/ephysics/collision/shapes/ConeShape.java

@@ -0,0 +1,50 @@
+package org.atriaSoft.ephysics.collision.shapes;
+
+	/**
+	 * @brief 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
+	 * ructor of the cone shape. Otherwise, it is recommended to use the
+	 * default margin distance by not using the "margin" parameter in the ructor.
+	 */
+	class ConeShape extends  ConvexShape {
+		public :
+			/**
+			 * @brief 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(float radius, float height, float margin = OBJECTMARGIN);
+		protected :
+			float radius; //!< Radius of the base
+			float halfHeight; //!< Half height of the cone
+			float sinTheta; //!< sine of the semi angle at the apex point
+			Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData)  ;
+			boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape)  ;
+			boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape)  ;
+			long getSizeInBytes()  ;
+		public:
+			/**
+			 * @brief Return the radius
+			 * @return Radius of the cone (in meters)
+			 */
+			float getRadius();
+			/**
+			 * @brief Return the height
+			 * @return Height of the cone (in meters)
+			 */
+			float getHeight();
+			
+			void setLocalScaling( Vector3f scaling) ;
+			void getLocalBounds(Vector3f min, Vector3f max)  ;
+			void computeLocalInertiaTensor(Matrix3f tensor, float mass)  ;
+	};
+}

src/org/atriaSoft/ephysics/collision/shapes/ConvexMeshShape.cpp

@@ -0,0 +1,248 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/configuration.hpp>
+#include <ephysics/collision/shapes/ConvexMeshShape.hpp>
+
+using namespace ephysics;
+
+ConvexMeshShape::ConvexMeshShape( float* arrayVertices,
+                                 int nbVertices,
+                                 int stride,
+                                 float margin):
+  ConvexShape(CONVEXMESH, margin),
+  this.numberVertices(nbVertices),
+  this.minBounds(0, 0, 0),
+  this.maxBounds(0, 0, 0),
+  this.isEdgesInformationUsed(false) {
+	assert(nbVertices > 0);
+	assert(stride > 0);
+	 unsigned char* vertexPointer = ( unsigned char*) arrayVertices;
+	// Copy all the vertices into the internal array
+	for (int iii=0; iii<this.numberVertices; iii++) {
+		 float* newPoint = ( float*) vertexPointer;
+		this.vertices.pushBack(Vector3f(newPoint[0], newPoint[1], newPoint[2]));
+		vertexPointer += stride;
+	}
+	// Recalculate the bounds of the mesh
+	recalculateBounds();
+}
+
+ConvexMeshShape::ConvexMeshShape(TriangleVertexArray* triangleVertexArray,
+                                 boolean isEdgesInformationUsed,
+                                 float margin):
+  ConvexShape(CONVEXMESH, margin),
+  this.minBounds(0, 0, 0),
+  this.maxBounds(0, 0, 0),
+  this.isEdgesInformationUsed(isEdgesInformationUsed) {
+	// For each vertex of the mesh
+	for (auto it: triangleVertexArray.getVertices()) {
+		this.vertices.pushBack(it*this.scaling);
+	}
+	// If we need to use the edges information of the mesh
+	if (this.isEdgesInformationUsed) {
+		// For each triangle of the mesh
+		for (long iii=0; iii<triangleVertexArray.getNbTriangles(); iii++) {
+			int vertexIndex[3] = {0, 0, 0};
+			vertexIndex[0] = triangleVertexArray.getIndices()[iii*3];
+			vertexIndex[1] = triangleVertexArray.getIndices()[iii*3+1];
+			vertexIndex[2] = triangleVertexArray.getIndices()[iii*3+2];
+			// Add information about the edges
+			addEdge(vertexIndex[0], vertexIndex[1]);
+			addEdge(vertexIndex[0], vertexIndex[2]);
+			addEdge(vertexIndex[1], vertexIndex[2]);
+		}
+	}
+	this.numberVertices = this.vertices.size();
+	recalculateBounds();
+}
+
+ConvexMeshShape::ConvexMeshShape(float margin):
+  ConvexShape(CONVEXMESH, margin),
+  this.numberVertices(0),
+  this.minBounds(0, 0, 0),
+  this.maxBounds(0, 0, 0),
+  this.isEdgesInformationUsed(false) {
+	
+}
+
+Vector3f ConvexMeshShape::getLocalSupportPointWithoutMargin( Vector3f direction,
+                                                        void** cachedCollisionData)  {
+	assert(this.numberVertices == this.vertices.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 (this.isEdgesInformationUsed) {
+		assert(this.edgesAdjacencyList.size() == this.numberVertices);
+		int maxVertex = *((int*)(*cachedCollisionData));
+		float maxDotProduct = direction.dot(this.vertices[maxVertex]);
+		boolean isOptimal;
+		// Perform hill-climbing (local search)
+		do {
+			isOptimal = true;
+			assert(this.edgesAdjacencyList[maxVertex].size() > 0);
+			// For all neighbors of the current vertex
+			Set<int>::Iterator it;
+			Set<int>::Iterator itBegin = this.edgesAdjacencyList[maxVertex].begin();
+			Set<int>::Iterator itEnd = this.edgesAdjacencyList[maxVertex].end();
+			for (it = itBegin; it != itEnd; ++it) {
+				// Compute the dot product
+				float dotProduct = direction.dot(this.vertices[*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 this.vertices[maxVertex] * this.scaling;
+	} else {
+		// If the edges information is not used
+		double maxDotProduct = FLTMIN;
+		int indexMaxDotProduct = 0;
+		// For each vertex of the mesh
+		for (int i=0; i<this.numberVertices; i++) {
+			// Compute the dot product of the current vertex
+			double dotProduct = direction.dot(this.vertices[i]);
+			// If the current dot product is larger than the maximum one
+			if (dotProduct > maxDotProduct) {
+				indexMaxDotProduct = i;
+				maxDotProduct = dotProduct;
+			}
+		}
+		assert(maxDotProduct >= 0.0f);
+		// Return the vertex with the largest dot product in the support direction
+		return this.vertices[indexMaxDotProduct] * this.scaling;
+	}
+}
+
+// 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)
+	this.minBounds.setZero();
+	this.maxBounds.setZero();
+	// For each vertex of the mesh
+	for (int i=0; i<this.numberVertices; i++) {
+		if (this.vertices[i].x() > this.maxBounds.x()) {
+			this.maxBounds.setX(this.vertices[i].x());
+		}
+		if (this.vertices[i].x() < this.minBounds.x()) {
+			this.minBounds.setX(this.vertices[i].x());
+		}
+		if (this.vertices[i].y() > this.maxBounds.y()) {
+			this.maxBounds.setY(this.vertices[i].y());
+		}
+		if (this.vertices[i].y() < this.minBounds.y()) {
+			this.minBounds.setY(this.vertices[i].y());
+		}
+		if (this.vertices[i].z() > this.maxBounds.z()) {
+			this.maxBounds.setZ(this.vertices[i].z());
+		}
+		if (this.vertices[i].z() < this.minBounds.z()) {
+			this.minBounds.setZ(this.vertices[i].z());
+		}
+	}
+	// Apply the local scaling factor
+	this.maxBounds = this.maxBounds * this.scaling;
+	this.minBounds = this.minBounds * this.scaling;
+	// Add the object margin to the bounds
+	this.maxBounds += Vector3f(this.margin, this.margin, this.margin);
+	this.minBounds -= Vector3f(this.margin, this.margin, this.margin);
+}
+
+boolean ConvexMeshShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape)  {
+	return proxyShape.this.body.this.world.collisionDetection.narrowPhaseGJKAlgorithm.raycast(ray, proxyShape, raycastInfo);
+}
+
+void ConvexMeshShape::setLocalScaling( Vector3f scaling) {
+	ConvexShape::setLocalScaling(scaling);
+	recalculateBounds();
+}
+
+long ConvexMeshShape::getSizeInBytes()  {
+	return sizeof(ConvexMeshShape);
+}
+
+void ConvexMeshShape::getLocalBounds(Vector3f min, Vector3f max)  {
+	min = this.minBounds;
+	max = this.maxBounds;
+}
+
+void ConvexMeshShape::computeLocalInertiaTensor(Matrix3f tensor, float mass)  {
+	float factor = (1.0f / float(3.0)) * mass;
+	Vector3f realExtent = 0.5f * (this.maxBounds - this.minBounds);
+	assert(realExtent.x() > 0 && realExtent.y() > 0 hjkhjkhjkhkj realExtent.z() > 0);
+	float xSquare = realExtent.x() * realExtent.x();
+	float ySquare = realExtent.y() * realExtent.y();
+	float zSquare = realExtent.z() * realExtent.z();
+	tensor.setValue(factor * (ySquare + zSquare), 0.0, 0.0,
+	                 0.0, factor * (xSquare + zSquare), 0.0,
+	                 0.0, 0.0, factor * (xSquare + ySquare));
+}
+
+void ConvexMeshShape::addVertex( Vector3f vertex) {
+	// Add the vertex in to vertices array
+	this.vertices.pushBack(vertex);
+	this.numberVertices++;
+	// Update the bounds of the mesh
+	if (vertex.x() * this.scaling.x() > this.maxBounds.x()) {
+		this.maxBounds.setX(vertex.x() * this.scaling.x());
+	}
+	if (vertex.x() * this.scaling.x() < this.minBounds.x()) {
+		this.minBounds.setX(vertex.x() * this.scaling.x());
+	}
+	if (vertex.y() * this.scaling.y() > this.maxBounds.y()) {
+		this.maxBounds.setY(vertex.y() * this.scaling.y());
+	}
+	if (vertex.y() * this.scaling.y() < this.minBounds.y()) {
+		this.minBounds.setY(vertex.y() * this.scaling.y());
+	}
+	if (vertex.z() * this.scaling.z() > this.maxBounds.z()) {
+		this.maxBounds.setZ(vertex.z() * this.scaling.z());
+	}
+	if (vertex.z() * this.scaling.z() < this.minBounds.z()) {
+		this.minBounds.setZ(vertex.z() * this.scaling.z());
+	}
+}
+
+void ConvexMeshShape::addEdge(int v1, int v2) {
+	// If the entry for vertex v1 does not exist in the adjacency list
+	if (this.edgesAdjacencyList.count(v1) == 0) {
+		this.edgesAdjacencyList.add(v1, Set<int>());
+	}
+	// If the entry for vertex v2 does not exist in the adjacency list
+	if (this.edgesAdjacencyList.count(v2) == 0) {
+		this.edgesAdjacencyList.add(v2, Set<int>());
+	}
+	// Add the edge in the adjacency list
+	this.edgesAdjacencyList[v1].add(v2);
+	this.edgesAdjacencyList[v2].add(v1);
+}
+
+boolean ConvexMeshShape::isEdgesInformationUsed()  {
+	return this.isEdgesInformationUsed;
+}
+
+void ConvexMeshShape::setIsEdgesInformationUsed(boolean isEdgesUsed) {
+	this.isEdgesInformationUsed = isEdgesUsed;
+}
+
+boolean ConvexMeshShape::testPointInside( Vector3f localPoint,
+                                      ProxyShape* proxyShape)  {
+	// Use the GJK algorithm to test if the point is inside the convex mesh
+	return proxyShape.this.body.this.world.collisionDetection.narrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape);
+}

src/org/atriaSoft/ephysics/collision/shapes/ConvexMeshShape.java

@@ -0,0 +1,92 @@
+package org.atriaSoft.ephysics.collision.shapes;
+
+	/**
+	 * @brief It 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 ructor 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) ant 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 extends  ConvexShape {
+		protected :
+			Vector<Vector3f> vertices; //!< Array with the vertices of the mesh
+			int numberVertices; //!< Number of vertices in the mesh
+			Vector3f minBounds; //!< Mesh minimum bounds in the three local x, y and z directions
+			Vector3f maxBounds; //!< Mesh maximum bounds in the three local x, y and z directions
+			boolean isEdgesInformationUsed; //!< True if the shape contains the edges of the convex mesh in order to make the collision detection faster
+			Map<int, Set<int> > edgesAdjacencyList; //!< Adjacency list representing the edges of the mesh
+			/// Recompute the bounds of the mesh
+			void recalculateBounds();
+			void setLocalScaling( Vector3f scaling) ;
+			Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData)  ;
+			boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape)  ;
+			boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape)  ;
+			long getSizeInBytes()  ;
+		public :
+			/**
+			 * @brief Constructor to initialize with an array of 3D vertices.
+			 * This method creates an internal copy of the input vertices.
+			 * @param[in] arrayVertices Array with the vertices of the convex mesh
+			 * @param[in] nbVertices Number of vertices in the convex mesh
+			 * @param[in] stride Stride between the beginning of two elements in the vertices array
+			 * @param[in] margin Collision margin (in meters) around the collision shape
+			 */
+			ConvexMeshShape( float* arrayVertices,
+			                int nbVertices,
+			                int stride,
+			                float margin = OBJECTMARGIN);
+			/**
+			 * @brief 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(TriangleVertexArray* triangleVertexArray,
+			                boolean isEdgesInformationUsed = true,
+			                float margin = OBJECTMARGIN);
+			/**
+			 * @brief Constructor.
+			 * If you use this ructor, you will need to set the vertices manually one by one using the addVertex() method.
+			 */
+			ConvexMeshShape(float margin = OBJECTMARGIN);
+		public:
+			void getLocalBounds(Vector3f min, Vector3f max)  ;
+			void computeLocalInertiaTensor(Matrix3f tensor, float mass)  ;
+			/**
+			 * @brief Add a vertex into the convex mesh
+			 * @param vertex Vertex to be added
+			 */
+			void addVertex( Vector3f vertex);
+			/**
+			 * @brief 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 ructor or the order of the calls
+			 * of the addVertex() methods that you use to add vertices into the convex mesh.
+			 * @param[in] v1 Index of the first vertex of the edge to add
+			 * @param[in] v2 Index of the second vertex of the edge to add
+			 */
+			void addEdge(int v1, int v2);
+			/**
+			 * @brief 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
+			 */
+			boolean isEdgesInformationUsed() ;
+			/**
+			 * @brief Set the variable to know if the edges information is used to speed up the
+			 * collision detection
+			 * @param[in] isEdgesUsed True if you want to use the edges information to speed up the collision detection with the convex mesh shape
+			 */
+			void setIsEdgesInformationUsed(boolean isEdgesUsed);
+	};
+}
+
+

src/org/atriaSoft/ephysics/collision/shapes/ConvexShape.cpp

@@ -0,0 +1,33 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/shapes/ConvexShape.hpp>
+
+ephysics::ConvexShape::ConvexShape(ephysics::CollisionShapeType type, float margin):
+  CollisionShape(type),
+  this.margin(margin) {
+	
+}
+
+ephysics::ConvexShape::~ConvexShape() {
+	
+}
+
+Vector3f ephysics::ConvexShape::getLocalSupportPointWithMargin( Vector3f direction, void** cachedCollisionData)  {
+	// Get the support point without margin
+	Vector3f supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);
+	if (this.margin != 0.0f) {
+		// Add the margin to the support point
+		Vector3f unitVec(0.0, -1.0, 0.0);
+		if (direction.length2() > FLTEPSILON * FLTEPSILON) {
+			unitVec = direction.safeNormalized();
+		}
+		supportPoint += unitVec * this.margin;
+	}
+	return supportPoint;
+}

src/org/atriaSoft/ephysics/collision/shapes/ConvexShape.java

@@ -0,0 +1,33 @@
+package org.atriaSoft.ephysics.collision.shapes;
+
+/**
+ * @brief It represents a convex collision shape associated with a
+ * body that is used during the narrow-phase collision detection.
+ */
+class ConvexShape: public CollisionShape {
+	protected:
+		float margin; //!< Margin used for the GJK collision detection algorithm
+		// Return a local support point in a given direction with the object margin
+		 Vector3f getLocalSupportPointWithMargin( Vector3f direction, void** cachedCollisionData) ;
+		/// Return a local support point in a given direction without the object margin
+		 Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) =0;
+		boolean testPointInside( Vector3f worldPoint, ProxyShape* proxyShape)   = 0;
+	public:
+		/// Constructor
+		ConvexShape(CollisionShapeType type, float margin);
+	public:
+		/**
+		 * @brief Get the current object margin
+		 * @return The margin (in meters) around the collision shape
+		 */
+		float getMargin()  {
+			return this.margin;
+		}
+		 boolean isConvex()   {
+			return true;
+		}
+};
+
+}
+
+

src/org/atriaSoft/ephysics/collision/shapes/CylinderShape.cpp

@@ -0,0 +1,238 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/shapes/CylinderShape.hpp>
+#include <ephysics/collision/ProxyShape.hpp>
+#include <ephysics/configuration.hpp>
+
+using namespace ephysics;
+
+CylinderShape::CylinderShape(float radius,
+                             float height,
+                             float margin):
+  ConvexShape(CYLINDER, margin), this.radius(radius), this.halfHeight(height/float(2.0)) {
+	assert(radius > 0.0f);
+	assert(height > 0.0f);
+}
+
+Vector3f CylinderShape::getLocalSupportPointWithoutMargin( Vector3f direction,
+                                                      void** cachedCollisionData)  {
+	Vector3f supportPoint(0.0, 0.0, 0.0);
+	float uDotv = direction.y();
+	Vector3f w(direction.x(), 0.0, direction.z());
+	float lengthW = sqrt(direction.x() * direction.x() + direction.z() * direction.z());
+	if (lengthW > FLTEPSILON) {
+		if (uDotv < 0.0) {
+			supportPoint.setY(-this.halfHeight);
+		} else {
+			supportPoint.setY(this.halfHeight);
+		}
+		supportPoint += (this.radius / lengthW) * w;
+	} else {
+		if (uDotv < 0.0) {
+			supportPoint.setY(-this.halfHeight);
+		} else {
+			supportPoint.setY(this.halfHeight);
+		}
+	}
+	return supportPoint;
+}
+
+boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape)  {
+	 Vector3f n = ray.point2 - ray.point1;
+	 float epsilon = float(0.01);
+	Vector3f p(float(0), -this.halfHeight, float(0));
+	Vector3f q(float(0), this.halfHeight, float(0));
+	Vector3f d = q - p;
+	Vector3f m = ray.point1 - p;
+	float t;
+	float mDotD = m.dot(d);
+	float nDotD = n.dot(d);
+	float dDotD = d.dot(d);
+	// Test if the segment is outside the cylinder
+	if (mDotD < 0.0f && mDotD + nDotD < float(0.0)) {
+		return false;
+	}
+	if (mDotD > dDotD && mDotD + nDotD > dDotD) {
+		return false;
+	}
+	float nDotN = n.dot(n);
+	float mDotN = m.dot(n);
+	float a = dDotD * nDotN - nDotD * nDotD;
+	float k = m.dot(m) - this.radius * this.radius;
+	float c = dDotD * k - mDotD * mDotD;
+	// If the ray is parallel to the cylinder axis
+	if (abs(a) < epsilon) {
+		// If the origin is outside the surface of the cylinder, we return no hit
+		if (c > 0.0f) {
+			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 < 0.0f) {
+			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 < 0.0f || t > ray.maxFraction) {
+				return false;
+			}
+			// Compute the hit information
+			Vector3f localHitPoint = ray.point1 + t * n;
+			raycastInfo.body = proxyShape.getBody();
+			raycastInfo.proxyShape = proxyShape;
+			raycastInfo.hitFraction = t;
+			raycastInfo.worldPoint = localHitPoint;
+			Vector3f normalDirection(0, float(-1), 0);
+			raycastInfo.worldNormal = normalDirection;
+			return true;
+		}
+		// If the ray intersects with the "q" endcap of the cylinder
+		if (mDotD > dDotD) {
+			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 < 0.0f || t > ray.maxFraction) {
+				return false;
+			}
+			// Compute the hit information
+			Vector3f localHitPoint = ray.point1 + t * n;
+			raycastInfo.body = proxyShape.getBody();
+			raycastInfo.proxyShape = proxyShape;
+			raycastInfo.hitFraction = t;
+			raycastInfo.worldPoint = localHitPoint;
+			Vector3f normalDirection(0, 1.0f, 0);
+			raycastInfo.worldNormal = normalDirection;
+			return true;
+		}
+		// If the origin is inside the cylinder, we return no hit
+		return false;
+	}
+	float b = dDotD * mDotN - nDotD * mDotD;
+	float discriminant = b * b - a * c;
+	// If the discriminant is negative, no real roots and therfore, no hit
+	if (discriminant < 0.0f) {
+		return false;
+	}
+	// Compute the smallest root (first intersection along the ray)
+	float t0 = t = (-b - sqrt(discriminant)) / a;
+	// If the intersection is outside the cylinder on "p" endcap side
+	float value = mDotD + t * nDotD;
+	if (value < 0.0f) {
+		// If the ray is pointing away from the "p" endcap, we return no hit
+		if (nDotD <= 0.0f) {
+			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 * (float(2.0) * mDotN + t) > 0.0f) {
+			return false;
+		}
+		// If the intersection is behind the origin of the ray or beyond the maximum
+		// raycasting distance, we return no hit
+		if (t < 0.0f || t > ray.maxFraction) {
+			return false;
+		}
+		// Compute the hit information
+		Vector3f localHitPoint = ray.point1 + t * n;
+		raycastInfo.body = proxyShape.getBody();
+		raycastInfo.proxyShape = proxyShape;
+		raycastInfo.hitFraction = t;
+		raycastInfo.worldPoint = localHitPoint;
+		Vector3f normalDirection(0, float(-1.0), 0);
+		raycastInfo.worldNormal = normalDirection;
+		return true;
+	}
+	// If the intersection is outside the cylinder on the "q" side
+	if (value > dDotD) {
+		// If the ray is pointing away from the "q" endcap, we return no hit
+		if (nDotD >= 0.0f) {
+			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 - float(2.0) * mDotD + t * (float(2.0) * (mDotN - nDotD) + t) > 0.0f) {
+			return false;
+		}
+		// If the intersection is behind the origin of the ray or beyond the maximum
+		// raycasting distance, we return no hit
+		if (t < 0.0f || t > ray.maxFraction) {
+			return false;
+		}
+		// Compute the hit information
+		Vector3f localHitPoint = ray.point1 + t * n;
+		raycastInfo.body = proxyShape.getBody();
+		raycastInfo.proxyShape = proxyShape;
+		raycastInfo.hitFraction = t;
+		raycastInfo.worldPoint = localHitPoint;
+		Vector3f normalDirection(0, 1.0f, 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 < 0.0f || t > ray.maxFraction) {
+		return false;
+	}
+	// Compute the hit information
+	Vector3f localHitPoint = ray.point1 + t * n;
+	raycastInfo.body = proxyShape.getBody();
+	raycastInfo.proxyShape = proxyShape;
+	raycastInfo.hitFraction = t;
+	raycastInfo.worldPoint = localHitPoint;
+	Vector3f v = localHitPoint - p;
+	Vector3f w = (v.dot(d) / d.length2()) * d;
+	Vector3f normalDirection = (localHitPoint - (p + w));
+	raycastInfo.worldNormal = normalDirection;
+	return true;
+}
+
+float CylinderShape::getRadius()  {
+	return this.radius;
+}
+
+float CylinderShape::getHeight()  {
+	return this.halfHeight + this.halfHeight;
+}
+
+void CylinderShape::setLocalScaling( Vector3f scaling) {
+	this.halfHeight = (this.halfHeight / this.scaling.y()) * scaling.y();
+	this.radius = (this.radius / this.scaling.x()) * scaling.x();
+	CollisionShape::setLocalScaling(scaling);
+}
+
+long CylinderShape::getSizeInBytes()  {
+	return sizeof(CylinderShape);
+}
+
+void CylinderShape::getLocalBounds(Vector3f min, Vector3f max)  {
+	// Maximum bounds
+	max.setX(this.radius + this.margin);
+	max.setY(this.halfHeight + this.margin);
+	max.setZ(max.x());
+	// Minimum bounds
+	min.setX(-max.x());
+	min.setY(-max.y());
+	min.setZ(min.x());
+}
+
+void CylinderShape::computeLocalInertiaTensor(Matrix3f tensor, float mass)  {
+	float height = float(2.0) * this.halfHeight;
+	float diag = (1.0f / float(12.0)) * mass * (3 * this.radius * this.radius + height * height);
+	tensor.setValue(diag, 0.0, 0.0, 0.0,
+	                 0.5f * mass * this.radius * this.radius, 0.0,
+	                 0.0, 0.0, diag);
+}
+
+boolean CylinderShape::testPointInside( Vector3f localPoint, ProxyShape* proxyShape) {
+	return (    (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z()) < this.radius * this.radius
+	         && localPoint.y() < this.halfHeight
+	         && localPoint.y() > -this.halfHeight);
+}

src/org/atriaSoft/ephysics/collision/shapes/CylinderShape.java

@@ -0,0 +1,49 @@
+package org.atriaSoft.ephysics.collision.shapes;
+
+	/**
+	 * @brief It 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
+	 * ructor of the cylinder shape. Otherwise, it is recommended to use the
+	 * default margin distance by not using the "margin" parameter in the ructor.
+	 */
+	class CylinderShape: public ConvexShape {
+		protected:
+			float radius; //!< Radius of the base
+			float halfHeight; //!< Half height of the cylinder
+			Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData)  ;
+			boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape)  ;
+			boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape)  ;
+			long getSizeInBytes()  ;
+		public:
+			/**
+			 * @brief Contructor
+			 * @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(float radius, float height, float margin = OBJECTMARGIN);
+			/**
+			 * @breif Get the Shape radius
+			 * @return Radius of the cylinder (in meters)
+			 */
+			float getRadius() ;
+			/**
+			 * @breif Get the Shape height
+			 * @return Height of the cylinder (in meters)
+			 */
+			float getHeight() ;
+			void setLocalScaling( Vector3f scaling) ;
+			void getLocalBounds(Vector3f min, Vector3f max)  ;
+			void computeLocalInertiaTensor(Matrix3f tensor, float mass)  ;
+	};
+
+}
+
+

src/org/atriaSoft/ephysics/collision/shapes/HeightFieldShape.cpp

@@ -0,0 +1,246 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/shapes/HeightFieldShape.hpp>
+
+// TODO: REMOVE this...
+using namespace ephysics;
+
+HeightFieldShape::HeightFieldShape(int nbGridColumns,
+                                   int nbGridRows,
+                                   float minHeight,
+                                   float maxHeight,
+                                    void* heightFieldData,
+                                   HeightDataType dataType,
+                                   int upAxis,
+                                   float integerHeightScale):
+  ConcaveShape(HEIGHTFIELD),
+  this.numberColumns(nbGridColumns),
+  this.numberRows(nbGridRows),
+  this.width(nbGridColumns - 1),
+  this.length(nbGridRows - 1),
+  this.minHeight(minHeight),
+  this.maxHeight(maxHeight),
+  this.upAxis(upAxis),
+  this.integerHeightScale(integerHeightScale),
+  this.heightDataType(dataType) {
+	assert(nbGridColumns >= 2);
+	assert(nbGridRows >= 2);
+	assert(this.width >= 1);
+	assert(this.length >= 1);
+	assert(minHeight <= maxHeight);
+	assert(upAxis == 0 || upAxis == 1 || upAxis == 2);
+	this.heightFieldData = heightFieldData;
+	float halfHeight = (this.maxHeight - this.minHeight) * 0.5f;
+	assert(halfHeight >= 0);
+	// Compute the local AABB of the height field
+	if (this.upAxis == 0) {
+		this.AABB.setMin(Vector3f(-halfHeight, -this.width * 0.5f, -this.length * float(0.5)));
+		this.AABB.setMax(Vector3f(halfHeight, this.width * 0.5f, this.length* float(0.5)));
+	} else if (this.upAxis == 1) {
+		this.AABB.setMin(Vector3f(-this.width * 0.5f, -halfHeight, -this.length * float(0.5)));
+		this.AABB.setMax(Vector3f(this.width * 0.5f, halfHeight, this.length * float(0.5)));
+	} else if (this.upAxis == 2) {
+		this.AABB.setMin(Vector3f(-this.width * 0.5f, -this.length * float(0.5), -halfHeight));
+		this.AABB.setMax(Vector3f(this.width * 0.5f, this.length * float(0.5), halfHeight));
+	}
+}
+
+void HeightFieldShape::getLocalBounds(Vector3f min, Vector3f max)  {
+	min = this.AABB.getMin() * this.scaling;
+	max = this.AABB.getMax() * this.scaling;
+}
+
+void HeightFieldShape::testAllTriangles(TriangleCallback callback,  AABB localAABB)  {
+	// Compute the non-scaled AABB
+	Vector3f inverseScaling(1.0f / this.scaling.x(), 1.0f / this.scaling.y(), float(1.0) / this.scaling.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(this.upAxis) {
+		case 0 :
+			iMin = clamp(minGridCoords[1], 0, this.numberColumns - 1);
+			iMax = clamp(maxGridCoords[1], 0, this.numberColumns - 1);
+			jMin = clamp(minGridCoords[2], 0, this.numberRows - 1);
+			jMax = clamp(maxGridCoords[2], 0, this.numberRows - 1);
+			break;
+		case 1 :
+			iMin = clamp(minGridCoords[0], 0, this.numberColumns - 1);
+			iMax = clamp(maxGridCoords[0], 0, this.numberColumns - 1);
+			jMin = clamp(minGridCoords[2], 0, this.numberRows - 1);
+			jMax = clamp(maxGridCoords[2], 0, this.numberRows - 1);
+			break;
+		case 2 :
+			iMin = clamp(minGridCoords[0], 0, this.numberColumns - 1);
+			iMax = clamp(maxGridCoords[0], 0, this.numberColumns - 1);
+			jMin = clamp(minGridCoords[1], 0, this.numberRows - 1);
+			jMax = clamp(maxGridCoords[1], 0, this.numberRows - 1);
+			break;
+	}
+	assert(iMin >= 0 && iMin < this.numberColumns);
+	assert(iMax >= 0 && iMax < this.numberColumns);
+	assert(jMin >= 0 && jMin < this.numberRows);
+	assert(jMax >= 0 && jMax < this.numberRows);
+	// 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
+			Vector3f p1 = getVertexAt(i, j);
+			Vector3f p2 = getVertexAt(i, j + 1);
+			Vector3f p3 = getVertexAt(i + 1, j);
+			Vector3f p4 = getVertexAt(i + 1, j + 1);
+			// Generate the first triangle for the current grid rectangle
+			Vector3f 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);
+		}
+	}
+}
+
+void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoords,  AABB aabbToCollide)  {
+	// Clamp the min/max coords of the AABB to collide inside the height field AABB
+	Vector3f minPoint = max(aabbToCollide.getMin(), this.AABB.getMin());
+	minPoint = min(minPoint, this.AABB.getMax());
+	Vector3f maxPoint = min(aabbToCollide.getMax(), this.AABB.getMax());
+	maxPoint = max(maxPoint, this.AABB.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 ... this.width/2]
+	// and [-this.length/2 ... this.length/2]
+	 Vector3f translateVec = this.AABB.getExtent() * 0.5f;
+	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;
+}
+
+boolean HeightFieldShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape)  {
+	// 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
+	 Vector3f rayEnd = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
+	 AABB rayAABB(min(ray.point1, rayEnd), max(ray.point1, rayEnd));
+	testAllTriangles(triangleCallback, rayAABB);
+	return triangleCallback.getIsHit();
+}
+
+Vector3f HeightFieldShape::getVertexAt(int xxx, int yyy)  {
+	// Get the height value
+	 float height = getHeightAt(xxx, yyy);
+	// Height values origin
+	 float heightOrigin = -(this.maxHeight - this.minHeight) * 0.5f - this.minHeight;
+	Vector3f vertex;
+	switch (this.upAxis) {
+		case 0:
+			vertex = Vector3f(heightOrigin + height, -this.width * 0.5f + xxx, -this.length * float(0.5) + yyy);
+			break;
+		case 1:
+			vertex = Vector3f(-this.width * 0.5f + xxx, heightOrigin + height, -this.length * float(0.5) + yyy);
+			break;
+		case 2:
+			vertex = Vector3f(-this.width * 0.5f + xxx, -this.length * float(0.5) + yyy, heightOrigin + height);
+			break;
+		default:
+			assert(false);
+	}
+	assert(this.AABB.contains(vertex));
+	return vertex * this.scaling;
+}
+
+void TriangleOverlapCallback::testTriangle( Vector3f* trianglePoints) {
+	// Create a triangle collision shape
+	float margin = this.heightFieldShape.getTriangleMargin();
+	TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
+	triangleShape.setRaycastTestType(this.heightFieldShape.getRaycastTestType());
+	// Ray casting test against the collision shape
+	RaycastInfo raycastInfo;
+	boolean isTriangleHit = triangleShape.raycast(this.ray, raycastInfo, this.proxyShape);
+	// If the ray hit the collision shape
+	if (    isTriangleHit
+	     && raycastInfo.hitFraction <= this.smallestHitFraction) {
+		assert(raycastInfo.hitFraction >= 0.0f);
+		this.raycastInfo.body = raycastInfo.body;
+		this.raycastInfo.proxyShape = raycastInfo.proxyShape;
+		this.raycastInfo.hitFraction = raycastInfo.hitFraction;
+		this.raycastInfo.worldPoint = raycastInfo.worldPoint;
+		this.raycastInfo.worldNormal = raycastInfo.worldNormal;
+		this.raycastInfo.meshSubpart = -1;
+		this.raycastInfo.triangleIndex = -1;
+		this.smallestHitFraction = raycastInfo.hitFraction;
+		this.isHit = true;
+	}
+}
+
+int HeightFieldShape::getNbRows()  {
+	return this.numberRows;
+}
+
+int HeightFieldShape::getNbColumns()  {
+	return this.numberColumns;
+}
+
+HeightFieldShape::HeightDataType HeightFieldShape::getHeightDataType()  {
+	return this.heightDataType;
+}
+
+long HeightFieldShape::getSizeInBytes()  {
+	return sizeof(HeightFieldShape);
+}
+
+void HeightFieldShape::setLocalScaling( Vector3f scaling) {
+	CollisionShape::setLocalScaling(scaling);
+}
+
+float HeightFieldShape::getHeightAt(int xxx, int yyy)  {
+	switch(this.heightDataType) {
+		case HEIGHTFLOATTYPE:
+			return ((float*)this.heightFieldData)[yyy * this.numberColumns + xxx];
+		case HEIGHTDOUBLETYPE:
+			return ((double*)this.heightFieldData)[yyy * this.numberColumns + xxx];
+		case HEIGHTINTTYPE:
+			return ((int*)this.heightFieldData)[yyy * this.numberColumns + xxx] * this.integerHeightScale;
+		default:
+			assert(false);
+			return 0;
+	}
+}
+
+int HeightFieldShape::computeIntegerGridValue(float value)  {
+	return (value < 0.0f) ? value - 0.5f : value + 0.5f;
+}
+
+void HeightFieldShape::computeLocalInertiaTensor(Matrix3f tensor, float mass)  {
+	// 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.setValue(mass, 0, 0,
+	                 0, mass, 0,
+	                 0, 0, mass);
+}

src/org/atriaSoft/ephysics/collision/shapes/HeightFieldShape.java

@@ -0,0 +1,118 @@
+package org.atriaSoft.ephysics.collision.shapes;
+
+	class HeightFieldShape;
+	/**
+	 * @brief This class is used for testing AABB and triangle overlap for raycasting
+	 */
+	class TriangleOverlapCallback extends  TriangleCallback {
+		protected:
+			Ray ray;
+			ProxyShape* proxyShape;
+			RaycastInfo raycastInfo;
+			boolean isHit;
+			float smallestHitFraction;
+			 HeightFieldShape heightFieldShape;
+		public:
+			TriangleOverlapCallback( Ray ray,
+			                        ProxyShape* proxyShape,
+			                        RaycastInfo raycastInfo,
+			                         HeightFieldShape heightFieldShape):
+			  this.ray(ray),
+			  this.proxyShape(proxyShape),
+			  this.raycastInfo(raycastInfo),
+			  this.heightFieldShape(heightFieldShape) {
+				this.isHit = false;
+				this.smallestHitFraction = this.ray.maxFraction;
+			}
+			boolean getIsHit()  {
+				return this.isHit;
+			}
+			/// Raycast test between a ray and a triangle of the heightfield
+			 void testTriangle( Vector3f* trianglePoints);
+	};
+
+
+	/**
+	 * @brief 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 extends  ConcaveShape {
+		public:
+			/**
+			 * @brief Data type for the height data of the height field
+			 */
+			enum HeightDataType {
+				HEIGHTFLOATTYPE,
+				HEIGHTDOUBLETYPE,
+				HEIGHTINTTYPE
+			};
+		protected:
+			int numberColumns; //!< Number of columns in the grid of the height field
+			int numberRows; //!< Number of rows in the grid of the height field
+			float width; //!< Height field width
+			float length; //!< Height field length
+			float minHeight; //!< Minimum height of the height field
+			float maxHeight; //!< Maximum height of the height field
+			int upAxis; //!< Up axis direction (0 => x, 1 => y, 2 => z)
+			float integerHeightScale; //!< Height values scale for height field with integer height values
+			HeightDataType heightDataType; //!< Data type of the height values
+			void*	heightFieldData; //!< Array of data with all the height values of the height field
+			AABB AABB; //!< Local AABB of the height field (without scaling)
+			boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape)  ;
+			long getSizeInBytes()  ;
+			/// 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(int subPart,
+			                                         int triangleIndex,
+			                                         Vector3f* outTriangleVertices) ;
+			/// Return the vertex (local-coordinates) of the height field at a given (x,y) position
+			Vector3f getVertexAt(int x, int y) ;
+			/// Return the height of a given (x,y) point in the height field
+			float getHeightAt(int x, int y) ;
+			/// Return the closest inside integer grid value of a given floating grid value
+			int computeIntegerGridValue(float value) ;
+			/// 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,  AABB aabbToCollide) ;
+		public:
+			/**
+			 * @brief Contructor
+			 * @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(int nbGridColumns,
+			                 int nbGridRows,
+			                 float minHeight,
+			                 float maxHeight,
+			                  void* heightFieldData,
+			                 HeightDataType dataType,
+			                 int upAxis = 1, float integerHeightScale = 1.0f);
+			/// Return the number of rows in the height field
+			int getNbRows() ;
+			/// Return the number of columns in the height field
+			int getNbColumns() ;
+			/// Return the type of height value in the height field
+			HeightDataType getHeightDataType() ;
+			void getLocalBounds(Vector3f min, Vector3f max)  ;
+			void setLocalScaling( Vector3f scaling) ;
+			void computeLocalInertiaTensor(Matrix3f tensor, float mass)  ;
+			void testAllTriangles(TriangleCallback callback,  AABB localAABB)  ;
+			friend class ConvexTriangleAABBOverlapCallback;
+			friend class ConcaveMeshRaycastCallback;
+	};
+
+}
+

src/org/atriaSoft/ephysics/collision/shapes/SphereShape.cpp

@@ -0,0 +1,89 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/shapes/SphereShape.hpp>
+#include <ephysics/collision/ProxyShape.hpp>
+#include <ephysics/configuration.hpp>
+
+// TODO: REMOVE this ...
+using namespace ephysics;
+
+SphereShape::SphereShape(float radius):
+  ConvexShape(SPHERE, radius) {
+	assert(radius > 0.0f);
+}
+
+void SphereShape::setLocalScaling( Vector3f scaling) {
+	this.margin = (this.margin / this.scaling.x()) * scaling.x();
+	CollisionShape::setLocalScaling(scaling);
+}
+
+void SphereShape::computeLocalInertiaTensor(Matrix3f tensor, float mass)  {
+	float diag = 0.4f * mass * this.margin * this.margin;
+	tensor.setValue(diag, 0.0f,  0.0f,
+	                 0.0f,  diag, 0.0f,
+	                 0.0f,  0.0f,  diag);
+}
+
+void SphereShape::computeAABB(AABB aabb,  Transform3D transform)  {
+	// Get the local extents in x,y and z direction
+	Vector3f extents(this.margin, this.margin, this.margin);
+	// Update the AABB with the new minimum and maximum coordinates
+	aabb.setMin(transform.getPosition() - extents);
+	aabb.setMax(transform.getPosition() + extents);
+}
+
+void SphereShape::getLocalBounds(Vector3f min, Vector3f max)  {
+	// Maximum bounds
+	max.setX(this.margin);
+	max.setY(this.margin);
+	max.setZ(this.margin);
+	// Minimum bounds
+	min.setX(-this.margin);
+	min.setY(min.x());
+	min.setZ(min.x());
+}
+
+boolean SphereShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape)  {
+	 Vector3f m = ray.point1;
+	float c = m.dot(m) - this.margin * this.margin;
+	// If the origin of the ray is inside the sphere, we return no intersection
+	if (c < 0.0f) {
+		return false;
+	}
+	 Vector3f rayDirection = ray.point2 - ray.point1;
+	float 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 > 0.0f) {
+		return false;
+	}
+	float raySquareLength = rayDirection.length2();
+	// Compute the discriminant of the quadratic equation
+	float discriminant = b * b - raySquareLength * c;
+	// If the discriminant is negative or the ray length is very small, there is no intersection
+	if (    discriminant < 0.0f
+	     || raySquareLength < FLTEPSILON) {
+		return false;
+	}
+	// Compute the solution "t" closest to the origin
+	float t = -b - sqrt(discriminant);
+	assert(t >= 0.0f);
+	// 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/org/atriaSoft/ephysics/collision/shapes/SphereShape.java

@@ -0,0 +1,42 @@
+package org.atriaSoft.ephysics.collision.shapes;
+
+	/**
+	 * @brief 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 extends  ConvexShape {
+		protected :
+			SphereShape( SphereShape shape);
+			Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData)   {
+				return Vector3f(0.0, 0.0, 0.0);
+			}
+			boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape)   {
+				return (localPoint.length2() < this.margin * this.margin);
+			}
+			boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape)  ;
+			long getSizeInBytes()   {
+				return sizeof(SphereShape);
+			}
+		public :
+			/**
+			 * @brief Constructor
+			 * @param[in] radius Radius of the sphere (in meters)
+			 */
+			SphereShape(float radius);
+			/**
+			 * @brief Get the radius of the sphere
+			 * @return Radius of the sphere (in meters)
+			 */
+			float getRadius()  {
+				return this.margin;
+			}
+			void setLocalScaling( Vector3f scaling) ;
+			void getLocalBounds(Vector3f min, Vector3f max)  ;
+			void computeLocalInertiaTensor(Matrix3f tensor, float mass)  ;
+			void computeAABB(AABB aabb,  Transform3D transform)  ;
+	};
+
+}

src/org/atriaSoft/ephysics/collision/shapes/TriangleShape.cpp

@@ -0,0 +1,163 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/collision/shapes/TriangleShape.hpp>
+#include <ephysics/collision/ProxyShape.hpp>
+#include <ephysics/engine/Profiler.hpp>
+#include <ephysics/configuration.hpp>
+
+// TODO: REMOVE this...
+using namespace ephysics;
+
+TriangleShape::TriangleShape( Vector3f point1,  Vector3f point2,  Vector3f point3, float margin)
+			  : ConvexShape(TRIANGLE, margin) {
+	this.points[0] = point1;
+	this.points[1] = point2;
+	this.points[2] = point3;
+	this.raycastTestType = FRONT;
+}
+
+boolean TriangleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape)  {
+	PROFILE("TriangleShape::raycast()");
+	 Vector3f pq = ray.point2 - ray.point1;
+	 Vector3f pa = this.points[0] - ray.point1;
+	 Vector3f pb = this.points[1] - ray.point1;
+	 Vector3f pc = this.points[2] - ray.point1;
+	// Test if the line PQ is inside the eges BC, CA and AB. We use the triple
+	// product for this test.
+	 Vector3f m = pq.cross(pc);
+	float u = pb.dot(m);
+	if (this.raycastTestType == FRONT) {
+		if (u < 0.0f) {
+			return false;
+		}
+	} else if (this.raycastTestType == BACK) {
+		if (u > 0.0f) {
+			return false;
+		}
+	}
+	float v = -pa.dot(m);
+	if (this.raycastTestType == FRONT) {
+		if (v < 0.0f) {
+			return false;
+		}
+	} else if (this.raycastTestType == BACK) {
+		if (v > 0.0f) {
+			return false;
+		}
+	} else if (this.raycastTestType == FRONTANDBACK) {
+		if (!sameSign(u, v)) {
+			return false;
+		}
+	}
+	float w = pa.dot(pq.cross(pb));
+	if (this.raycastTestType == FRONT) {
+		if (w < 0.0f) {
+			return false;
+		}
+	} else if (this.raycastTestType == BACK) {
+		if (w > 0.0f) {
+			return false;
+		}
+	} else if (this.raycastTestType == FRONTANDBACK) {
+		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
+	float denom = 1.0f / (u + v + w);
+	u *= denom;
+	v *= denom;
+	w *= denom;
+	// Compute the local hit point using the barycentric coordinates
+	 Vector3f localHitPoint = u * this.points[0] + v * this.points[1] + w * this.points[2];
+	 float hitFraction = (localHitPoint - ray.point1).length() / pq.length();
+	if (    hitFraction < 0.0f
+	     || hitFraction > ray.maxFraction) {
+		return false;
+	}
+	Vector3f localHitNormal = (this.points[1] - this.points[0]).cross(this.points[2] - this.points[0]);
+	if (localHitNormal.dot(pq) > 0.0f) {
+		localHitNormal = -localHitNormal;
+	}
+	raycastInfo.body = proxyShape.getBody();
+	raycastInfo.proxyShape = proxyShape;
+	raycastInfo.worldPoint = localHitPoint;
+	raycastInfo.hitFraction = hitFraction;
+	raycastInfo.worldNormal = localHitNormal;
+	return true;
+}
+
+long TriangleShape::getSizeInBytes()  {
+	return sizeof(TriangleShape);
+}
+
+Vector3f TriangleShape::getLocalSupportPointWithoutMargin( Vector3f direction,
+                                                      void** cachedCollisionData)  {
+	Vector3f dotProducts(direction.dot(this.points[0]), direction.dot(this.points[1]), direction.dot(this.points[2]));
+	return this.points[dotProducts.getMaxAxis()];
+}
+
+void TriangleShape::getLocalBounds(Vector3f min, Vector3f max)  {
+	 Vector3f xAxis(this.points[0].x(), this.points[1].x(), this.points[2].x());
+	 Vector3f yAxis(this.points[0].y(), this.points[1].y(), this.points[2].y());
+	 Vector3f zAxis(this.points[0].z(), this.points[1].z(), this.points[2].z());
+	min.setValue(xAxis.getMin(), yAxis.getMin(), zAxis.getMin());
+	max.setValue(xAxis.getMax(), yAxis.getMax(), zAxis.getMax());
+	min -= Vector3f(this.margin, this.margin, this.margin);
+	max += Vector3f(this.margin, this.margin, this.margin);
+}
+
+void TriangleShape::setLocalScaling( Vector3f scaling) {
+	this.points[0] = (this.points[0] / this.scaling) * scaling;
+	this.points[1] = (this.points[1] / this.scaling) * scaling;
+	this.points[2] = (this.points[2] / this.scaling) * scaling;
+	CollisionShape::setLocalScaling(scaling);
+}
+
+void TriangleShape::computeLocalInertiaTensor(Matrix3f tensor, float mass)  {
+	tensor.setZero();
+}
+
+void TriangleShape::computeAABB(AABB aabb,  Transform3D transform)  {
+	 Vector3f worldPoint1 = transform * this.points[0];
+	 Vector3f worldPoint2 = transform * this.points[1];
+	 Vector3f worldPoint3 = transform * this.points[2];
+	 Vector3f xAxis(worldPoint1.x(), worldPoint2.x(), worldPoint3.x());
+	 Vector3f yAxis(worldPoint1.y(), worldPoint2.y(), worldPoint3.y());
+	 Vector3f zAxis(worldPoint1.z(), worldPoint2.z(), worldPoint3.z());
+	aabb.setMin(Vector3f(xAxis.getMin(), yAxis.getMin(), zAxis.getMin()));
+	aabb.setMax(Vector3f(xAxis.getMax(), yAxis.getMax(), zAxis.getMax()));
+}
+
+boolean TriangleShape::testPointInside( Vector3f localPoint, ProxyShape* proxyShape)  {
+	return false;
+}
+
+TriangleRaycastSide TriangleShape::getRaycastTestType()  {
+	return this.raycastTestType;
+}
+
+
+void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) {
+	this.raycastTestType = testType;
+}
+
+Vector3f TriangleShape::getVertex(int index)  {
+	assert(    index >= 0
+	        && index < 3);
+	return this.points[index];
+}
+

src/org/atriaSoft/ephysics/collision/shapes/TriangleShape.java

@@ -0,0 +1,60 @@
+package org.atriaSoft.ephysics.collision.shapes;
+
+	/**
+	 * @brief Raycast test side for the triangle
+	 */
+	enum TriangleRaycastSide {
+		FRONT, //!< Raycast against front triangle
+		BACK, //!< Raycast against back triangle
+		FRONTANDBACK //!< Raycast against front and back triangle
+	};
+	
+	/**
+	 * This class represents a triangle collision shape that is centered
+	 * at the origin and defined three points.
+	 */
+	class TriangleShape: public ConvexShape {
+		protected:
+			Vector3f this.points[3]; //!< Three points of the triangle
+			TriangleRaycastSide raycastTestType; //!< Raycast test type for the triangle (front, back, front-back)
+			/// Private copy-ructor
+			TriangleShape( TriangleShape shape);
+			/// Private assignment operator
+			TriangleShape operator=( TriangleShape shape);
+			Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData)  ;
+			boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape)  ;
+			boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape)  ;
+			long getSizeInBytes()  ;
+		public:
+			/**
+			 * @brief 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( Vector3f point1,
+			               Vector3f point2,
+			               Vector3f point3,
+			              float margin = OBJECTMARGIN);
+			void getLocalBounds(Vector3f min, Vector3f max)  ;
+			void setLocalScaling( Vector3f scaling) ;
+			void computeLocalInertiaTensor(Matrix3f tensor, float mass)  ;
+			void computeAABB(AABB aabb,  Transform3D transform)  ;
+			/// Return the raycast test type (front, back, front-back)
+			TriangleRaycastSide getRaycastTestType() ;
+			/**
+			 * @brief Set the raycast test type (front, back, front-back)
+			 * @param[in] testType Raycast test type for the triangle (front, back, front-back)
+			 */
+			void setRaycastTestType(TriangleRaycastSide testType);
+			/**
+			 * @brief Return the coordinates of a given vertex of the triangle
+			 * @param[in] index Index (0 to 2) of a vertex of the triangle
+			 */
+			Vector3f getVertex(int index) ;
+			friend class ConcaveMeshRaycastCallback;
+			friend class TriangleOverlapCallback;
+	};
+}
+

src/org/atriaSoft/ephysics/constraint/BallAndSocketJoint.cpp

@@ -0,0 +1,212 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+// Libraries
+#include <ephysics/raint/BallAndSocketJoint.hpp>
+#include <ephysics/engine/ConstraintSolver.hpp>
+
+using namespace ephysics;
+
+// Static variables definition
+ float BallAndSocketJoint::BETA = float(0.2);
+
+// Constructor
+BallAndSocketJoint::BallAndSocketJoint( BallAndSocketJointInfo jointInfo)
+				   : Joint(jointInfo), this.impulse(Vector3f(0, 0, 0)) {
+
+	// Compute the local-space anchor point for each body
+	this.localAnchorPointBody1 = this.body1.getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
+	this.localAnchorPointBody2 = this.body2.getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
+}
+
+// Initialize before solving the raint
+void BallAndSocketJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
+
+	// Initialize the bodies index in the velocity array
+	this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1).second;
+	this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2).second;
+
+	// Get the bodies center of mass and orientations
+	 Vector3f x1 = this.body1.this.centerOfMassWorld;
+	 Vector3f x2 = this.body2.this.centerOfMassWorld;
+	 Quaternion orientationBody1 = this.body1.getTransform().getOrientation();
+	 Quaternion orientationBody2 = this.body2.getTransform().getOrientation();
+
+	// Get the inertia tensor of bodies
+	this.i1 = this.body1.getInertiaTensorInverseWorld();
+	this.i2 = this.body2.getInertiaTensorInverseWorld();
+
+	// Compute the vector from body center to the anchor point in world-space
+	this.r1World = orientationBody1 * this.localAnchorPointBody1;
+	this.r2World = orientationBody2 * this.localAnchorPointBody2;
+
+	// Compute the corresponding skew-symmetric matrices
+	Matrix3f skewSymmetricMatrixU1= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r1World);
+	Matrix3f skewSymmetricMatrixU2= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r2World);
+
+	// Compute the matrix K=JM^-1J^t (3x3 matrix)
+	float inverseMassBodies = this.body1.this.massInverse + this.body2.this.massInverse;
+	Matrix3f massMatrix = Matrix3f(inverseMassBodies, 0, 0,
+									0, inverseMassBodies, 0,
+									0, 0, inverseMassBodies) +
+						   skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose() +
+						   skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose();
+
+	// Compute the inverse mass matrix K^-1
+	this.inverseMassMatrix.setZero();
+	if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
+		this.inverseMassMatrix = massMatrix.getInverse();
+	}
+
+	// Compute the bias "b" of the raint
+	this.biasVector.setZero();
+	if (this.positionCorrectionTechnique == BAUMGARTEJOINTS) {
+		float biasFactor = (BETA / raintSolverData.timeStep);
+		this.biasVector = biasFactor * (x2 + this.r2World - x1 - this.r1World);
+	}
+
+	// If warm-starting is not enabled
+	if (!raintSolverData.isWarmStartingActive) {
+
+		// Reset the accumulated impulse
+		this.impulse.setZero();
+	}
+}
+
+// Warm start the raint (apply the previous impulse at the beginning of the step)
+void BallAndSocketJoint::warmstart( ConstraintSolverData raintSolverData) {
+
+	// Get the velocities
+	Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+	Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+	Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+	Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+
+	// Compute the impulse P=J^T * lambda for the body 1
+	 Vector3f linearImpulseBody1 = -this.impulse;
+	 Vector3f angularImpulseBody1 = this.impulse.cross(this.r1World);
+
+	// Apply the impulse to the body 1
+	v1 += this.body1.this.massInverse * linearImpulseBody1;
+	w1 += this.i1 * angularImpulseBody1;
+
+	// Compute the impulse P=J^T * lambda for the body 2
+	 Vector3f angularImpulseBody2 = -this.impulse.cross(this.r2World);
+
+	// Apply the impulse to the body to the body 2
+	v2 += this.body2.this.massInverse * this.impulse;
+	w2 += this.i2 * angularImpulseBody2;
+}
+
+// Solve the velocity raint
+void BallAndSocketJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) {
+
+	// Get the velocities
+	Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+	Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+	Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+	Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+
+	// Compute J*v
+	 Vector3f Jv = v2 + w2.cross(this.r2World) - v1 - w1.cross(this.r1World);
+
+	// Compute the Lagrange multiplier lambda
+	 Vector3f deltaLambda = this.inverseMassMatrix * (-Jv - this.biasVector);
+	this.impulse += deltaLambda;
+
+	// Compute the impulse P=J^T * lambda for the body 1
+	 Vector3f linearImpulseBody1 = -deltaLambda;
+	 Vector3f angularImpulseBody1 = deltaLambda.cross(this.r1World);
+
+	// Apply the impulse to the body 1
+	v1 += this.body1.this.massInverse * linearImpulseBody1;
+	w1 += this.i1 * angularImpulseBody1;
+
+	// Compute the impulse P=J^T * lambda for the body 2
+	 Vector3f angularImpulseBody2 = -deltaLambda.cross(this.r2World);
+
+	// Apply the impulse to the body 2
+	v2 += this.body2.this.massInverse * deltaLambda;
+	w2 += this.i2 * angularImpulseBody2;
+}
+
+// Solve the position raint (for position error correction)
+void BallAndSocketJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) {
+
+	// If the error position correction technique is not the non-linear-gauss-seidel, we do
+	// do not execute this method
+	if (this.positionCorrectionTechnique != NONLINEARGAUSSSEIDEL) return;
+
+	// Get the bodies center of mass and orientations
+	Vector3f x1 = raintSolverData.positions[this.indexBody1];
+	Vector3f x2 = raintSolverData.positions[this.indexBody2];
+	Quaternion q1 = raintSolverData.orientations[this.indexBody1];
+	Quaternion q2 = raintSolverData.orientations[this.indexBody2];
+
+	// Get the inverse mass and inverse inertia tensors of the bodies
+	float inverseMassBody1 = this.body1.this.massInverse;
+	float inverseMassBody2 = this.body2.this.massInverse;
+
+	// Recompute the inverse inertia tensors
+	this.i1 = this.body1.getInertiaTensorInverseWorld();
+	this.i2 = this.body2.getInertiaTensorInverseWorld();
+
+	// Compute the vector from body center to the anchor point in world-space
+	this.r1World = q1 * this.localAnchorPointBody1;
+	this.r2World = q2 * this.localAnchorPointBody2;
+
+	// Compute the corresponding skew-symmetric matrices
+	Matrix3f skewSymmetricMatrixU1= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r1World);
+	Matrix3f skewSymmetricMatrixU2= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r2World);
+
+	// Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation raints
+	float inverseMassBodies = inverseMassBody1 + inverseMassBody2;
+	Matrix3f massMatrix = Matrix3f(inverseMassBodies, 0, 0,
+									0, inverseMassBodies, 0,
+									0, 0, inverseMassBodies) +
+						   skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose() +
+						   skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose();
+	this.inverseMassMatrix.setZero();
+	if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
+		this.inverseMassMatrix = massMatrix.getInverse();
+	}
+
+	// Compute the raint error (value of the C(x) function)
+	 Vector3f raintError = (x2 + this.r2World - x1 - this.r1World);
+
+	// 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.
+	 Vector3f lambda = this.inverseMassMatrix * (-raintError);
+
+	// Compute the impulse of body 1
+	 Vector3f linearImpulseBody1 = -lambda;
+	 Vector3f angularImpulseBody1 = lambda.cross(this.r1World);
+
+	// Compute the pseudo velocity of body 1
+	 Vector3f v1 = inverseMassBody1 * linearImpulseBody1;
+	 Vector3f w1 = this.i1 * angularImpulseBody1;
+
+	// Update the body center of mass and orientation of body 1
+	x1 += v1;
+	q1 += Quaternion(0, w1) * q1 * 0.5f;
+	q1.normalize();
+
+	// Compute the impulse of body 2
+	 Vector3f angularImpulseBody2 = -lambda.cross(this.r2World);
+
+	// Compute the pseudo velocity of body 2
+	 Vector3f v2 = inverseMassBody2 * lambda;
+	 Vector3f w2 = this.i2 * angularImpulseBody2;
+
+	// Update the body position/orientation of body 2
+	x2 += v2;
+	q2 += Quaternion(0, w2) * q2 * 0.5f;
+	q2.normalize();
+}
+

src/org/atriaSoft/ephysics/constraint/BallAndSocketJoint.java

@@ -0,0 +1,52 @@
+package org.atriaSoft.ephysics.constraint;
+
+	/**
+	 * @brief It 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 extends  JointInfo {
+		public :
+			Vector3f anchorPointWorldSpace; //!< Anchor point (in world-space coordinates)
+			/**
+			 * @brief 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,
+			                        Vector3f initAnchorPointWorldSpace):
+			  JointInfo(rigidBody1, rigidBody2, BALLSOCKETJOINT),
+			  this.anchorPointWorldSpace(initAnchorPointWorldSpace) {
+				
+			}
+	};
+	/**
+	 * @brief 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 extends  Joint {
+		private:
+			static float BETA; //!< Beta value for the bias factor of position correction
+			Vector3f localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1)
+			Vector3f localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2)
+			Vector3f r1World; //!< Vector from center of body 2 to anchor point in world-space
+			Vector3f r2World; //!< Vector from center of body 2 to anchor point in world-space
+			Matrix3f i1; //!< Inertia tensor of body 1 (in world-space coordinates)
+			Matrix3f i2; //!< Inertia tensor of body 2 (in world-space coordinates)
+			Vector3f biasVector; //!< Bias vector for the raint
+			Matrix3f inverseMassMatrix; //!< Inverse mass matrix K=JM^-1J^-t of the raint
+			Vector3f impulse; //!< Accumulated impulse
+			long getSizeInBytes() {
+				return sizeof(BallAndSocketJoint);
+			}
+			void initBeforeSolve( ConstraintSolverData raintSolverData) ;
+			void warmstart( ConstraintSolverData raintSolverData) ;
+			void solveVelocityConstraint( ConstraintSolverData raintSolverData) ;
+			void solvePositionConstraint( ConstraintSolverData raintSolverData) ;
+		public:
+			/// Constructor
+			BallAndSocketJoint( BallAndSocketJointInfo jointInfo);
+	};
+}

src/org/atriaSoft/ephysics/constraint/ContactPoint.cpp

@@ -0,0 +1,171 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/raint/ContactPoint.hpp>
+#include <ephysics/collision/ProxyShape.hpp>
+
+using namespace ephysics;
+using namespace std;
+
+// Constructor
+ContactPoint::ContactPoint( ContactPointInfo contactInfo):
+  this.body1(contactInfo.shape1.getBody()),
+  this.body2(contactInfo.shape2.getBody()),
+  this.normal(contactInfo.normal),
+  this.penetrationDepth(contactInfo.penetrationDepth),
+  this.localPointOnBody1(contactInfo.localPoint1),
+  this.localPointOnBody2(contactInfo.localPoint2),
+  this.worldPointOnBody1(contactInfo.shape1.getBody().getTransform() *
+                      contactInfo.shape1.getLocalToBodyTransform() *
+                      contactInfo.localPoint1),
+  this.worldPointOnBody2(contactInfo.shape2.getBody().getTransform() *
+                      contactInfo.shape2.getLocalToBodyTransform() *
+                      contactInfo.localPoint2),
+  this.isRestingContact(false) {
+
+	this.frictionVectors[0] = Vector3f(0, 0, 0);
+	this.frictionVectors[1] = Vector3f(0, 0, 0);
+
+	assert(this.penetrationDepth >= 0.0);
+
+}
+
+// Destructor
+ContactPoint::~ContactPoint() {
+
+}
+
+// Return the reference to the body 1
+CollisionBody* ContactPoint::getBody1()  {
+	return this.body1;
+}
+
+// Return the reference to the body 2
+CollisionBody* ContactPoint::getBody2()  {
+	return this.body2;
+}
+
+// Return the normal vector of the contact
+Vector3f ContactPoint::getNormal()  {
+	return this.normal;
+}
+
+// Set the penetration depth of the contact
+void ContactPoint::setPenetrationDepth(float penetrationDepth) {
+	this.this.penetrationDepth = penetrationDepth;
+}
+
+// Return the contact point on body 1
+Vector3f ContactPoint::getLocalPointOnBody1()  {
+	return this.localPointOnBody1;
+}
+
+// Return the contact point on body 2
+Vector3f ContactPoint::getLocalPointOnBody2()  {
+	return this.localPointOnBody2;
+}
+
+// Return the contact world point on body 1
+Vector3f ContactPoint::getWorldPointOnBody1()  {
+	return this.worldPointOnBody1;
+}
+
+// Return the contact world point on body 2
+Vector3f ContactPoint::getWorldPointOnBody2()  {
+	return this.worldPointOnBody2;
+}
+
+// Return the cached penetration impulse
+float ContactPoint::getPenetrationImpulse()  {
+	return this.penetrationImpulse;
+}
+
+// Return the cached first friction impulse
+float ContactPoint::getFrictionImpulse1()  {
+	return this.frictionImpulse1;
+}
+
+// Return the cached second friction impulse
+float ContactPoint::getFrictionImpulse2()  {
+	return this.frictionImpulse2;
+}
+
+// Return the cached rolling resistance impulse
+Vector3f ContactPoint::getRollingResistanceImpulse()  {
+	return this.rollingResistanceImpulse;
+}
+
+// Set the cached penetration impulse
+void ContactPoint::setPenetrationImpulse(float impulse) {
+	this.penetrationImpulse = impulse;
+}
+
+// Set the first cached friction impulse
+void ContactPoint::setFrictionImpulse1(float impulse) {
+	this.frictionImpulse1 = impulse;
+}
+
+// Set the second cached friction impulse
+void ContactPoint::setFrictionImpulse2(float impulse) {
+	this.frictionImpulse2 = impulse;
+}
+
+// Set the cached rolling resistance impulse
+void ContactPoint::setRollingResistanceImpulse( Vector3f impulse) {
+	this.rollingResistanceImpulse = impulse;
+}
+
+// Set the contact world point on body 1
+void ContactPoint::setWorldPointOnBody1( Vector3f worldPoint) {
+	this.worldPointOnBody1 = worldPoint;
+}
+
+// Set the contact world point on body 2
+void ContactPoint::setWorldPointOnBody2( Vector3f worldPoint) {
+	this.worldPointOnBody2 = worldPoint;
+}
+
+// Return true if the contact is a resting contact
+boolean ContactPoint::getIsRestingContact()  {
+	return this.isRestingContact;
+}
+
+// Set the this.isRestingContact variable
+void ContactPoint::setIsRestingContact(boolean isRestingContact) {
+	this.isRestingContact = isRestingContact;
+}
+
+// Get the first friction vector
+Vector3f ContactPoint::getFrictionVector1()  {
+	return this.frictionVectors[0];
+}
+
+// Set the first friction vector
+void ContactPoint::setFrictionVector1( Vector3f frictionVector1) {
+	this.frictionVectors[0] = frictionVector1;
+}
+
+// Get the second friction vector
+Vector3f ContactPoint::getFrictionvec2()  {
+	return this.frictionVectors[1];
+}
+
+// Set the second friction vector
+void ContactPoint::setFrictionvec2( Vector3f frictionvec2) {
+	this.frictionVectors[1] = frictionvec2;
+}
+
+// Return the penetration depth of the contact
+float ContactPoint::getPenetrationDepth()  {
+	return this.penetrationDepth;
+}
+
+// Return the number of bytes used by the contact point
+long ContactPoint::getSizeInBytes()  {
+	return sizeof(ContactPoint);
+}

src/org/atriaSoft/ephysics/constraint/ContactPoint.java

@@ -0,0 +1,123 @@
+package org.atriaSoft.ephysics.constraint;
+
+	/**
+	 * @brief 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 {
+		public:
+			ProxyShape* shape1; //!< First proxy shape of the contact
+			ProxyShape* shape2; //!< Second proxy shape of the contact
+			 CollisionShape* collisionShape1; //!< First collision shape
+			 CollisionShape* collisionShape2; //!< Second collision shape
+			Vector3f normal; //!< Normalized normal vector of the collision contact in world space
+			float penetrationDepth; //!< Penetration depth of the contact
+			Vector3f localPoint1; //!< Contact point of body 1 in local space of body 1
+			Vector3f localPoint2; //!< Contact point of body 2 in local space of body 2
+			ContactPointInfo(ProxyShape* proxyShape1,
+			                 ProxyShape* proxyShape2,
+			                  CollisionShape* collShape1,
+			                  CollisionShape* collShape2,
+			                  Vector3f normal,
+			                 float penetrationDepth,
+			                  Vector3f localPoint1,
+			                  Vector3f localPoint2):
+			  shape1(proxyShape1),
+			  shape2(proxyShape2),
+			  collisionShape1(collShape1),
+			  collisionShape2(collShape2),
+			  normal(normal),
+			  penetrationDepth(penetrationDepth),
+			  localPoint1(localPoint1),
+			  localPoint2(localPoint2) {
+				
+			}
+			ContactPointInfo():
+			  shape1(null),
+			  shape2(null),
+			  collisionShape1(null),
+			  collisionShape2(null) {
+				// TODO: add it for Vector
+			}
+	};
+
+	/**
+	 * @brief This class represents a collision contact point between two
+	 * bodies in the physics engine.
+	 */
+	class ContactPoint {
+		private :
+			CollisionBody* body1; //!< First rigid body of the contact
+			CollisionBody* body2; //!< Second rigid body of the contact
+			 Vector3f normal; //!< Normalized normal vector of the contact (from body1 toward body2) in world space
+			float penetrationDepth; //!< Penetration depth
+			 Vector3f localPointOnBody1; //!< Contact point on body 1 in local space of body 1
+			 Vector3f localPointOnBody2; //!< Contact point on body 2 in local space of body 2
+			Vector3f worldPointOnBody1; //!< Contact point on body 1 in world space
+			Vector3f worldPointOnBody2; //!< Contact point on body 2 in world space
+			boolean isRestingContact; //!< True if the contact is a resting contact (exists for more than one time step)
+			Vector3f frictionVectors[2]; //!< Two orthogonal vectors that span the tangential friction plane
+			float penetrationImpulse; //!< Cached penetration impulse
+			float frictionImpulse1; //!< Cached first friction impulse
+			float frictionImpulse2; //!< Cached second friction impulse
+			Vector3f rollingResistanceImpulse; //!< Cached rolling resistance impulse
+		public :
+			/// Constructor
+			ContactPoint( ContactPointInfo contactInfo);
+			/// Return the reference to the body 1
+			CollisionBody* getBody1() ;
+			/// Return the reference to the body 2
+			CollisionBody* getBody2() ;
+			/// Return the normal vector of the contact
+			Vector3f getNormal() ;
+			/// Set the penetration depth of the contact
+			void setPenetrationDepth(float penetrationDepth);
+			/// Return the contact local point on body 1
+			Vector3f getLocalPointOnBody1() ;
+			/// Return the contact local point on body 2
+			Vector3f getLocalPointOnBody2() ;
+			/// Return the contact world point on body 1
+			Vector3f getWorldPointOnBody1() ;
+			/// Return the contact world point on body 2
+			Vector3f getWorldPointOnBody2() ;
+			/// Return the cached penetration impulse
+			float getPenetrationImpulse() ;
+			/// Return the cached first friction impulse
+			float getFrictionImpulse1() ;
+			/// Return the cached second friction impulse
+			float getFrictionImpulse2() ;
+			/// Return the cached rolling resistance impulse
+			Vector3f getRollingResistanceImpulse() ;
+			/// Set the cached penetration impulse
+			void setPenetrationImpulse(float impulse);
+			/// Set the first cached friction impulse
+			void setFrictionImpulse1(float impulse);
+			/// Set the second cached friction impulse
+			void setFrictionImpulse2(float impulse);
+			/// Set the cached rolling resistance impulse
+			void setRollingResistanceImpulse( Vector3f impulse);
+			/// Set the contact world point on body 1
+			void setWorldPointOnBody1( Vector3f worldPoint);
+			/// Set the contact world point on body 2
+			void setWorldPointOnBody2( Vector3f worldPoint);
+			/// Return true if the contact is a resting contact
+			boolean getIsRestingContact() ;
+			/// Set the this.isRestingContact variable
+			void setIsRestingContact(boolean isRestingContact);
+			/// Get the first friction vector
+			Vector3f getFrictionVector1() ;
+			/// Set the first friction vector
+			void setFrictionVector1( Vector3f frictionVector1);
+			/// Get the second friction vector
+			Vector3f getFrictionvec2() ;
+			/// Set the second friction vector
+			void setFrictionvec2( Vector3f frictionvec2);
+			/// Return the penetration depth
+			float getPenetrationDepth() ;
+			/// Return the number of bytes used by the contact point
+			long getSizeInBytes() ;
+	};
+
+}

src/org/atriaSoft/ephysics/constraint/FixedJoint.cpp

@@ -0,0 +1,311 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/raint/FixedJoint.hpp>
+#include <ephysics/engine/ConstraintSolver.hpp>
+
+using namespace ephysics;
+
+// Static variables definition
+ float FixedJoint::BETA = float(0.2);
+
+// Constructor
+FixedJoint::FixedJoint( FixedJointInfo jointInfo)
+		   : Joint(jointInfo), this.impulseTranslation(0, 0, 0), this.impulseRotation(0, 0, 0) {
+
+	// Compute the local-space anchor point for each body
+	 Transform3D transform1 = this.body1.getTransform();
+	 Transform3D transform2 = this.body2.getTransform();
+	this.localAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace;
+	this.localAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace;
+
+	// Compute the inverse of the initial orientation difference between the two bodies
+	this.initOrientationDifferenceInv = transform2.getOrientation() *
+									transform1.getOrientation().getInverse();
+	this.initOrientationDifferenceInv.normalize();
+	this.initOrientationDifferenceInv.inverse();
+}
+
+// Destructor
+FixedJoint::~FixedJoint() {
+
+}
+
+// Initialize before solving the raint
+void FixedJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
+
+	// Initialize the bodies index in the velocity array
+	this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1).second;
+	this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2).second;
+
+	// Get the bodies positions and orientations
+	 Vector3f x1 = this.body1.this.centerOfMassWorld;
+	 Vector3f x2 = this.body2.this.centerOfMassWorld;
+	 Quaternion orientationBody1 = this.body1.getTransform().getOrientation();
+	 Quaternion orientationBody2 = this.body2.getTransform().getOrientation();
+
+	// Get the inertia tensor of bodies
+	this.i1 = this.body1.getInertiaTensorInverseWorld();
+	this.i2 = this.body2.getInertiaTensorInverseWorld();
+
+	// Compute the vector from body center to the anchor point in world-space
+	this.r1World = orientationBody1 * this.localAnchorPointBody1;
+	this.r2World = orientationBody2 * this.localAnchorPointBody2;
+
+	// Compute the corresponding skew-symmetric matrices
+	Matrix3f skewSymmetricMatrixU1= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r1World);
+	Matrix3f skewSymmetricMatrixU2= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r2World);
+
+	// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints
+	float inverseMassBodies = this.body1.this.massInverse + this.body2.this.massInverse;
+	Matrix3f massMatrix = Matrix3f(inverseMassBodies, 0, 0,
+	                                           0, inverseMassBodies, 0,
+	                                           0, 0, inverseMassBodies)
+	                            + skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose()
+	                            + skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose();
+
+	// Compute the inverse mass matrix K^-1 for the 3 translation raints
+	this.inverseMassMatrixTranslation.setZero();
+	if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
+		this.inverseMassMatrixTranslation = massMatrix.getInverse();
+	}
+
+	// Compute the bias "b" of the raint for the 3 translation raints
+	float biasFactor = (BETA / raintSolverData.timeStep);
+	this.biasTranslation.setZero();
+	if (this.positionCorrectionTechnique == BAUMGARTEJOINTS) {
+		this.biasTranslation = biasFactor * (x2 + this.r2World - x1 - this.r1World);
+	}
+
+	// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
+	// contraints (3x3 matrix)
+	this.inverseMassMatrixRotation = this.i1 + this.i2;
+	if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
+		this.inverseMassMatrixRotation = this.inverseMassMatrixRotation.getInverse();
+	}
+
+	// Compute the bias "b" for the 3 rotation raints
+	this.biasRotation.setZero();
+	if (this.positionCorrectionTechnique == BAUMGARTEJOINTS) {
+		Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
+		currentOrientationDifference.normalize();
+		 Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv;
+		this.biasRotation = biasFactor * float(2.0) * qError.getVectorV();
+	}
+
+	// If warm-starting is not enabled
+	if (!raintSolverData.isWarmStartingActive) {
+
+		// Reset the accumulated impulses
+		this.impulseTranslation.setZero();
+		this.impulseRotation.setZero();
+	}
+}
+
+// Warm start the raint (apply the previous impulse at the beginning of the step)
+void FixedJoint::warmstart( ConstraintSolverData raintSolverData) {
+
+	// Get the velocities
+	Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+	Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+	Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+	Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+
+	// Get the inverse mass of the bodies
+	 float inverseMassBody1 = this.body1.this.massInverse;
+	 float inverseMassBody2 = this.body2.this.massInverse;
+
+	// Compute the impulse P=J^T * lambda for the 3 translation raints for body 1
+	Vector3f linearImpulseBody1 = -this.impulseTranslation;
+	Vector3f angularImpulseBody1 = this.impulseTranslation.cross(this.r1World);
+
+	// Compute the impulse P=J^T * lambda for the 3 rotation raints for body 1
+	angularImpulseBody1 += -this.impulseRotation;
+
+	// Apply the impulse to the body 1
+	v1 += inverseMassBody1 * linearImpulseBody1;
+	w1 += this.i1 * angularImpulseBody1;
+
+	// Compute the impulse P=J^T * lambda for the 3 translation raints for body 2
+	Vector3f angularImpulseBody2 = -this.impulseTranslation.cross(this.r2World);
+
+	// Compute the impulse P=J^T * lambda for the 3 rotation raints for body 2
+	angularImpulseBody2 += this.impulseRotation;
+
+	// Apply the impulse to the body 2
+	v2 += inverseMassBody2 * this.impulseTranslation;
+	w2 += this.i2 * angularImpulseBody2;
+}
+
+// Solve the velocity raint
+void FixedJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) {
+
+	// Get the velocities
+	Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+	Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+	Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+	Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+
+	// Get the inverse mass of the bodies
+	float inverseMassBody1 = this.body1.this.massInverse;
+	float inverseMassBody2 = this.body2.this.massInverse;
+
+	// --------------- Translation Constraints --------------- //
+
+	// Compute J*v for the 3 translation raints
+	 Vector3f JvTranslation = v2 + w2.cross(this.r2World) - v1 - w1.cross(this.r1World);
+
+	// Compute the Lagrange multiplier lambda
+	 Vector3f deltaLambda = this.inverseMassMatrixTranslation *
+							   (-JvTranslation - this.biasTranslation);
+	this.impulseTranslation += deltaLambda;
+
+	// Compute the impulse P=J^T * lambda for body 1
+	 Vector3f linearImpulseBody1 = -deltaLambda;
+	Vector3f angularImpulseBody1 = deltaLambda.cross(this.r1World);
+
+	// Apply the impulse to the body 1
+	v1 += inverseMassBody1 * linearImpulseBody1;
+	w1 += this.i1 * angularImpulseBody1;
+
+	// Compute the impulse P=J^T * lambda  for body 2
+	 Vector3f angularImpulseBody2 = -deltaLambda.cross(this.r2World);
+
+	// Apply the impulse to the body 2
+	v2 += inverseMassBody2 * deltaLambda;
+	w2 += this.i2 * angularImpulseBody2;
+
+	// --------------- Rotation Constraints --------------- //
+
+	// Compute J*v for the 3 rotation raints
+	 Vector3f JvRotation = w2 - w1;
+
+	// Compute the Lagrange multiplier lambda for the 3 rotation raints
+	Vector3f deltaLambda2 = this.inverseMassMatrixRotation * (-JvRotation - this.biasRotation);
+	this.impulseRotation += deltaLambda2;
+
+	// Compute the impulse P=J^T * lambda for the 3 rotation raints for body 1
+	angularImpulseBody1 = -deltaLambda2;
+
+	// Apply the impulse to the body 1
+	w1 += this.i1 * angularImpulseBody1;
+
+	// Apply the impulse to the body 2
+	w2 += this.i2 * deltaLambda2;
+}
+
+// Solve the position raint (for position error correction)
+void FixedJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) {
+
+	// If the error position correction technique is not the non-linear-gauss-seidel, we do
+	// do not execute this method
+	if (this.positionCorrectionTechnique != NONLINEARGAUSSSEIDEL) return;
+
+	// Get the bodies positions and orientations
+	Vector3f x1 = raintSolverData.positions[this.indexBody1];
+	Vector3f x2 = raintSolverData.positions[this.indexBody2];
+	Quaternion q1 = raintSolverData.orientations[this.indexBody1];
+	Quaternion q2 = raintSolverData.orientations[this.indexBody2];
+
+	// Get the inverse mass and inverse inertia tensors of the bodies
+	float inverseMassBody1 = this.body1.this.massInverse;
+	float inverseMassBody2 = this.body2.this.massInverse;
+
+	// Recompute the inverse inertia tensors
+	this.i1 = this.body1.getInertiaTensorInverseWorld();
+	this.i2 = this.body2.getInertiaTensorInverseWorld();
+
+	// Compute the vector from body center to the anchor point in world-space
+	this.r1World = q1 * this.localAnchorPointBody1;
+	this.r2World = q2 * this.localAnchorPointBody2;
+
+	// Compute the corresponding skew-symmetric matrices
+	Matrix3f skewSymmetricMatrixU1= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r1World);
+	Matrix3f skewSymmetricMatrixU2= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r2World);
+
+	// --------------- Translation Constraints --------------- //
+
+	// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints
+	float inverseMassBodies = this.body1.this.massInverse + this.body2.this.massInverse;
+	Matrix3f massMatrix = Matrix3f(inverseMassBodies, 0, 0,
+	                                           0, inverseMassBodies, 0,
+	                                           0, 0, inverseMassBodies)
+	                            + skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose()
+	                            + skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose();
+	this.inverseMassMatrixTranslation.setZero();
+	if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
+		this.inverseMassMatrixTranslation = massMatrix.getInverse();
+	}
+
+	// Compute position error for the 3 translation raints
+	 Vector3f errorTranslation = x2 + this.r2World - x1 - this.r1World;
+
+	// Compute the Lagrange multiplier lambda
+	 Vector3f lambdaTranslation = this.inverseMassMatrixTranslation * (-errorTranslation);
+
+	// Compute the impulse of body 1
+	Vector3f linearImpulseBody1 = -lambdaTranslation;
+	Vector3f angularImpulseBody1 = lambdaTranslation.cross(this.r1World);
+
+	// Compute the pseudo velocity of body 1
+	 Vector3f v1 = inverseMassBody1 * linearImpulseBody1;
+	Vector3f w1 = this.i1 * angularImpulseBody1;
+
+	// Update the body position/orientation of body 1
+	x1 += v1;
+	q1 += Quaternion(0, w1) * q1 * 0.5f;
+	q1.normalize();
+
+	// Compute the impulse of body 2
+	Vector3f angularImpulseBody2 = -lambdaTranslation.cross(this.r2World);
+
+	// Compute the pseudo velocity of body 2
+	 Vector3f v2 = inverseMassBody2 * lambdaTranslation;
+	Vector3f w2 = this.i2 * angularImpulseBody2;
+
+	// Update the body position/orientation of body 2
+	x2 += v2;
+	q2 += Quaternion(0, w2) * q2 * 0.5f;
+	q2.normalize();
+
+	// --------------- Rotation Constraints --------------- //
+
+	// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
+	// contraints (3x3 matrix)
+	this.inverseMassMatrixRotation = this.i1 + this.i2;
+	if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
+		this.inverseMassMatrixRotation = this.inverseMassMatrixRotation.getInverse();
+	}
+
+	// Compute the position error for the 3 rotation raints
+	Quaternion currentOrientationDifference = q2 * q1.getInverse();
+	currentOrientationDifference.normalize();
+	 Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv;
+	 Vector3f errorRotation = float(2.0) * qError.getVectorV();
+
+	// Compute the Lagrange multiplier lambda for the 3 rotation raints
+	Vector3f lambdaRotation = this.inverseMassMatrixRotation * (-errorRotation);
+
+	// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
+	angularImpulseBody1 = -lambdaRotation;
+
+	// Compute the pseudo velocity of body 1
+	w1 = this.i1 * angularImpulseBody1;
+
+	// Update the body position/orientation of body 1
+	q1 += Quaternion(0, w1) * q1 * 0.5f;
+	q1.normalize();
+
+	// Compute the pseudo velocity of body 2
+	w2 = this.i2 * lambdaRotation;
+
+	// Update the body position/orientation of body 2
+	q2 += Quaternion(0, w2) * q2 * 0.5f;
+	q2.normalize();
+}
+

src/org/atriaSoft/ephysics/constraint/FixedJoint.java

@@ -0,0 +1,58 @@
+package org.atriaSoft.ephysics.constraint;
+
+	/**
+	 * This structure is used to gather the information needed to create a fixed
+	 * joint. This structure will be used to create the actual fixed joint.
+	 */
+	struct FixedJointInfo extends  JointInfo {
+		public :
+			Vector3f anchorPointWorldSpace; //!< Anchor point (in world-space coordinates)
+			/**
+			 * @breif Contructor
+			 * @param rigidBody1 The first body of the joint
+			 * @param rigidBody2 The second body of the joint
+			 * @param initAnchorPointWorldSpace The initial anchor point of the joint in world-space coordinates
+			 */
+			FixedJointInfo(RigidBody* rigidBody1,
+			               RigidBody* rigidBody2,
+			                Vector3f initAnchorPointWorldSpace):
+			  JointInfo(rigidBody1, rigidBody2, FIXEDJOINT),
+			  this.anchorPointWorldSpace(initAnchorPointWorldSpace){
+				
+			}
+	};
+	
+	/**
+	 * @breif It represents a fixed joint that is used to forbid any translation or rotation
+	 * between two bodies.
+	 */
+	class FixedJoint extends  Joint {
+		private:
+			static  float BETA; //!< Beta value for the bias factor of position correction
+			Vector3f localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1)
+			Vector3f localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2)
+			Vector3f r1World; //!< Vector from center of body 2 to anchor point in world-space
+			Vector3f r2World; //!< Vector from center of body 2 to anchor point in world-space
+			Matrix3f i1; //!< Inertia tensor of body 1 (in world-space coordinates)
+			Matrix3f i2; //!< Inertia tensor of body 2 (in world-space coordinates)
+			Vector3f impulseTranslation; //!< Accumulated impulse for the 3 translation raints
+			Vector3f impulseRotation; //!< Accumulate impulse for the 3 rotation raints
+			Matrix3f inverseMassMatrixTranslation; //!< Inverse mass matrix K=JM^-1J^-t of the 3 translation raints (3x3 matrix)
+			Matrix3f inverseMassMatrixRotation; //!< Inverse mass matrix K=JM^-1J^-t of the 3 rotation raints (3x3 matrix)
+			Vector3f biasTranslation; //!< Bias vector for the 3 translation raints
+			Vector3f biasRotation; //!< Bias vector for the 3 rotation raints
+			Quaternion initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the two bodies
+			
+			long getSizeInBytes()   {
+				return sizeof(FixedJoint);
+			}
+			void initBeforeSolve( ConstraintSolverData raintSolverData) ;
+			void warmstart( ConstraintSolverData raintSolverData) ;
+			void solveVelocityConstraint( ConstraintSolverData raintSolverData) ;
+			void solvePositionConstraint( ConstraintSolverData raintSolverData) ;
+		public:
+			/// Constructor
+			FixedJoint( FixedJointInfo jointInfo);
+	};
+}
+

src/org/atriaSoft/ephysics/constraint/HingeJoint.cpp

@@ -0,0 +1,849 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/raint/HingeJoint.hpp>
+#include <ephysics/engine/ConstraintSolver.hpp>
+
+using namespace ephysics;
+
+// Static variables definition
+ float HingeJoint::BETA = float(0.2);
+
+// Constructor
+HingeJoint::HingeJoint( HingeJointInfo jointInfo)
+		   : Joint(jointInfo), this.impulseTranslation(0, 0, 0), this.impulseRotation(0, 0),
+			 this.impulseLowerLimit(0), this.impulseUpperLimit(0), this.impulseMotor(0),
+			 this.isLimitEnabled(jointInfo.isLimitEnabled), this.isMotorEnabled(jointInfo.isMotorEnabled),
+			 this.lowerLimit(jointInfo.minAngleLimit), this.upperLimit(jointInfo.maxAngleLimit),
+			 this.isLowerLimitViolated(false), this.isUpperLimitViolated(false),
+			 this.motorSpeed(jointInfo.motorSpeed), this.maxMotorTorque(jointInfo.maxMotorTorque) {
+
+	assert(this.lowerLimit <= 0 && this.lowerLimit >= -2.0 * PI);
+	assert(this.upperLimit >= 0 && this.upperLimit <= 2.0 * PI);
+
+	// Compute the local-space anchor point for each body
+	Transform3D transform1 = this.body1.getTransform();
+	Transform3D transform2 = this.body2.getTransform();
+	this.localAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace;
+	this.localAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace;
+
+	// Compute the local-space hinge axis
+	this.hingeLocalAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.rotationAxisWorld;
+	this.hingeLocalAxisBody2 = transform2.getOrientation().getInverse() * jointInfo.rotationAxisWorld;
+	this.hingeLocalAxisBody1.normalize();
+	this.hingeLocalAxisBody2.normalize();
+
+	// Compute the inverse of the initial orientation difference between the two bodies
+	this.initOrientationDifferenceInv = transform2.getOrientation() *
+									transform1.getOrientation().getInverse();
+	this.initOrientationDifferenceInv.normalize();
+	this.initOrientationDifferenceInv.inverse();
+}
+
+// Destructor
+HingeJoint::~HingeJoint() {
+
+}
+
+// Initialize before solving the raint
+void HingeJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
+
+	// Initialize the bodies index in the velocity array
+	this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1).second;
+	this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2).second;
+
+	// Get the bodies positions and orientations
+	 Vector3f x1 = this.body1.this.centerOfMassWorld;
+	 Vector3f x2 = this.body2.this.centerOfMassWorld;
+	 Quaternion orientationBody1 = this.body1.getTransform().getOrientation();
+	 Quaternion orientationBody2 = this.body2.getTransform().getOrientation();
+
+	// Get the inertia tensor of bodies
+	this.i1 = this.body1.getInertiaTensorInverseWorld();
+	this.i2 = this.body2.getInertiaTensorInverseWorld();
+
+	// Compute the vector from body center to the anchor point in world-space
+	this.r1World = orientationBody1 * this.localAnchorPointBody1;
+	this.r2World = orientationBody2 * this.localAnchorPointBody2;
+
+	// Compute the current angle around the hinge axis
+	float hingeAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2);
+
+	// Check if the limit raints are violated or not
+	float lowerLimitError = hingeAngle - this.lowerLimit;
+	float upperLimitError = this.upperLimit - hingeAngle;
+	boolean oldIsLowerLimitViolated = this.isLowerLimitViolated;
+	this.isLowerLimitViolated = lowerLimitError <= 0;
+	if (this.isLowerLimitViolated != oldIsLowerLimitViolated) {
+		this.impulseLowerLimit = 0.0;
+	}
+	boolean oldIsUpperLimitViolated = this.isUpperLimitViolated;
+	this.isUpperLimitViolated = upperLimitError <= 0;
+	if (this.isUpperLimitViolated != oldIsUpperLimitViolated) {
+		this.impulseUpperLimit = 0.0;
+	}
+
+	// Compute vectors needed in the Jacobian
+	mA1 = orientationBody1 * this.hingeLocalAxisBody1;
+	Vector3f a2 = orientationBody2 * this.hingeLocalAxisBody2;
+	mA1.normalize();
+	a2.normalize();
+	 Vector3f b2 = a2.getOrthoVector();
+	 Vector3f c2 = a2.cross(b2);
+	this.b2CrossA1 = b2.cross(mA1);
+	this.c2CrossA1 = c2.cross(mA1);
+
+	// Compute the corresponding skew-symmetric matrices
+	Matrix3f skewSymmetricMatrixU1= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r1World);
+	Matrix3f skewSymmetricMatrixU2= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r2World);
+
+	// Compute the inverse mass matrix K=JM^-1J^t for the 3 translation raints (3x3 matrix)
+	float inverseMassBodies = this.body1.this.massInverse + this.body2.this.massInverse;
+	Matrix3f massMatrix = Matrix3f(inverseMassBodies, 0, 0,
+	                                           0, inverseMassBodies, 0,
+	                                           0, 0, inverseMassBodies)
+	                            + skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose()
+	                            + skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose();
+	this.inverseMassMatrixTranslation.setZero();
+	if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
+		this.inverseMassMatrixTranslation = massMatrix.getInverse();
+	}
+
+	// Compute the bias "b" of the translation raints
+	this.bTranslation.setZero();
+	float biasFactor = (BETA / raintSolverData.timeStep);
+	if (this.positionCorrectionTechnique == BAUMGARTEJOINTS) {
+		this.bTranslation = biasFactor * (x2 + this.r2World - x1 - this.r1World);
+	}
+
+	// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation raints (2x2 matrix)
+	Vector3f I1B2CrossA1 = this.i1 * this.b2CrossA1;
+	Vector3f I1C2CrossA1 = this.i1 * this.c2CrossA1;
+	Vector3f I2B2CrossA1 = this.i2 * this.b2CrossA1;
+	Vector3f I2C2CrossA1 = this.i2 * this.c2CrossA1;
+	 float el11 = this.b2CrossA1.dot(I1B2CrossA1) +
+						 this.b2CrossA1.dot(I2B2CrossA1);
+	 float el12 = this.b2CrossA1.dot(I1C2CrossA1) +
+						 this.b2CrossA1.dot(I2C2CrossA1);
+	 float el21 = this.c2CrossA1.dot(I1B2CrossA1) +
+						 this.c2CrossA1.dot(I2B2CrossA1);
+	 float el22 = this.c2CrossA1.dot(I1C2CrossA1) +
+						 this.c2CrossA1.dot(I2C2CrossA1);
+	 Matrix2x2 matrixKRotation(el11, el12, el21, el22);
+	this.inverseMassMatrixRotation.setZero();
+	if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
+		this.inverseMassMatrixRotation = matrixKRotation.getInverse();
+	}
+
+	// Compute the bias "b" of the rotation raints
+	this.bRotation.setZero();
+	if (this.positionCorrectionTechnique == BAUMGARTEJOINTS) {
+		this.bRotation = biasFactor * vec2(mA1.dot(b2), mA1.dot(c2));
+	}
+
+	// If warm-starting is not enabled
+	if (!raintSolverData.isWarmStartingActive) {
+
+		// Reset all the accumulated impulses
+		this.impulseTranslation.setZero();
+		this.impulseRotation.setZero();
+		this.impulseLowerLimit = 0.0;
+		this.impulseUpperLimit = 0.0;
+		this.impulseMotor = 0.0;
+	}
+
+	// If the motor or limits are enabled
+	if (this.isMotorEnabled || (this.isLimitEnabled && (this.isLowerLimitViolated || this.isUpperLimitViolated))) {
+
+		// Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix)
+		this.inverseMassMatrixLimitMotor = mA1.dot(this.i1 * mA1) + mA1.dot(this.i2 * mA1);
+		this.inverseMassMatrixLimitMotor = (this.inverseMassMatrixLimitMotor > 0.0) ?
+								  1.0f / this.inverseMassMatrixLimitMotor : 0.0f;
+
+		if (this.isLimitEnabled) {
+
+			// Compute the bias "b" of the lower limit raint
+			this.bLowerLimit = 0.0;
+			if (this.positionCorrectionTechnique == BAUMGARTEJOINTS) {
+				this.bLowerLimit = biasFactor * lowerLimitError;
+			}
+
+			// Compute the bias "b" of the upper limit raint
+			this.bUpperLimit = 0.0;
+			if (this.positionCorrectionTechnique == BAUMGARTEJOINTS) {
+				this.bUpperLimit = biasFactor * upperLimitError;
+			}
+		}
+	}
+}
+
+// Warm start the raint (apply the previous impulse at the beginning of the step)
+void HingeJoint::warmstart( ConstraintSolverData raintSolverData) {
+
+	// Get the velocities
+	Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+	Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+	Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+	Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+
+	// Get the inverse mass and inverse inertia tensors of the bodies
+	 float inverseMassBody1 = this.body1.this.massInverse;
+	 float inverseMassBody2 = this.body2.this.massInverse;
+
+	// Compute the impulse P=J^T * lambda for the 2 rotation raints
+	Vector3f rotationImpulse = -this.b2CrossA1 * this.impulseRotation.x() - this.c2CrossA1 * this.impulseRotation.y();
+
+	// Compute the impulse P=J^T * lambda for the lower and upper limits raints
+	 Vector3f limitsImpulse = (this.impulseUpperLimit - this.impulseLowerLimit) * mA1;
+
+	// Compute the impulse P=J^T * lambda for the motor raint
+	 Vector3f motorImpulse = -this.impulseMotor * mA1;
+
+	// Compute the impulse P=J^T * lambda for the 3 translation raints of body 1
+	Vector3f linearImpulseBody1 = -this.impulseTranslation;
+	Vector3f angularImpulseBody1 = this.impulseTranslation.cross(this.r1World);
+
+	// Compute the impulse P=J^T * lambda for the 2 rotation raints of body 1
+	angularImpulseBody1 += rotationImpulse;
+
+	// Compute the impulse P=J^T * lambda for the lower and upper limits raints of body 1
+	angularImpulseBody1 += limitsImpulse;
+
+	// Compute the impulse P=J^T * lambda for the motor raint of body 1
+	angularImpulseBody1 += motorImpulse;
+
+	// Apply the impulse to the body 1
+	v1 += inverseMassBody1 * linearImpulseBody1;
+	w1 += this.i1 * angularImpulseBody1;
+
+	// Compute the impulse P=J^T * lambda for the 3 translation raints of body 2
+	Vector3f angularImpulseBody2 = -this.impulseTranslation.cross(this.r2World);
+
+	// Compute the impulse P=J^T * lambda for the 2 rotation raints of body 2
+	angularImpulseBody2 += -rotationImpulse;
+
+	// Compute the impulse P=J^T * lambda for the lower and upper limits raints of body 2
+	angularImpulseBody2 += -limitsImpulse;
+
+	// Compute the impulse P=J^T * lambda for the motor raint of body 2
+	angularImpulseBody2 += -motorImpulse;
+
+	// Apply the impulse to the body 2
+	v2 += inverseMassBody2 * this.impulseTranslation;
+	w2 += this.i2 * angularImpulseBody2;
+}
+
+// Solve the velocity raint
+void HingeJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) {
+
+	// Get the velocities
+	Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+	Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+	Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+	Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+
+	// Get the inverse mass and inverse inertia tensors of the bodies
+	float inverseMassBody1 = this.body1.this.massInverse;
+	float inverseMassBody2 = this.body2.this.massInverse;
+
+	// --------------- Translation Constraints --------------- //
+
+	// Compute J*v
+	 Vector3f JvTranslation = v2 + w2.cross(this.r2World) - v1 - w1.cross(this.r1World);
+
+	// Compute the Lagrange multiplier lambda
+	 Vector3f deltaLambdaTranslation = this.inverseMassMatrixTranslation *
+										  (-JvTranslation - this.bTranslation);
+	this.impulseTranslation += deltaLambdaTranslation;
+
+	// Compute the impulse P=J^T * lambda of body 1
+	 Vector3f linearImpulseBody1 = -deltaLambdaTranslation;
+	Vector3f angularImpulseBody1 = deltaLambdaTranslation.cross(this.r1World);
+
+	// Apply the impulse to the body 1
+	v1 += inverseMassBody1 * linearImpulseBody1;
+	w1 += this.i1 * angularImpulseBody1;
+
+	// Compute the impulse P=J^T * lambda of body 2
+	Vector3f angularImpulseBody2 = -deltaLambdaTranslation.cross(this.r2World);
+
+	// Apply the impulse to the body 2
+	v2 += inverseMassBody2 * deltaLambdaTranslation;
+	w2 += this.i2 * angularImpulseBody2;
+
+	// --------------- Rotation Constraints --------------- //
+
+	// Compute J*v for the 2 rotation raints
+	 vec2 JvRotation(-this.b2CrossA1.dot(w1) + this.b2CrossA1.dot(w2),
+							 -this.c2CrossA1.dot(w1) + this.c2CrossA1.dot(w2));
+
+	// Compute the Lagrange multiplier lambda for the 2 rotation raints
+	vec2 deltaLambdaRotation = this.inverseMassMatrixRotation * (-JvRotation - this.bRotation);
+	this.impulseRotation += deltaLambdaRotation;
+
+	// Compute the impulse P=J^T * lambda for the 2 rotation raints of body 1
+	angularImpulseBody1 = -this.b2CrossA1 * deltaLambdaRotation.x() -
+										this.c2CrossA1 * deltaLambdaRotation.y();
+
+	// Apply the impulse to the body 1
+	w1 += this.i1 * angularImpulseBody1;
+
+	// Compute the impulse P=J^T * lambda for the 2 rotation raints of body 2
+	angularImpulseBody2 = this.b2CrossA1 * deltaLambdaRotation.x() +
+			this.c2CrossA1 * deltaLambdaRotation.y();
+
+	// Apply the impulse to the body 2
+	w2 += this.i2 * angularImpulseBody2;
+
+	// --------------- Limits Constraints --------------- //
+
+	if (this.isLimitEnabled) {
+
+		// If the lower limit is violated
+		if (this.isLowerLimitViolated) {
+
+			// Compute J*v for the lower limit raint
+			 float JvLowerLimit = (w2 - w1).dot(mA1);
+
+			// Compute the Lagrange multiplier lambda for the lower limit raint
+			float deltaLambdaLower = this.inverseMassMatrixLimitMotor * (-JvLowerLimit -this.bLowerLimit);
+			float lambdaTemp = this.impulseLowerLimit;
+			this.impulseLowerLimit = max(this.impulseLowerLimit + deltaLambdaLower, 0.0f);
+			deltaLambdaLower = this.impulseLowerLimit - lambdaTemp;
+
+			// Compute the impulse P=J^T * lambda for the lower limit raint of body 1
+			 Vector3f angularImpulseBody1 = -deltaLambdaLower * mA1;
+
+			// Apply the impulse to the body 1
+			w1 += this.i1 * angularImpulseBody1;
+
+			// Compute the impulse P=J^T * lambda for the lower limit raint of body 2
+			 Vector3f angularImpulseBody2 = deltaLambdaLower * mA1;
+
+			// Apply the impulse to the body 2
+			w2 += this.i2 * angularImpulseBody2;
+		}
+
+		// If the upper limit is violated
+		if (this.isUpperLimitViolated) {
+
+			// Compute J*v for the upper limit raint
+			 float JvUpperLimit = -(w2 - w1).dot(mA1);
+
+			// Compute the Lagrange multiplier lambda for the upper limit raint
+			float deltaLambdaUpper = this.inverseMassMatrixLimitMotor * (-JvUpperLimit -this.bUpperLimit);
+			float lambdaTemp = this.impulseUpperLimit;
+			this.impulseUpperLimit = max(this.impulseUpperLimit + deltaLambdaUpper, 0.0f);
+			deltaLambdaUpper = this.impulseUpperLimit - lambdaTemp;
+
+			// Compute the impulse P=J^T * lambda for the upper limit raint of body 1
+			 Vector3f angularImpulseBody1 = deltaLambdaUpper * mA1;
+
+			// Apply the impulse to the body 1
+			w1 += this.i1 * angularImpulseBody1;
+
+			// Compute the impulse P=J^T * lambda for the upper limit raint of body 2
+			 Vector3f angularImpulseBody2 = -deltaLambdaUpper * mA1;
+
+			// Apply the impulse to the body 2
+			w2 += this.i2 * angularImpulseBody2;
+		}
+	}
+
+	// --------------- Motor --------------- //
+
+	// If the motor is enabled
+	if (this.isMotorEnabled) {
+
+		// Compute J*v for the motor
+		 float JvMotor = mA1.dot(w1 - w2);
+
+		// Compute the Lagrange multiplier lambda for the motor
+		 float maxMotorImpulse = this.maxMotorTorque * raintSolverData.timeStep;
+		float deltaLambdaMotor = this.inverseMassMatrixLimitMotor * (-JvMotor - this.motorSpeed);
+		float lambdaTemp = this.impulseMotor;
+		this.impulseMotor = clamp(this.impulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
+		deltaLambdaMotor = this.impulseMotor - lambdaTemp;
+
+		// Compute the impulse P=J^T * lambda for the motor of body 1
+		 Vector3f angularImpulseBody1 = -deltaLambdaMotor * mA1;
+
+		// Apply the impulse to the body 1
+		w1 += this.i1 * angularImpulseBody1;
+
+		// Compute the impulse P=J^T * lambda for the motor of body 2
+		 Vector3f angularImpulseBody2 = deltaLambdaMotor * mA1;
+
+		// Apply the impulse to the body 2
+		w2 += this.i2 * angularImpulseBody2;
+	}
+}
+
+// Solve the position raint (for position error correction)
+void HingeJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) {
+
+	// If the error position correction technique is not the non-linear-gauss-seidel, we do
+	// do not execute this method
+	if (this.positionCorrectionTechnique != NONLINEARGAUSSSEIDEL) return;
+
+	// Get the bodies positions and orientations
+	Vector3f x1 = raintSolverData.positions[this.indexBody1];
+	Vector3f x2 = raintSolverData.positions[this.indexBody2];
+	Quaternion q1 = raintSolverData.orientations[this.indexBody1];
+	Quaternion q2 = raintSolverData.orientations[this.indexBody2];
+
+	// Get the inverse mass and inverse inertia tensors of the bodies
+	float inverseMassBody1 = this.body1.this.massInverse;
+	float inverseMassBody2 = this.body2.this.massInverse;
+
+	// Recompute the inverse inertia tensors
+	this.i1 = this.body1.getInertiaTensorInverseWorld();
+	this.i2 = this.body2.getInertiaTensorInverseWorld();
+
+	// Compute the vector from body center to the anchor point in world-space
+	this.r1World = q1 * this.localAnchorPointBody1;
+	this.r2World = q2 * this.localAnchorPointBody2;
+
+	// Compute the current angle around the hinge axis
+	float hingeAngle = computeCurrentHingeAngle(q1, q2);
+
+	// Check if the limit raints are violated or not
+	float lowerLimitError = hingeAngle - this.lowerLimit;
+	float upperLimitError = this.upperLimit - hingeAngle;
+	this.isLowerLimitViolated = lowerLimitError <= 0;
+	this.isUpperLimitViolated = upperLimitError <= 0;
+
+	// Compute vectors needed in the Jacobian
+	mA1 = q1 * this.hingeLocalAxisBody1;
+	Vector3f a2 = q2 * this.hingeLocalAxisBody2;
+	mA1.normalize();
+	a2.normalize();
+	 Vector3f b2 = a2.getOrthoVector();
+	 Vector3f c2 = a2.cross(b2);
+	this.b2CrossA1 = b2.cross(mA1);
+	this.c2CrossA1 = c2.cross(mA1);
+
+	// Compute the corresponding skew-symmetric matrices
+	Matrix3f skewSymmetricMatrixU1= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r1World);
+	Matrix3f skewSymmetricMatrixU2= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r2World);
+
+	// --------------- Translation Constraints --------------- //
+
+	// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints
+	float inverseMassBodies = this.body1.this.massInverse + this.body2.this.massInverse;
+	Matrix3f massMatrix = Matrix3f(inverseMassBodies, 0, 0,
+	                                           0, inverseMassBodies, 0,
+	                                           0, 0, inverseMassBodies)
+	                            + skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose()
+	                            + skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose();
+	this.inverseMassMatrixTranslation.setZero();
+	if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
+		this.inverseMassMatrixTranslation = massMatrix.getInverse();
+	}
+
+	// Compute position error for the 3 translation raints
+	 Vector3f errorTranslation = x2 + this.r2World - x1 - this.r1World;
+
+	// Compute the Lagrange multiplier lambda
+	 Vector3f lambdaTranslation = this.inverseMassMatrixTranslation * (-errorTranslation);
+
+	// Compute the impulse of body 1
+	Vector3f linearImpulseBody1 = -lambdaTranslation;
+	Vector3f angularImpulseBody1 = lambdaTranslation.cross(this.r1World);
+
+	// Compute the pseudo velocity of body 1
+	 Vector3f v1 = inverseMassBody1 * linearImpulseBody1;
+	Vector3f w1 = this.i1 * angularImpulseBody1;
+
+	// Update the body position/orientation of body 1
+	x1 += v1;
+	q1 += Quaternion(0, w1) * q1 * 0.5f;
+	q1.normalize();
+
+	// Compute the impulse of body 2
+	Vector3f angularImpulseBody2 = -lambdaTranslation.cross(this.r2World);
+
+	// Compute the pseudo velocity of body 2
+	 Vector3f v2 = inverseMassBody2 * lambdaTranslation;
+	Vector3f w2 = this.i2 * angularImpulseBody2;
+
+	// Update the body position/orientation of body 2
+	x2 += v2;
+	q2 += Quaternion(0, w2) * q2 * 0.5f;
+	q2.normalize();
+
+	// --------------- Rotation Constraints --------------- //
+
+	// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation raints (2x2 matrix)
+	Vector3f I1B2CrossA1 = this.i1 * this.b2CrossA1;
+	Vector3f I1C2CrossA1 = this.i1 * this.c2CrossA1;
+	Vector3f I2B2CrossA1 = this.i2 * this.b2CrossA1;
+	Vector3f I2C2CrossA1 = this.i2 * this.c2CrossA1;
+	 float el11 = this.b2CrossA1.dot(I1B2CrossA1) +
+						 this.b2CrossA1.dot(I2B2CrossA1);
+	 float el12 = this.b2CrossA1.dot(I1C2CrossA1) +
+						 this.b2CrossA1.dot(I2C2CrossA1);
+	 float el21 = this.c2CrossA1.dot(I1B2CrossA1) +
+						 this.c2CrossA1.dot(I2B2CrossA1);
+	 float el22 = this.c2CrossA1.dot(I1C2CrossA1) +
+						 this.c2CrossA1.dot(I2C2CrossA1);
+	 Matrix2x2 matrixKRotation(el11, el12, el21, el22);
+	this.inverseMassMatrixRotation.setZero();
+	if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
+		this.inverseMassMatrixRotation = matrixKRotation.getInverse();
+	}
+
+	// Compute the position error for the 3 rotation raints
+	 vec2 errorRotation = vec2(mA1.dot(b2), mA1.dot(c2));
+
+	// Compute the Lagrange multiplier lambda for the 3 rotation raints
+	vec2 lambdaRotation = this.inverseMassMatrixRotation * (-errorRotation);
+
+	// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
+	angularImpulseBody1 = -this.b2CrossA1 * lambdaRotation.x() - this.c2CrossA1 * lambdaRotation.y();
+
+	// Compute the pseudo velocity of body 1
+	w1 = this.i1 * angularImpulseBody1;
+
+	// Update the body position/orientation of body 1
+	q1 += Quaternion(0, w1) * q1 * 0.5f;
+	q1.normalize();
+
+	// Compute the impulse of body 2
+	angularImpulseBody2 = this.b2CrossA1 * lambdaRotation.x() + this.c2CrossA1 * lambdaRotation.y();
+
+	// Compute the pseudo velocity of body 2
+	w2 = this.i2 * angularImpulseBody2;
+
+	// Update the body position/orientation of body 2
+	q2 += Quaternion(0, w2) * q2 * 0.5f;
+	q2.normalize();
+
+	// --------------- Limits Constraints --------------- //
+
+	if (this.isLimitEnabled) {
+
+		if (this.isLowerLimitViolated || this.isUpperLimitViolated) {
+
+			// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
+			this.inverseMassMatrixLimitMotor = mA1.dot(this.i1 * mA1) + mA1.dot(this.i2 * mA1);
+			this.inverseMassMatrixLimitMotor = (this.inverseMassMatrixLimitMotor > 0.0) ?
+									  1.0f / this.inverseMassMatrixLimitMotor : 0.0f;
+		}
+
+		// If the lower limit is violated
+		if (this.isLowerLimitViolated) {
+
+			// Compute the Lagrange multiplier lambda for the lower limit raint
+			float lambdaLowerLimit = this.inverseMassMatrixLimitMotor * (-lowerLimitError );
+
+			// Compute the impulse P=J^T * lambda of body 1
+			 Vector3f angularImpulseBody1 = -lambdaLowerLimit * mA1;
+
+			// Compute the pseudo velocity of body 1
+			 Vector3f w1 = this.i1 * angularImpulseBody1;
+
+			// Update the body position/orientation of body 1
+			q1 += Quaternion(0, w1) * q1 * 0.5f;
+			q1.normalize();
+
+			// Compute the impulse P=J^T * lambda of body 2
+			 Vector3f angularImpulseBody2 = lambdaLowerLimit * mA1;
+
+			// Compute the pseudo velocity of body 2
+			 Vector3f w2 = this.i2 * angularImpulseBody2;
+
+			// Update the body position/orientation of body 2
+			q2 += Quaternion(0, w2) * q2 * 0.5f;
+			q2.normalize();
+		}
+
+		// If the upper limit is violated
+		if (this.isUpperLimitViolated) {
+
+			// Compute the Lagrange multiplier lambda for the upper limit raint
+			float lambdaUpperLimit = this.inverseMassMatrixLimitMotor * (-upperLimitError);
+
+			// Compute the impulse P=J^T * lambda of body 1
+			 Vector3f angularImpulseBody1 = lambdaUpperLimit * mA1;
+
+			// Compute the pseudo velocity of body 1
+			 Vector3f w1 = this.i1 * angularImpulseBody1;
+
+			// Update the body position/orientation of body 1
+			q1 += Quaternion(0, w1) * q1 * 0.5f;
+			q1.normalize();
+
+			// Compute the impulse P=J^T * lambda of body 2
+			 Vector3f angularImpulseBody2 = -lambdaUpperLimit * mA1;
+
+			// Compute the pseudo velocity of body 2
+			 Vector3f w2 = this.i2 * angularImpulseBody2;
+
+			// Update the body position/orientation of body 2
+			q2 += Quaternion(0, w2) * q2 * 0.5f;
+			q2.normalize();
+		}
+	}
+}
+
+
+// Enable/Disable the limits of the joint
+/**
+ * @param isLimitEnabled True if you want to enable the limits of the joint and
+ *					   false otherwise
+ */
+void HingeJoint::enableLimit(boolean isLimitEnabled) {
+
+	if (isLimitEnabled != this.isLimitEnabled) {
+
+		this.isLimitEnabled = isLimitEnabled;
+
+		// Reset the limits
+		resetLimits();
+	}
+}
+
+// Enable/Disable the motor of the joint
+/**
+ * @param isMotorEnabled True if you want to enable the motor of the joint and
+ *					   false otherwise
+ */
+void HingeJoint::enableMotor(boolean isMotorEnabled) {
+
+	this.isMotorEnabled = isMotorEnabled;
+	this.impulseMotor = 0.0;
+
+	// Wake up the two bodies of the joint
+	this.body1.setIsSleeping(false);
+	this.body2.setIsSleeping(false);
+}
+
+// Set the minimum angle limit
+/**
+ * @param lowerLimit The minimum limit angle of the joint (in radian)
+ */
+void HingeJoint::setMinAngleLimit(float lowerLimit) {
+
+	assert(this.lowerLimit <= 0 && this.lowerLimit >= -2.0 * PI);
+
+	if (lowerLimit != this.lowerLimit) {
+
+		this.lowerLimit = lowerLimit;
+
+		// Reset the limits
+		resetLimits();
+	}
+}
+
+// Set the maximum angle limit
+/**
+ * @param upperLimit The maximum limit angle of the joint (in radian)
+ */
+void HingeJoint::setMaxAngleLimit(float upperLimit) {
+
+	assert(upperLimit >= 0 && upperLimit <= 2.0 * PI);
+
+	if (upperLimit != this.upperLimit) {
+
+		this.upperLimit = upperLimit;
+
+		// Reset the limits
+		resetLimits();
+	}
+}
+
+// Reset the limits
+void HingeJoint::resetLimits() {
+
+	// Reset the accumulated impulses for the limits
+	this.impulseLowerLimit = 0.0;
+	this.impulseUpperLimit = 0.0;
+
+	// Wake up the two bodies of the joint
+	this.body1.setIsSleeping(false);
+	this.body2.setIsSleeping(false);
+}
+
+// Set the motor speed
+void HingeJoint::setMotorSpeed(float motorSpeed) {
+
+	if (motorSpeed != this.motorSpeed) {
+
+		this.motorSpeed = motorSpeed;
+
+		// Wake up the two bodies of the joint
+		this.body1.setIsSleeping(false);
+		this.body2.setIsSleeping(false);
+	}
+}
+
+// Set the maximum motor torque
+/**
+ * @param maxMotorTorque The maximum torque (in Newtons) of the joint motor
+ */
+void HingeJoint::setMaxMotorTorque(float maxMotorTorque) {
+
+	if (maxMotorTorque != this.maxMotorTorque) {
+
+		assert(this.maxMotorTorque >= 0.0);
+		this.maxMotorTorque = maxMotorTorque;
+
+		// Wake up the two bodies of the joint
+		this.body1.setIsSleeping(false);
+		this.body2.setIsSleeping(false);
+	}
+}
+
+// Given an angle in radian, this method returns the corresponding angle in the range [-pi; pi]
+float HingeJoint::computeNormalizedAngle(float angle)  {
+
+	// Convert it into the range [-2*pi; 2*pi]
+	angle = fmod(angle, PITIMES2);
+
+	// Convert it into the range [-pi; pi]
+	if (angle < -PI) {
+		return angle + PITIMES2;
+	}
+	else if (angle > PI) {
+		return angle - PITIMES2;
+	}
+	else {
+		return angle;
+	}
+}
+
+// Given an "inputAngle" in the range [-pi, pi], this method returns an
+// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the
+// two angle limits in arguments.
+float HingeJoint::computeCorrespondingAngleNearLimits(float inputAngle, float lowerLimitAngle,
+														float upperLimitAngle)  {
+	if (upperLimitAngle <= lowerLimitAngle) {
+		return inputAngle;
+	}
+	else if (inputAngle > upperLimitAngle) {
+		float diffToUpperLimit = fabs(computeNormalizedAngle(inputAngle - upperLimitAngle));
+		float diffToLowerLimit = fabs(computeNormalizedAngle(inputAngle - lowerLimitAngle));
+		return (diffToUpperLimit > diffToLowerLimit) ? (inputAngle - PITIMES2) : inputAngle;
+	}
+	else if (inputAngle < lowerLimitAngle) {
+		float diffToUpperLimit = fabs(computeNormalizedAngle(upperLimitAngle - inputAngle));
+		float diffToLowerLimit = fabs(computeNormalizedAngle(lowerLimitAngle - inputAngle));
+		return (diffToUpperLimit > diffToLowerLimit) ? inputAngle : (inputAngle + PITIMES2);
+	}
+	else {
+		return inputAngle;
+	}
+}
+
+// Compute the current angle around the hinge axis
+float HingeJoint::computeCurrentHingeAngle( Quaternion orientationBody1,
+											  Quaternion orientationBody2) {
+
+	float hingeAngle;
+
+	// Compute the current orientation difference between the two bodies
+	Quaternion currentOrientationDiff = orientationBody2 * orientationBody1.getInverse();
+	currentOrientationDiff.normalize();
+
+	// Compute the relative rotation considering the initial orientation difference
+	Quaternion relativeRotation = currentOrientationDiff * this.initOrientationDifferenceInv;
+	relativeRotation.normalize();
+
+	// A quaternion q = [cos(theta/2); sin(theta/2) * rotAxis] where rotAxis is a unit
+	// length vector. We can extract cos(theta/2) with q.w and we can extract |sin(theta/2)| with :
+	// |sin(theta/2)| = q.getVectorV().length() since rotAxis is unit length. Note that any
+	// rotation can be represented by a quaternion q and -q. Therefore, if the relative rotation
+	// axis is not pointing in the same direction as the hinge axis, we use the rotation -q which
+	// has the same |sin(theta/2)| value but the value cos(theta/2) is sign inverted. Some details
+	// about this trick is explained in the source code of OpenTissue (http://www.opentissue.org).
+	float cosHalfAngle = relativeRotation.w();
+	float sinHalfAngleAbs = relativeRotation.getVectorV().length();
+
+	// Compute the dot product of the relative rotation axis and the hinge axis
+	float dotProduct = relativeRotation.getVectorV().dot(mA1);
+
+	// If the relative rotation axis and the hinge axis are pointing the same direction
+	if (dotProduct >= 0.0f) {
+		hingeAngle = float(2.0) * atan2(sinHalfAngleAbs, cosHalfAngle);
+	}
+	else {
+		hingeAngle = float(2.0) * atan2(sinHalfAngleAbs, -cosHalfAngle);
+	}
+
+	// Convert the angle from range [-2*pi; 2*pi] into the range [-pi; pi]
+	hingeAngle = computeNormalizedAngle(hingeAngle);
+
+	// Compute and return the corresponding angle near one the two limits
+	return computeCorrespondingAngleNearLimits(hingeAngle, this.lowerLimit, this.upperLimit);
+}
+
+
+// Return true if the limits of the joint are enabled
+/**
+ * @return True if the limits of the joint are enabled and false otherwise
+ */
+boolean HingeJoint::isLimitEnabled()  {
+	return this.isLimitEnabled;
+}
+
+// Return true if the motor of the joint is enabled
+/**
+ * @return True if the motor of joint is enabled and false otherwise
+ */
+boolean HingeJoint::isMotorEnabled()  {
+	return this.isMotorEnabled;
+}
+
+// Return the minimum angle limit
+/**
+ * @return The minimum limit angle of the joint (in radian)
+ */
+float HingeJoint::getMinAngleLimit()  {
+	return this.lowerLimit;
+}
+
+// Return the maximum angle limit
+/**
+ * @return The maximum limit angle of the joint (in radian)
+ */
+float HingeJoint::getMaxAngleLimit()  {
+	return this.upperLimit;
+}
+
+// Return the motor speed
+/**
+ * @return The current speed of the joint motor (in radian per second)
+ */
+float HingeJoint::getMotorSpeed()  {
+	return this.motorSpeed;
+}
+
+// Return the maximum motor torque
+/**
+ * @return The maximum torque of the joint motor (in Newtons)
+ */
+float HingeJoint::getMaxMotorTorque()  {
+	return this.maxMotorTorque;
+}
+
+// Return the intensity of the current torque applied for the joint motor
+/**
+ * @param timeStep The current time step (in seconds)
+ * @return The intensity of the current torque (in Newtons) of the joint motor
+ */
+float HingeJoint::getMotorTorque(float timeStep)  {
+	return this.impulseMotor / timeStep;
+}
+
+// Return the number of bytes used by the joint
+long HingeJoint::getSizeInBytes()  {
+	return sizeof(HingeJoint);
+}
+
+

src/org/atriaSoft/ephysics/constraint/HingeJoint.java

@@ -0,0 +1,188 @@
+package org.atriaSoft.ephysics.constraint;
+
+	/**
+	 * @brief It is used to gather the information needed to create a hinge joint.
+	 * This structure will be used to create the actual hinge joint.
+	 */
+	struct HingeJointInfo extends  JointInfo {
+		public :
+			Vector3f anchorPointWorldSpace; //!< Anchor point (in world-space coordinates)
+			Vector3f rotationAxisWorld; //!< Hinge rotation axis (in world-space coordinates)
+			boolean isLimitEnabled; //!< True if the hinge joint limits are enabled
+			boolean isMotorEnabled; //!< True if the hinge joint motor is enabled
+			float minAngleLimit; //!< Minimum allowed rotation angle (in radian) if limits are enabled. The angle must be in the range [-2*pi, 0]
+			float maxAngleLimit; //!< Maximum allowed rotation angle (in radian) if limits are enabled. The angle must be in the range [0, 2*pi]
+			float motorSpeed; //!< Motor speed (in radian/second)
+			float maxMotorTorque; //!< Maximum motor torque (in Newtons * meters) that can be applied to reach to desired motor speed
+			/**
+			 * @brief Constructor without limits and without motor
+			 * @param[in] rigidBody1 The first body of the joint
+			 * @param[in] rigidBody2 The second body of the joint
+			 * @param[in] initAnchorPointWorldSpace The initial anchor point in world-space coordinates
+			 * @param[in] initRotationAxisWorld The initial rotation axis in world-space coordinates
+			 */
+			HingeJointInfo(RigidBody* rigidBody1,
+			               RigidBody* rigidBody2,
+			                Vector3f initAnchorPointWorldSpace,
+			                Vector3f initRotationAxisWorld):
+			  JointInfo(rigidBody1, rigidBody2, HINGEJOINT),
+			  this.anchorPointWorldSpace(initAnchorPointWorldSpace),
+			  rotationAxisWorld(initRotationAxisWorld),
+			  isLimitEnabled(false),
+			  isMotorEnabled(false),
+			  minAngleLimit(-1),
+			  maxAngleLimit(1),
+			  motorSpeed(0),
+			  maxMotorTorque(0) {
+				
+			}
+			/**
+			 * @brief Constructor with limits but without motor
+			 * @param[in] rigidBody1 The first body of the joint
+			 * @param[in] rigidBody2 The second body of the joint
+			 * @param[in] initAnchorPointWorldSpace The initial anchor point in world-space coordinates
+			 * @param[in] initRotationAxisWorld The intial rotation axis in world-space coordinates
+			 * @param[in] initMinAngleLimit The initial minimum limit angle (in radian)
+			 * @param[in] initMaxAngleLimit The initial maximum limit angle (in radian)
+			 */
+			HingeJointInfo(RigidBody* rigidBody1,
+			               RigidBody* rigidBody2,
+			                Vector3f initAnchorPointWorldSpace,
+			                Vector3f initRotationAxisWorld,
+			               float initMinAngleLimit,
+			               float initMaxAngleLimit):
+			  JointInfo(rigidBody1, rigidBody2, HINGEJOINT),
+			  this.anchorPointWorldSpace(initAnchorPointWorldSpace),
+			  rotationAxisWorld(initRotationAxisWorld),
+			  isLimitEnabled(true),
+			  isMotorEnabled(false),
+			  minAngleLimit(initMinAngleLimit),
+			  maxAngleLimit(initMaxAngleLimit),
+			  motorSpeed(0),
+			  maxMotorTorque(0) {
+				
+			}
+			/**
+			 * @brief Constructor with limits and motor
+			 * @param[in] rigidBody1 The first body of the joint
+			 * @param[in] rigidBody2 The second body of the joint
+			 * @param[in] initAnchorPointWorldSpace The initial anchor point in world-space
+			 * @param[in] initRotationAxisWorld The initial rotation axis in world-space
+			 * @param[in] initMinAngleLimit The initial minimum limit angle (in radian)
+			 * @param[in] initMaxAngleLimit The initial maximum limit angle (in radian)
+			 * @param[in] initMotorSpeed The initial motor speed of the joint (in radian per second)
+			 * @param[in] initMaxMotorTorque The initial maximum motor torque (in Newtons)
+			 */
+			HingeJointInfo(RigidBody* rigidBody1,
+			               RigidBody* rigidBody2,
+			                Vector3f initAnchorPointWorldSpace,
+			                Vector3f initRotationAxisWorld,
+			               float initMinAngleLimit,
+			               float initMaxAngleLimit,
+			               float initMotorSpeed,
+			               float initMaxMotorTorque):
+			  JointInfo(rigidBody1, rigidBody2, HINGEJOINT),
+			  this.anchorPointWorldSpace(initAnchorPointWorldSpace),
+			  rotationAxisWorld(initRotationAxisWorld),
+			  isLimitEnabled(true),
+			  isMotorEnabled(false),
+			  minAngleLimit(initMinAngleLimit),
+			  maxAngleLimit(initMaxAngleLimit),
+			  motorSpeed(initMotorSpeed),
+			  maxMotorTorque(initMaxMotorTorque) {
+			
+			}
+	};
+
+	/**
+	 * @brief It represents a hinge joint that allows arbitrary rotation
+	 * between two bodies around a single axis. This joint has one degree of freedom. It
+	 * can be useful to simulate doors or pendulumns.
+	 */
+	class HingeJoint extends  Joint {
+		private :
+			static float BETA; //!< Beta value for the bias factor of position correction
+			Vector3f localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1)
+			Vector3f localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2)
+			Vector3f hingeLocalAxisBody1; //!< Hinge rotation axis (in local-space coordinates of body 1)
+			Vector3f hingeLocalAxisBody2; //!< Hinge rotation axis (in local-space coordiantes of body 2)
+			Matrix3f i1; //!< Inertia tensor of body 1 (in world-space coordinates)
+			Matrix3f i2; //!< Inertia tensor of body 2 (in world-space coordinates)
+			Vector3f mA1; //!< Hinge rotation axis (in world-space coordinates) computed from body 1
+			Vector3f r1World; //!< Vector from center of body 2 to anchor point in world-space
+			Vector3f r2World; //!< Vector from center of body 2 to anchor point in world-space
+			Vector3f b2CrossA1; //!< Cross product of vector b2 and a1
+			Vector3f c2CrossA1; //!< Cross product of vector c2 and a1;
+			Vector3f impulseTranslation; //!< Impulse for the 3 translation raints
+			vec2 impulseRotation; //!< Impulse for the 2 rotation raints
+			float impulseLowerLimit; //!< Accumulated impulse for the lower limit raint
+			float impulseUpperLimit; //!< Accumulated impulse for the upper limit raint
+			float impulseMotor; //!< Accumulated impulse for the motor raint;
+			Matrix3f inverseMassMatrixTranslation; //!< Inverse mass matrix K=JM^-1J^t for the 3 translation raints
+			Matrix2x2 inverseMassMatrixRotation; //!< Inverse mass matrix K=JM^-1J^t for the 2 rotation raints
+			float inverseMassMatrixLimitMotor; //!< Inverse of mass matrix K=JM^-1J^t for the limits and motor raints (1x1 matrix)
+			float inverseMassMatrixMotor; //!< Inverse of mass matrix K=JM^-1J^t for the motor
+			Vector3f bTranslation; //!< Bias vector for the error correction for the translation raints
+			vec2 bRotation; //!< Bias vector for the error correction for the rotation raints
+			float bLowerLimit; //!< Bias of the lower limit raint
+			float bUpperLimit; //!< Bias of the upper limit raint
+			Quaternion initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the bodies
+			boolean isLimitEnabled; //!< True if the joint limits are enabled
+			boolean isMotorEnabled; //!< True if the motor of the joint in enabled
+			float lowerLimit; //!< Lower limit (minimum allowed rotation angle in radian)
+			float upperLimit; //!< Upper limit (maximum translation distance)
+			boolean isLowerLimitViolated; //!< True if the lower limit is violated
+			boolean isUpperLimitViolated; //!< True if the upper limit is violated
+			float motorSpeed; //!< Motor speed (in rad/s)
+			float maxMotorTorque; //!< Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed
+			/// Reset the limits
+			void resetLimits();
+			/// Given an angle in radian, this method returns the corresponding
+			/// angle in the range [-pi; pi]
+			float computeNormalizedAngle(float angle) ;
+			/// Given an "inputAngle" in the range [-pi, pi], this method returns an
+			/// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the
+			/// two angle limits in arguments.
+			float computeCorrespondingAngleNearLimits(float inputAngle,
+			                                          float lowerLimitAngle,
+			                                          float upperLimitAngle) ;
+			/// Compute the current angle around the hinge axis
+			float computeCurrentHingeAngle( Quaternion orientationBody1,  Quaternion orientationBody2);
+			long getSizeInBytes()  ;
+			void initBeforeSolve( ConstraintSolverData raintSolverData) ;
+			void warmstart( ConstraintSolverData raintSolverData) ;
+			void solveVelocityConstraint( ConstraintSolverData raintSolverData) ;
+			void solvePositionConstraint( ConstraintSolverData raintSolverData) ;
+		public :
+			/// Constructor
+			HingeJoint( HingeJointInfo jointInfo);
+			/// Return true if the limits or the joint are enabled
+			boolean isLimitEnabled() ;
+			/// Return true if the motor of the joint is enabled
+			boolean isMotorEnabled() ;
+			/// Enable/Disable the limits of the joint
+			void enableLimit(boolean isLimitEnabled);
+			/// Enable/Disable the motor of the joint
+			void enableMotor(boolean isMotorEnabled);
+			/// Return the minimum angle limit
+			float getMinAngleLimit() ;
+			/// Set the minimum angle limit
+			void setMinAngleLimit(float lowerLimit);
+			/// Return the maximum angle limit
+			float getMaxAngleLimit() ;
+			/// Set the maximum angle limit
+			void setMaxAngleLimit(float upperLimit);
+			/// Return the motor speed
+			float getMotorSpeed() ;
+			/// Set the motor speed
+			void setMotorSpeed(float motorSpeed);
+			/// Return the maximum motor torque
+			float getMaxMotorTorque() ;
+			/// Set the maximum motor torque
+			void setMaxMotorTorque(float maxMotorTorque);
+			/// Return the intensity of the current torque applied for the joint motor
+			float getMotorTorque(float timeStep) ;
+	};
+
+}
+

src/org/atriaSoft/ephysics/constraint/Joint.cpp

@@ -0,0 +1,71 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/raint/Joint.hpp>
+
+using namespace ephysics;
+
+Joint::Joint( JointInfo jointInfo)
+		   :this.body1(jointInfo.body1), this.body2(jointInfo.body2), this.type(jointInfo.type),
+			this.positionCorrectionTechnique(jointInfo.positionCorrectionTechnique),
+			this.isCollisionEnabled(jointInfo.isCollisionEnabled), this.isAlreadyInIsland(false) {
+
+	assert(this.body1 != null);
+	assert(this.body2 != null);
+}
+
+Joint::~Joint() {
+
+}
+
+// Return the reference to the body 1
+/**
+ * @return The first body involved in the joint
+ */
+RigidBody* Joint::getBody1()  {
+	return this.body1;
+}
+
+// Return the reference to the body 2
+/**
+ * @return The second body involved in the joint
+ */
+RigidBody* Joint::getBody2()  {
+	return this.body2;
+}
+
+// Return true if the joint is active
+/**
+ * @return True if the joint is active
+ */
+boolean Joint::isActive()  {
+	return (this.body1.isActive() && this.body2.isActive());
+}
+
+// Return the type of the joint
+/**
+ * @return The type of the joint
+ */
+JointType Joint::getType()  {
+	return this.type;
+}
+
+// Return true if the collision between the two bodies of the joint is enabled
+/**
+ * @return True if the collision is enabled between the two bodies of the joint
+ *			  is enabled and false otherwise
+ */
+boolean Joint::isCollisionEnabled()  {
+	return this.isCollisionEnabled;
+}
+
+// Return true if the joint has already been added into an island
+boolean Joint::isAlreadyInIsland()  {
+	return this.isAlreadyInIsland;
+}
+

src/org/atriaSoft/ephysics/constraint/Joint.java

@@ -0,0 +1,110 @@
+package org.atriaSoft.ephysics.constraint;
+
+	/// Enumeration for the type of a raint
+	enum JointType {BALLSOCKETJOINT, SLIDERJOINT, HINGEJOINT, FIXEDJOINT};
+	
+	struct ConstraintSolverData;
+	class Joint;
+	
+	/**
+	 * @brief It represents a single element of a linked list of joints
+	 */
+	struct JointListElement {
+		public:
+			Joint* joint; //!< Pointer to the actual joint
+			JointListElement* next; //!< Next element of the list
+			/**
+			 * @breif Constructor
+			 */
+			JointListElement(Joint* initJoint,
+			                 JointListElement* initNext):
+			  joint(initJoint),
+			  next(initNext) {
+				
+			}
+	};
+	
+	/**
+	 * @brief It is used to gather the information needed to create a joint.
+	 */
+	struct JointInfo {
+		public :
+			RigidBody* body1; //!< First rigid body of the joint
+			RigidBody* body2; //!< Second rigid body of the joint
+			JointType type; //!< Type of the joint
+			JointsPositionCorrectionTechnique positionCorrectionTechnique; //!< Position correction technique used for the raint (used for joints). By default, the BAUMGARTE technique is used
+			boolean isCollisionEnabled; //!< True if the two bodies of the joint are allowed to collide with each other
+			/// Constructor
+			JointInfo(JointType raintType):
+			  body1(null),
+			  body2(null),
+			  type(raintType),
+			  positionCorrectionTechnique(NONLINEARGAUSSSEIDEL),
+			  isCollisionEnabled(true) {
+				
+			}
+			/// Constructor
+			JointInfo(RigidBody* rigidBody1,
+			          RigidBody* rigidBody2,
+			          JointType raintType):
+			  body1(rigidBody1),
+			  body2(rigidBody2),
+			  type(raintType),
+			  positionCorrectionTechnique(NONLINEARGAUSSSEIDEL),
+			  isCollisionEnabled(true) {
+				
+			}
+			/// Destructor
+			 ~JointInfo() = default;
+	};
+	
+	/**
+	 * @brief It represents a joint between two bodies.
+	 */
+	class Joint {
+		protected :
+			RigidBody* body1; //!< Pointer to the first body of the joint
+			RigidBody* body2; //!< Pointer to the second body of the joint
+			 JointType type; //!< Type of the joint
+			int indexBody1; //!< Body 1 index in the velocity array to solve the raint
+			int indexBody2; //!< Body 2 index in the velocity array to solve the raint
+			JointsPositionCorrectionTechnique positionCorrectionTechnique; //!< Position correction technique used for the raint (used for joints)
+			boolean isCollisionEnabled; //!< True if the two bodies of the raint are allowed to collide with each other
+			boolean isAlreadyInIsland; //!< True if the joint has already been added into an island
+			/// Private copy-ructor
+			Joint( Joint raint);
+			/// Private assignment operator
+			Joint operator=( Joint raint);
+			/// Return true if the joint has already been added into an island
+			boolean isAlreadyInIsland() ;
+			/// Return the number of bytes used by the joint
+			 long getSizeInBytes()  = 0;
+			/// Initialize before solving the joint
+			 void initBeforeSolve( ConstraintSolverData raintSolverData) = 0;
+			/// Warm start the joint (apply the previous impulse at the beginning of the step)
+			 void warmstart( ConstraintSolverData raintSolverData) = 0;
+			/// Solve the velocity raint
+			 void solveVelocityConstraint( ConstraintSolverData raintSolverData) = 0;
+			/// Solve the position raint
+			 void solvePositionConstraint( ConstraintSolverData raintSolverData) = 0;
+		public :
+			/// Constructor
+			Joint( JointInfo jointInfo);
+			/// Destructor
+			 ~Joint();
+			/// Return the reference to the body 1
+			RigidBody* getBody1() ;
+			/// Return the reference to the body 2
+			RigidBody* getBody2() ;
+			/// Return true if the raint is active
+			boolean isActive() ;
+			/// Return the type of the raint
+			JointType getType() ;
+			/// Return true if the collision between the two bodies of the joint is enabled
+			boolean isCollisionEnabled() ;
+			friend class DynamicsWorld;
+			friend class Island;
+			friend class ConstraintSolver;
+	};
+
+}

src/org/atriaSoft/ephysics/constraint/SliderJoint.cpp

@@ -0,0 +1,840 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/raint/SliderJoint.hpp>
+
+using namespace ephysics;
+
+// Static variables definition
+ float SliderJoint::BETA = float(0.2);
+
+// Constructor
+SliderJoint::SliderJoint( SliderJointInfo jointInfo)
+			: Joint(jointInfo), this.impulseTranslation(0, 0), this.impulseRotation(0, 0, 0),
+			  this.impulseLowerLimit(0), this.impulseUpperLimit(0), this.impulseMotor(0),
+			  this.isLimitEnabled(jointInfo.isLimitEnabled), this.isMotorEnabled(jointInfo.isMotorEnabled),
+			  this.lowerLimit(jointInfo.minTranslationLimit),
+			  this.upperLimit(jointInfo.maxTranslationLimit), this.isLowerLimitViolated(false),
+			  this.isUpperLimitViolated(false), this.motorSpeed(jointInfo.motorSpeed),
+			  this.maxMotorForce(jointInfo.maxMotorForce){
+
+	assert(this.upperLimit >= 0.0);
+	assert(this.lowerLimit <= 0.0);
+	assert(this.maxMotorForce >= 0.0);
+
+	// Compute the local-space anchor point for each body
+	 Transform3D transform1 = this.body1.getTransform();
+	 Transform3D transform2 = this.body2.getTransform();
+	this.localAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace;
+	this.localAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace;
+
+	// Compute the inverse of the initial orientation difference between the two bodies
+	this.initOrientationDifferenceInv = transform2.getOrientation() *
+								 transform1.getOrientation().getInverse();
+	this.initOrientationDifferenceInv.normalize();
+	this.initOrientationDifferenceInv.inverse();
+
+	// Compute the slider axis in local-space of body 1
+	this.sliderAxisBody1 = this.body1.getTransform().getOrientation().getInverse() *
+					   jointInfo.sliderAxisWorldSpace;
+	this.sliderAxisBody1.normalize();
+}
+
+// Destructor
+SliderJoint::~SliderJoint() {
+
+}
+
+// Initialize before solving the raint
+void SliderJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
+
+	// Initialize the bodies index in the veloc ity array
+	this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1).second;
+	this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2).second;
+
+	// Get the bodies positions and orientations
+	 Vector3f x1 = this.body1.this.centerOfMassWorld;
+	 Vector3f x2 = this.body2.this.centerOfMassWorld;
+	 Quaternion orientationBody1 = this.body1.getTransform().getOrientation();
+	 Quaternion orientationBody2 = this.body2.getTransform().getOrientation();
+
+	// Get the inertia tensor of bodies
+	this.i1 = this.body1.getInertiaTensorInverseWorld();
+	this.i2 = this.body2.getInertiaTensorInverseWorld();
+
+	// Vector from body center to the anchor point
+	this.R1 = orientationBody1 * this.localAnchorPointBody1;
+	this.R2 = orientationBody2 * this.localAnchorPointBody2;
+
+	// Compute the vector u (difference between anchor points)
+	 Vector3f u = x2 + this.R2 - x1 - this.R1;
+
+	// Compute the two orthogonal vectors to the slider axis in world-space
+	this.sliderAxisWorld = orientationBody1 * this.sliderAxisBody1;
+	this.sliderAxisWorld.normalize();
+	this.N1 = this.sliderAxisWorld.getOrthoVector();
+	this.N2 = this.sliderAxisWorld.cross(this.N1);
+
+	// Check if the limit raints are violated or not
+	float uDotSliderAxis = u.dot(this.sliderAxisWorld);
+	float lowerLimitError = uDotSliderAxis - this.lowerLimit;
+	float upperLimitError = this.upperLimit - uDotSliderAxis;
+	boolean oldIsLowerLimitViolated = this.isLowerLimitViolated;
+	this.isLowerLimitViolated = lowerLimitError <= 0;
+	if (this.isLowerLimitViolated != oldIsLowerLimitViolated) {
+		this.impulseLowerLimit = 0.0;
+	}
+	boolean oldIsUpperLimitViolated = this.isUpperLimitViolated;
+	this.isUpperLimitViolated = upperLimitError <= 0;
+	if (this.isUpperLimitViolated != oldIsUpperLimitViolated) {
+		this.impulseUpperLimit = 0.0;
+	}
+
+	// Compute the cross products used in the Jacobians
+	this.R2CrossN1 = this.R2.cross(this.N1);
+	this.R2CrossN2 = this.R2.cross(this.N2);
+	this.R2CrossSliderAxis = this.R2.cross(this.sliderAxisWorld);
+	 Vector3f r1PlusU = this.R1 + u;
+	this.R1PlusUCrossN1 = (r1PlusU).cross(this.N1);
+	this.R1PlusUCrossN2 = (r1PlusU).cross(this.N2);
+	this.R1PlusUCrossSliderAxis = (r1PlusU).cross(this.sliderAxisWorld);
+
+	// Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
+	// raints (2x2 matrix)
+	float sumInverseMass = this.body1.this.massInverse + this.body2.this.massInverse;
+	Vector3f I1R1PlusUCrossN1 = this.i1 * this.R1PlusUCrossN1;
+	Vector3f I1R1PlusUCrossN2 = this.i1 * this.R1PlusUCrossN2;
+	Vector3f I2R2CrossN1 = this.i2 * this.R2CrossN1;
+	Vector3f I2R2CrossN2 = this.i2 * this.R2CrossN2;
+	 float el11 = sumInverseMass + this.R1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
+						 this.R2CrossN1.dot(I2R2CrossN1);
+	 float el12 = this.R1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
+						 this.R2CrossN1.dot(I2R2CrossN2);
+	 float el21 = this.R1PlusUCrossN2.dot(I1R1PlusUCrossN1) +
+						 this.R2CrossN2.dot(I2R2CrossN1);
+	 float el22 = sumInverseMass + this.R1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
+						 this.R2CrossN2.dot(I2R2CrossN2);
+	Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
+	this.inverseMassMatrixTranslationConstraint.setZero();
+	if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
+		this.inverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse();
+	}
+
+	// Compute the bias "b" of the translation raint
+	this.bTranslation.setZero();
+	float biasFactor = (BETA / raintSolverData.timeStep);
+	if (this.positionCorrectionTechnique == BAUMGARTEJOINTS) {
+		this.bTranslation.setX(u.dot(this.N1));
+		this.bTranslation.setY(u.dot(this.N2));
+		this.bTranslation *= biasFactor;
+	}
+
+	// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
+	// contraints (3x3 matrix)
+	this.inverseMassMatrixRotationConstraint = this.i1 + this.i2;
+	if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
+		this.inverseMassMatrixRotationConstraint = this.inverseMassMatrixRotationConstraint.getInverse();
+	}
+
+	// Compute the bias "b" of the rotation raint
+	this.bRotation.setZero();
+	if (this.positionCorrectionTechnique == BAUMGARTEJOINTS) {
+		Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
+		currentOrientationDifference.normalize();
+		 Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv;
+		this.bRotation = biasFactor * float(2.0) * qError.getVectorV();
+	}
+
+	// If the limits are enabled
+	if (this.isLimitEnabled && (this.isLowerLimitViolated || this.isUpperLimitViolated)) {
+
+		// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
+		this.inverseMassMatrixLimit = this.body1.this.massInverse + this.body2.this.massInverse +
+								  this.R1PlusUCrossSliderAxis.dot(this.i1 * this.R1PlusUCrossSliderAxis) +
+								  this.R2CrossSliderAxis.dot(this.i2 * this.R2CrossSliderAxis);
+		this.inverseMassMatrixLimit = (this.inverseMassMatrixLimit > 0.0) ?
+								  1.0f / this.inverseMassMatrixLimit : 0.0f;
+
+		// Compute the bias "b" of the lower limit raint
+		this.bLowerLimit = 0.0;
+		if (this.positionCorrectionTechnique == BAUMGARTEJOINTS) {
+			this.bLowerLimit = biasFactor * lowerLimitError;
+		}
+
+		// Compute the bias "b" of the upper limit raint
+		this.bUpperLimit = 0.0;
+		if (this.positionCorrectionTechnique == BAUMGARTEJOINTS) {
+			this.bUpperLimit = biasFactor * upperLimitError;
+		}
+	}
+
+	// If the motor is enabled
+	if (this.isMotorEnabled) {
+
+		// Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix)
+		this.inverseMassMatrixMotor = this.body1.this.massInverse + this.body2.this.massInverse;
+		this.inverseMassMatrixMotor = (this.inverseMassMatrixMotor > 0.0) ?
+					1.0f / this.inverseMassMatrixMotor : 0.0f;
+	}
+
+	// If warm-starting is not enabled
+	if (!raintSolverData.isWarmStartingActive) {
+
+		// Reset all the accumulated impulses
+		this.impulseTranslation.setZero();
+		this.impulseRotation.setZero();
+		this.impulseLowerLimit = 0.0;
+		this.impulseUpperLimit = 0.0;
+		this.impulseMotor = 0.0;
+	}
+}
+
+// Warm start the raint (apply the previous impulse at the beginning of the step)
+void SliderJoint::warmstart( ConstraintSolverData raintSolverData) {
+
+	// Get the velocities
+	Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+	Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+	Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+	Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+
+	// Get the inverse mass and inverse inertia tensors of the bodies
+	 float inverseMassBody1 = this.body1.this.massInverse;
+	 float inverseMassBody2 = this.body2.this.massInverse;
+
+	// Compute the impulse P=J^T * lambda for the lower and upper limits raints of body 1
+	float impulseLimits = this.impulseUpperLimit - this.impulseLowerLimit;
+	Vector3f linearImpulseLimits = impulseLimits * this.sliderAxisWorld;
+
+	// Compute the impulse P=J^T * lambda for the motor raint of body 1
+	Vector3f impulseMotor = this.impulseMotor * this.sliderAxisWorld;
+
+	// Compute the impulse P=J^T * lambda for the 2 translation raints of body 1
+	Vector3f linearImpulseBody1 = -this.N1 * this.impulseTranslation.x() - this.N2 * this.impulseTranslation.y();
+	Vector3f angularImpulseBody1 = -this.R1PlusUCrossN1 * this.impulseTranslation.x() -
+			this.R1PlusUCrossN2 * this.impulseTranslation.y();
+
+	// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
+	angularImpulseBody1 += -this.impulseRotation;
+
+	// Compute the impulse P=J^T * lambda for the lower and upper limits raints of body 1
+	linearImpulseBody1 += linearImpulseLimits;
+	angularImpulseBody1 += impulseLimits * this.R1PlusUCrossSliderAxis;
+
+	// Compute the impulse P=J^T * lambda for the motor raint of body 1
+	linearImpulseBody1 += impulseMotor;
+
+	// Apply the impulse to the body 1
+	v1 += inverseMassBody1 * linearImpulseBody1;
+	w1 += this.i1 * angularImpulseBody1;
+
+	// Compute the impulse P=J^T * lambda for the 2 translation raints of body 2
+	Vector3f linearImpulseBody2 = this.N1 * this.impulseTranslation.x() + this.N2 * this.impulseTranslation.y();
+	Vector3f angularImpulseBody2 = this.R2CrossN1 * this.impulseTranslation.x() +
+			this.R2CrossN2 * this.impulseTranslation.y();
+
+	// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 2
+	angularImpulseBody2 += this.impulseRotation;
+
+	// Compute the impulse P=J^T * lambda for the lower and upper limits raints of body 2
+	linearImpulseBody2 += -linearImpulseLimits;
+	angularImpulseBody2 += -impulseLimits * this.R2CrossSliderAxis;
+
+	// Compute the impulse P=J^T * lambda for the motor raint of body 2
+	linearImpulseBody2 += -impulseMotor;
+
+	// Apply the impulse to the body 2
+	v2 += inverseMassBody2 * linearImpulseBody2;
+	w2 += this.i2 * angularImpulseBody2;
+}
+
+// Solve the velocity raint
+void SliderJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) {
+
+	// Get the velocities
+	Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+	Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+	Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+	Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+
+	// Get the inverse mass and inverse inertia tensors of the bodies
+	float inverseMassBody1 = this.body1.this.massInverse;
+	float inverseMassBody2 = this.body2.this.massInverse;
+
+	// --------------- Translation Constraints --------------- //
+
+	// Compute J*v for the 2 translation raints
+	 float el1 = -this.N1.dot(v1) - w1.dot(this.R1PlusUCrossN1) +
+						 this.N1.dot(v2) + w2.dot(this.R2CrossN1);
+	 float el2 = -this.N2.dot(v1) - w1.dot(this.R1PlusUCrossN2) +
+						 this.N2.dot(v2) + w2.dot(this.R2CrossN2);
+	 vec2 JvTranslation(el1, el2);
+
+	// Compute the Lagrange multiplier lambda for the 2 translation raints
+	vec2 deltaLambda = this.inverseMassMatrixTranslationConstraint * (-JvTranslation -this.bTranslation);
+	this.impulseTranslation += deltaLambda;
+
+	// Compute the impulse P=J^T * lambda for the 2 translation raints of body 1
+	 Vector3f linearImpulseBody1 = -this.N1 * deltaLambda.x() - this.N2 * deltaLambda.y();
+	Vector3f angularImpulseBody1 = -this.R1PlusUCrossN1 * deltaLambda.x() -
+			this.R1PlusUCrossN2 * deltaLambda.y();
+
+	// Apply the impulse to the body 1
+	v1 += inverseMassBody1 * linearImpulseBody1;
+	w1 += this.i1 * angularImpulseBody1;
+
+	// Compute the impulse P=J^T * lambda for the 2 translation raints of body 2
+	 Vector3f linearImpulseBody2 = this.N1 * deltaLambda.x() + this.N2 * deltaLambda.y();
+	Vector3f angularImpulseBody2 = this.R2CrossN1 * deltaLambda.x() + this.R2CrossN2 * deltaLambda.y();
+
+	// Apply the impulse to the body 2
+	v2 += inverseMassBody2 * linearImpulseBody2;
+	w2 += this.i2 * angularImpulseBody2;
+
+	// --------------- Rotation Constraints --------------- //
+
+	// Compute J*v for the 3 rotation raints
+	 Vector3f JvRotation = w2 - w1;
+
+	// Compute the Lagrange multiplier lambda for the 3 rotation raints
+	Vector3f deltaLambda2 = this.inverseMassMatrixRotationConstraint * (-JvRotation - this.bRotation);
+	this.impulseRotation += deltaLambda2;
+
+	// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
+	angularImpulseBody1 = -deltaLambda2;
+
+	// Apply the impulse to the body to body 1
+	w1 += this.i1 * angularImpulseBody1;
+
+	// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 2
+	angularImpulseBody2 = deltaLambda2;
+
+	// Apply the impulse to the body 2
+	w2 += this.i2 * angularImpulseBody2;
+
+	// --------------- Limits Constraints --------------- //
+
+	if (this.isLimitEnabled) {
+
+		// If the lower limit is violated
+		if (this.isLowerLimitViolated) {
+
+			// Compute J*v for the lower limit raint
+			 float JvLowerLimit = this.sliderAxisWorld.dot(v2) + this.R2CrossSliderAxis.dot(w2) -
+										 this.sliderAxisWorld.dot(v1) - this.R1PlusUCrossSliderAxis.dot(w1);
+
+			// Compute the Lagrange multiplier lambda for the lower limit raint
+			float deltaLambdaLower = this.inverseMassMatrixLimit * (-JvLowerLimit -this.bLowerLimit);
+			float lambdaTemp = this.impulseLowerLimit;
+			this.impulseLowerLimit = max(this.impulseLowerLimit + deltaLambdaLower, 0.0f);
+			deltaLambdaLower = this.impulseLowerLimit - lambdaTemp;
+
+			// Compute the impulse P=J^T * lambda for the lower limit raint of body 1
+			 Vector3f linearImpulseBody1 = -deltaLambdaLower * this.sliderAxisWorld;
+			 Vector3f angularImpulseBody1 = -deltaLambdaLower * this.R1PlusUCrossSliderAxis;
+
+			// Apply the impulse to the body 1
+			v1 += inverseMassBody1 * linearImpulseBody1;
+			w1 += this.i1 * angularImpulseBody1;
+
+			// Compute the impulse P=J^T * lambda for the lower limit raint of body 2
+			 Vector3f linearImpulseBody2 = deltaLambdaLower * this.sliderAxisWorld;
+			 Vector3f angularImpulseBody2 = deltaLambdaLower * this.R2CrossSliderAxis;
+
+			// Apply the impulse to the body 2
+			v2 += inverseMassBody2 * linearImpulseBody2;
+			w2 += this.i2 * angularImpulseBody2;
+		}
+
+		// If the upper limit is violated
+		if (this.isUpperLimitViolated) {
+
+			// Compute J*v for the upper limit raint
+			 float JvUpperLimit = this.sliderAxisWorld.dot(v1) + this.R1PlusUCrossSliderAxis.dot(w1)
+										- this.sliderAxisWorld.dot(v2) - this.R2CrossSliderAxis.dot(w2);
+
+			// Compute the Lagrange multiplier lambda for the upper limit raint
+			float deltaLambdaUpper = this.inverseMassMatrixLimit * (-JvUpperLimit -this.bUpperLimit);
+			float lambdaTemp = this.impulseUpperLimit;
+			this.impulseUpperLimit = max(this.impulseUpperLimit + deltaLambdaUpper, 0.0f);
+			deltaLambdaUpper = this.impulseUpperLimit - lambdaTemp;
+
+			// Compute the impulse P=J^T * lambda for the upper limit raint of body 1
+			 Vector3f linearImpulseBody1 = deltaLambdaUpper * this.sliderAxisWorld;
+			 Vector3f angularImpulseBody1 = deltaLambdaUpper * this.R1PlusUCrossSliderAxis;
+
+			// Apply the impulse to the body 1
+			v1 += inverseMassBody1 * linearImpulseBody1;
+			w1 += this.i1 * angularImpulseBody1;
+
+			// Compute the impulse P=J^T * lambda for the upper limit raint of body 2
+			 Vector3f linearImpulseBody2 = -deltaLambdaUpper * this.sliderAxisWorld;
+			 Vector3f angularImpulseBody2 = -deltaLambdaUpper * this.R2CrossSliderAxis;
+
+			// Apply the impulse to the body 2
+			v2 += inverseMassBody2 * linearImpulseBody2;
+			w2 += this.i2 * angularImpulseBody2;
+		}
+	}
+
+	// --------------- Motor --------------- //
+
+	if (this.isMotorEnabled) {
+
+		// Compute J*v for the motor
+		 float JvMotor = this.sliderAxisWorld.dot(v1) - this.sliderAxisWorld.dot(v2);
+
+		// Compute the Lagrange multiplier lambda for the motor
+		 float maxMotorImpulse = this.maxMotorForce * raintSolverData.timeStep;
+		float deltaLambdaMotor = this.inverseMassMatrixMotor * (-JvMotor - this.motorSpeed);
+		float lambdaTemp = this.impulseMotor;
+		this.impulseMotor = clamp(this.impulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
+		deltaLambdaMotor = this.impulseMotor - lambdaTemp;
+
+		// Compute the impulse P=J^T * lambda for the motor of body 1
+		 Vector3f linearImpulseBody1 = deltaLambdaMotor * this.sliderAxisWorld;
+
+		// Apply the impulse to the body 1
+		v1 += inverseMassBody1 * linearImpulseBody1;
+
+		// Compute the impulse P=J^T * lambda for the motor of body 2
+		 Vector3f linearImpulseBody2 = -deltaLambdaMotor * this.sliderAxisWorld;
+
+		// Apply the impulse to the body 2
+		v2 += inverseMassBody2 * linearImpulseBody2;
+	}
+}
+
+// Solve the position raint (for position error correction)
+void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) {
+
+	// If the error position correction technique is not the non-linear-gauss-seidel, we do
+	// do not execute this method
+	if (this.positionCorrectionTechnique != NONLINEARGAUSSSEIDEL) return;
+
+	// Get the bodies positions and orientations
+	Vector3f x1 = raintSolverData.positions[this.indexBody1];
+	Vector3f x2 = raintSolverData.positions[this.indexBody2];
+	Quaternion q1 = raintSolverData.orientations[this.indexBody1];
+	Quaternion q2 = raintSolverData.orientations[this.indexBody2];
+
+	// Get the inverse mass and inverse inertia tensors of the bodies
+	float inverseMassBody1 = this.body1.this.massInverse;
+	float inverseMassBody2 = this.body2.this.massInverse;
+
+	// Recompute the inertia tensor of bodies
+	this.i1 = this.body1.getInertiaTensorInverseWorld();
+	this.i2 = this.body2.getInertiaTensorInverseWorld();
+
+	// Vector from body center to the anchor point
+	this.R1 = q1 * this.localAnchorPointBody1;
+	this.R2 = q2 * this.localAnchorPointBody2;
+
+	// Compute the vector u (difference between anchor points)
+	 Vector3f u = x2 + this.R2 - x1 - this.R1;
+
+	// Compute the two orthogonal vectors to the slider axis in world-space
+	this.sliderAxisWorld = q1 * this.sliderAxisBody1;
+	this.sliderAxisWorld.normalize();
+	this.N1 = this.sliderAxisWorld.getOrthoVector();
+	this.N2 = this.sliderAxisWorld.cross(this.N1);
+
+	// Check if the limit raints are violated or not
+	float uDotSliderAxis = u.dot(this.sliderAxisWorld);
+	float lowerLimitError = uDotSliderAxis - this.lowerLimit;
+	float upperLimitError = this.upperLimit - uDotSliderAxis;
+	this.isLowerLimitViolated = lowerLimitError <= 0;
+	this.isUpperLimitViolated = upperLimitError <= 0;
+
+	// Compute the cross products used in the Jacobians
+	this.R2CrossN1 = this.R2.cross(this.N1);
+	this.R2CrossN2 = this.R2.cross(this.N2);
+	this.R2CrossSliderAxis = this.R2.cross(this.sliderAxisWorld);
+	 Vector3f r1PlusU = this.R1 + u;
+	this.R1PlusUCrossN1 = (r1PlusU).cross(this.N1);
+	this.R1PlusUCrossN2 = (r1PlusU).cross(this.N2);
+	this.R1PlusUCrossSliderAxis = (r1PlusU).cross(this.sliderAxisWorld);
+
+	// --------------- Translation Constraints --------------- //
+
+	// Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
+	// raints (2x2 matrix)
+	float sumInverseMass = this.body1.this.massInverse + this.body2.this.massInverse;
+	Vector3f I1R1PlusUCrossN1 = this.i1 * this.R1PlusUCrossN1;
+	Vector3f I1R1PlusUCrossN2 = this.i1 * this.R1PlusUCrossN2;
+	Vector3f I2R2CrossN1 = this.i2 * this.R2CrossN1;
+	Vector3f I2R2CrossN2 = this.i2 * this.R2CrossN2;
+	 float el11 = sumInverseMass + this.R1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
+						 this.R2CrossN1.dot(I2R2CrossN1);
+	 float el12 = this.R1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
+						 this.R2CrossN1.dot(I2R2CrossN2);
+	 float el21 = this.R1PlusUCrossN2.dot(I1R1PlusUCrossN1) +
+						 this.R2CrossN2.dot(I2R2CrossN1);
+	 float el22 = sumInverseMass + this.R1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
+						 this.R2CrossN2.dot(I2R2CrossN2);
+	Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
+	this.inverseMassMatrixTranslationConstraint.setZero();
+	if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
+		this.inverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse();
+	}
+
+	// Compute the position error for the 2 translation raints
+	 vec2 translationError(u.dot(this.N1), u.dot(this.N2));
+
+	// Compute the Lagrange multiplier lambda for the 2 translation raints
+	vec2 lambdaTranslation = this.inverseMassMatrixTranslationConstraint * (-translationError);
+
+	// Compute the impulse P=J^T * lambda for the 2 translation raints of body 1
+	 Vector3f linearImpulseBody1 = -this.N1 * lambdaTranslation.x() - this.N2 * lambdaTranslation.y();
+	Vector3f angularImpulseBody1 = -this.R1PlusUCrossN1 * lambdaTranslation.x() -
+										this.R1PlusUCrossN2 * lambdaTranslation.y();
+
+	// Apply the impulse to the body 1
+	 Vector3f v1 = inverseMassBody1 * linearImpulseBody1;
+	Vector3f w1 = this.i1 * angularImpulseBody1;
+
+	// Update the body position/orientation of body 1
+	x1 += v1;
+	q1 += Quaternion(0, w1) * q1 * 0.5f;
+	q1.normalize();
+
+	// Compute the impulse P=J^T * lambda for the 2 translation raints of body 2
+	 Vector3f linearImpulseBody2 = this.N1 * lambdaTranslation.x() + this.N2 * lambdaTranslation.y();
+	Vector3f angularImpulseBody2 = this.R2CrossN1 * lambdaTranslation.x() +
+			this.R2CrossN2 * lambdaTranslation.y();
+
+	// Apply the impulse to the body 2
+	 Vector3f v2 = inverseMassBody2 * linearImpulseBody2;
+	Vector3f w2 = this.i2 * angularImpulseBody2;
+
+	// Update the body position/orientation of body 2
+	x2 += v2;
+	q2 += Quaternion(0, w2) * q2 * 0.5f;
+	q2.normalize();
+
+	// --------------- Rotation Constraints --------------- //
+
+	// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
+	// contraints (3x3 matrix)
+	this.inverseMassMatrixRotationConstraint = this.i1 + this.i2;
+	if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) {
+		this.inverseMassMatrixRotationConstraint = this.inverseMassMatrixRotationConstraint.getInverse();
+	}
+
+	// Compute the position error for the 3 rotation raints
+	Quaternion currentOrientationDifference = q2 * q1.getInverse();
+	currentOrientationDifference.normalize();
+	 Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv;
+	 Vector3f errorRotation = float(2.0) * qError.getVectorV();
+
+	// Compute the Lagrange multiplier lambda for the 3 rotation raints
+	Vector3f lambdaRotation = this.inverseMassMatrixRotationConstraint * (-errorRotation);
+
+	// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
+	angularImpulseBody1 = -lambdaRotation;
+
+	// Apply the impulse to the body 1
+	w1 = this.i1 * angularImpulseBody1;
+
+	// Update the body position/orientation of body 1
+	q1 += Quaternion(0, w1) * q1 * 0.5f;
+	q1.normalize();
+
+	// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 2
+	angularImpulseBody2 = lambdaRotation;
+
+	// Apply the impulse to the body 2
+	w2 = this.i2 * angularImpulseBody2;
+
+	// Update the body position/orientation of body 2
+	q2 += Quaternion(0, w2) * q2 * 0.5f;
+	q2.normalize();
+
+	// --------------- Limits Constraints --------------- //
+
+	if (this.isLimitEnabled) {
+
+		if (this.isLowerLimitViolated || this.isUpperLimitViolated) {
+
+			// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
+			this.inverseMassMatrixLimit = this.body1.this.massInverse + this.body2.this.massInverse +
+									this.R1PlusUCrossSliderAxis.dot(this.i1 * this.R1PlusUCrossSliderAxis) +
+									this.R2CrossSliderAxis.dot(this.i2 * this.R2CrossSliderAxis);
+			this.inverseMassMatrixLimit = (this.inverseMassMatrixLimit > 0.0) ?
+									  1.0f / this.inverseMassMatrixLimit : 0.0f;
+		}
+
+		// If the lower limit is violated
+		if (this.isLowerLimitViolated) {
+
+			// Compute the Lagrange multiplier lambda for the lower limit raint
+			float lambdaLowerLimit = this.inverseMassMatrixLimit * (-lowerLimitError);
+
+			// Compute the impulse P=J^T * lambda for the lower limit raint of body 1
+			 Vector3f linearImpulseBody1 = -lambdaLowerLimit * this.sliderAxisWorld;
+			 Vector3f angularImpulseBody1 = -lambdaLowerLimit * this.R1PlusUCrossSliderAxis;
+
+			// Apply the impulse to the body 1
+			 Vector3f v1 = inverseMassBody1 * linearImpulseBody1;
+			 Vector3f w1 = this.i1 * angularImpulseBody1;
+
+			// Update the body position/orientation of body 1
+			x1 += v1;
+			q1 += Quaternion(0, w1) * q1 * 0.5f;
+			q1.normalize();
+
+			// Compute the impulse P=J^T * lambda for the lower limit raint of body 2
+			 Vector3f linearImpulseBody2 = lambdaLowerLimit * this.sliderAxisWorld;
+			 Vector3f angularImpulseBody2 = lambdaLowerLimit * this.R2CrossSliderAxis;
+
+			// Apply the impulse to the body 2
+			 Vector3f v2 = inverseMassBody2 * linearImpulseBody2;
+			 Vector3f w2 = this.i2 * angularImpulseBody2;
+
+			// Update the body position/orientation of body 2
+			x2 += v2;
+			q2 += Quaternion(0, w2) * q2 * 0.5f;
+			q2.normalize();
+		}
+
+		// If the upper limit is violated
+		if (this.isUpperLimitViolated) {
+
+			// Compute the Lagrange multiplier lambda for the upper limit raint
+			float lambdaUpperLimit = this.inverseMassMatrixLimit * (-upperLimitError);
+
+			// Compute the impulse P=J^T * lambda for the upper limit raint of body 1
+			 Vector3f linearImpulseBody1 = lambdaUpperLimit * this.sliderAxisWorld;
+			 Vector3f angularImpulseBody1 = lambdaUpperLimit * this.R1PlusUCrossSliderAxis;
+
+			// Apply the impulse to the body 1
+			 Vector3f v1 = inverseMassBody1 * linearImpulseBody1;
+			 Vector3f w1 = this.i1 * angularImpulseBody1;
+
+			// Update the body position/orientation of body 1
+			x1 += v1;
+			q1 += Quaternion(0, w1) * q1 * 0.5f;
+			q1.normalize();
+
+			// Compute the impulse P=J^T * lambda for the upper limit raint of body 2
+			 Vector3f linearImpulseBody2 = -lambdaUpperLimit * this.sliderAxisWorld;
+			 Vector3f angularImpulseBody2 = -lambdaUpperLimit * this.R2CrossSliderAxis;
+
+			// Apply the impulse to the body 2
+			 Vector3f v2 = inverseMassBody2 * linearImpulseBody2;
+			 Vector3f w2 = this.i2 * angularImpulseBody2;
+
+			// Update the body position/orientation of body 2
+			x2 += v2;
+			q2 += Quaternion(0, w2) * q2 * 0.5f;
+			q2.normalize();
+		}
+	}
+}
+
+// Enable/Disable the limits of the joint
+/**
+ * @param isLimitEnabled True if you want to enable the joint limits and false
+ *					   otherwise
+ */
+void SliderJoint::enableLimit(boolean isLimitEnabled) {
+
+	if (isLimitEnabled != this.isLimitEnabled) {
+
+		this.isLimitEnabled = isLimitEnabled;
+
+		// Reset the limits
+		resetLimits();
+	}
+}
+
+// Enable/Disable the motor of the joint
+/**
+ * @param isMotorEnabled True if you want to enable the joint motor and false
+ *					   otherwise
+ */
+void SliderJoint::enableMotor(boolean isMotorEnabled) {
+
+	this.isMotorEnabled = isMotorEnabled;
+	this.impulseMotor = 0.0;
+
+	// Wake up the two bodies of the joint
+	this.body1.setIsSleeping(false);
+	this.body2.setIsSleeping(false);
+}
+
+// Return the current translation value of the joint
+/**
+ * @return The current translation distance of the joint (in meters)
+ */
+float SliderJoint::getTranslation()  {
+
+	// TODO : Check if we need to compare rigid body position or center of mass here
+
+	// Get the bodies positions and orientations
+	 Vector3f x1 = this.body1.getTransform().getPosition();
+	 Vector3f x2 = this.body2.getTransform().getPosition();
+	 Quaternion q1 = this.body1.getTransform().getOrientation();
+	 Quaternion q2 = this.body2.getTransform().getOrientation();
+
+	// Compute the two anchor points in world-space coordinates
+	 Vector3f anchorBody1 = x1 + q1 * this.localAnchorPointBody1;
+	 Vector3f anchorBody2 = x2 + q2 * this.localAnchorPointBody2;
+
+	// Compute the vector u (difference between anchor points)
+	 Vector3f u = anchorBody2 - anchorBody1;
+
+	// Compute the slider axis in world-space
+	Vector3f sliderAxisWorld = q1 * this.sliderAxisBody1;
+	sliderAxisWorld.normalize();
+
+	// Compute and return the translation value
+	return u.dot(sliderAxisWorld);
+}
+
+// Set the minimum translation limit
+/**
+ * @param lowerLimit The minimum translation limit of the joint (in meters)
+ */
+void SliderJoint::setMinTranslationLimit(float lowerLimit) {
+
+	assert(lowerLimit <= this.upperLimit);
+
+	if (lowerLimit != this.lowerLimit) {
+
+		this.lowerLimit = lowerLimit;
+
+		// Reset the limits
+		resetLimits();
+	}
+}
+
+// Set the maximum translation limit
+/**
+ * @param lowerLimit The maximum translation limit of the joint (in meters)
+ */
+void SliderJoint::setMaxTranslationLimit(float upperLimit) {
+
+	assert(this.lowerLimit <= upperLimit);
+
+	if (upperLimit != this.upperLimit) {
+
+		this.upperLimit = upperLimit;
+
+		// Reset the limits
+		resetLimits();
+	}
+}
+
+// Reset the limits
+void SliderJoint::resetLimits() {
+
+	// Reset the accumulated impulses for the limits
+	this.impulseLowerLimit = 0.0;
+	this.impulseUpperLimit = 0.0;
+
+	// Wake up the two bodies of the joint
+	this.body1.setIsSleeping(false);
+	this.body2.setIsSleeping(false);
+}
+
+// Set the motor speed
+/**
+ * @param motorSpeed The speed of the joint motor (in meters per second)
+ */
+void SliderJoint::setMotorSpeed(float motorSpeed) {
+
+	if (motorSpeed != this.motorSpeed) {
+
+		this.motorSpeed = motorSpeed;
+
+		// Wake up the two bodies of the joint
+		this.body1.setIsSleeping(false);
+		this.body2.setIsSleeping(false);
+	}
+}
+
+// Set the maximum motor force
+/**
+ * @param maxMotorForce The maximum force of the joint motor (in Newton x meters)
+ */
+void SliderJoint::setMaxMotorForce(float maxMotorForce) {
+
+	if (maxMotorForce != this.maxMotorForce) {
+
+		assert(this.maxMotorForce >= 0.0);
+		this.maxMotorForce = maxMotorForce;
+
+		// Wake up the two bodies of the joint
+		this.body1.setIsSleeping(false);
+		this.body2.setIsSleeping(false);
+	}
+}
+
+// Return true if the limits or the joint are enabled
+/**
+ * @return True if the joint limits are enabled
+ */
+boolean SliderJoint::isLimitEnabled()  {
+	return this.isLimitEnabled;
+}
+
+// Return true if the motor of the joint is enabled
+/**
+ * @return True if the joint motor is enabled
+ */
+boolean SliderJoint::isMotorEnabled()  {
+	return this.isMotorEnabled;
+}
+
+// Return the minimum translation limit
+/**
+ * @return The minimum translation limit of the joint (in meters)
+ */
+float SliderJoint::getMinTranslationLimit()  {
+	return this.lowerLimit;
+}
+
+// Return the maximum translation limit
+/**
+ * @return The maximum translation limit of the joint (in meters)
+ */
+float SliderJoint::getMaxTranslationLimit()  {
+	return this.upperLimit;
+}
+
+// Return the motor speed
+/**
+ * @return The current motor speed of the joint (in meters per second)
+ */
+float SliderJoint::getMotorSpeed()  {
+	return this.motorSpeed;
+}
+
+// Return the maximum motor force
+/**
+ * @return The maximum force of the joint motor (in Newton x meters)
+ */
+float SliderJoint::getMaxMotorForce()  {
+	return this.maxMotorForce;
+}
+
+// Return the intensity of the current force applied for the joint motor
+/**
+ * @param timeStep Time step (in seconds)
+ * @return The current force of the joint motor (in Newton x meters)
+ */
+float SliderJoint::getMotorForce(float timeStep)  {
+	return this.impulseMotor / timeStep;
+}
+
+// Return the number of bytes used by the joint
+long SliderJoint::getSizeInBytes()  {
+	return sizeof(SliderJoint);
+}
+

src/org/atriaSoft/ephysics/constraint/SliderJoint.java

@@ -0,0 +1,191 @@
+package org.atriaSoft.ephysics.constraint;
+
+	/**
+	 * This structure is used to gather the information needed to create a slider
+	 * joint. This structure will be used to create the actual slider joint.
+	 */
+	struct SliderJointInfo extends  JointInfo {
+	
+		public :
+			Vector3f anchorPointWorldSpace; //!< Anchor point (in world-space coordinates)
+			Vector3f sliderAxisWorldSpace; //!< Slider axis (in world-space coordinates)
+			boolean isLimitEnabled; //!< True if the slider limits are enabled
+			boolean isMotorEnabled; //!< True if the slider motor is enabled
+			float minTranslationLimit; //!< Mininum allowed translation if limits are enabled
+			float maxTranslationLimit; //!< Maximum allowed translation if limits are enabled
+			float motorSpeed; //!< Motor speed
+			float maxMotorForce; //!< Maximum motor force (in Newtons) that can be applied to reach to desired motor speed
+			/** 
+			 * @brief Constructor without limits and without motor
+			 * @param[in] rigidBody1 The first body of the joint
+			 * @param[in] rigidBody2 The second body of the joint
+			 * @param[in] initAnchorPointWorldSpace The initial anchor point in world-space
+			 * @param[in] initSliderAxisWorldSpace The initial slider axis in world-space
+			 */
+			SliderJointInfo(RigidBody* rigidBody1,
+			                RigidBody* rigidBody2,
+			                 Vector3f initAnchorPointWorldSpace,
+			                 Vector3f initSliderAxisWorldSpace):
+			  JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
+			  this.anchorPointWorldSpace(initAnchorPointWorldSpace),
+			  sliderAxisWorldSpace(initSliderAxisWorldSpace),
+			  isLimitEnabled(false),
+			  isMotorEnabled(false),
+			  minTranslationLimit(-1.0),
+			  maxTranslationLimit(1.0),
+			  motorSpeed(0),
+			  maxMotorForce(0) {
+				
+			}
+			/**
+			 * @brief Constructor with limits and no motor
+			 * @param[in] rigidBody1 The first body of the joint
+			 * @param[in] rigidBody2 The second body of the joint
+			 * @param[in] initAnchorPointWorldSpace The initial anchor point in world-space
+			 * @param[in] initSliderAxisWorldSpace The initial slider axis in world-space
+			 * @param[in] initMinTranslationLimit The initial minimum translation limit (in meters)
+			 * @param[in] initMaxTranslationLimit The initial maximum translation limit (in meters)
+			 */
+			SliderJointInfo(RigidBody* rigidBody1,
+			                RigidBody* rigidBody2,
+			                 Vector3f initAnchorPointWorldSpace,
+			                 Vector3f initSliderAxisWorldSpace,
+			                float initMinTranslationLimit,
+			                float initMaxTranslationLimit):
+			  JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
+			  this.anchorPointWorldSpace(initAnchorPointWorldSpace),
+			  sliderAxisWorldSpace(initSliderAxisWorldSpace),
+			  isLimitEnabled(true),
+			  isMotorEnabled(false),
+			  minTranslationLimit(initMinTranslationLimit),
+			  maxTranslationLimit(initMaxTranslationLimit),
+			  motorSpeed(0),
+			  maxMotorForce(0) {
+				
+			}
+			/**
+			 * @brief Constructor with limits and motor
+			 * @param[in] rigidBody1 The first body of the joint
+			 * @param[in] rigidBody2 The second body of the joint
+			 * @param[in] initAnchorPointWorldSpace The initial anchor point in world-space
+			 * @param[in] initSliderAxisWorldSpace The initial slider axis in world-space
+			 * @param[in] initMinTranslationLimit The initial minimum translation limit (in meters)
+			 * @param[in] initMaxTranslationLimit The initial maximum translation limit (in meters)
+			 * @param[in] initMotorSpeed The initial speed of the joint motor (in meters per second)
+			 * @param[in] initMaxMotorForce The initial maximum motor force of the joint (in Newtons x meters)
+			 */
+			SliderJointInfo(RigidBody* rigidBody1,
+			                RigidBody* rigidBody2,
+			                 Vector3f initAnchorPointWorldSpace,
+			                 Vector3f initSliderAxisWorldSpace,
+			                float initMinTranslationLimit,
+			                float initMaxTranslationLimit,
+			                float initMotorSpeed,
+			                float initMaxMotorForce):
+			  JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
+			  this.anchorPointWorldSpace(initAnchorPointWorldSpace),
+			  sliderAxisWorldSpace(initSliderAxisWorldSpace),
+			  isLimitEnabled(true),
+			  isMotorEnabled(true),
+			  minTranslationLimit(initMinTranslationLimit),
+			  maxTranslationLimit(initMaxTranslationLimit),
+			  motorSpeed(initMotorSpeed),
+			  maxMotorForce(initMaxMotorForce) {
+				
+			}
+	};
+	
+	/**
+	 * @brief This class represents a slider joint. This joint has a one degree of freedom.
+	 * It only allows relative translation of the bodies along a single direction and no
+	 * rotation.
+	 */
+	class SliderJoint: public Joint {
+		private:
+			static  float BETA; //!< Beta value for the position correction bias factor
+			Vector3f this.localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1)
+			Vector3f this.localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2)
+			Vector3f this.sliderAxisBody1; //!< Slider axis (in local-space coordinates of body 1)
+			Matrix3f this.i1; //!< Inertia tensor of body 1 (in world-space coordinates)
+			Matrix3f this.i2; //!< Inertia tensor of body 2 (in world-space coordinates)
+			Quaternion this.initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the two bodies
+			Vector3f this.N1; //!< First vector orthogonal to the slider axis local-space of body 1
+			Vector3f this.N2; //!< Second vector orthogonal to the slider axis and this.N1 in local-space of body 1
+			Vector3f this.R1; //!< Vector r1 in world-space coordinates
+			Vector3f this.R2; //!< Vector r2 in world-space coordinates
+			Vector3f this.R2CrossN1; //!< Cross product of r2 and n1
+			Vector3f this.R2CrossN2; //!< Cross product of r2 and n2
+			Vector3f this.R2CrossSliderAxis; //!< Cross product of r2 and the slider axis
+			Vector3f this.R1PlusUCrossN1; //!< Cross product of vector (r1 + u) and n1
+			Vector3f this.R1PlusUCrossN2; //!< Cross product of vector (r1 + u) and n2
+			Vector3f this.R1PlusUCrossSliderAxis; //!< Cross product of vector (r1 + u) and the slider axis
+			vec2 this.bTranslation; //!< Bias of the 2 translation raints
+			Vector3f this.bRotation; //!< Bias of the 3 rotation raints
+			float this.bLowerLimit; //!< Bias of the lower limit raint
+			float this.bUpperLimit; //!< Bias of the upper limit raint
+			Matrix2x2 this.inverseMassMatrixTranslationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the translation raint (2x2 matrix)
+			Matrix3f this.inverseMassMatrixRotationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the rotation raint (3x3 matrix)
+			float this.inverseMassMatrixLimit; //!< Inverse of mass matrix K=JM^-1J^t for the upper and lower limit raints (1x1 matrix)
+			float this.inverseMassMatrixMotor; //!< Inverse of mass matrix K=JM^-1J^t for the motor
+			vec2 this.impulseTranslation; //!< Accumulated impulse for the 2 translation raints
+			Vector3f this.impulseRotation; //!< Accumulated impulse for the 3 rotation raints
+			float this.impulseLowerLimit; //!< Accumulated impulse for the lower limit raint
+			float this.impulseUpperLimit; //!< Accumulated impulse for the upper limit raint
+			float this.impulseMotor; //!< Accumulated impulse for the motor
+			boolean this.isLimitEnabled; //!< True if the slider limits are enabled
+			boolean this.isMotorEnabled; //!< True if the motor of the joint in enabled
+			Vector3f this.sliderAxisWorld; //!< Slider axis in world-space coordinates
+			float this.lowerLimit; //!< Lower limit (minimum translation distance)
+			float this.upperLimit; //!< Upper limit (maximum translation distance)
+			boolean this.isLowerLimitViolated; //!< True if the lower limit is violated
+			boolean this.isUpperLimitViolated; //!< True if the upper limit is violated
+			float this.motorSpeed; //!< Motor speed (in m/s)
+			float this.maxMotorForce; //!< Maximum motor force (in Newtons) that can be applied to reach to desired motor speed
+			/// Private copy-ructor
+			SliderJoint( SliderJoint raint);
+			/// Private assignment operator
+			SliderJoint operator=( SliderJoint raint);
+			/// Reset the limits
+			void resetLimits();
+			long getSizeInBytes()  ;
+			void initBeforeSolve( ConstraintSolverData raintSolverData) ;
+			void warmstart( ConstraintSolverData raintSolverData) ;
+			void solveVelocityConstraint( ConstraintSolverData raintSolverData) ;
+			void solvePositionConstraint( ConstraintSolverData raintSolverData) ;
+		public :
+			/// Constructor
+			SliderJoint( SliderJointInfo jointInfo);
+			/// Destructor
+			 ~SliderJoint();
+			/// Return true if the limits or the joint are enabled
+			boolean isLimitEnabled() ;
+			/// Return true if the motor of the joint is enabled
+			boolean isMotorEnabled() ;
+			/// Enable/Disable the limits of the joint
+			void enableLimit(boolean isLimitEnabled);
+			/// Enable/Disable the motor of the joint
+			void enableMotor(boolean isMotorEnabled);
+			/// Return the current translation value of the joint
+			float getTranslation() ;
+			/// Return the minimum translation limit
+			float getMinTranslationLimit() ;
+			/// Set the minimum translation limit
+			void setMinTranslationLimit(float lowerLimit);
+			/// Return the maximum translation limit
+			float getMaxTranslationLimit() ;
+			/// Set the maximum translation limit
+			void setMaxTranslationLimit(float upperLimit);
+			/// Return the motor speed
+			float getMotorSpeed() ;
+			/// Set the motor speed
+			void setMotorSpeed(float motorSpeed);
+			/// Return the maximum motor force
+			float getMaxMotorForce() ;
+			/// Set the maximum motor force
+			void setMaxMotorForce(float maxMotorForce);
+			/// Return the intensity of the current force applied for the joint motor
+			float getMotorForce(float timeStep) ;
+	};
+
+
+}

src/org/atriaSoft/ephysics/engine/CollisionWorld.cpp

@@ -0,0 +1,154 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/engine/CollisionWorld.hpp>
+#include <ephysics/debug.hpp>
+
+using namespace ephysics;
+using namespace std;
+
+CollisionWorld::CollisionWorld() :
+  this.collisionDetection(this),
+  this.currentBodyID(0),
+  this.eventListener(null) {
+	
+}
+
+CollisionWorld::~CollisionWorld() {
+	while(this.bodies.size() != 0) {
+		destroyCollisionBody(this.bodies[0]);
+	}
+}
+
+
+CollisionBody* CollisionWorld::createCollisionBody( Transform3D transform) {
+	// Get the next available body ID
+	long bodyID = computeNextAvailableBodyID();
+	// Largest index cannot be used (it is used for invalid index)
+	EPHYASSERT(bodyID < UINT64MAX, "index too big");
+	// Create the collision body
+	CollisionBody* collisionBody = ETKNEW(CollisionBody, transform, *this, bodyID);
+	EPHYASSERT(collisionBody != null, "empty Body collision");
+	// Add the collision body to the world
+	this.bodies.add(collisionBody);
+	// Return the pointer to the rigid body
+	return collisionBody;
+}
+
+void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) {
+	// Remove all the collision shapes of the body
+	collisionBody.removeAllCollisionShapes();
+	// Add the body ID to the list of free IDs
+	this.freeBodiesIDs.pushBack(collisionBody.getID());
+	// Remove the collision body from the list of bodies
+	this.bodies.erase(this.bodies.find(collisionBody));
+	ETKDELETE(CollisionBody, collisionBody);
+	collisionBody = null;
+}
+
+long CollisionWorld::computeNextAvailableBodyID() {
+	// Compute the body ID
+	long bodyID;
+	if (!this.freeBodiesIDs.empty()) {
+		bodyID = this.freeBodiesIDs.back();
+		this.freeBodiesIDs.popBack();
+	} else {
+		bodyID = this.currentBodyID;
+		this.currentBodyID++;
+	}
+	return bodyID;
+}
+
+void CollisionWorld::resetContactManifoldListsOfBodies() {
+	// For each rigid body of the world
+	for (Set<CollisionBody*>::Iterator it = this.bodies.begin(); it != this.bodies.end(); ++it) {
+		// Reset the contact manifold list of the body
+		(*it).resetContactManifoldsList();
+	}
+}
+
+boolean CollisionWorld::testAABBOverlap( CollisionBody* body1,  CollisionBody* body2)  {
+	// If one of the body is not active, we return no overlap
+	if (    !body1.isActive()
+	     || !body2.isActive()) {
+		return false;
+	}
+	// Compute the AABBs of both bodies
+	AABB body1AABB = body1.getAABB();
+	AABB body2AABB = body2.getAABB();
+	// Return true if the two AABBs overlap
+	return body1AABB.testCollision(body2AABB);
+}
+
+void CollisionWorld::testCollision( ProxyShape* shape, CollisionCallback* callback) {
+	// Reset all the contact manifolds lists of each body
+	resetContactManifoldListsOfBodies();
+	// Create the sets of shapes
+	Set<int> shapes;
+	shapes.add(shape.this.broadPhaseID);
+	Set<int> emptySet;
+	// Perform the collision detection and report contacts
+	this.collisionDetection.testCollisionBetweenShapes(callback, shapes, emptySet);
+}
+
+void CollisionWorld::testCollision( ProxyShape* shape1,  ProxyShape* shape2, CollisionCallback* callback) {
+	// Reset all the contact manifolds lists of each body
+	resetContactManifoldListsOfBodies();
+	// Create the sets of shapes
+	Set<int> shapes1;
+	shapes1.add(shape1.this.broadPhaseID);
+	Set<int> shapes2;
+	shapes2.add(shape2.this.broadPhaseID);
+	// Perform the collision detection and report contacts
+	this.collisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2);
+}
+
+void CollisionWorld::testCollision( CollisionBody* body, CollisionCallback* callback) {
+	// Reset all the contact manifolds lists of each body
+	resetContactManifoldListsOfBodies();
+	// Create the sets of shapes
+	Set<int> shapes1;
+	// For each shape of the body
+	for ( ProxyShape* shape = body.getProxyShapesList();
+	     shape != null;
+	     shape = shape.getNext()) {
+		shapes1.add(shape.this.broadPhaseID);
+	}
+	Set<int> emptySet;
+	// Perform the collision detection and report contacts
+	this.collisionDetection.testCollisionBetweenShapes(callback, shapes1, emptySet);
+}
+
+void CollisionWorld::testCollision( CollisionBody* body1,  CollisionBody* body2, CollisionCallback* callback) {
+	// Reset all the contact manifolds lists of each body
+	resetContactManifoldListsOfBodies();
+	// Create the sets of shapes
+	Set<int> shapes1;
+	for ( ProxyShape* shape = body1.getProxyShapesList();
+	     shape != null;
+	     shape = shape.getNext()) {
+		shapes1.add(shape.this.broadPhaseID);
+	}
+	Set<int> shapes2;
+	for ( ProxyShape* shape = body2.getProxyShapesList();
+	     shape != null;
+	     shape = shape.getNext()) {
+		shapes2.add(shape.this.broadPhaseID);
+	}
+	// Perform the collision detection and report contacts
+	this.collisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2);
+}
+
+void CollisionWorld::testCollision(CollisionCallback* callback) {
+	// Reset all the contact manifolds lists of each body
+	resetContactManifoldListsOfBodies();
+	Set<int> emptySet;
+	// Perform the collision detection and report contacts
+	this.collisionDetection.testCollisionBetweenShapes(callback, emptySet, emptySet);
+}
+

src/org/atriaSoft/ephysics/engine/CollisionWorld.java

@@ -0,0 +1,148 @@
+package org.atriaSoft.ephysics.engine;
+
+	/**
+	 * @brief This class represent a world where it is possible to move bodies
+	 * by hand and to test collision between each other. In this kind of
+	 * world, the bodies movement is not computed using the laws of physics.
+	 */
+	class CollisionWorld {
+		protected :
+			CollisionDetection collisionDetection; //!< Reference to the collision detection
+			Set<CollisionBody*> bodies; //!< All the bodies (rigid and soft) of the world
+			long currentBodyID; //!< Current body ID
+			Vector<long> freeBodiesIDs; //!< List of free ID for rigid bodies
+			EventListener* eventListener; //!< Pointer to an event listener object
+			/// Return the next available body ID
+			long computeNextAvailableBodyID();
+			/// Reset all the contact manifolds linked list of each body
+			void resetContactManifoldListsOfBodies();
+		public :
+			/// Constructor
+			CollisionWorld();
+			/// Destructor
+			 ~CollisionWorld();
+			/**
+			 * @brief Get an iterator to the beginning of the bodies of the physics world
+			 * @return An starting iterator to the set of bodies of the world
+			 */
+			Set<CollisionBody*>::Iterator getBodiesBeginIterator() {
+				return this.bodies.begin();
+			}
+			/**
+			 * @brief Get an iterator to the end of the bodies of the physics world
+			 * @return An ending iterator to the set of bodies of the world
+			 */
+			Set<CollisionBody*>::Iterator getBodiesEndIterator() {
+				return this.bodies.end();
+			}
+			/**
+			 * @brief Create a collision body and add it to the world
+			 * @param transform Transform3Dation mapping the local-space of the body to world-space
+			 * @return A pointer to the body that has been created in the world
+			 */
+			CollisionBody* createCollisionBody( Transform3D transform);
+			/**
+			 * @brief Destroy a collision body
+			 * @param collisionBody Pointer to the body to destroy
+			 */
+			void destroyCollisionBody(CollisionBody* collisionBody);
+			/**
+			 * @brief Set the collision dispatch configuration
+			 * This can be used to replace default collision detection algorithms by your
+			 * custom algorithm for instance.
+			 * @param[in] CollisionDispatch Pointer to a collision dispatch object describing
+			 * which collision detection algorithm to use for two given collision shapes
+			 */
+			void setCollisionDispatch(CollisionDispatch* collisionDispatch) {
+				this.collisionDetection.setCollisionDispatch(collisionDispatch);
+			}
+			/**
+			 * @brief Ray cast method
+			 * @param ray Ray to use for raycasting
+			 * @param raycastCallback Pointer to the class with the callback method
+			 * @param raycastWithCategoryMaskBits Bits mask corresponding to the category of bodies to be raycasted
+			 */
+			void raycast( Ray ray,
+			             RaycastCallback* raycastCallback,
+			             int raycastWithCategoryMaskBits = 0xFFFF)  {
+				this.collisionDetection.raycast(raycastCallback, ray, raycastWithCategoryMaskBits);
+			}
+			/**
+			 * @brief Test if the AABBs of two bodies overlap
+			 * @param body1 Pointer to the first body to test
+			 * @param body2 Pointer to the second body to test
+			 * @return True if the AABBs of the two bodies overlap and false otherwise
+			 */
+			boolean testAABBOverlap( CollisionBody* body1,
+			                      CollisionBody* body2) ;
+			/**
+			 * @brief Test if the AABBs of two proxy shapes overlap
+			 * @param shape1 Pointer to the first proxy shape to test
+			 * @param shape2 Pointer to the second proxy shape to test
+			 */
+			boolean testAABBOverlap( ProxyShape* shape1,
+			                      ProxyShape* shape2)  {
+				return this.collisionDetection.testAABBOverlap(shape1, shape2);
+			}
+			/**
+			 * @brief Test and report collisions between a given shape and all the others shapes of the world.
+			 * @param shape Pointer to the proxy shape to test
+			 * @param callback Pointer to the object with the callback method
+			 */
+			 void testCollision( ProxyShape* shape,
+			                           CollisionCallback* callback);
+			/**
+			 * @briefTest and report collisions between two given shapes
+			 * @param shape1 Pointer to the first proxy shape to test
+			 * @param shape2 Pointer to the second proxy shape to test
+			 * @param callback Pointer to the object with the callback method
+			 */
+			 void testCollision( ProxyShape* shape1,
+			                            ProxyShape* shape2,
+			                           CollisionCallback* callback);
+			/**
+			 * @brief Test and report collisions between a body and all the others bodies of the world.
+			 * @param body Pointer to the first body to test
+			 * @param callback Pointer to the object with the callback method
+			 */
+			 void testCollision( CollisionBody* body,
+			                           CollisionCallback* callback);
+			/**
+			 * @brief Test and report collisions between two bodies
+			 * @param body1 Pointer to the first body to test
+			 * @param body2 Pointer to the second body to test
+			 * @param callback Pointer to the object with the callback method
+			 */
+			 void testCollision( CollisionBody* body1,
+			                            CollisionBody* body2,
+			                           CollisionCallback* callback);
+			/**
+			 * @brief Test and report collisions between all shapes of the world
+			 * @param callback Pointer to the object with the callback method
+			 */
+			 void testCollision(CollisionCallback* callback);
+			friend class CollisionDetection;
+			friend class CollisionBody;
+			friend class RigidBody;
+			friend class ConvexMeshShape;
+	};
+	
+	/**
+	 * @brief This class can be used to register a callback for collision test 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 CollisionCallback {
+		public:
+			/**
+			 * @brief Virtualisation of the destructor.
+			 */
+			 ~CollisionCallback() = default;
+			/**
+			 * @brief This method will be called for contact.
+			 * @param[in] contactPointInfo Contact information property.
+			 */
+			 void notifyContact( ContactPointInfo contactPointInfo)=0;
+	};
+}

src/org/atriaSoft/ephysics/engine/ConstraintSolver.cpp

@@ -0,0 +1,78 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/engine/ConstraintSolver.hpp>
+#include <ephysics/engine/Profiler.hpp>
+
+using namespace ephysics;
+
+ConstraintSolver::ConstraintSolver( Map<RigidBody*, int> mapBodyToVelocityIndex):
+  this.mapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
+  this.isWarmStartingActive(true),
+  this.raintSolverData(mapBodyToVelocityIndex) {
+	
+}
+
+void ConstraintSolver::initializeForIsland(float dt, Island* island) {
+	PROFILE("ConstraintSolver::initializeForIsland()");
+	assert(island != null);
+	assert(island.getNbBodies() > 0);
+	assert(island.getNbJoints() > 0);
+	// Set the current time step
+	this.timeStep = dt;
+	// Initialize the raint solver data used to initialize and solve the raints
+	this.raintSolverData.timeStep = this.timeStep;
+	this.raintSolverData.isWarmStartingActive = this.isWarmStartingActive;
+	// For each joint of the island
+	Joint** joints = island.getJoints();
+	for (int iii=0; iii<island.getNbJoints(); ++iii) {
+		// Initialize the raint before solving it
+		joints[iii].initBeforeSolve(this.raintSolverData);
+		// Warm-start the raint if warm-starting is enabled
+		if (this.isWarmStartingActive) {
+			joints[iii].warmstart(this.raintSolverData);
+		}
+	}
+}
+
+void ConstraintSolver::solveVelocityConstraints(Island* island) {
+	PROFILE("ConstraintSolver::solveVelocityConstraints()");
+	assert(island != null);
+	assert(island.getNbJoints() > 0);
+	// For each joint of the island
+	Joint** joints = island.getJoints();
+	for (int iii=0; iii<island.getNbJoints(); ++iii) {
+		joints[iii].solveVelocityConstraint(this.raintSolverData);
+	}
+}
+
+void ConstraintSolver::solvePositionConstraints(Island* island) {
+	PROFILE("ConstraintSolver::solvePositionConstraints()");
+	assert(island != null);
+	assert(island.getNbJoints() > 0);
+	Joint** joints = island.getJoints();
+	for (int iii=0; iii < island.getNbJoints(); ++iii) {
+		joints[iii].solvePositionConstraint(this.raintSolverData);
+	}
+}
+
+void ConstraintSolver::setConstrainedVelocitiesArrays(Vector3f* rainedLinearVelocities,
+                                                      Vector3f* rainedAngularVelocities) {
+	assert(rainedLinearVelocities != null);
+	assert(rainedAngularVelocities != null);
+	this.raintSolverData.linearVelocities = rainedLinearVelocities;
+	this.raintSolverData.angularVelocities = rainedAngularVelocities;
+}
+
+void ConstraintSolver::setConstrainedPositionsArrays(Vector3f* rainedPositions,
+                                                     Quaternion* rainedOrientations) {
+	assert(rainedPositions != null);
+	assert(rainedOrientations != null);
+	this.raintSolverData.positions = rainedPositions;
+	this.raintSolverData.orientations = rainedOrientations;
+}

src/org/atriaSoft/ephysics/engine/ConstraintSolver.java

@@ -0,0 +1,126 @@
+package org.atriaSoft.ephysics.engine;
+
+	/**
+	 * This structure contains data from the raint solver that are used to solve
+	 * each joint raint.
+	 */
+	struct ConstraintSolverData {
+		public :
+			float timeStep; //!< Current time step of the simulation
+			Vector3f* linearVelocities; //!< Array with the bodies linear velocities
+			Vector3f* angularVelocities; //!< Array with the bodies angular velocities
+			Vector3f* positions; //!< Reference to the bodies positions
+			Quaternion* orientations; //!< Reference to the bodies orientations
+			 Map<RigidBody*, int> mapBodyToConstrainedVelocityIndex; //!< Reference to the map that associates rigid body to their index in the rained velocities array
+			boolean isWarmStartingActive; //!< True if warm starting of the solver is active
+			/// Constructor
+			ConstraintSolverData( Map<RigidBody*, int> refMapBodyToConstrainedVelocityIndex):
+			  linearVelocities(null),
+			  angularVelocities(null),
+			  positions(null),
+			  orientations(null),
+			  mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex) {
+				
+			}
+	};
+	
+	/**
+	 * @brief This class represents the raint solver that is used to solve raints between
+	 * the rigid bodies. The raint solver is based on the "Sequential Impulse" technique
+	 * described by Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list).
+	 *
+	 * A raint between two bodies is represented by a function C(x) which is equal to zero
+	 * when the raint is satisfied. The condition C(x)=0 describes a valid position and the
+	 * condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is
+	 * the Jacobian matrix of the raint, v is a vector that contains the velocity of both
+	 * bodies and b is the raint bias. We are looking for a force Fc that will act on the
+	 * bodies to keep the raint satisfied. Note that from the  work principle, we have
+	 * Fc = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a
+	 * Lagrange multiplier. Therefore, finding the force Fc is equivalent to finding the Lagrange
+	 * multiplier lambda.
+	
+	 * An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a
+	 * body to change its velocity. The idea of the Sequential Impulse technique is to apply
+	 * impulses to bodies of each raints in order to keep the raint satisfied.
+	 *
+	 * --- Step 1 ---
+	 *
+	 * First, we integrate the applied force Fa acting of each rigid body (like gravity, ...) and
+	 * we obtain some new velocities v2' that tends to violate the raints.
+	 *
+	 * v2' = v1 + dt * M^-1 * Fa
+	 *
+	 * where M is a matrix that contains mass and inertia tensor information.
+	 *
+	 * --- Step 2 ---
+	 *
+	 * During the second step, we iterate over all the raints for a certain number of
+	 * iterations and for each raint we compute the impulse to apply to the bodies needed
+	 * so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that
+	 * M * deltaV = Pc where M is the mass of the body, deltaV is the difference of velocity and
+	 * Pc is the raint impulse to apply to the body. Therefore, we have
+	 * v2 = v2' + M^-1 * Pc. For each raint, we can compute the Lagrange multiplier lambda
+	 * using : lambda = -this.c (Jv2' + b) where this.c = 1 / (J * M^-1 * J^t). Now that we have the
+	 * Lagrange multiplier lambda, we can compute the impulse Pc = J^t * lambda * dt to apply to
+	 * the bodies to satisfy the raint.
+	 *
+	 * --- Step 3 ---
+	 *
+	 * In the third step, we integrate the new position x2 of the bodies using the new velocities
+	 * v2 computed in the second step with : x2 = x1 + dt * v2.
+	 *
+	 * Note that in the following code (as it is also explained in the slides from Erin Catto),
+	 * the value lambda is not only the lagrange multiplier but is the multiplication of the
+	 * Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use
+	 * lambda, we mean (lambda * dt).
+	 *
+	 * We are using the accumulated impulse technique that is also described in the slides from
+	 * Erin Catto.
+	 *
+	 * We are also using warm starting. The idea is to warm start the solver at the beginning of
+	 * each step by applying the last impulstes for the raints that we already existing at the
+	 * previous step. This allows the iterative solver to converge faster towards the solution.
+	 *
+	 * For contact raints, we are also using split impulses so that the position correction
+	 * that uses Baumgarte stabilization does not change the momentum of the bodies.
+	 *
+	 * There are two ways to apply the friction raints. Either the friction raints are
+	 * applied at each contact point or they are applied only at the center of the contact manifold
+	 * between two bodies. If we solve the friction raints at each contact point, we need
+	 * two raints (two tangential friction directions) and if we solve the friction
+	 * raints at the center of the contact manifold, we need two raints for tangential
+	 * friction but also another twist friction raint to prevent spin of the body around the
+	 * contact manifold center.
+	 */
+	class ConstraintSolver {
+		private :
+			 Map<RigidBody*, int> mapBodyToConstrainedVelocityIndex; //!< Reference to the map that associates rigid body to their index in the rained velocities array
+			float timeStep; //!< Current time step
+			boolean isWarmStartingActive; //!< True if the warm starting of the solver is active
+			ConstraintSolverData raintSolverData; //!< Constraint solver data used to initialize and solve the raints
+		public :
+			/// Constructor
+			ConstraintSolver( Map<RigidBody*, int> mapBodyToVelocityIndex);
+			/// Initialize the raint solver for a given island
+			void initializeForIsland(float dt, Island* island);
+			/// Solve the raints
+			void solveVelocityConstraints(Island* island);
+			/// Solve the position raints
+			void solvePositionConstraints(Island* island);
+			/// Return true if the Non-Linear-Gauss-Seidel position correction technique is active
+			boolean getIsNonLinearGaussSeidelPositionCorrectionActive()  {
+				return this.isWarmStartingActive;
+			}
+			/// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique.
+			void setIsNonLinearGaussSeidelPositionCorrectionActive(boolean isActive) {
+				this.isWarmStartingActive = isActive;
+			}
+			/// Set the rained velocities arrays
+			void setConstrainedVelocitiesArrays(Vector3f* rainedLinearVelocities,
+			                                    Vector3f* rainedAngularVelocities);
+			/// Set the rained positions/orientations arrays
+			void setConstrainedPositionsArrays(Vector3f* rainedPositions,
+			                                   Quaternion* rainedOrientations);
+	};
+
+}

src/org/atriaSoft/ephysics/engine/ContactSolver.cpp

@@ -0,0 +1,712 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/engine/ContactSolver.hpp>
+#include <ephysics/engine/DynamicsWorld.hpp>
+#include <ephysics/body/RigidBody.hpp>
+#include <ephysics/engine/Profiler.hpp>
+
+using namespace ephysics;
+
+ float ContactSolver::BETA = float(0.2);
+ float ContactSolver::BETASPLITIMPULSE = float(0.2);
+ float ContactSolver::SLOP = float(0.01);
+
+ContactSolver::ContactSolver( Map<RigidBody*, int> mapBodyToVelocityIndex) :
+  this.splitLinearVelocities(null),
+  this.splitAngularVelocities(null),
+  this.linearVelocities(null),
+  this.angularVelocities(null),
+  this.mapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
+  this.isWarmStartingActive(true),
+  this.isSplitImpulseActive(true),
+  this.isSolveFrictionAtContactManifoldCenterActive(true) {
+	
+}
+
+void ContactSolver::initializeForIsland(float dt, Island* island) {
+	PROFILE("ContactSolver::initializeForIsland()");
+	assert(island != null);
+	assert(island.getNbBodies() > 0);
+	assert(island.getNbContactManifolds() > 0);
+	assert(this.splitLinearVelocities != null);
+	assert(this.splitAngularVelocities != null);
+	// Set the current time step
+	this.timeStep = dt;
+	this.contactConstraints.resize(island.getNbContactManifolds());
+	// For each contact manifold of the island
+	ContactManifold** contactManifolds = island.getContactManifold();
+	for (int iii=0; iii<this.contactConstraints.size(); ++iii) {
+		ContactManifold* externalManifold = contactManifolds[iii];
+		ContactManifoldSolver internalManifold = this.contactConstraints[iii];
+		assert(externalManifold.getNbContactPoints() > 0);
+		// Get the two bodies of the contact
+		RigidBody* body1 = staticcast<RigidBody*>(externalManifold.getContactPoint(0).getBody1());
+		RigidBody* body2 = staticcast<RigidBody*>(externalManifold.getContactPoint(0).getBody2());
+		assert(body1 != null);
+		assert(body2 != null);
+		// Get the position of the two bodies
+		 Vector3f x1 = body1.this.centerOfMassWorld;
+		 Vector3f x2 = body2.this.centerOfMassWorld;
+		// Initialize the internal contact manifold structure using the external
+		// contact manifold
+		internalManifold.indexBody1 = this.mapBodyToConstrainedVelocityIndex.find(body1).second;
+		internalManifold.indexBody2 = this.mapBodyToConstrainedVelocityIndex.find(body2).second;
+		internalManifold.inverseInertiaTensorBody1 = body1.getInertiaTensorInverseWorld();
+		internalManifold.inverseInertiaTensorBody2 = body2.getInertiaTensorInverseWorld();
+		internalManifold.massInverseBody1 = body1.this.massInverse;
+		internalManifold.massInverseBody2 = body2.this.massInverse;
+		internalManifold.nbContacts = externalManifold.getNbContactPoints();
+		internalManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2);
+		internalManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
+		internalManifold.rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
+		internalManifold.externalContactManifold = externalManifold;
+		internalManifold.isBody1DynamicType = body1.getType() == DYNAMIC;
+		internalManifold.isBody2DynamicType = body2.getType() == DYNAMIC;
+		// If we solve the friction raints at the center of the contact manifold
+		if (this.isSolveFrictionAtContactManifoldCenterActive) {
+			internalManifold.frictionPointBody1 = Vector3f(0.0f,0.0f,0.0f);
+			internalManifold.frictionPointBody2 = Vector3f(0.0f,0.0f,0.0f);
+		}
+		// For each  contact point of the contact manifold
+		for (int ccc=0; ccc<externalManifold.getNbContactPoints(); ++ccc) {
+			ContactPointSolver contactPoint = internalManifold.contacts[ccc];
+			// Get a contact point
+			ContactPoint* externalContact = externalManifold.getContactPoint(ccc);
+			// Get the contact point on the two bodies
+			Vector3f p1 = externalContact.getWorldPointOnBody1();
+			Vector3f p2 = externalContact.getWorldPointOnBody2();
+			contactPoint.externalContact = externalContact;
+			contactPoint.normal = externalContact.getNormal();
+			contactPoint.r1 = p1 - x1;
+			contactPoint.r2 = p2 - x2;
+			contactPoint.penetrationDepth = externalContact.getPenetrationDepth();
+			contactPoint.isRestingContact = externalContact.getIsRestingContact();
+			externalContact.setIsRestingContact(true);
+			contactPoint.oldFrictionVector1 = externalContact.getFrictionVector1();
+			contactPoint.oldFrictionvec2 = externalContact.getFrictionvec2();
+			contactPoint.penetrationImpulse = 0.0;
+			contactPoint.friction1Impulse = 0.0;
+			contactPoint.friction2Impulse = 0.0;
+			contactPoint.rollingResistanceImpulse = Vector3f(0.0f,0.0f,0.0f);
+			// If we solve the friction raints at the center of the contact manifold
+			if (this.isSolveFrictionAtContactManifoldCenterActive) {
+				internalManifold.frictionPointBody1 += p1;
+				internalManifold.frictionPointBody2 += p2;
+			}
+		}
+		// If we solve the friction raints at the center of the contact manifold
+		if (this.isSolveFrictionAtContactManifoldCenterActive) {
+			internalManifold.frictionPointBody1 /=staticcast<float>(internalManifold.nbContacts);
+			internalManifold.frictionPointBody2 /=staticcast<float>(internalManifold.nbContacts);
+			internalManifold.r1Friction = internalManifold.frictionPointBody1 - x1;
+			internalManifold.r2Friction = internalManifold.frictionPointBody2 - x2;
+			internalManifold.oldFrictionVector1 = externalManifold.getFrictionVector1();
+			internalManifold.oldFrictionvec2 = externalManifold.getFrictionvec2();
+			// If warm starting is active
+			if (this.isWarmStartingActive) {
+				// Initialize the accumulated impulses with the previous step accumulated impulses
+				internalManifold.friction1Impulse = externalManifold.getFrictionImpulse1();
+				internalManifold.friction2Impulse = externalManifold.getFrictionImpulse2();
+				internalManifold.frictionTwistImpulse = externalManifold.getFrictionTwistImpulse();
+			} else {
+				// Initialize the accumulated impulses to zero
+				internalManifold.friction1Impulse = 0.0;
+				internalManifold.friction2Impulse = 0.0;
+				internalManifold.frictionTwistImpulse = 0.0;
+				internalManifold.rollingResistanceImpulse = Vector3f(0, 0, 0);
+			}
+		}
+	}
+	// Fill-in all the matrices needed to solve the LCP problem
+	initializeContactConstraints();
+}
+
+void ContactSolver::initializeContactConstraints() {
+	// For each contact raint
+	for (int c=0; c<this.contactConstraints.size(); c++) {
+		ContactManifoldSolver manifold = this.contactConstraints[c];
+		// Get the inertia tensors of both bodies
+		Matrix3f I1 = manifold.inverseInertiaTensorBody1;
+		Matrix3f I2 = manifold.inverseInertiaTensorBody2;
+		// If we solve the friction raints at the center of the contact manifold
+		if (this.isSolveFrictionAtContactManifoldCenterActive) {
+			manifold.normal = Vector3f(0.0, 0.0, 0.0);
+		}
+		// Get the velocities of the bodies
+		 Vector3f v1 = this.linearVelocities[manifold.indexBody1];
+		 Vector3f w1 = this.angularVelocities[manifold.indexBody1];
+		 Vector3f v2 = this.linearVelocities[manifold.indexBody2];
+		 Vector3f w2 = this.angularVelocities[manifold.indexBody2];
+		// For each contact point raint
+		for (int i=0; i<manifold.nbContacts; i++) {
+			ContactPointSolver contactPoint = manifold.contacts[i];
+			ContactPoint* externalContact = contactPoint.externalContact;
+			// Compute the velocity difference
+			Vector3f deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
+			contactPoint.r1CrossN = contactPoint.r1.cross(contactPoint.normal);
+			contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal);
+			// Compute the inverse mass matrix K for the penetration raint
+			float massPenetration =   manifold.massInverseBody1
+			                        + manifold.massInverseBody2
+			                        + ((I1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal)
+			                        + ((I2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal);
+			if (massPenetration > 0.0) {
+				contactPoint.inversePenetrationMass = 1.0f / massPenetration;
+			}
+			// If we do not solve the friction raints at the center of the contact manifold
+			if (!this.isSolveFrictionAtContactManifoldCenterActive) {
+				// Compute the friction vectors
+				computeFrictionVectors(deltaV, contactPoint);
+				contactPoint.r1CrossT1 = contactPoint.r1.cross(contactPoint.frictionVector1);
+				contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionvec2);
+				contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1);
+				contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionvec2);
+				// Compute the inverse mass matrix K for the friction
+				// raints at each contact point
+				float friction1Mass =   manifold.massInverseBody1
+				                      + manifold.massInverseBody2
+				                      + ((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot(contactPoint.frictionVector1)
+				                      + ((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot(contactPoint.frictionVector1);
+				float friction2Mass =   manifold.massInverseBody1
+				                      + manifold.massInverseBody2
+				                      + ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot(contactPoint.frictionvec2)
+				                      + ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot(contactPoint.frictionvec2);
+				if (friction1Mass > 0.0) {
+					contactPoint.inverseFriction1Mass = 1.0f / friction1Mass;
+				}
+				if (friction2Mass > 0.0) {
+					contactPoint.inverseFriction2Mass = 1.0f / friction2Mass;
+				}
+			}
+			// Compute the restitution velocity bias "b". We compute this here instead
+			// of inside the solve() method because we need to use the velocity difference
+			// at the beginning of the contact. Note that if it is a resting contact (normal
+			// velocity bellow a given threshold), we do not add a restitution velocity bias
+			contactPoint.restitutionBias = 0.0;
+			float deltaVDotN = deltaV.dot(contactPoint.normal);
+			if (deltaVDotN < -RESTITUTIONVELOCITYTHRESHOLD) {
+				contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN;
+			}
+			// If the warm starting of the contact solver is active
+			if (this.isWarmStartingActive) {
+				// Get the cached accumulated impulses from the previous step
+				contactPoint.penetrationImpulse = externalContact.getPenetrationImpulse();
+				contactPoint.friction1Impulse = externalContact.getFrictionImpulse1();
+				contactPoint.friction2Impulse = externalContact.getFrictionImpulse2();
+				contactPoint.rollingResistanceImpulse = externalContact.getRollingResistanceImpulse();
+			}
+			// Initialize the split impulses to zero
+			contactPoint.penetrationSplitImpulse = 0.0;
+			// If we solve the friction raints at the center of the contact manifold
+			if (this.isSolveFrictionAtContactManifoldCenterActive) {
+				manifold.normal += contactPoint.normal;
+			}
+		}
+		// Compute the inverse K matrix for the rolling resistance raint
+		manifold.inverseRollingResistance.setZero();
+		if (manifold.rollingResistanceFactor > 0 && (manifold.isBody1DynamicType || manifold.isBody2DynamicType)) {
+			manifold.inverseRollingResistance = manifold.inverseInertiaTensorBody1 + manifold.inverseInertiaTensorBody2;
+			manifold.inverseRollingResistance = manifold.inverseRollingResistance.getInverse();
+		}
+		// If we solve the friction raints at the center of the contact manifold
+		if (this.isSolveFrictionAtContactManifoldCenterActive) {
+			manifold.normal.normalize();
+			Vector3f deltaVFrictionPoint =   v2 + w2.cross(manifold.r2Friction)
+			                           - v1 - w1.cross(manifold.r1Friction);
+			// Compute the friction vectors
+			computeFrictionVectors(deltaVFrictionPoint, manifold);
+			// Compute the inverse mass matrix K for the friction raints at the center of
+			// the contact manifold
+			manifold.r1CrossT1 = manifold.r1Friction.cross(manifold.frictionVector1);
+			manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionvec2);
+			manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1);
+			manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionvec2);
+			float friction1Mass =   manifold.massInverseBody1
+			                      + manifold.massInverseBody2
+			                      + ((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot(manifold.frictionVector1)
+			                      + ((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot(manifold.frictionVector1);
+			float friction2Mass =   manifold.massInverseBody1
+			                      + manifold.massInverseBody2
+			                      + ((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot(manifold.frictionvec2)
+			                      + ((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot(manifold.frictionvec2);
+			float frictionTwistMass =   manifold.normal.dot(manifold.inverseInertiaTensorBody1 * manifold.normal)
+			                          + manifold.normal.dot(manifold.inverseInertiaTensorBody2 * manifold.normal);
+			if (friction1Mass > 0.0) {
+				manifold.inverseFriction1Mass = 1.0f/friction1Mass;
+			}
+			if (friction2Mass > 0.0) {
+				manifold.inverseFriction2Mass = 1.0f/friction2Mass;
+			}
+			if (frictionTwistMass > 0.0) {
+				manifold.inverseTwistFrictionMass = 1.0f / frictionTwistMass;
+			}
+		}
+	}
+}
+
+void ContactSolver::warmStart() {
+	// Check that warm starting is active
+	if (!this.isWarmStartingActive) {
+		return;
+	}
+	// For each raint
+	for (int ccc=0; ccc<this.contactConstraints.size(); ++ccc) {
+		ContactManifoldSolver contactManifold = this.contactConstraints[ccc];
+		boolean atLeastOneRestingContactPoint = false;
+		for (int iii=0; iii<contactManifold.nbContacts; ++iii) {
+			ContactPointSolver contactPoint = contactManifold.contacts[iii];
+			// If it is not a new contact (this contact was already existing at last time step)
+			if (contactPoint.isRestingContact) {
+				atLeastOneRestingContactPoint = true;
+				// --------- Penetration --------- //
+				// Compute the impulse P = J^T * lambda
+				 Impulse impulsePenetration = computePenetrationImpulse(contactPoint.penetrationImpulse, contactPoint);
+				// Apply the impulse to the bodies of the raint
+				applyImpulse(impulsePenetration, contactManifold);
+				// If we do not solve the friction raints at the center of the contact manifold
+				if (!this.isSolveFrictionAtContactManifoldCenterActive) {
+					// Project the old friction impulses (with old friction vectors) into
+					// the new friction vectors to get the new friction impulses
+					Vector3f oldFrictionImpulse = contactPoint.friction1Impulse *
+					                          contactPoint.oldFrictionVector1 +
+					                          contactPoint.friction2Impulse *
+					                          contactPoint.oldFrictionvec2;
+					contactPoint.friction1Impulse = oldFrictionImpulse.dot(contactPoint.frictionVector1);
+					contactPoint.friction2Impulse = oldFrictionImpulse.dot(contactPoint.frictionvec2);
+					// --------- Friction 1 --------- //
+					// Compute the impulse P = J^T * lambda
+					 Impulse impulseFriction1 = computeFriction1Impulse(contactPoint.friction1Impulse, contactPoint);
+					// Apply the impulses to the bodies of the raint
+					applyImpulse(impulseFriction1, contactManifold);
+					// --------- Friction 2 --------- //
+					// Compute the impulse P=J^T * lambda
+					 Impulse impulseFriction2 = computeFriction2Impulse(contactPoint.friction2Impulse, contactPoint);
+					// Apply the impulses to the bodies of the raint
+					applyImpulse(impulseFriction2, contactManifold);
+					// ------ Rolling resistance------ //
+					if (contactManifold.rollingResistanceFactor > 0) {
+						// Compute the impulse P = J^T * lambda
+						 Impulse impulseRollingResistance(Vector3f(0.0f,0.0f,0.0f), -contactPoint.rollingResistanceImpulse,
+						                                       Vector3f(0.0f,0.0f,0.0f), contactPoint.rollingResistanceImpulse);
+						// Apply the impulses to the bodies of the raint
+						applyImpulse(impulseRollingResistance, contactManifold);
+					}
+				}
+			} else {
+				// If it is a new contact point
+				// Initialize the accumulated impulses to zero
+				contactPoint.penetrationImpulse = 0.0;
+				contactPoint.friction1Impulse = 0.0;
+				contactPoint.friction2Impulse = 0.0;
+				contactPoint.rollingResistanceImpulse = Vector3f(0.0f,0.0f,0.0f);
+			}
+		}
+		// If we solve the friction raints at the center of the contact manifold and there is
+		// at least one resting contact point in the contact manifold
+		if (this.isSolveFrictionAtContactManifoldCenterActive && atLeastOneRestingContactPoint) {
+			// Project the old friction impulses (with old friction vectors) into the new friction
+			// vectors to get the new friction impulses
+			Vector3f oldFrictionImpulse = contactManifold.friction1Impulse *
+			                          contactManifold.oldFrictionVector1 +
+			                          contactManifold.friction2Impulse *
+			                          contactManifold.oldFrictionvec2;
+			contactManifold.friction1Impulse = oldFrictionImpulse.dot(contactManifold.frictionVector1);
+			contactManifold.friction2Impulse = oldFrictionImpulse.dot(contactManifold.frictionvec2);
+			// ------ First friction raint at the center of the contact manifold ------ //
+			// Compute the impulse P = J^T * lambda
+			Vector3f linearImpulseBody1 = -contactManifold.frictionVector1 * contactManifold.friction1Impulse;
+			Vector3f angularImpulseBody1 = -contactManifold.r1CrossT1 * contactManifold.friction1Impulse;
+			Vector3f linearImpulseBody2 = contactManifold.frictionVector1 * contactManifold.friction1Impulse;
+			Vector3f angularImpulseBody2 = contactManifold.r2CrossT1 * contactManifold.friction1Impulse;
+			 Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
+			                               linearImpulseBody2, angularImpulseBody2);
+			// Apply the impulses to the bodies of the raint
+			applyImpulse(impulseFriction1, contactManifold);
+			// ------ Second friction raint at the center of the contact manifold ----- //
+			// Compute the impulse P = J^T * lambda
+			linearImpulseBody1 = -contactManifold.frictionvec2 * contactManifold.friction2Impulse;
+			angularImpulseBody1 = -contactManifold.r1CrossT2 * contactManifold.friction2Impulse;
+			linearImpulseBody2 = contactManifold.frictionvec2 * contactManifold.friction2Impulse;
+			angularImpulseBody2 = contactManifold.r2CrossT2 * contactManifold.friction2Impulse;
+			 Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
+			                               linearImpulseBody2, angularImpulseBody2);
+			// Apply the impulses to the bodies of the raint
+			applyImpulse(impulseFriction2, contactManifold);
+			// ------ Twist friction raint at the center of the contact manifold ------ //
+			// Compute the impulse P = J^T * lambda
+			linearImpulseBody1 = Vector3f(0.0, 0.0, 0.0);
+			angularImpulseBody1 = -contactManifold.normal * contactManifold.frictionTwistImpulse;
+			linearImpulseBody2 = Vector3f(0.0, 0.0, 0.0);
+			angularImpulseBody2 = contactManifold.normal * contactManifold.frictionTwistImpulse;
+			 Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
+			                                   linearImpulseBody2, angularImpulseBody2);
+			// Apply the impulses to the bodies of the raint
+			applyImpulse(impulseTwistFriction, contactManifold);
+			// ------ Rolling resistance at the center of the contact manifold ------ //
+			// Compute the impulse P = J^T * lambda
+			angularImpulseBody1 = -contactManifold.rollingResistanceImpulse;
+			angularImpulseBody2 = contactManifold.rollingResistanceImpulse;
+			 Impulse impulseRollingResistance(Vector3f(0.0f,0.0f,0.0f), angularImpulseBody1,
+			                                       Vector3f(0.0f,0.0f,0.0f), angularImpulseBody2);
+			// Apply the impulses to the bodies of the raint
+			applyImpulse(impulseRollingResistance, contactManifold);
+		} else {
+			// If it is a new contact manifold
+			// Initialize the accumulated impulses to zero
+			contactManifold.friction1Impulse = 0.0;
+			contactManifold.friction2Impulse = 0.0;
+			contactManifold.frictionTwistImpulse = 0.0;
+			contactManifold.rollingResistanceImpulse = Vector3f(0.0f,0.0f,0.0f);
+		}
+	}
+}
+
+void ContactSolver::solve() {
+	PROFILE("ContactSolver::solve()");
+	float deltaLambda;
+	float lambdaTemp;
+	// For each contact manifold
+	for (int ccc=0; ccc<this.contactConstraints.size(); ++ccc) {
+		ContactManifoldSolver contactManifold = this.contactConstraints[ccc];
+		float suthis.penetrationImpulse = 0.0;
+		// Get the rained velocities
+		 Vector3f v1 = this.linearVelocities[contactManifold.indexBody1];
+		 Vector3f w1 = this.angularVelocities[contactManifold.indexBody1];
+		 Vector3f v2 = this.linearVelocities[contactManifold.indexBody2];
+		 Vector3f w2 = this.angularVelocities[contactManifold.indexBody2];
+		for (int iii=0; iii<contactManifold.nbContacts; ++iii) {
+			ContactPointSolver contactPoint = contactManifold.contacts[iii];
+			// --------- Penetration --------- //
+			// Compute J*v
+			Vector3f deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
+			float deltaVDotN = deltaV.dot(contactPoint.normal);
+			float Jv = deltaVDotN;
+			// Compute the bias "b" of the raint
+			float beta = this.isSplitImpulseActive ? BETASPLITIMPULSE : BETA;
+			float biasPenetrationDepth = 0.0;
+			if (contactPoint.penetrationDepth > SLOP) {
+				biasPenetrationDepth = -(beta/this.timeStep) * max(0.0f, float(contactPoint.penetrationDepth - SLOP));
+			}
+			float b = biasPenetrationDepth + contactPoint.restitutionBias;
+			// Compute the Lagrange multiplier lambda
+			if (this.isSplitImpulseActive) {
+				deltaLambda = - (Jv + contactPoint.restitutionBias) * contactPoint.inversePenetrationMass;
+			} else {
+				deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass;
+			}
+			lambdaTemp = contactPoint.penetrationImpulse;
+			contactPoint.penetrationImpulse = max(contactPoint.penetrationImpulse + deltaLambda, 0.0f);
+			deltaLambda = contactPoint.penetrationImpulse - lambdaTemp;
+			// Compute the impulse P=J^T * lambda
+			 Impulse impulsePenetration = computePenetrationImpulse(deltaLambda, contactPoint);
+			// Apply the impulse to the bodies of the raint
+			applyImpulse(impulsePenetration, contactManifold);
+			suthis.penetrationImpulse += contactPoint.penetrationImpulse;
+			// If the split impulse position correction is active
+			if (this.isSplitImpulseActive) {
+				// Split impulse (position correction)
+				 Vector3f v1Split = this.splitLinearVelocities[contactManifold.indexBody1];
+				 Vector3f w1Split = this.splitAngularVelocities[contactManifold.indexBody1];
+				 Vector3f v2Split = this.splitLinearVelocities[contactManifold.indexBody2];
+				 Vector3f w2Split = this.splitAngularVelocities[contactManifold.indexBody2];
+				Vector3f deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) - v1Split - w1Split.cross(contactPoint.r1);
+				float JvSplit = deltaVSplit.dot(contactPoint.normal);
+				float deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * contactPoint.inversePenetrationMass;
+				float lambdaTempSplit = contactPoint.penetrationSplitImpulse;
+				contactPoint.penetrationSplitImpulse = max(contactPoint.penetrationSplitImpulse + deltaLambdaSplit, 0.0f);
+				deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit;
+				// Compute the impulse P=J^T * lambda
+				 Impulse splitImpulsePenetration = computePenetrationImpulse(deltaLambdaSplit, contactPoint);
+				applySplitImpulse(splitImpulsePenetration, contactManifold);
+			}
+			// If we do not solve the friction raints at the center of the contact manifold
+			if (!this.isSolveFrictionAtContactManifoldCenterActive) {
+				// --------- Friction 1 --------- //
+				// Compute J*v
+				deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
+				Jv = deltaV.dot(contactPoint.frictionVector1);
+				// Compute the Lagrange multiplier lambda
+				deltaLambda = -Jv;
+				deltaLambda *= contactPoint.inverseFriction1Mass;
+				float frictionLimit = contactManifold.frictionCoefficient * contactPoint.penetrationImpulse;
+				lambdaTemp = contactPoint.friction1Impulse;
+				contactPoint.friction1Impulse = max(-frictionLimit,
+				                                         min(contactPoint.friction1Impulse + deltaLambda, frictionLimit));
+				deltaLambda = contactPoint.friction1Impulse - lambdaTemp;
+				// Compute the impulse P=J^T * lambda
+				 Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda, contactPoint);
+				// Apply the impulses to the bodies of the raint
+				applyImpulse(impulseFriction1, contactManifold);
+				// --------- Friction 2 --------- //
+				// Compute J*v
+				deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
+				Jv = deltaV.dot(contactPoint.frictionvec2);
+				// Compute the Lagrange multiplier lambda
+				deltaLambda = -Jv;
+				deltaLambda *= contactPoint.inverseFriction2Mass;
+				frictionLimit = contactManifold.frictionCoefficient * contactPoint.penetrationImpulse;
+				lambdaTemp = contactPoint.friction2Impulse;
+				contactPoint.friction2Impulse = max(-frictionLimit, min(contactPoint.friction2Impulse + deltaLambda, frictionLimit));
+				deltaLambda = contactPoint.friction2Impulse - lambdaTemp;
+				// Compute the impulse P=J^T * lambda
+				 Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda, contactPoint);
+				// Apply the impulses to the bodies of the raint
+				applyImpulse(impulseFriction2, contactManifold);
+				// --------- Rolling resistance raint --------- //
+				if (contactManifold.rollingResistanceFactor > 0) {
+					// Compute J*v
+					 Vector3f JvRolling = w2 - w1;
+					// Compute the Lagrange multiplier lambda
+					Vector3f deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
+					float rollingLimit = contactManifold.rollingResistanceFactor * contactPoint.penetrationImpulse;
+					Vector3f lambdaTempRolling = contactPoint.rollingResistanceImpulse;
+					contactPoint.rollingResistanceImpulse = clamp(contactPoint.rollingResistanceImpulse + deltaLambdaRolling, rollingLimit);
+					deltaLambdaRolling = contactPoint.rollingResistanceImpulse - lambdaTempRolling;
+					// Compute the impulse P=J^T * lambda
+					 Impulse impulseRolling(Vector3f(0.0f,0.0f,0.0f), -deltaLambdaRolling,
+					                             Vector3f(0.0f,0.0f,0.0f), deltaLambdaRolling);
+					// Apply the impulses to the bodies of the raint
+					applyImpulse(impulseRolling, contactManifold);
+				}
+			}
+		}
+		// If we solve the friction raints at the center of the contact manifold
+		if (this.isSolveFrictionAtContactManifoldCenterActive) {
+			// ------ First friction raint at the center of the contact manifol ------ //
+			// Compute J*v
+			Vector3f deltaV =   v2 + w2.cross(contactManifold.r2Friction)
+			              - v1 - w1.cross(contactManifold.r1Friction);
+			float Jv = deltaV.dot(contactManifold.frictionVector1);
+			// Compute the Lagrange multiplier lambda
+			float deltaLambda = -Jv * contactManifold.inverseFriction1Mass;
+			float frictionLimit = contactManifold.frictionCoefficient * suthis.penetrationImpulse;
+			lambdaTemp = contactManifold.friction1Impulse;
+			contactManifold.friction1Impulse = max(-frictionLimit, min(contactManifold.friction1Impulse + deltaLambda, frictionLimit));
+			deltaLambda = contactManifold.friction1Impulse - lambdaTemp;
+			// Compute the impulse P=J^T * lambda
+			Vector3f linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda;
+			Vector3f angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda;
+			Vector3f linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda;
+			Vector3f angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda;
+			 Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
+			                               linearImpulseBody2, angularImpulseBody2);
+			// Apply the impulses to the bodies of the raint
+			applyImpulse(impulseFriction1, contactManifold);
+			// ------ Second friction raint at the center of the contact manifol ----- //
+			// Compute J*v
+			deltaV =   v2 + w2.cross(contactManifold.r2Friction)
+			         - v1 - w1.cross(contactManifold.r1Friction);
+			Jv = deltaV.dot(contactManifold.frictionvec2);
+			// Compute the Lagrange multiplier lambda
+			deltaLambda = -Jv * contactManifold.inverseFriction2Mass;
+			frictionLimit = contactManifold.frictionCoefficient * suthis.penetrationImpulse;
+			lambdaTemp = contactManifold.friction2Impulse;
+			contactManifold.friction2Impulse = max(-frictionLimit, min(contactManifold.friction2Impulse + deltaLambda, frictionLimit));
+			deltaLambda = contactManifold.friction2Impulse - lambdaTemp;
+			// Compute the impulse P=J^T * lambda
+			linearImpulseBody1 = -contactManifold.frictionvec2 * deltaLambda;
+			angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda;
+			linearImpulseBody2 = contactManifold.frictionvec2 * deltaLambda;
+			angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda;
+			 Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
+			                               linearImpulseBody2, angularImpulseBody2);
+			// Apply the impulses to the bodies of the raint
+			applyImpulse(impulseFriction2, contactManifold);
+			// ------ Twist friction raint at the center of the contact manifol ------ //
+			// Compute J*v
+			deltaV = w2 - w1;
+			Jv = deltaV.dot(contactManifold.normal);
+			deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass);
+			frictionLimit = contactManifold.frictionCoefficient * suthis.penetrationImpulse;
+			lambdaTemp = contactManifold.frictionTwistImpulse;
+			contactManifold.frictionTwistImpulse = max(-frictionLimit, min(contactManifold.frictionTwistImpulse + deltaLambda, frictionLimit));
+			deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp;
+			// Compute the impulse P=J^T * lambda
+			linearImpulseBody1 = Vector3f(0.0, 0.0, 0.0);
+			angularImpulseBody1 = -contactManifold.normal * deltaLambda;
+			linearImpulseBody2 = Vector3f(0.0, 0.0, 0.0);;
+			angularImpulseBody2 = contactManifold.normal * deltaLambda;
+			 Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
+			                                   linearImpulseBody2, angularImpulseBody2);
+			// Apply the impulses to the bodies of the raint
+			applyImpulse(impulseTwistFriction, contactManifold);
+			// --------- Rolling resistance raint at the center of the contact manifold --------- //
+			if (contactManifold.rollingResistanceFactor > 0) {
+				// Compute J*v
+				 Vector3f JvRolling = w2 - w1;
+				// Compute the Lagrange multiplier lambda
+				Vector3f deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
+				float rollingLimit = contactManifold.rollingResistanceFactor * suthis.penetrationImpulse;
+				Vector3f lambdaTempRolling = contactManifold.rollingResistanceImpulse;
+				contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse + deltaLambdaRolling,
+				                                                 rollingLimit);
+				deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling;
+				// Compute the impulse P=J^T * lambda
+				angularImpulseBody1 = -deltaLambdaRolling;
+				angularImpulseBody2 = deltaLambdaRolling;
+				 Impulse impulseRolling(Vector3f(0.0f,0.0f,0.0f),
+				                             angularImpulseBody1,
+				                             Vector3f(0.0f,0.0f,0.0f),
+				                             angularImpulseBody2);
+				// Apply the impulses to the bodies of the raint
+				applyImpulse(impulseRolling, contactManifold);
+			}
+		}
+	}
+}
+
+void ContactSolver::storeImpulses() {
+	// For each contact manifold
+	for (int ccc=0; ccc<this.contactConstraints.size(); ++ccc) {
+		ContactManifoldSolver manifold = this.contactConstraints[ccc];
+		for (int iii=0; iii<manifold.nbContacts; ++iii) {
+			ContactPointSolver contactPoint = manifold.contacts[iii];
+			contactPoint.externalContact.setPenetrationImpulse(contactPoint.penetrationImpulse);
+			contactPoint.externalContact.setFrictionImpulse1(contactPoint.friction1Impulse);
+			contactPoint.externalContact.setFrictionImpulse2(contactPoint.friction2Impulse);
+			contactPoint.externalContact.setRollingResistanceImpulse(contactPoint.rollingResistanceImpulse);
+			contactPoint.externalContact.setFrictionVector1(contactPoint.frictionVector1);
+			contactPoint.externalContact.setFrictionvec2(contactPoint.frictionvec2);
+		}
+		manifold.externalContactManifold.setFrictionImpulse1(manifold.friction1Impulse);
+		manifold.externalContactManifold.setFrictionImpulse2(manifold.friction2Impulse);
+		manifold.externalContactManifold.setFrictionTwistImpulse(manifold.frictionTwistImpulse);
+		manifold.externalContactManifold.setRollingResistanceImpulse(manifold.rollingResistanceImpulse);
+		manifold.externalContactManifold.setFrictionVector1(manifold.frictionVector1);
+		manifold.externalContactManifold.setFrictionvec2(manifold.frictionvec2);
+	}
+}
+
+void ContactSolver::applyImpulse( Impulse impulse,  ContactManifoldSolver manifold) {
+	// Update the velocities of the body 1 by applying the impulse P
+	this.linearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * impulse.linearImpulseBody1;
+	this.angularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * impulse.angularImpulseBody1;
+	// Update the velocities of the body 1 by applying the impulse P
+	this.linearVelocities[manifold.indexBody2] += manifold.massInverseBody2 * impulse.linearImpulseBody2;
+	this.angularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * impulse.angularImpulseBody2;
+}
+
+void ContactSolver::applySplitImpulse( Impulse impulse,  ContactManifoldSolver manifold) {
+	// Update the velocities of the body 1 by applying the impulse P
+	this.splitLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * impulse.linearImpulseBody1;
+	this.splitAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * impulse.angularImpulseBody1;
+	// Update the velocities of the body 1 by applying the impulse P
+	this.splitLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 * impulse.linearImpulseBody2;
+	this.splitAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * impulse.angularImpulseBody2;
+}
+
+void ContactSolver::computeFrictionVectors( Vector3f deltaVelocity, ContactPointSolver contactPoint)  {
+	assert(contactPoint.normal.length() > 0.0);
+	// Compute the velocity difference vector in the tangential plane
+	Vector3f normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal;
+	Vector3f tangentVelocity = deltaVelocity - normalVelocity;
+	// If the velocty difference in the tangential plane is not zero
+	float lengthTangenVelocity = tangentVelocity.length();
+	if (lengthTangenVelocity > FLTEPSILON) {
+		// Compute the first friction vector in the direction of the tangent
+		// velocity difference
+		contactPoint.frictionVector1 = tangentVelocity / lengthTangenVelocity;
+	} else {
+		// Get any orthogonal vector to the normal as the first friction vector
+		contactPoint.frictionVector1 = contactPoint.normal.getOrthoVector();
+	}
+	// The second friction vector is computed by the cross product of the firs
+	// friction vector and the contact normal
+	contactPoint.frictionvec2 =contactPoint.normal.cross(contactPoint.frictionVector1).safeNormalized();
+}
+
+void ContactSolver::computeFrictionVectors( Vector3f deltaVelocity, ContactManifoldSolver contactManifold)  {
+	assert(contactManifold.normal.length() > 0.0);
+	// Compute the velocity difference vector in the tangential plane
+	Vector3f normalVelocity = deltaVelocity.dot(contactManifold.normal) * contactManifold.normal;
+	Vector3f tangentVelocity = deltaVelocity - normalVelocity;
+	// If the velocty difference in the tangential plane is not zero
+	float lengthTangenVelocity = tangentVelocity.length();
+	if (lengthTangenVelocity > FLTEPSILON) {
+		// Compute the first friction vector in the direction of the tangent
+		// velocity difference
+		contactManifold.frictionVector1 = tangentVelocity / lengthTangenVelocity;
+	} else {
+		// Get any orthogonal vector to the normal as the first friction vector
+		contactManifold.frictionVector1 = contactManifold.normal.getOrthoVector();
+	}
+	// The second friction vector is computed by the cross product of the firs
+	// friction vector and the contact normal
+	contactManifold.frictionvec2 = contactManifold.normal.cross(contactManifold.frictionVector1).safeNormalized();
+}
+
+void ContactSolver::cleanup() {
+	this.contactConstraints.clear();
+}
+
+void ContactSolver::setSplitVelocitiesArrays(Vector3f* splitLinearVelocities, Vector3f* splitAngularVelocities) {
+	assert(splitLinearVelocities != NULL);
+	assert(splitAngularVelocities != NULL);
+	this.splitLinearVelocities = splitLinearVelocities;
+	this.splitAngularVelocities = splitAngularVelocities;
+}
+
+void ContactSolver::setConstrainedVelocitiesArrays(Vector3f* rainedLinearVelocities, Vector3f* rainedAngularVelocities) {
+	assert(rainedLinearVelocities != NULL);
+	assert(rainedAngularVelocities != NULL);
+	this.linearVelocities = rainedLinearVelocities;
+	this.angularVelocities = rainedAngularVelocities;
+}
+
+boolean ContactSolver::isSplitImpulseActive()  {
+	return this.isSplitImpulseActive;
+}
+
+void ContactSolver::setIsSplitImpulseActive(boolean isActive) {
+	this.isSplitImpulseActive = isActive;
+}
+
+void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(boolean isActive) {
+	this.isSolveFrictionAtContactManifoldCenterActive = isActive;
+}
+
+float ContactSolver::computeMixedRestitutionFactor(RigidBody* body1, RigidBody* body2)  {
+	float restitution1 = body1.getMaterial().getBounciness();
+	float restitution2 = body2.getMaterial().getBounciness();
+	// Return the largest restitution factor
+	return (restitution1 > restitution2) ? restitution1 : restitution2;
+}
+
+float ContactSolver::computeMixedFrictionCoefficient(RigidBody* body1, RigidBody* body2)  {
+	// Use the geometric mean to compute the mixed friction coefficient
+	return sqrt(body1.getMaterial().getFrictionCoefficient() *
+	            body2.getMaterial().getFrictionCoefficient());
+}
+
+float ContactSolver::computeMixedRollingResistance(RigidBody* body1, RigidBody* body2)  {
+	return   0.5f
+	       * (body1.getMaterial().getRollingResistance()
+	       + body2.getMaterial().getRollingResistance());
+}
+
+ Impulse ContactSolver::computePenetrationImpulse(float deltaLambda,  ContactPointSolver contactPoint)  {
+	return Impulse(-contactPoint.normal * deltaLambda,
+	               -contactPoint.r1CrossN * deltaLambda,
+	                contactPoint.normal * deltaLambda,
+	                contactPoint.r2CrossN * deltaLambda);
+}
+
+ Impulse ContactSolver::computeFriction1Impulse(float deltaLambda,  ContactPointSolver contactPoint)  {
+	return Impulse(-contactPoint.frictionVector1 * deltaLambda,
+	               -contactPoint.r1CrossT1 * deltaLambda,
+	                contactPoint.frictionVector1 * deltaLambda,
+	                contactPoint.r2CrossT1 * deltaLambda);
+}
+
+ Impulse ContactSolver::computeFriction2Impulse(float deltaLambda,  ContactPointSolver contactPoint)  {
+	return Impulse(-contactPoint.frictionvec2 * deltaLambda,
+	               -contactPoint.r1CrossT2 * deltaLambda,
+	                contactPoint.frictionvec2 * deltaLambda,
+	                contactPoint.r2CrossT2 * deltaLambda);
+}

src/org/atriaSoft/ephysics/engine/ContactSolver.java

@@ -0,0 +1,295 @@
+package org.atriaSoft.ephysics.engine;
+
+	/**
+	 * @brief This class represents the contact solver that is used to solve rigid bodies contacts.
+	 * The raint solver is based on the "Sequential Impulse" technique described by
+	 * Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list).
+	 *
+	 * A raint between two bodies is represented by a function C(x) which is equal to zero
+	 * when the raint is satisfied. The condition C(x)=0 describes a valid position and the
+	 * condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is
+	 * the Jacobian matrix of the raint, v is a vector that contains the velocity of both
+	 * bodies and b is the raint bias. We are looking for a force Fc that will act on the
+	 * bodies to keep the raint satisfied. Note that from the  work principle, we have
+	 * Fc = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a
+	 * Lagrange multiplier. Therefore, finding the force Fc is equivalent to finding the Lagrange
+	 * multiplier lambda.
+	 *
+	 * An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a
+	 * body to change its velocity. The idea of the Sequential Impulse technique is to apply
+	 * impulses to bodies of each raints in order to keep the raint satisfied.
+	 *
+	 * --- Step 1 ---
+	 *
+	 * First, we integrate the applied force Fa acting of each rigid body (like gravity, ...) and
+	 * we obtain some new velocities v2' that tends to violate the raints.
+	 *
+	 * v2' = v1 + dt * M^-1 * Fa
+	 *
+	 * where M is a matrix that contains mass and inertia tensor information.
+	 *
+	 * --- Step 2 ---
+	 *
+	 * During the second step, we iterate over all the raints for a certain number of
+	 * iterations and for each raint we compute the impulse to apply to the bodies needed
+	 * so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that
+	 * M * deltaV = Pc where M is the mass of the body, deltaV is the difference of velocity and
+	 * Pc is the raint impulse to apply to the body. Therefore, we have
+	 * v2 = v2' + M^-1 * Pc. For each raint, we can compute the Lagrange multiplier lambda
+	 * using : lambda = -this.c (Jv2' + b) where this.c = 1 / (J * M^-1 * J^t). Now that we have the
+	 * Lagrange multiplier lambda, we can compute the impulse Pc = J^t * lambda * dt to apply to
+	 * the bodies to satisfy the raint.
+	 *
+	 * --- Step 3 ---
+	 *
+	 * In the third step, we integrate the new position x2 of the bodies using the new velocities
+	 * v2 computed in the second step with : x2 = x1 + dt * v2.
+	 *
+	 * Note that in the following code (as it is also explained in the slides from Erin Catto),
+	 * the value lambda is not only the lagrange multiplier but is the multiplication of the
+	 * Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use
+	 * lambda, we mean (lambda * dt).
+	 *
+	 * We are using the accumulated impulse technique that is also described in the slides from
+	 * Erin Catto.
+	 *
+	 * We are also using warm starting. The idea is to warm start the solver at the beginning of
+	 * each step by applying the last impulstes for the raints that we already existing at the
+	 * previous step. This allows the iterative solver to converge faster towards the solution.
+	 *
+	 * For contact raints, we are also using split impulses so that the position correction
+	 * that uses Baumgarte stabilization does not change the momentum of the bodies.
+	 *
+	 * There are two ways to apply the friction raints. Either the friction raints are
+	 * applied at each contact point or they are applied only at the center of the contact manifold
+	 * between two bodies. If we solve the friction raints at each contact point, we need
+	 * two raints (two tangential friction directions) and if we solve the friction
+	 * raints at the center of the contact manifold, we need two raints for tangential
+	 * friction but also another twist friction raint to prevent spin of the body around the
+	 * contact manifold center.
+	 */
+	class ContactSolver {
+		private:
+			/**
+			 * Contact solver internal data structure that to store all the
+			 * information relative to a contact point
+			 */
+			struct ContactPointSolver {
+				float penetrationImpulse; //!< Accumulated normal impulse
+				float friction1Impulse; //!< Accumulated impulse in the 1st friction direction
+				float friction2Impulse; //!< Accumulated impulse in the 2nd friction direction
+				float penetrationSplitImpulse; //!< Accumulated split impulse for penetration correction
+				Vector3f rollingResistanceImpulse; //!< Accumulated rolling resistance impulse
+				Vector3f normal; //!< Normal vector of the contact
+				Vector3f frictionVector1; //!< First friction vector in the tangent plane
+				Vector3f frictionvec2; //!< Second friction vector in the tangent plane
+				Vector3f oldFrictionVector1; //!< Old first friction vector in the tangent plane
+				Vector3f oldFrictionvec2; //!< Old second friction vector in the tangent plane
+				Vector3f r1; //!< Vector from the body 1 center to the contact point
+				Vector3f r2; //!< Vector from the body 2 center to the contact point
+				Vector3f r1CrossT1; //!< Cross product of r1 with 1st friction vector
+				Vector3f r1CrossT2; //!< Cross product of r1 with 2nd friction vector
+				Vector3f r2CrossT1; //!< Cross product of r2 with 1st friction vector
+				Vector3f r2CrossT2; //!< Cross product of r2 with 2nd friction vector
+				Vector3f r1CrossN; //!< Cross product of r1 with the contact normal
+				Vector3f r2CrossN; //!< Cross product of r2 with the contact normal
+				float penetrationDepth; //!< Penetration depth
+				float restitutionBias; //!< Velocity restitution bias
+				float inversePenetrationMass; //!< Inverse of the matrix K for the penenetration
+				float inverseFriction1Mass; //!< Inverse of the matrix K for the 1st friction
+				float inverseFriction2Mass; //!< Inverse of the matrix K for the 2nd friction
+				boolean isRestingContact; //!< True if the contact was existing last time step
+				ContactPoint* externalContact; //!< Pointer to the external contact
+			};
+			/**
+			 * @brief Contact solver internal data structure to store all the information relative to a contact manifold.
+			 */
+			struct ContactManifoldSolver {
+				int indexBody1; //!< Index of body 1 in the raint solver
+				int indexBody2; //!< Index of body 2 in the raint solver
+				float massInverseBody1; //!< Inverse of the mass of body 1
+				float massInverseBody2; //!< Inverse of the mass of body 2
+				Matrix3f inverseInertiaTensorBody1; //!< Inverse inertia tensor of body 1
+				Matrix3f inverseInertiaTensorBody2; //!< Inverse inertia tensor of body 2
+				ContactPointSolver contacts[MAXCONTACTPOINTSINMANIFOLD]; //!< Contact point raints
+				int nbContacts; //!< Number of contact points
+				boolean isBody1DynamicType; //!< True if the body 1 is of type dynamic
+				boolean isBody2DynamicType; //!< True if the body 2 is of type dynamic
+				float restitutionFactor; //!< Mix of the restitution factor for two bodies
+				float frictionCoefficient; //!< Mix friction coefficient for the two bodies
+				float rollingResistanceFactor; //!< Rolling resistance factor between the two bodies
+				ContactManifold* externalContactManifold; //!< Pointer to the external contact manifold
+				// - Variables used when friction raints are apply at the center of the manifold-//
+				Vector3f normal; //!< Average normal vector of the contact manifold
+				Vector3f frictionPointBody1; //!< Point on body 1 where to apply the friction raints
+				Vector3f frictionPointBody2; //!< Point on body 2 where to apply the friction raints
+				Vector3f r1Friction; //!< R1 vector for the friction raints
+				Vector3f r2Friction; //!< R2 vector for the friction raints
+				Vector3f r1CrossT1; //!< Cross product of r1 with 1st friction vector
+				Vector3f r1CrossT2; //!< Cross product of r1 with 2nd friction vector
+				Vector3f r2CrossT1; //!< Cross product of r2 with 1st friction vector
+				Vector3f r2CrossT2; //!< Cross product of r2 with 2nd friction vector
+				float inverseFriction1Mass; //!< Matrix K for the first friction raint
+				float inverseFriction2Mass; //!< Matrix K for the second friction raint
+				float inverseTwistFrictionMass; //!< Matrix K for the twist friction raint
+				Matrix3f inverseRollingResistance; //!< Matrix K for the rolling resistance raint
+				Vector3f frictionVector1; //!< First friction direction at contact manifold center
+				Vector3f frictionvec2; //!< Second friction direction at contact manifold center
+				Vector3f oldFrictionVector1; //!< Old 1st friction direction at contact manifold center
+				Vector3f oldFrictionvec2; //!< Old 2nd friction direction at contact manifold center
+				float friction1Impulse; //!< First friction direction impulse at manifold center
+				float friction2Impulse; //!< Second friction direction impulse at manifold center
+				float frictionTwistImpulse; //!< Twist friction impulse at contact manifold center
+				Vector3f rollingResistanceImpulse; //!< Rolling resistance impulse
+			};
+			static float BETA; //!< Beta value for the penetration depth position correction without split impulses
+			static float BETASPLITIMPULSE; //!< Beta value for the penetration depth position correction with split impulses
+			static float SLOP; //!< Slop distance (allowed penetration distance between bodies)
+			Vector3f* this.splitLinearVelocities; //!< Split linear velocities for the position contact solver (split impulse)
+			Vector3f* this.splitAngularVelocities; //!< Split angular velocities for the position contact solver (split impulse)
+			float this.timeStep; //!< Current time step
+			Vector<ContactManifoldSolver> this.contactConstraints; //!< Contact raints
+			Vector3f* this.linearVelocities; //!< Array of linear velocities
+			Vector3f* this.angularVelocities; //!< Array of angular velocities
+			 Map<RigidBody*, int> this.mapBodyToConstrainedVelocityIndex; //!< Reference to the map of rigid body to their index in the rained velocities array
+			boolean this.isWarmStartingActive; //!< True if the warm starting of the solver is active
+			boolean this.isSplitImpulseActive; //!< True if the split impulse position correction is active
+			boolean this.isSolveFrictionAtContactManifoldCenterActive; //!< True if we solve 3 friction raints at the contact manifold center only instead of 2 friction raints at each contact point
+			/**
+			 * @brief Initialize the contact raints before solving the system
+			 */
+			void initializeContactConstraints();
+			/**
+			 * @brief Apply an impulse to the two bodies of a raint
+			 * @param[in] impulse Impulse to apply
+			 * @param[in] manifold Constraint to apply the impulse
+			 */
+			void applyImpulse( Impulse impulse,  ContactManifoldSolver manifold);
+			/**
+			 * @brief Apply an impulse to the two bodies of a raint
+			 * @param[in] impulse Impulse to apply
+			 * @param[in] manifold Constraint to apply the impulse
+			 */
+			void applySplitImpulse( Impulse impulse,  ContactManifoldSolver manifold);
+			/**
+			 * @brief Compute the collision restitution factor from the restitution factor of each body
+			 * @param[in] body1 First body to compute
+			 * @param[in] body2 Second body to compute
+			 * @return Collision restitution factor
+			 */
+			float computeMixedRestitutionFactor(RigidBody* body1, RigidBody* body2) ;
+			/**
+			 * @brief Compute the mixed friction coefficient from the friction coefficient of each body
+			 * @param[in] body1 First body to compute
+			 * @param[in] body2 Second body to compute
+			 * @return Mixed friction coefficient
+			 */
+			float computeMixedFrictionCoefficient(RigidBody* body1, RigidBody* body2) ;
+			/**
+			 * @brief Compute the mixed rolling resistance factor between two bodies
+			 * @param[in] body1 First body to compute
+			 * @param[in] body2 Second body to compute
+			 * @return Mixed rolling resistance
+			 */
+			float computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) ;
+			/**
+			 * @brief Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
+			 *        plane for a contact point. The two vectors have to be such that : t1 x t2 = contactNormal.
+			 * @param[in] deltaVelocity Velocity ratio (with the delta time step)
+			 * @param[in,out] contactPoint Contact point property
+			 */
+			void computeFrictionVectors( Vector3f deltaVelocity, ContactPointSolver contactPoint) ;
+			/**
+			 * @brief Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
+			 *        plane for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal.
+			 * @param[in] deltaVelocity Velocity ratio (with the delta time step)
+			 * @param[in,out] contactPoint Contact point property
+			 */
+			void computeFrictionVectors( Vector3f deltaVelocity, ContactManifoldSolver contactPoint) ;
+			/**
+			 * @brief Compute a penetration raint impulse
+			 * @param[in] deltaLambda Ratio to apply at the calculation.
+			 * @param[in,out] contactPoint Contact point property
+			 * @return Impulse of the penetration result
+			 */
+			 Impulse computePenetrationImpulse(float deltaLambda,  ContactPointSolver contactPoint) ;
+			/**
+			 * @brief Compute the first friction raint impulse
+			 * @param[in] deltaLambda Ratio to apply at the calculation.
+			 * @param[in] contactPoint Contact point property
+			 * @return Impulse of the friction result
+			 */
+			 Impulse computeFriction1Impulse(float deltaLambda,  ContactPointSolver contactPoint) ;
+			/**
+			 * @brief Compute the second friction raint impulse
+			 * @param[in] deltaLambda Ratio to apply at the calculation.
+			 * @param[in] contactPoint Contact point property
+			 * @return Impulse of the friction result
+			 */
+			 Impulse computeFriction2Impulse(float deltaLambda,  ContactPointSolver contactPoint) ;
+		public:
+			/**
+			 * @brief Constructor
+			 * @param[in] mapBodyToVelocityIndex
+			 */
+			ContactSolver( Map<RigidBody*, int> mapBodyToVelocityIndex);
+			/**
+			 * @brief Virtualize the destructor
+			 */
+			 ~ContactSolver() = default;
+			/**
+			 * @brief Initialize the raint solver for a given island
+			 * @param[in] dt Delta step time
+			 * @param[in] island Island list property
+			 */
+			void initializeForIsland(float dt, Island* island);
+			/**
+			 * @brief Set the split velocities arrays
+			 * @param[in] splitLinearVelocities Split linear velocities Table pointer (not free)
+			 * @param[in] splitAngularVelocities Split angular velocities Table pointer (not free)
+			 */
+			void setSplitVelocitiesArrays(Vector3f* splitLinearVelocities, Vector3f* splitAngularVelocities);
+			/**
+			 * @brief Set the rained velocities arrays
+			 * @param[in] rainedLinearVelocities Constrained Linear velocities Table pointer (not free)
+			 * @param[in] rainedAngularVelocities Constrained angular velocities Table pointer (not free)
+			 */
+			void setConstrainedVelocitiesArrays(Vector3f* rainedLinearVelocities, Vector3f* rainedAngularVelocities);
+			/**
+			 * @brief Warm start the solver.
+			 * For each raint, we apply the previous impulse (from the previous step)
+			 * at the beginning. With this technique, we will converge faster towards the solution of the linear system
+			 */
+			void warmStart();
+			/**
+			 * @brief Store the computed impulses to use them to warm start the solver at the next iteration
+			 */
+			void storeImpulses();
+			/**
+			 * @brief Solve the contacts
+			 */
+			void solve();
+			/**
+			 * @brief Get the split impulses position correction technique is used for contacts
+			 * @return true the split status is Enable
+			 * @return true the split status is Disable
+			 */
+			boolean isSplitImpulseActive() ;
+			/**
+			 * @brief Activate or Deactivate the split impulses for contacts
+			 * @param[in] isActive status to set.
+			 */
+			void setIsSplitImpulseActive(boolean isActive);
+			/**
+			 * @brief Activate or deactivate the solving of friction raints at the center of
+			/// the contact manifold instead of solving them at each contact point
+			 * @param[in] isActive Enable or not the center inertie
+			 */
+			void setIsSolveFrictionAtContactManifoldCenterActive(boolean isActive);
+			/**
+			 * @brief Clean up the raint solver
+			 */
+			void cleanup();
+	};
+
+}

src/org/atriaSoft/ephysics/engine/DynamicsWorld..java

@@ -0,0 +1,293 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#pragma once
+
+#include <ephysics/engine/CollisionWorld.hpp>
+#include <ephysics/collision/CollisionDetection.hpp>
+#include <ephysics/engine/ContactSolver.hpp>
+#include <ephysics/engine/ConstraintSolver.hpp>
+#include <ephysics/body/RigidBody.hpp>
+#include <ephysics/engine/Island.hpp>
+#include <ephysics/configuration.hpp>
+
+namespace ephysics {
+	/**
+	 * @brief This class represents a dynamics world. This class inherits from
+	 * the CollisionWorld class. In a dynamics world, bodies can collide
+	 * and their movements are simulated using the laws of physics.
+	 */
+	class DynamicsWorld extends  CollisionWorld {
+		protected :
+			ContactSolver contactSolver; //!< Contact solver
+			ConstraintSolver raintSolver; //!< Constraint solver
+			int nbVelocitySolverIterations; //!< Number of iterations for the velocity solver of the Sequential Impulses technique
+			int nbPositionSolverIterations; //!< Number of iterations for the position solver of the Sequential Impulses technique
+			boolean isSleepingEnabled; //!< True if the spleeping technique for inactive bodies is enabled
+			Set<RigidBody*> rigidBodies; //!< All the rigid bodies of the physics world
+			Set<Joint*> joints; //!< All the joints of the world
+			Vector3f gravity; //!< Gravity vector of the world
+			float timeStep; //!< Current frame time step (in seconds)
+			boolean isGravityEnabled; //!< True if the gravity force is on
+			Vector<Vector3f> rainedLinearVelocities; //!< Array of rained linear velocities (state of the linear velocities after solving the raints)
+			Vector<Vector3f> rainedAngularVelocities; //!< Array of rained angular velocities (state of the angular velocities after solving the raints)
+			Vector<Vector3f> splitLinearVelocities; //!< Split linear velocities for the position contact solver (split impulse)
+			Vector<Vector3f> splitAngularVelocities; //!< Split angular velocities for the position contact solver (split impulse)
+			Vector<Vector3f> rainedPositions; //!< Array of rained rigid bodies position (for position error correction)
+			Vector<Quaternion> rainedOrientations; //!< Array of rained rigid bodies orientation (for position error correction)
+			Map<RigidBody*, int> mapBodyToConstrainedVelocityIndex; //!< Map body to their index in the rained velocities array
+			Vector<Island*> islands; //!< Array with all the islands of awaken bodies
+			int numberBodiesCapacity; //!< Current allocated capacity for the bodies
+			float sleepLinearVelocity; //!< Sleep linear velocity threshold
+			float sleepAngularVelocity; //!< Sleep angular velocity threshold
+			float timeBeforeSleep; //!< Time (in seconds) before a body is put to sleep if its velocity becomes smaller than the sleep velocity.
+			/**
+			 * @brief Integrate position and orientation of the rigid bodies.
+			 * The positions and orientations of the bodies are integrated using
+			 * the sympletic Euler time stepping scheme.
+			 */
+			void integrateRigidBodiesPositions();
+			/**
+			 * @brief Reset the external force and torque applied to the bodies
+			 */
+			void resetBodiesForceAndTorque();
+			/**
+			 * @brief Initialize the bodies velocities arrays for the next simulation step.
+			 */
+			void initVelocityArrays();
+			/**
+			 * @brief Integrate the velocities of rigid bodies.
+			 * This method only set the temporary velocities but does not update
+			 * the actual velocitiy of the bodies. The velocities updated in this method
+			 * might violate the raints and will be corrected in the raint and
+			 * contact solver.
+			 */
+			void integrateRigidBodiesVelocities();
+			/**
+			 * @brief Solve the contacts and raints
+			 */
+			void solveContactsAndConstraints();
+			/**
+			 * @brief Solve the position error correction of the raints
+			 */
+			void solvePositionCorrection();
+			/**
+			 * @brief Compute the islands of awake bodies.
+			 * An island is an isolated group of rigid bodies that have raints (joints or contacts)
+			 * between each other. This method computes the islands at each time step as follows: For each
+			 * awake rigid body, we run a Depth First Search (DFS) through the raint graph of that body
+			 * (graph where nodes are the bodies and where the edges are the raints between the bodies) to
+			 * find all the bodies that are connected with it (the bodies that share joints or contacts with
+			 * it). Then, we create an island with this group of connected bodies.
+			 */
+			void computeIslands();
+			/**
+			 * @brief Update the postion/orientation of the bodies
+			 */
+			void updateBodiesState();
+			/**
+			 * @brief Put bodies to sleep if needed.
+			 * For each island, if all the bodies have been almost still for a long enough period of
+			 * time, we put all the bodies of the island to sleep.
+			 */
+			void updateSleepingBodies();
+			/**
+			 * @brief Add the joint to the list of joints of the two bodies involved in the joint
+			 * @param[in,out] joint Joint to add at the body.
+			 */
+			void addJointToBody(Joint* joint);
+		public :
+			/**
+			 * @brief Constructor
+			 * @param gravity Gravity vector in the world (in meters per second squared)
+			 */
+			DynamicsWorld( Vector3f gravity);
+			/**
+			 * @brief Vitualize the Destructor
+			 */
+			 ~DynamicsWorld();
+			/**
+			 * @brief Update the physics simulation
+			 * @param timeStep The amount of time to step the simulation by (in seconds)
+			 */
+			void update(float timeStep);
+			/**
+			 * @brief Get the number of iterations for the velocity raint solver
+			 * @return Number if iteration.
+			 */
+			int getNbIterationsVelocitySolver() ;
+			/**
+			 * @brief Set the number of iterations for the velocity raint solver
+			 * @param[in] nbIterations Number of iterations for the velocity solver
+			 */
+			void setNbIterationsVelocitySolver(int nbIterations);
+			/**
+			 * @brief Get the number of iterations for the position raint solver
+			 */
+			int getNbIterationsPositionSolver() ;
+			/**
+			 * @brief Set the number of iterations for the position raint solver
+			 * @param[in] nbIterations Number of iterations for the position solver
+			 */
+			void setNbIterationsPositionSolver(int nbIterations);
+			/**
+			 * @brief Set the position correction technique used for contacts
+			 * @param[in] technique Technique used for the position correction (Baumgarte or Split Impulses)
+			 */
+			void setContactsPositionCorrectionTechnique(ContactsPositionCorrectionTechnique technique);
+			/**
+			 * @brief Set the position correction technique used for joints
+			 * @param[in] technique Technique used for the joins position correction (Baumgarte or Non Linear Gauss Seidel)
+			 */
+			void setJointsPositionCorrectionTechnique(JointsPositionCorrectionTechnique technique);
+			/**
+			 * @brief Activate or deactivate the solving of friction raints at the center of the contact
+			 * manifold instead of solving them at each contact point
+			 * @param[in] isActive True if you want the friction to be solved at the center of
+			 * the contact manifold and false otherwise
+			 */
+			void setIsSolveFrictionAtContactManifoldCenterActive(boolean isActive);
+			/**
+			 * @brief Create a rigid body into the physics world
+			 * @param[in] transform Transform3Dation from body local-space to world-space
+			 * @return A pointer to the body that has been created in the world
+			 */
+			RigidBody* createRigidBody( Transform3D transform);
+			/**
+			 * @brief Destroy a rigid body and all the joints which it belongs
+			 * @param[in,out] rigidBody Pointer to the body you want to destroy
+			 */
+			void destroyRigidBody(RigidBody* rigidBody);
+			/**
+			 * @brief Create a joint between two bodies in the world and return a pointer to the new joint
+			 * @param[in] jointInfo The information that is necessary to create the joint
+			 * @return A pointer to the joint that has been created in the world
+			 */
+			Joint* createJoint( JointInfo jointInfo);
+			/**
+			 * @brief Destroy a joint
+			 * @param[in,out] joint Pointer to the joint you want to destroy
+			 */
+			void destroyJoint(Joint* joint);
+			/**
+			 * @brief Get the gravity vector of the world
+			 * @return The current gravity vector (in meter per seconds squared)
+			 */
+			Vector3f getGravity() ;
+			/**
+			 * @brief Set the gravity vector of the world
+			 * @param[in] gravity The gravity vector (in meter per seconds squared)
+			 */
+			void setGravity(Vector3f gravity);
+			/**
+			 * @brief Get if the gravity is enaled
+			 * @return True if the gravity is enabled in the world
+			 */
+			boolean isGravityEnabled() ;
+			/**
+			 * @brief Enable/Disable the gravity
+			 * @param[in] isGravityEnabled True if you want to enable the gravity in the world
+			 * and false otherwise
+			 */
+			void setIsGratityEnabled(boolean isGravityEnabled);
+			/**
+			 * @brief Get the number of rigid bodies in the world
+			 * @return Number of rigid bodies in the world
+			 */
+			int getNbRigidBodies() ;
+			/**
+			 * @brief Get the number of all joints
+			 * @return Number of joints in the world
+			 */
+			int getNbJoints() ;
+			/**
+			 * @brief Get an iterator to the beginning of the bodies of the physics world
+			 * @return Starting iterator of the set of rigid bodies
+			 */
+			Set<RigidBody*>::Iterator getRigidBodiesBeginIterator();
+			/**
+			 * @brief Get an iterator to the end of the bodies of the physics world
+			 * @return Ending iterator of the set of rigid bodies
+			 */
+			Set<RigidBody*>::Iterator getRigidBodiesEndIterator();
+			/**
+			 * @brief Get if the sleeping technique is enabled
+			 * @return True if the sleeping technique is enabled and false otherwise
+			 */
+			boolean isSleepingEnabled() ;
+			 /**
+			 * @brief Enable/Disable the sleeping technique.
+			 * The sleeping technique is used to put bodies that are not moving into sleep
+			 * to speed up the simulation.
+			 * @param[in] isSleepingEnabled True if you want to enable the sleeping technique and false otherwise
+			 */
+			void enableSleeping(boolean isSleepingEnabled);
+			/**
+			 * @brief Get the sleep linear velocity
+			 * @return The current sleep linear velocity (in meters per second)
+			 */
+			float getSleepLinearVelocity() ;
+			/**
+			 * @brief Set the sleep linear velocity
+			 * @param[in] sleepLinearVelocity The new sleep linear velocity (in meters per second)
+			 */
+			void setSleepLinearVelocity(float sleepLinearVelocity);
+			/**
+			 * @brief Return the current sleep angular velocity
+			 * @return The sleep angular velocity (in radian per second)
+			 */
+			float getSleepAngularVelocity() ;
+			/**
+			 * @brief Set the sleep angular velocity.
+			 * When the velocity of a body becomes smaller than the sleep linear/angular
+			 * velocity for a given amount of time, the body starts sleeping and does not need
+			 * to be simulated anymore.
+			 * @param[in] sleepAngularVelocity The sleep angular velocity (in radian per second)
+			 */
+			void setSleepAngularVelocity(float sleepAngularVelocity);
+			/**
+			 * @brief Get the time a body is required to stay still before sleeping
+			 * @return Time a body is required to stay still before sleeping (in seconds)
+			 */
+			float getTimeBeforeSleep() ;
+			/**
+			 * @brief Set the time a body is required to stay still before sleeping
+			 * @param timeBeforeSleep Time a body is required to stay still before sleeping (in seconds)
+			 */
+			void setTimeBeforeSleep(float timeBeforeSleep);
+			/**
+			 * @brief Set an event listener object to receive events callbacks.
+			 * @note If you use null as an argument, the events callbacks will be disabled.
+			 * @param[in] eventListener Pointer to the event listener object that will receive
+			 * event callbacks during the simulation
+			 */
+			void setEventListener(EventListener* eventListener);
+			void testCollision( ProxyShape* shape,
+			                   CollisionCallback* callback) ;
+			 void testCollision( ProxyShape* shape1,
+			                            ProxyShape* shape2,
+			                           CollisionCallback* callback) ;
+			 void testCollision( CollisionBody* body,
+			                           CollisionCallback* callback) ;
+			 void testCollision( CollisionBody* body1,
+			                            CollisionBody* body2,
+			                           CollisionCallback* callback) ;
+			/// Test and report collisions between all shapes of the world
+			 void testCollision(CollisionCallback* callback) ;
+			/**
+			 * @brief Get list of all contacts.
+			 * @return The list of all contacts of the world
+			 */
+			Vector< ContactManifold*> getContactsList() ;
+			friend class RigidBody;
+	};
+
+
+
+}
+

src/org/atriaSoft/ephysics/engine/DynamicsWorld.cpp

@@ -0,0 +1,789 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/engine/DynamicsWorld.hpp>
+#include <ephysics/raint/BallAndSocketJoint.hpp>
+#include <ephysics/raint/SliderJoint.hpp>
+#include <ephysics/raint/HingeJoint.hpp>
+#include <ephysics/raint/FixedJoint.hpp>
+#include <ephysics/debug.hpp>
+
+ephysics::DynamicsWorld::DynamicsWorld( Vector3f gravity):
+  CollisionWorld(),
+  this.contactSolver(this.mapBodyToConstrainedVelocityIndex),
+  this.raintSolver(this.mapBodyToConstrainedVelocityIndex),
+  this.nbVelocitySolverIterations(DEFAULTVELOCITYSOLVERNBITERATIONS),
+  this.nbPositionSolverIterations(DEFAULTPOSITIONSOLVERNBITERATIONS),
+  this.isSleepingEnabled(SPLEEPINGENABLED),
+  this.gravity(gravity),
+  this.isGravityEnabled(true),
+  this.numberBodiesCapacity(0),
+  this.sleepLinearVelocity(DEFAULTSLEEPLINEARVELOCITY),
+  this.sleepAngularVelocity(DEFAULTSLEEPANGULARVELOCITY),
+  this.timeBeforeSleep(DEFAULTTIMEBEFORESLEEP) {
+	
+}
+
+ephysics::DynamicsWorld::~DynamicsWorld() {
+	// Destroy all the joints that have not been removed
+	Set<ephysics::Joint*>::Iterator itJoints;
+	for (itJoints = this.joints.begin(); itJoints != this.joints.end();) {
+		Set<ephysics::Joint*>::Iterator itToRemove = itJoints;
+		++itJoints;
+		destroyJoint(*itToRemove);
+	}
+	// Destroy all the rigid bodies that have not been removed
+	Set<RigidBody*>::Iterator itRigidBodies;
+	for (itRigidBodies = this.rigidBodies.begin(); itRigidBodies != this.rigidBodies.end();) {
+		Set<RigidBody*>::Iterator itToRemove = itRigidBodies;
+		++itRigidBodies;
+		destroyRigidBody(*itToRemove);
+	}
+	// Release the memory allocated for the islands
+	for (auto it: this.islands) {
+		// Call the island destructor
+		ETKDELETE(Island, it);
+		it = null;
+	}
+	this.islands.clear();
+	// Release the memory allocated for the bodies velocity arrays
+	if (this.numberBodiesCapacity > 0) {
+		this.splitLinearVelocities.clear();
+		this.splitAngularVelocities.clear();
+		this.rainedLinearVelocities.clear();
+		this.rainedAngularVelocities.clear();
+		this.rainedPositions.clear();
+		this.rainedOrientations.clear();
+	}
+	assert(this.joints.size() == 0);
+	assert(this.rigidBodies.size() == 0);
+#ifdef ISPROFILINGACTIVE
+	// Print the profiling report
+	Stream tmp;
+	Profiler::printReport(tmp);
+	Log.print(tmp.str());
+	// Destroy the profiler (release the allocated memory)
+	Profiler::destroy();
+#endif
+}
+
+void ephysics::DynamicsWorld::update(float timeStep) {
+	#ifdef ISPROFILINGACTIVE
+		// Increment the frame counter of the profiler
+		Profiler::incrementFrameCounter();
+	#endif
+	PROFILE("ephysics::DynamicsWorld::update()");
+	this.timeStep = timeStep;
+	// Notify the event listener about the beginning of an internal tick
+	if (this.eventListener != null) {
+		this.eventListener.beginInternalTick();
+	}
+	// Reset all the contact manifolds lists of each body
+	resetContactManifoldListsOfBodies();
+	if (this.rigidBodies.size() == 0) {
+		// no rigid body ==> no process to do ...
+		return;
+	}
+	// Compute the collision detection
+	this.collisionDetection.computeCollisionDetection();
+	// Compute the islands (separate groups of bodies with raints between each others)
+	computeIslands();
+	// Integrate the velocities
+	integrateRigidBodiesVelocities();
+	// Solve the contacts and raints
+	solveContactsAndConstraints();
+	// Integrate the position and orientation of each body
+	integrateRigidBodiesPositions();
+	// Solve the position correction for raints
+	solvePositionCorrection();
+	// Update the state (positions and velocities) of the bodies
+	updateBodiesState();
+	if (this.isSleepingEnabled) {
+		updateSleepingBodies();
+	}
+	// Notify the event listener about the end of an internal tick
+	if (this.eventListener != null) {
+		this.eventListener.endInternalTick();
+	}
+	// Reset the external force and torque applied to the bodies
+	resetBodiesForceAndTorque();
+}
+
+void ephysics::DynamicsWorld::integrateRigidBodiesPositions() {
+	PROFILE("ephysics::DynamicsWorld::integrateRigidBodiesPositions()");
+	// For each island of the world
+	for (int i=0; i < this.islands.size(); i++) {
+		RigidBody** bodies = this.islands[i].getBodies();
+		// For each body of the island
+		for (int b=0; b < this.islands[i].getNbBodies(); b++) {
+			// Get the rained velocity
+			int indexArray = this.mapBodyToConstrainedVelocityIndex.find(bodies[b]).second;
+			Vector3f newLinVelocity = this.rainedLinearVelocities[indexArray];
+			Vector3f newAngVelocity = this.rainedAngularVelocities[indexArray];
+			// Add the split impulse velocity from Contact Solver (only used
+			// to update the position)
+			if (this.contactSolver.isSplitImpulseActive()) {
+				newLinVelocity += this.splitLinearVelocities[indexArray];
+				newAngVelocity += this.splitAngularVelocities[indexArray];
+			}
+			// Get current position and orientation of the body
+			 Vector3f currentPosition = bodies[b].this.centerOfMassWorld;
+			 Quaternion currentOrientation = bodies[b].getTransform().getOrientation();
+			// Update the new rained position and orientation of the body
+			this.rainedPositions[indexArray] = currentPosition + newLinVelocity * this.timeStep;
+			this.rainedOrientations[indexArray] = currentOrientation;
+			this.rainedOrientations[indexArray] +=   Quaternion(0, newAngVelocity)
+			                                         * currentOrientation
+			                                         * 0.5f
+			                                         * this.timeStep;
+		}
+	}
+}
+
+void ephysics::DynamicsWorld::updateBodiesState() {
+	PROFILE("ephysics::DynamicsWorld::updateBodiesState()");
+	// For each island of the world
+	for (int islandIndex = 0; islandIndex < this.islands.size(); islandIndex++) {
+		// For each body of the island
+		RigidBody** bodies = this.islands[islandIndex].getBodies();
+		for (int b=0; b < this.islands[islandIndex].getNbBodies(); b++) {
+			int index = this.mapBodyToConstrainedVelocityIndex.find(bodies[b]).second;
+			// Update the linear and angular velocity of the body
+			bodies[b].this.linearVelocity = this.rainedLinearVelocities[index];
+			bodies[b].this.angularVelocity = this.rainedAngularVelocities[index];
+			// Update the position of the center of mass of the body
+			bodies[b].this.centerOfMassWorld = this.rainedPositions[index];
+			// Update the orientation of the body
+			bodies[b].this.transform.setOrientation(this.rainedOrientations[index].safeNormalized());
+			// Update the transform of the body (using the new center of mass and new orientation)
+			bodies[b].updateTransformWithCenterOfMass();
+			// Update the broad-phase state of the body
+			bodies[b].updateBroadPhaseState();
+		}
+	}
+}
+
+void ephysics::DynamicsWorld::initVelocityArrays() {
+	// Allocate memory for the bodies velocity arrays
+	int nbBodies = this.rigidBodies.size();
+	if (this.numberBodiesCapacity != nbBodies && nbBodies > 0) {
+		if (this.numberBodiesCapacity > 0) {
+			this.splitLinearVelocities.clear();
+			this.splitAngularVelocities.clear();
+		}
+		this.numberBodiesCapacity = nbBodies;
+		this.splitLinearVelocities.clear();
+		this.splitAngularVelocities.clear();
+		this.rainedLinearVelocities.clear();
+		this.rainedAngularVelocities.clear();
+		this.rainedPositions.clear();
+		this.rainedOrientations.clear();
+		this.splitLinearVelocities.resize(this.numberBodiesCapacity, Vector3f(0,0,0));
+		this.splitAngularVelocities.resize(this.numberBodiesCapacity, Vector3f(0,0,0));
+		this.rainedLinearVelocities.resize(this.numberBodiesCapacity, Vector3f(0,0,0));
+		this.rainedAngularVelocities.resize(this.numberBodiesCapacity, Vector3f(0,0,0));
+		this.rainedPositions.resize(this.numberBodiesCapacity, Vector3f(0,0,0));
+		this.rainedOrientations.resize(this.numberBodiesCapacity, Quaternion::identity());
+	}
+	// Reset the velocities arrays
+	for (int i=0; i<this.numberBodiesCapacity; i++) {
+		this.splitLinearVelocities[i].setZero();
+		this.splitAngularVelocities[i].setZero();
+	}
+	// Initialize the map of body indexes in the velocity arrays
+	this.mapBodyToConstrainedVelocityIndex.clear();
+	Set<RigidBody*>::Iterator it;
+	int indexBody = 0;
+	for (it = this.rigidBodies.begin(); it != this.rigidBodies.end(); ++it) {
+		// Add the body into the map
+		this.mapBodyToConstrainedVelocityIndex.add(*it, indexBody);
+		indexBody++;
+	}
+}
+
+void ephysics::DynamicsWorld::integrateRigidBodiesVelocities() {
+	PROFILE("ephysics::DynamicsWorld::integrateRigidBodiesVelocities()");
+	// Initialize the bodies velocity arrays
+	initVelocityArrays();
+	// For each island of the world
+	for (int i=0; i < this.islands.size(); i++) {
+		RigidBody** bodies = this.islands[i].getBodies();
+		// For each body of the island
+		for (int b=0; b < this.islands[i].getNbBodies(); b++) {
+			// Insert the body into the map of rained velocities
+			int indexBody = this.mapBodyToConstrainedVelocityIndex.find(bodies[b]).second;
+			assert(this.splitLinearVelocities[indexBody] == Vector3f(0, 0, 0));
+			assert(this.splitAngularVelocities[indexBody] == Vector3f(0, 0, 0));
+			// Integrate the external force to get the new velocity of the body
+			this.rainedLinearVelocities[indexBody] = bodies[b].getLinearVelocity();
+			this.rainedLinearVelocities[indexBody] += bodies[b].this.massInverse * bodies[b].this.externalForce * this.timeStep;
+			this.rainedAngularVelocities[indexBody] = bodies[b].getAngularVelocity();
+			this.rainedAngularVelocities[indexBody] += bodies[b].getInertiaTensorInverseWorld() * bodies[b].this.externalTorque * this.timeStep;
+			// If the gravity has to be applied to this rigid body
+			if (bodies[b].isGravityEnabled() && this.isGravityEnabled) {
+				// Integrate the gravity force
+				this.rainedLinearVelocities[indexBody] += this.timeStep * bodies[b].this.massInverse * bodies[b].getMass() * this.gravity;
+			}
+			// Apply the velocity damping
+			// Damping force : Fc = -c' * v (c=damping factor)
+			// Equation	  : m * dv/dt = -c' * v
+			//				 => dv/dt = -c * v (with c=c'/m)
+			//				 => dv/dt + c * v = 0
+			// Solution	  : v(t) = v0 * e^(-c * t)
+			//				 => v(t + dt) = v0 * e^(-c(t + dt))
+			//							  = v0 * e^(-ct) * e^(-c * dt)
+			//							  = v(t) * e^(-c * dt)
+			//				 => v2 = v1 * e^(-c * dt)
+			// Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ...
+			//							  => e^(-x) ~ 1 - x
+			//				 => v2 = v1 * (1 - c * dt)
+			float linDampingFactor = bodies[b].getLinearDamping();
+			float angDampingFactor = bodies[b].getAngularDamping();
+			float linearDamping = pow(1.0f - linDampingFactor, this.timeStep);
+			float angularDamping = pow(1.0f - angDampingFactor, this.timeStep);
+			this.rainedLinearVelocities[indexBody] *= linearDamping;
+			this.rainedAngularVelocities[indexBody] *= angularDamping;
+			indexBody++;
+		}
+	}
+}
+
+void ephysics::DynamicsWorld::solveContactsAndConstraints() {
+	PROFILE("ephysics::DynamicsWorld::solveContactsAndConstraints()");
+	// Set the velocities arrays
+	this.contactSolver.setSplitVelocitiesArrays(this.splitLinearVelocities[0], this.splitAngularVelocities[0]);
+	this.contactSolver.setConstrainedVelocitiesArrays(this.rainedLinearVelocities[0],
+	                                               this.rainedAngularVelocities[0]);
+	this.raintSolver.setConstrainedVelocitiesArrays(this.rainedLinearVelocities[0],
+	                                                  this.rainedAngularVelocities[0]);
+	this.raintSolver.setConstrainedPositionsArrays(this.rainedPositions[0],
+	                                                 this.rainedOrientations[0]);
+	// ---------- Solve velocity raints for joints and contacts ---------- //
+	// For each island of the world
+	for (int islandIndex = 0; islandIndex < this.islands.size(); islandIndex++) {
+		// Check if there are contacts and raints to solve
+		boolean isConstraintsToSolve = this.islands[islandIndex].getNbJoints() > 0;
+		boolean isContactsToSolve = this.islands[islandIndex].getNbContactManifolds() > 0;
+		if (!isConstraintsToSolve && !isContactsToSolve) {
+			continue;
+		}
+		// If there are contacts in the current island
+		if (isContactsToSolve) {
+			// Initialize the solver
+			this.contactSolver.initializeForIsland(this.timeStep, this.islands[islandIndex]);
+			// Warm start the contact solver
+			this.contactSolver.warmStart();
+		}
+		// If there are raints
+		if (isConstraintsToSolve) {
+			// Initialize the raint solver
+			this.raintSolver.initializeForIsland(this.timeStep, this.islands[islandIndex]);
+		}
+		// For each iteration of the velocity solver
+		for (int i=0; i<this.nbVelocitySolverIterations; i++) {
+			// Solve the raints
+			if (isConstraintsToSolve) {
+				this.raintSolver.solveVelocityConstraints(this.islands[islandIndex]);
+			}
+			// Solve the contacts
+			if (isContactsToSolve) this.contactSolver.solve();
+		}
+		// Cache the lambda values in order to use them in the next
+		// step and cleanup the contact solver
+		if (isContactsToSolve) {
+			this.contactSolver.storeImpulses();
+			this.contactSolver.cleanup();
+		}
+	}
+}
+
+void ephysics::DynamicsWorld::solvePositionCorrection() {
+	PROFILE("ephysics::DynamicsWorld::solvePositionCorrection()");
+	// Do not continue if there is no raints
+	if (this.joints.empty()) {
+		return;
+	}
+	// For each island of the world
+	for (int islandIndex = 0; islandIndex < this.islands.size(); islandIndex++) {
+		// ---------- Solve the position error correction for the raints ---------- //
+		// For each iteration of the position (error correction) solver
+		for (int i=0; i<this.nbPositionSolverIterations; i++) {
+			// Solve the position raints
+			this.raintSolver.solvePositionConstraints(this.islands[islandIndex]);
+		}
+	}
+}
+
+ephysics::RigidBody* ephysics::DynamicsWorld::createRigidBody( Transform3D transform) {
+	// Compute the body ID
+	ephysics::long bodyID = computeNextAvailableBodyID();
+	// Largest index cannot be used (it is used for invalid index)
+	assert(bodyID < UINT64MAX);
+	// Create the rigid body
+	ephysics::RigidBody* rigidBody = ETKNEW(RigidBody, transform, *this, bodyID);
+	assert(rigidBody != null);
+	// Add the rigid body to the physics world
+	this.bodies.add(rigidBody);
+	this.rigidBodies.add(rigidBody);
+	// Return the pointer to the rigid body
+	return rigidBody;
+}
+
+void ephysics::DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
+	// Remove all the collision shapes of the body
+	rigidBody.removeAllCollisionShapes();
+	// Add the body ID to the list of free IDs
+	this.freeBodiesIDs.pushBack(rigidBody.getID());
+	// Destroy all the joints in which the rigid body to be destroyed is involved
+	for (ephysics::JointListElement* element = rigidBody.this.jointsList;
+	     element != null;
+	     element = element.next) {
+		destroyJoint(element.joint);
+	}
+	// Reset the contact manifold list of the body
+	rigidBody.resetContactManifoldsList();
+	// Remove the rigid body from the list of rigid bodies
+	this.bodies.erase(this.bodies.find(rigidBody));
+	this.rigidBodies.erase(this.rigidBodies.find(rigidBody));
+	// Call the destructor of the rigid body
+	ETKDELETE(RigidBody, rigidBody);
+	rigidBody = null;
+}
+
+ephysics::Joint* ephysics::DynamicsWorld::createJoint( ephysics::JointInfo jointInfo) {
+	Joint* newJoint = null;
+	// Allocate memory to create the new joint
+	switch(jointInfo.type) {
+		// Ball-and-Socket joint
+		case BALLSOCKETJOINT:
+			newJoint = ETKNEW(BallAndSocketJoint, staticcast< ephysics::BallAndSocketJointInfo>(jointInfo));
+			break;
+		// Slider joint
+		case SLIDERJOINT:
+			newJoint = ETKNEW(SliderJoint, staticcast< ephysics::SliderJointInfo>(jointInfo));
+			break;
+		// Hinge joint
+		case HINGEJOINT:
+			newJoint = ETKNEW(HingeJoint, staticcast< ephysics::HingeJointInfo>(jointInfo));
+			break;
+		// Fixed joint
+		case FIXEDJOINT:
+			newJoint = ETKNEW(FixedJoint, staticcast< ephysics::FixedJointInfo>(jointInfo));
+			break;
+		default:
+			assert(false);
+			return null;
+	}
+	// If the collision between the two bodies of the raint is disabled
+	if (!jointInfo.isCollisionEnabled) {
+		// Add the pair of bodies in the set of body pairs that cannot collide with each other
+		this.collisionDetection.addNoCollisionPair(jointInfo.body1, jointInfo.body2);
+	}
+	// Add the joint into the world
+	this.joints.add(newJoint);
+	// Add the joint into the joint list of the bodies involved in the joint
+	addJointToBody(newJoint);
+	// Return the pointer to the created joint
+	return newJoint;
+}
+
+void ephysics::DynamicsWorld::destroyJoint(Joint* joint) {
+	if (joint == null) {
+		EPHYWARNING("Request destroy null joint");
+		return;
+	}
+	// If the collision between the two bodies of the raint was disabled
+	if (!joint.isCollisionEnabled()) {
+		// Remove the pair of bodies from the set of body pairs that cannot collide with each other
+		this.collisionDetection.removeNoCollisionPair(joint.getBody1(), joint.getBody2());
+	}
+	// Wake up the two bodies of the joint
+	joint.getBody1().setIsSleeping(false);
+	joint.getBody2().setIsSleeping(false);
+	// Remove the joint from the world
+	this.joints.erase(this.joints.find(joint));
+	// Remove the joint from the joint list of the bodies involved in the joint
+	joint.this.body1.removeJointFrothis.jointsList(joint);
+	joint.this.body2.removeJointFrothis.jointsList(joint);
+	long nbBytes = joint.getSizeInBytes();
+	// Call the destructor of the joint
+	ETKDELETE(Joint, joint);
+	joint = null;
+}
+
+void ephysics::DynamicsWorld::addJointToBody(ephysics::Joint* joint) {
+	if (joint == null) {
+		EPHYWARNING("Request add null joint");
+		return;
+	}
+	// Add the joint at the beginning of the linked list of joints of the first body
+	joint.this.body1.this.jointsList = ETKNEW(JointListElement, joint, joint.this.body1.this.jointsList);
+	// Add the joint at the beginning of the linked list of joints of the second body
+	joint.this.body2.this.jointsList = ETKNEW(JointListElement, joint, joint.this.body2.this.jointsList);
+}
+
+void ephysics::DynamicsWorld::computeIslands() {
+	PROFILE("ephysics::DynamicsWorld::computeIslands()");
+	int nbBodies = this.rigidBodies.size();
+	// Clear all the islands
+	for (auto it: this.islands) {
+		ETKDELETE(Island, it);
+		it = null;
+	}
+	// Call the island destructor
+	this.islands.clear();
+	int nbContactManifolds = 0;
+	// Reset all the isAlreadyInIsland variables of bodies, joints and contact manifolds
+	for (Set<ephysics::RigidBody*>::Iterator it = this.rigidBodies.begin(); it != this.rigidBodies.end(); ++it) {
+		int nbBodyManifolds = (*it).resetIsAlreadyInIslandAndCountManifolds();
+		nbContactManifolds += nbBodyManifolds;
+	}
+	for (Set<ephysics::Joint*>::Iterator it = this.joints.begin(); it != this.joints.end(); ++it) {
+		(*it).this.isAlreadyInIsland = false;
+	}
+	// Create a stack (using an array) for the rigid bodies to visit during the Depth First Search
+	Vector<ephysics::RigidBody*> stackBodiesToVisit;
+	stackBodiesToVisit.resize(nbBodies, null);
+	// For each rigid body of the world
+	for (Set<ephysics::RigidBody*>::Iterator it = this.rigidBodies.begin(); it != this.rigidBodies.end(); ++it) {
+		ephysics::RigidBody* body = *it;
+		// If the body has already been added to an island, we go to the next body
+		if (body.this.isAlreadyInIsland) {
+			continue;
+		}
+		// If the body is static, we go to the next body
+		if (body.getType() == STATIC) {
+			continue;
+		}
+		// If the body is sleeping or inactive, we go to the next body
+		if (body.isSleeping() || !body.isActive()) {
+			continue;
+		}
+		// Reset the stack of bodies to visit
+		int stackIndex = 0;
+		stackBodiesToVisit[stackIndex] = body;
+		stackIndex++;
+		body.this.isAlreadyInIsland = true;
+		// Create the new island
+		this.islands.pushBack(ETKNEW(Island, nbBodies, nbContactManifolds, this.joints.size()));
+		// While there are still some bodies to visit in the stack
+		while (stackIndex > 0) {
+			// Get the next body to visit from the stack
+			stackIndex--;
+			ephysics::RigidBody* bodyToVisit = stackBodiesToVisit[stackIndex];
+			assert(bodyToVisit.isActive());
+			// Awake the body if it is slepping
+			bodyToVisit.setIsSleeping(false);
+			// Add the body into the island
+			this.islands.back().addBody(bodyToVisit);
+			// If the current body is static, we do not want to perform the DFS
+			// search across that body
+			if (bodyToVisit.getType() == STATIC) {
+				continue;
+			}
+			// For each contact manifold in which the current body is involded
+			ephysics::ContactManifoldListElement* contactElement;
+			for (contactElement = bodyToVisit.this.contactManifoldsList;
+			     contactElement != null;
+			     contactElement = contactElement.next) {
+				ephysics::ContactManifold* contactManifold = contactElement.contactManifold;
+				assert(contactManifold.getNbContactPoints() > 0);
+				// Check if the current contact manifold has already been added into an island
+				if (contactManifold.isAlreadyInIsland()) {
+					continue;
+				}
+				// Add the contact manifold into the island
+				this.islands.back().addContactManifold(contactManifold);
+				contactManifold.this.isAlreadyInIsland = true;
+				// Get the other body of the contact manifold
+				ephysics::RigidBody* body1 = staticcast<ephysics::RigidBody*>(contactManifold.getBody1());
+				ephysics::RigidBody* body2 = staticcast<ephysics::RigidBody*>(contactManifold.getBody2());
+				ephysics::RigidBody* otherBody = (body1.getID() == bodyToVisit.getID()) ? body2 : body1;
+				// Check if the other body has already been added to the island
+				if (otherBody.this.isAlreadyInIsland) {
+					continue;
+				}
+				// Insert the other body into the stack of bodies to visit
+				stackBodiesToVisit[stackIndex] = otherBody;
+				stackIndex++;
+				otherBody.this.isAlreadyInIsland = true;
+			}
+			// For each joint in which the current body is involved
+			ephysics::JointListElement* jointElement;
+			for (jointElement = bodyToVisit.this.jointsList;
+			     jointElement != null;
+			     jointElement = jointElement.next) {
+				ephysics::Joint* joint = jointElement.joint;
+				// Check if the current joint has already been added into an island
+				if (joint.isAlreadyInIsland()) continue;
+				// Add the joint into the island
+				this.islands.back().addJoint(joint);
+				joint.this.isAlreadyInIsland = true;
+				// Get the other body of the contact manifold
+				ephysics::RigidBody* body1 = staticcast<ephysics::RigidBody*>(joint.getBody1());
+				ephysics::RigidBody* body2 = staticcast<ephysics::RigidBody*>(joint.getBody2());
+				ephysics::RigidBody* otherBody = (body1.getID() == bodyToVisit.getID()) ? body2 : body1;
+				// Check if the other body has already been added to the island
+				if (otherBody.this.isAlreadyInIsland) continue;
+				// Insert the other body into the stack of bodies to visit
+				stackBodiesToVisit[stackIndex] = otherBody;
+				stackIndex++;
+				otherBody.this.isAlreadyInIsland = true;
+			}
+		}
+		this.islands.back().resetStaticBobyNotInIsland();
+	}
+}
+
+void ephysics::DynamicsWorld::updateSleepingBodies() {
+	PROFILE("ephysics::DynamicsWorld::updateSleepingBodies()");
+	 float sleepLinearVelocitySquare = this.sleepLinearVelocity * this.sleepLinearVelocity;
+	 float sleepAngularVelocitySquare = this.sleepAngularVelocity * this.sleepAngularVelocity;
+	// For each island of the world
+	for (int i=0; i<this.islands.size(); i++) {
+		float minSleepTime = FLTMAX;
+		// For each body of the island
+		ephysics::RigidBody** bodies = this.islands[i].getBodies();
+		for (int b=0; b < this.islands[i].getNbBodies(); b++) {
+			// Skip static bodies
+			if (bodies[b].getType() == STATIC) continue;
+			// If the body is velocity is large enough to stay awake
+			if (bodies[b].getLinearVelocity().length2() > sleepLinearVelocitySquare ||
+				bodies[b].getAngularVelocity().length2() > sleepAngularVelocitySquare ||
+				!bodies[b].isAllowedToSleep()) {
+				// Reset the sleep time of the body
+				bodies[b].this.sleepTime = 0.0f;
+				minSleepTime = 0.0f;
+			} else {  // If the body velocity is bellow the sleeping velocity threshold
+				// Increase the sleep time
+				bodies[b].this.sleepTime += this.timeStep;
+				if (bodies[b].this.sleepTime < minSleepTime) {
+					minSleepTime = bodies[b].this.sleepTime;
+				}
+			}
+		}
+		// If the velocity of all the bodies of the island is under the
+		// sleeping velocity threshold for a period of time larger than
+		// the time required to become a sleeping body
+		if (minSleepTime >= this.timeBeforeSleep) {
+			// Put all the bodies of the island to sleep
+			for (int b=0; b < this.islands[i].getNbBodies(); b++) {
+				bodies[b].setIsSleeping(true);
+			}
+		}
+	}
+}
+
+void ephysics::DynamicsWorld::enableSleeping(boolean isSleepingEnabled) {
+	this.isSleepingEnabled = isSleepingEnabled;
+	if (!this.isSleepingEnabled) {
+		// For each body of the world
+		Set<ephysics::RigidBody*>::Iterator it;
+		for (it = this.rigidBodies.begin(); it != this.rigidBodies.end(); ++it) {
+			// Wake up the rigid body
+			(*it).setIsSleeping(false);
+		}
+	}
+}
+
+void ephysics::DynamicsWorld::testCollision( ephysics::ProxyShape* shape, ephysics::CollisionCallback* callback) {
+	// Create the sets of shapes
+	Set<int> shapes;
+	shapes.add(shape.this.broadPhaseID);
+	Set<int> emptySet;
+	// Perform the collision detection and report contacts
+	this.collisionDetection.reportCollisionBetweenShapes(callback, shapes, emptySet);
+}
+
+void ephysics::DynamicsWorld::testCollision( ephysics::ProxyShape* shape1,  ephysics::ProxyShape* shape2, ephysics::CollisionCallback* callback) {
+	// Create the sets of shapes
+	Set<int> shapes1;
+	shapes1.add(shape1.this.broadPhaseID);
+	Set<int> shapes2;
+	shapes2.add(shape2.this.broadPhaseID);
+	// Perform the collision detection and report contacts
+	this.collisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2);
+}
+
+void ephysics::DynamicsWorld::testCollision( ephysics::CollisionBody* body, ephysics::CollisionCallback* callback) {
+	// Create the sets of shapes
+	Set<int> shapes1;
+	// For each shape of the body
+	for ( ProxyShape* shape = body.getProxyShapesList();
+	     shape != null;
+	     shape = shape.getNext()) {
+		shapes1.add(shape.this.broadPhaseID);
+	}
+	Set<int> emptySet;
+	// Perform the collision detection and report contacts
+	this.collisionDetection.reportCollisionBetweenShapes(callback, shapes1, emptySet);
+}
+
+void ephysics::DynamicsWorld::testCollision( ephysics::CollisionBody* body1,  ephysics::CollisionBody* body2, ephysics::CollisionCallback* callback) {
+	// Create the sets of shapes
+	Set<int> shapes1;
+	for ( ProxyShape* shape=body1.getProxyShapesList(); shape != null;
+		 shape = shape.getNext()) {
+		shapes1.add(shape.this.broadPhaseID);
+	}
+	Set<int> shapes2;
+	for ( ProxyShape* shape=body2.getProxyShapesList(); shape != null;
+		 shape = shape.getNext()) {
+		shapes2.add(shape.this.broadPhaseID);
+	}
+	// Perform the collision detection and report contacts
+	this.collisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2);
+}
+
+void ephysics::DynamicsWorld::testCollision(ephysics::CollisionCallback* callback) {
+	Set<int> emptySet;
+	// Perform the collision detection and report contacts
+	this.collisionDetection.reportCollisionBetweenShapes(callback, emptySet, emptySet);
+}
+
+Vector< ephysics::ContactManifold*> ephysics::DynamicsWorld::getContactsList()  {
+	Vector< ephysics::ContactManifold*> contactManifolds;
+	// For each currently overlapping pair of bodies
+	Map<ephysics::overlappingpairid, ephysics::OverlappingPair*>::Iterator it;
+	for (it = this.collisionDetection.overlappingPairs.begin();
+	     it != this.collisionDetection.overlappingPairs.end();
+	     ++it) {
+		ephysics::OverlappingPair* pair = it.second;
+		// For each contact manifold of the pair
+		 ephysics::ContactManifoldSet manifoldSet = pair.getContactManifoldSet();
+		for (int i=0; i<manifoldSet.getNbContactManifolds(); i++) {
+			ContactManifold* manifold = manifoldSet.getContactManifold(i);
+			// Get the contact manifold
+			contactManifolds.pushBack(manifold);
+		}
+	}
+	// Return all the contact manifold
+	return contactManifolds;
+}
+
+void ephysics::DynamicsWorld::resetBodiesForceAndTorque() {
+	// For each body of the world
+	Set<ephysics::RigidBody*>::Iterator it;
+	for (it = this.rigidBodies.begin(); it != this.rigidBodies.end(); ++it) {
+		(*it).this.externalForce.setZero();
+		(*it).this.externalTorque.setZero();
+	}
+}
+
+int ephysics::DynamicsWorld::getNbIterationsVelocitySolver()  {
+	return this.nbVelocitySolverIterations;
+}
+
+void ephysics::DynamicsWorld::setNbIterationsVelocitySolver(int nbIterations) {
+	this.nbVelocitySolverIterations = nbIterations;
+}
+
+int ephysics::DynamicsWorld::getNbIterationsPositionSolver()  {
+	return this.nbPositionSolverIterations;
+}
+
+void ephysics::DynamicsWorld::setNbIterationsPositionSolver(int nbIterations) {
+	this.nbPositionSolverIterations = nbIterations;
+}
+
+void ephysics::DynamicsWorld::setContactsPositionCorrectionTechnique(ephysics::ContactsPositionCorrectionTechnique technique) {
+	if (technique == BAUMGARTECONTACTS) {
+		this.contactSolver.setIsSplitImpulseActive(false);
+	} else {
+		this.contactSolver.setIsSplitImpulseActive(true);
+	}
+}
+
+void ephysics::DynamicsWorld::setJointsPositionCorrectionTechnique(ephysics::JointsPositionCorrectionTechnique technique) {
+	if (technique == BAUMGARTEJOINTS) {
+		this.raintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(false);
+	} else {
+		this.raintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(true);
+	}
+}
+
+void ephysics::DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(boolean isActive) {
+	this.contactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive);
+}
+
+Vector3f ephysics::DynamicsWorld::getGravity()  {
+	return this.gravity;
+}
+
+void ephysics::DynamicsWorld::setGravity(Vector3f gravity) {
+	this.gravity = gravity;
+}
+
+boolean ephysics::DynamicsWorld::isGravityEnabled()  {
+	return this.isGravityEnabled;
+}
+
+void ephysics::DynamicsWorld::setIsGratityEnabled(boolean isGravityEnabled) {
+	this.isGravityEnabled = isGravityEnabled;
+}
+
+int ephysics::DynamicsWorld::getNbRigidBodies()  {
+	return this.rigidBodies.size();
+}
+
+int ephysics::DynamicsWorld::getNbJoints()  {
+	return this.joints.size();
+}
+
+Set<ephysics::RigidBody*>::Iterator ephysics::DynamicsWorld::getRigidBodiesBeginIterator() {
+	return this.rigidBodies.begin();
+}
+
+Set<ephysics::RigidBody*>::Iterator ephysics::DynamicsWorld::getRigidBodiesEndIterator() {
+	return this.rigidBodies.end();
+}
+
+boolean ephysics::DynamicsWorld::isSleepingEnabled()  {
+	return this.isSleepingEnabled;
+}
+
+float ephysics::DynamicsWorld::getSleepLinearVelocity()  {
+	return this.sleepLinearVelocity;
+}
+
+void ephysics::DynamicsWorld::setSleepLinearVelocity(float sleepLinearVelocity) {
+	if(sleepLinearVelocity < 0.0f) {
+		Log.error("Can not set sleepLinearVelocity=" + sleepLinearVelocity + " < 0 ");
+		return;
+	}
+	this.sleepLinearVelocity = sleepLinearVelocity;
+}
+
+float ephysics::DynamicsWorld::getSleepAngularVelocity()  {
+	return this.sleepAngularVelocity;
+}
+
+void ephysics::DynamicsWorld::setSleepAngularVelocity(float sleepAngularVelocity) {
+	if(sleepAngularVelocity < 0.0f) {
+		Log.error("Can not set sleepAngularVelocity=" + sleepAngularVelocity + " < 0 ");
+		return;
+	}
+	this.sleepAngularVelocity = sleepAngularVelocity;
+}
+
+float ephysics::DynamicsWorld::getTimeBeforeSleep()  {
+	return this.timeBeforeSleep;
+}
+
+void ephysics::DynamicsWorld::setTimeBeforeSleep(float timeBeforeSleep) {
+	if(timeBeforeSleep < 0.0f) {
+		Log.error("Can not set timeBeforeSleep=" + timeBeforeSleep + " < 0 ");
+		return;
+	}
+	this.timeBeforeSleep = timeBeforeSleep;
+}
+
+void ephysics::DynamicsWorld::setEventListener(ephysics::EventListener* eventListener) {
+	this.eventListener = eventListener;
+}
+

src/org/atriaSoft/ephysics/engine/EventListener.java

@@ -0,0 +1,41 @@
+package org.atriaSoft.ephysics.engine;
+
+	/**
+	 * @brief This class can be used to receive event callbacks from the physics engine.
+	 * In order to receive callbacks, you need to create a new class that inherits from
+	 * this one and you must  the methods you need. Then, you need to register your
+	 * new event listener class to the physics world using the DynamicsWorld::setEventListener()
+	 * method.
+	 */
+	class EventListener {
+		public :
+			/**
+			 * @brief Generic Constructor
+			 */
+			EventListener() {}
+			/**
+			 * @brief Called when a new contact point is found between two bodies that were separated before
+			 * @param contact Information about the contact
+			 */
+			 void beginContact( ContactPointInfo contact) {};
+			/**
+			 * @brief Called when a new contact point is found between two bodies
+			 * @param contact Information about the contact
+			 */
+			 void newContact( ContactPointInfo contact) {}
+			/**
+			 * @brief Called at the beginning of an internal tick of the simulation step.
+			 * Each time the DynamicsWorld::update() method is called, the physics
+			 * engine will do several internal simulation steps. This method is
+			 * called at the beginning of each internal simulation step.
+			 */
+			 void beginInternalTick() {}
+			/**
+			 * @brief Called at the end of an internal tick of the simulation step.
+			 * Each time the DynamicsWorld::update() metho is called, the physics
+			 * engine will do several internal simulation steps. This method is
+			 * called at the end of each internal simulation step.
+			 */
+			 void endInternalTick() {}
+	};
+}

src/org/atriaSoft/ephysics/engine/Impulse.java

@@ -0,0 +1,32 @@
+package org.atriaSoft.ephysics.engine;
+
+	/**
+	 * @brief Represents an impulse that we can apply to bodies in the contact or raint solver.
+	 */
+	class Impulse {
+		public:
+			 Vector3f linearImpulseBody1; //!< Linear impulse applied to the first body
+			 Vector3f angularImpulseBody1; //!< Angular impulse applied to the first body
+			 Vector3f linearImpulseBody2; //!< Linear impulse applied to the second body
+			 Vector3f angularImpulseBody2; //!< Angular impulse applied to the second body
+			/// Constructor
+			Impulse( Vector3f initLinearImpulseBody1,
+			         Vector3f initAngularImpulseBody1,
+			         Vector3f initLinearImpulseBody2,
+			         Vector3f initAngularImpulseBody2):
+			  linearImpulseBody1(initLinearImpulseBody1),
+			  angularImpulseBody1(initAngularImpulseBody1),
+			  linearImpulseBody2(initLinearImpulseBody2),
+			  angularImpulseBody2(initAngularImpulseBody2) {
+				
+			}
+			/// Copy-ructor
+			Impulse( Impulse impulse):
+			  linearImpulseBody1(impulse.linearImpulseBody1),
+			  angularImpulseBody1(impulse.angularImpulseBody1),
+			  linearImpulseBody2(impulse.linearImpulseBody2),
+			  angularImpulseBody2(impulse.angularImpulseBody2) {
+				
+			}
+	};
+}

src/org/atriaSoft/ephysics/engine/Island.cpp

@@ -0,0 +1,70 @@
+/** @file
+ * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
+ * @author Daniel CHAPPUIS
+ * @author Edouard DUPIN
+ * @copyright 2010-2016, Daniel Chappuis
+ * @copyright 2017, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+#include <ephysics/engine/Island.hpp>
+#include <ephysics/debug.hpp>
+
+using namespace ephysics;
+
+ephysics::Island::Island(long nbMaxBodies,
+                         long nbMaxContactManifolds,
+                         long nbMaxJoints) {
+	// Allocate memory for the arrays
+	this.bodies.reserve(nbMaxBodies);
+	this.contactManifolds.reserve(nbMaxContactManifolds);
+	this.joints.reserve(nbMaxJoints);
+}
+
+
+void ephysics::Island::addBody(ephysics::RigidBody* body) {
+	if (body.isSleeping() == true) {
+		Log.error("Try to add a body that is sleeping ...");
+		return;
+	}
+	this.bodies.pushBack(body);
+}
+
+void ephysics::Island::addContactManifold(ephysics::ContactManifold* contactManifold) {
+	this.contactManifolds.pushBack(contactManifold);
+}
+
+void ephysics::Island::addJoint(ephysics::Joint* joint) {
+	this.joints.pushBack(joint);
+}
+
+long ephysics::Island::getNbBodies()  {
+	return this.bodies.size();
+}
+
+long ephysics::Island::getNbContactManifolds()  {
+	return this.contactManifolds.size();
+}
+
+long ephysics::Island::getNbJoints()  {
+	return this.joints.size();
+}
+
+ephysics::RigidBody** ephysics::Island::getBodies() {
+	return this.bodies[0];
+}
+
+ephysics::ContactManifold** ephysics::Island::getContactManifold() {
+	return this.contactManifolds[0];
+}
+
+ephysics::Joint** ephysics::Island::getJoints() {
+	return this.joints[0];
+}
+
+void ephysics::Island::resetStaticBobyNotInIsland() {
+	for (auto it: this.bodies) {
+		if (it.getType() == STATIC) {
+			it.this.isAlreadyInIsland = false;
+		}
+	}
+}