[DEV] end port of c++ ephysics engine

.classpath

@@ -0,0 +1,29 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<classpath>
+	<classpathentry excluding="**/*.java__|**/__*.java" kind="src" path="src">
+		<attributes>
+			<attribute name="optional" value="true"/>
+		</attributes>
+	</classpathentry>
+	<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER/org.eclipse.jdt.internal.debug.ui.launcher.StandardVMType/JavaSE-14">
+		<attributes>
+			<attribute name="module" value="true"/>
+		</attributes>
+	</classpathentry>
+	<classpathentry kind="con" path="org.eclipse.jdt.junit.JUNIT_CONTAINER/5">
+		<attributes>
+			<attribute name="test" value="true"/>
+		</attributes>
+	</classpathentry>
+	<classpathentry combineaccessrules="false" kind="src" path="/atriasoft-etk">
+		<attributes>
+			<attribute name="module" value="true"/>
+		</attributes>
+	</classpathentry>
+	<classpathentry combineaccessrules="false" kind="src" path="/scenarium-logger">
+		<attributes>
+			<attribute name="module" value="true"/>
+		</attributes>
+	</classpathentry>
+	<classpathentry kind="output" path="out/eclipse/classes"/>
+</classpath>

.gitignore

@@ -0,0 +1,3 @@
+/examples/target/
+/examples/framework.log.*
+/target/

.project

@@ -0,0 +1,18 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>atriasoft-ephysics</name>
+	<comment></comment>
+	<projects>
+		<project>atriasoft-ephysics</project>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>org.eclipse.jdt.core.javabuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>org.eclipse.jdt.core.javanature</nature>
+	</natures>
+</projectDescription>

src/module-info.java

@@ -0,0 +1,21 @@
+/** Basic module interface.
+ *
+ * @author Edouard DUPIN */
+
+open module org.atriasoft.ephysics {
+	exports org.atriasoft.ephysics.body;
+	exports org.atriasoft.ephysics.collision;
+	exports org.atriasoft.ephysics.collision.broadphase;
+	exports org.atriasoft.ephysics.collision.narrowphase;
+	exports org.atriasoft.ephysics.collision.narrowphase.EPA;
+	exports org.atriasoft.ephysics.collision.narrowphase.GJK;
+	exports org.atriasoft.ephysics.collision.shapes;
+	exports org.atriasoft.ephysics.configuration;
+	exports org.atriasoft.ephysics.constraint;
+	exports org.atriasoft.ephysics.engine;
+	exports org.atriasoft.ephysics.mathematics;
+	exports org.atriasoft.ephysics;
+	
+	requires transitive org.atriasoft.etk;
+	requires javafx.base;
+}

src/org/atriasoft/ephysics/Configuration.java

@@ -0,0 +1,54 @@
+package org.atriasoft.ephysics;
+
+import org.atriasoft.etk.math.Constant;
+
+public class Configuration {
+	
+	/// 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 static final float DYNAMIC_TREE_AABB_GAP = 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 static final float DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER = 1.7f;
+	
+	/// Distance threshold for two contact points for a valid persistent contact (in meters)
+	public static final float PERSISTENT_CONTACT_DIST_THRESHOLD = 0.03f;
+	
+	/// Default friction coefficient for a rigid body
+	public static final float DEFAULT_FRICTION_COEFFICIENT = 0.3f;
+	
+	/// Default bounciness factor for a rigid body
+	public static final float DEFAULT_BOUNCINESS = 0.5f;
+	
+	/// Default rolling resistance
+	public static final float DEFAULT_ROLLING_RESISTANCE = 0.0f;
+	
+	/// True if the spleeping technique is enabled
+	public static final boolean SPLEEPING_ENABLED = true;
+	
+	/// Object margin for collision detection in meters (for the GJK-EPA Algorithm)
+	public static final float OBJECT_MARGIN = 0.04f;
+	
+	/// Velocity threshold for contact velocity restitution
+	public static final float RESTITUTION_VELOCITY_THRESHOLD = 1.0f;
+	
+	/// Number of iterations when solving the velocity raints of the Sequential Impulse technique
+	public static final int DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 10;
+	
+	/// Number of iterations when solving the position raints of the Sequential Impulse technique
+	public static final int DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 5;
+	
+	/// Time (in seconds) that a body must stay still to be idered sleeping
+	public static final float DEFAULT_TIME_BEFORE_SLEEP = 0.3f; //(old default : 3.0)
+	
+	/// A body with a linear velocity smaller than the sleep linear velocity (in m/s)
+	/// might enter sleeping mode.
+	public static final float DEFAULT_SLEEP_LINEAR_VELOCITY = 0.3f; //(old default : 0.01)
+	
+	/// A body with angular velocity smaller than the sleep angular velocity (in rad/s)
+	/// might enter sleeping mode
+	public static final float DEFAULT_SLEEP_ANGULAR_VELOCITY = 5.0f * (Constant.PI / 180.0f); //(old default : 0.3)
+}

src/org/atriasoft/ephysics/RaycastCallback.java

@@ -0,0 +1,20 @@
+package org.atriasoft.ephysics;
+
+public interface RaycastCallback {
+	/**
+	 * @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);
+}

src/org/atriasoft/ephysics/RaycastInfo.java

@@ -0,0 +1,16 @@
+package org.atriasoft.ephysics;
+
+import org.atriasoft.etk.math.Vector3f;
+
+import org.atriasoft.ephysics.body.CollisionBody;
+import org.atriasoft.ephysics.collision.ProxyShape;
+
+public class RaycastInfo {
+	public Vector3f worldPoint = new Vector3f(); //!< Hit point in world-space coordinates
+	public Vector3f worldNormal = new Vector3f(); //!< Surface normal at hit point in world-space coordinates
+	public float hitFraction = 0.0f; //!< 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)
+	public long meshSubpart = -1; //!< Mesh subpart index that has been hit (only used for triangles mesh and -1 otherwise)
+	public long triangleIndex = -1; //!< Hit triangle index (only used for triangles mesh and -1 otherwise)
+	public CollisionBody body; //!< Pointer to the hit collision body;
+	public ProxyShape proxyShape; //!< Pointer to the hit proxy collision shape
+}

src/org/atriasoft/ephysics/RaycastTest.java

@@ -0,0 +1,28 @@
+package org.atriasoft.ephysics;
+
+import org.atriasoft.ephysics.collision.ProxyShape;
+import org.atriasoft.ephysics.mathematics.Ray;
+
+public class RaycastTest {
+	
+	public RaycastCallback userCallback; //!< User callback class
+	/// Constructor
+	
+	public RaycastTest(final RaycastCallback _callback) {
+		this.userCallback = _callback;
+	}
+	
+	/// Ray cast test against a proxy shape
+	public float raycastAgainstShape(final ProxyShape _shape, final Ray _ray) {
+		// Ray casting test against the collision shape
+		final RaycastInfo raycastInfo = new RaycastInfo();
+		final 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 this.userCallback.notifyRaycastHit(raycastInfo);
+		}
+		return _ray.maxFraction;
+	}
+}

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

@@ -0,0 +1,154 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.body;
+
+/**
+ * This class is an abstract class to represent a body of the physics engine.
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public abstract class Body {
+	// Unique ID of the body
+	protected int id; // TODO check where it came from ...
+	// True if the body has already been added in an island (for sleeping technique)
+	public boolean isAlreadyInIsland = false;
+	// True if the body is allowed to go to sleep for better efficiency
+	protected boolean isAllowedToSleep = true;
+	/**
+	 * 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 = true;
+	// True if the body is sleeping (for sleeping technique)
+	protected boolean isSleeping = false;
+	// Elapsed time since the body velocity was bellow the sleep velocity
+	public float sleepTime = 0;
+	// user data
+	protected Object userData = null;
+	
+	/**
+	 * @brief Constructor
+	 * @param[in] _id ID of the new body
+	 */
+	public Body(final int bodyID) {
+		this.id = bodyID;
+	}
+	
+	// Return the id of the body
+	public int getID() {
+		return this.id;
+	}
+	
+	public float getSleepTime() {
+		return this.sleepTime;
+	}
+	
+	/**
+	 * @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
+	 */
+	Object getUserData() {
+		return this.userData;
+	}
+	
+	/**
+	 * @brief Return the id of the body
+	 * @return The ID of the body
+	 */
+	public boolean isActive() {
+		return this.isActive;
+	}
+	
+	/**
+	 * @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 boolean isAllowedToSleep() {
+		return this.isAllowedToSleep;
+	}
+	
+	public boolean isAlreadyInIsland() {
+		return this.isAlreadyInIsland;
+	}
+	
+	/**
+	 * @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 Set whether or not the body is active
+	 * @param[in] _isActive True if you want to activate the body
+	 */
+	void setIsActive(final boolean isActive) {
+		this.isActive = isActive;
+	}
+	
+	// Set whether or not the body is allowed to go to sleep
+	public void setIsAllowedToSleep(final boolean isAllowedToSleep) {
+		this.isAllowedToSleep = isAllowedToSleep;
+		
+		if (!this.isAllowedToSleep) {
+			setIsSleeping(false);
+		}
+	}
+	
+	public void setIsAlreadyInIsland(final boolean isAlreadyInIsland) {
+		this.isAlreadyInIsland = isAlreadyInIsland;
+	}
+	
+	/**
+	 * @brief Set the variable to know whether or not the body is sleeping
+	 * @param[in] _isSleeping Set the new status
+	 */
+	public void setIsSleeping(final boolean isSleeping) {
+		if (isSleeping) {
+			this.sleepTime = 0.0f;
+		} else if (this.isSleeping) {
+			this.sleepTime = 0.0f;
+		}
+		this.isSleeping = isSleeping;
+	}
+	
+	public void setSleepTime(final float sleepTime) {
+		this.sleepTime = sleepTime;
+	}
+	
+	/**
+	 * @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(final Object userData) {
+		this.userData = userData;
+	}
+	
+}

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,404 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.body;
+
+import org.atriasoft.ephysics.RaycastInfo;
+import org.atriasoft.ephysics.collision.ContactManifoldListElement;
+import org.atriasoft.ephysics.collision.ProxyShape;
+import org.atriasoft.ephysics.collision.shapes.AABB;
+import org.atriasoft.ephysics.collision.shapes.CollisionShape;
+import org.atriasoft.ephysics.engine.CollisionWorld;
+import org.atriasoft.ephysics.internal.Log;
+import org.atriasoft.ephysics.mathematics.Ray;
+import org.atriasoft.etk.math.Transform3D;
+import org.atriasoft.etk.math.Vector3f;
+
+/**
+ * This class represents a body that is able to collide with others bodies. This class inherits from the Body class.
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class CollisionBody extends Body {
+	// Type of body (static, kinematic or dynamic)
+	protected BodyType type;
+	// Position and orientation of the body
+	protected Transform3D transform;
+	// First element of the linked list of proxy collision shapes of this body
+	protected ProxyShape proxyCollisionShapes;
+	// Number of collision shapes
+	protected long numberCollisionShapes;
+	// First element of the linked list of contact manifolds involving this body
+	public ContactManifoldListElement contactManifoldsList;
+	// Reference to the world the body belongs to
+	protected CollisionWorld world;
+	
+	/**
+	 * @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(final Transform3D _transform, final CollisionWorld _world, final int _id) {
+		super(_id);
+		this.type = BodyType.DYNAMIC;
+		this.transform = _transform;
+		this.proxyCollisionShapes = null;
+		this.numberCollisionShapes = 0;
+		this.contactManifoldsList = null;
+		this.world = _world;
+		
+		//Log.debug("         set transform: " + _transform);
+		if (Float.isNaN(_transform.getPosition().x)) {
+			Log.critical("         set transform: " + _transform);
+		}
+		if (Float.isInfinite(_transform.getOrientation().z)) {
+			Log.critical("         set transform: " + _transform);
+		}
+	}
+	
+	/**
+	 * @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 int32_to 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(final CollisionShape _collisionShape, final Transform3D _transform) {
+		// Create a proxy collision shape to attach the collision shape to the body
+		final ProxyShape proxyShape = new ProxyShape(this, _collisionShape, _transform, 1.0f);
+		// Add it to the list of proxy collision shapes of the body
+		if (this.proxyCollisionShapes == null) {
+			this.proxyCollisionShapes = proxyShape;
+		} else {
+			proxyShape.next = this.proxyCollisionShapes;
+			this.proxyCollisionShapes = proxyShape;
+		}
+		final AABB aabb = new AABB();
+		_collisionShape.computeAABB(aabb, this.transform.multiplyNew(_transform));
+		this.world.collisionDetection.addProxyCollisionShape(proxyShape, aabb);
+		this.numberCollisionShapes++;
+		return proxyShape;
+	}
+	
+	/**
+	 * @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.getNext()) {
+			this.world.getCollisionDetection().askForBroadPhaseCollisionCheck(shape);
+		}
+	}
+	
+	/**
+	 * @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() {
+		final AABB bodyAABB = new AABB();
+		if (this.proxyCollisionShapes == null) {
+			return bodyAABB;
+		}
+		//Log.info("tmp this.transform : " + this.transform);
+		//Log.info("tmp this.proxyCollisionShapes.getLocalToBodyTransform() : " + this.proxyCollisionShapes.getLocalToBodyTransform());
+		//Log.info("tmp this.transform.multiplyNew(this.proxyCollisionShapes.getLocalToBodyTransform()) : " + this.transform.multiplyNew(this.proxyCollisionShapes.getLocalToBodyTransform()));
+		this.proxyCollisionShapes.getCollisionShape().computeAABB(bodyAABB, this.transform.multiplyNew(this.proxyCollisionShapes.getLocalToBodyTransform()));
+		for (ProxyShape shape = this.proxyCollisionShapes.next; shape != null; shape = shape.next) {
+			final AABB aabb = new AABB();
+			shape.getCollisionShape().computeAABB(aabb, this.transform.multiplyNew(shape.getLocalToBodyTransform()));
+			bodyAABB.mergeWithAABB(aabb);
+		}
+		//Log.info("tmp aabb : " + bodyAABB);
+		//Log.info("tmp aabb : " + bodyAABB.getMax().lessNew(bodyAABB.getMin()));
+		return bodyAABB;
+	}
+	
+	/**
+	 * @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 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(final Vector3f _worldPoint) {
+		return this.transform.inverseNew().multiply(_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(final Vector3f _worldVector) {
+		return this.transform.getOrientation().inverseNew().multiply(_worldVector);
+	}
+	
+	/**
+	 * @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 Return the current position and orientation
+	 * @return The current transformation of the body that transforms the local-space of the body int32_to world-space
+	 */
+	public Transform3D getTransform() {
+		return this.transform;
+	}
+	
+	/**
+	 * @brief Return the type of the body
+	 * @return the type of the body (STATIC, KINEMATIC, DYNAMIC)
+	 */
+	public BodyType getType() {
+		return this.type;
+	}
+	
+	public CollisionWorld getWorld() {
+		return this.world;
+	}
+	
+	/**
+	 * @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(final Vector3f _localPoint) {
+		return this.transform.multiplyNew(_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(final Vector3f _localVector) {
+		return this.transform.getOrientation().multiply(_localVector);
+	}
+	
+	/**
+	 * @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(final Ray _ray, final RaycastInfo _raycastInfo) {
+		if (this.isActive == false) {
+			return false;
+		}
+		boolean isHit = false;
+		
+		final Ray rayTemp = new Ray(_ray);
+		for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) {
+			// Test if the ray hits the collision shape
+			if (shape.raycast(rayTemp, _raycastInfo)) {
+				rayTemp.maxFraction = _raycastInfo.hitFraction;
+				isHit = true;
+			}
+		}
+		return isHit;
+	}
+	
+	/**
+	 * @brief Remove all the collision shapes
+	 */
+	public 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
+			final ProxyShape nextElement = current.next;
+			if (this.isActive) {
+				this.world.collisionDetection.removeProxyCollisionShape(current);
+			}
+			// Get the next element in the list
+			current = nextElement;
+		}
+		this.proxyCollisionShapes = null;
+	}
+	
+	/**
+	 * @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(final ProxyShape _proxyShape) {
+		ProxyShape current = this.proxyCollisionShapes;
+		// If the the first proxy shape is the one to remove
+		if (current == _proxyShape) {
+			this.proxyCollisionShapes = current.next;
+			if (this.isActive) {
+				this.world.collisionDetection.removeProxyCollisionShape(current);
+			}
+			current = null;
+			this.numberCollisionShapes--;
+			return;
+		}
+		// Look for the proxy shape that contains the collision shape in parameter
+		while (current.next != null) {
+			// If we have found the collision shape to remove
+			if (current.next == _proxyShape) {
+				// Remove the proxy collision shape
+				ProxyShape elementToRemove = current.next;
+				current.next = elementToRemove.next;
+				if (this.isActive) {
+					this.world.collisionDetection.removeProxyCollisionShape(elementToRemove);
+				}
+				elementToRemove = null;
+				this.numberCollisionShapes--;
+				return;
+			}
+			// Get the next element in the list
+			current = current.next;
+		}
+	}
+	
+	/**
+	 * @brief Reset the contact manifold lists
+	 */
+	public void resetContactManifoldsList() {
+		// Delete the linked list of contact manifolds of that body
+		this.contactManifoldsList = null;
+	}
+	
+	/**
+	 * @brief Reset the this.isAlreadyInIsland variable of the body and contact manifolds.
+	 * This method also returns the number of contact manifolds of the body.
+	 */
+	public 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.isAlreadyInIsland = false;
+			currentElement = currentElement.next;
+			nbManifolds++;
+		}
+		return nbManifolds;
+	}
+	
+	/**
+	 * @brief Set whether or not the body is active
+	 * @param[in] _isActive True if you want to activate the body
+	 */
+	@Override
+	public void setIsActive(final boolean _isActive) {
+		// If the state does not change
+		if (this.isActive == _isActive) {
+			return;
+		}
+		super.setIsActive(_isActive);
+		// If we have to activate the body
+		if (_isActive == true) {
+			for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) {
+				final AABB aabb = new AABB();
+				shape.getCollisionShape().computeAABB(aabb, this.transform.multiplyNew(shape.localToBodyTransform));
+				this.world.collisionDetection.addProxyCollisionShape(shape, aabb);
+			}
+		} else {
+			for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) {
+				this.world.collisionDetection.removeProxyCollisionShape(shape);
+			}
+			resetContactManifoldsList();
+		}
+	}
+	
+	/**
+	 * @brief Set the current position and orientation
+	 * @param transform The transformation of the body that transforms the local-space of the body int32_to world-space
+	 */
+	public void setTransform(final Transform3D _transform) {
+		//Log.info("         set transform: " + this.transform + " ==> " + _transform);
+		if (Float.isNaN(_transform.getPosition().x)) {
+			Log.critical("         set transform: " + this.transform + " ==> " + _transform);
+		}
+		if (Float.isInfinite(_transform.getOrientation().z)) {
+			Log.critical("         set transform: " + this.transform + " ==> " + _transform);
+		}
+		this.transform = _transform;
+		updateBroadPhaseState();
+	}
+	
+	/**
+	 * @brief Set the type of the body
+	 * @param[in] type The type of the body (STATIC, KINEMATIC, DYNAMIC)
+	 */
+	public void setType(final BodyType _type) {
+		this.type = _type;
+		if (this.type == BodyType.STATIC) {
+			// Update the broad-phase state of the body
+			updateBroadPhaseState();
+		}
+	}
+	
+	/**
+	 * @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(final Vector3f _worldPoint) {
+		for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) {
+			if (shape.testPointInside(_worldPoint)) {
+				return true;
+			}
+		}
+		return false;
+	}
+	
+	/**
+	 * @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.getNext()) {
+			// Update the proxy
+			updateProxyShapeInBroadPhase(shape, false);
+		}
+	}
+	
+	/**
+	 * @brief Update the broad-phase state of a proxy collision shape of the body
+	 */
+	public void updateProxyShapeInBroadPhase(final ProxyShape _proxyShape, final boolean _forceReinsert /* false */) {
+		final AABB aabb = new AABB();
+		_proxyShape.getCollisionShape().computeAABB(aabb, this.transform.multiplyNew(_proxyShape.getLocalToBodyTransform()));
+		this.world.getCollisionDetection().updateProxyCollisionShape(_proxyShape, aabb, new Vector3f(0, 0, 0), _forceReinsert);
+	}
+}

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

@@ -0,0 +1,574 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.body;
+
+import org.atriasoft.ephysics.collision.ProxyShape;
+import org.atriasoft.ephysics.collision.shapes.AABB;
+import org.atriasoft.ephysics.collision.shapes.CollisionShape;
+import org.atriasoft.ephysics.constraint.Joint;
+import org.atriasoft.ephysics.constraint.JointListElement;
+import org.atriasoft.ephysics.engine.CollisionWorld;
+import org.atriasoft.ephysics.engine.DynamicsWorld;
+import org.atriasoft.ephysics.engine.Material;
+import org.atriasoft.ephysics.internal.Log;
+import org.atriasoft.etk.math.Matrix3f;
+import org.atriasoft.etk.math.Transform3D;
+import org.atriasoft.etk.math.Vector3f;
+
+/**
+ *  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.
+ */
+public class RigidBody extends CollisionBody {
+	
+	protected float initMass = 1.0f; //!< Intial mass of the body
+	protected Vector3f centerOfMassLocal = new Vector3f(0, 0, 0); //!< Center of mass of the body in local-space coordinates. The center of mass can therefore be different from the body origin
+	public Vector3f centerOfMassWorld = new Vector3f(); //!< Center of mass of the body in world-space coordinates
+	public Vector3f linearVelocity = new Vector3f(); //!< Linear velocity of the body
+	public Vector3f angularVelocity = new Vector3f(); //!< Angular velocity of the body
+	public Vector3f externalForce = new Vector3f(); //!< Current external force on the body
+	public Vector3f externalTorque = new Vector3f(); //!< Current external torque on the body
+	public Matrix3f inertiaTensorLocal = new Matrix3f(); //!< Local inertia tensor of the body (in local-space) with respect to the center of mass of the body
+	public Matrix3f inertiaTensorLocalInverse = new Matrix3f(); //!< Inverse of the inertia tensor of the body
+	public float massInverse; //!< Inverse of the mass of the body
+	protected boolean isGravityEnabled = true; //!< True if the gravity needs to be applied to this rigid body
+	protected Material material = new Material(); //!< Material properties of the rigid body
+	protected float linearDamping = 0.0f; //!< Linear velocity damping factor
+	protected float angularDamping = 0.0f; //!< Angular velocity damping factor
+	public JointListElement jointsList = null; //!< First element of the linked list of joints involving this body
+	protected boolean angularReactionEnable = true;
+	
+	/**
+	 *  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(final Transform3D transform, final CollisionWorld world, final int id) {
+		super(transform, world, id);
+		this.centerOfMassWorld = new Vector3f(transform.getPosition());
+		this.massInverse = 1.0f / this.initMass;
+	}
+	
+	/**
+	 *  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 _collisionShape The collision shape you want to add to the body
+	 * @param _transform The transformation of the collision shape that transforms the local-space of the collision shape into the local-space of the body
+	 * @param _mass Mass (in kilograms) of the collision shape you want to add
+	 * @return A pointer to the proxy shape that has been created to link the body to the new collision shape you have added.
+	 */
+	public ProxyShape addCollisionShape(final CollisionShape _collisionShape, final Transform3D _transform, final float _mass) {
+		assert (_mass > 0.0f);
+		// Create a new proxy collision shape to attach the collision shape to the body
+		final ProxyShape proxyShape = new 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.next = this.proxyCollisionShapes;
+			this.proxyCollisionShapes = proxyShape;
+		}
+		// Compute the world-space AABB of the new collision shape
+		final AABB aabb = new AABB();
+		_collisionShape.computeAABB(aabb, this.transform.multiplyNew(_transform));
+		// Notify the collision detection about this new collision shape
+		this.world.collisionDetection.addProxyCollisionShape(proxyShape, aabb);
+		this.numberCollisionShapes++;
+		recomputeMassInformation();
+		return proxyShape;
+	}
+	
+	/**
+	 *  Apply an external force to the body at a given point (in world-space coordinates).
+	 * If the point is not at the center of mass of the body, it will also
+	 * generate some torque and therefore, change the angular velocity of the body.
+	 * If the body is sleeping, calling this method will wake it up. Note that the
+	 * force will we added to the sum of the applied forces and that this sum will be
+	 * reset to zero at the end of each call of the DynamicsWorld::update() method.
+	 * You can only apply a force to a dynamic body otherwise, this method will do nothing.
+	 * @param _force The force to apply on the body
+	 * @param _point The point where the force is applied (in world-space coordinates)
+	 */
+	public void applyForce(final Vector3f _force, final Vector3f _point) {
+		if (this.type != BodyType.DYNAMIC) {
+			return;
+		}
+		if (this.isSleeping) {
+			setIsSleeping(false);
+		}
+		this.externalForce.add(_force);
+		this.externalTorque.add(_point.lessNew(this.centerOfMassWorld).cross(_force));
+	}
+	
+	/**
+	 *  Apply an external force to the body at its center of mass.
+	 * If the body is sleeping, calling this method will wake it up. Note that the
+	 * force will we added to the sum of the applied forces and that this sum will be
+	 * reset to zero at the end of each call of the DynamicsWorld::update() method.
+	 * You can only apply a force to a dynamic body otherwise, this method will do nothing.
+	 * @param _force The external force to apply on the center of mass of the body
+	 */
+	public void applyForceToCenterOfMass(final Vector3f _force) {
+		if (this.type != BodyType.DYNAMIC) {
+			return;
+		}
+		if (this.isSleeping) {
+			setIsSleeping(false);
+		}
+		this.externalForce.add(_force);
+	}
+	
+	/**
+	 *  Apply an external torque to the body.
+	 * If the body is sleeping, calling this method will wake it up. Note that the
+	 * force will we added to the sum of the applied torques and that this sum will be
+	 * reset to zero at the end of each call of the DynamicsWorld::update() method.
+	 * You can only apply a force to a dynamic body otherwise, this method will do nothing.
+	 * @param _torque The external torque to apply on the body
+	 */
+	public void applyTorque(final Vector3f _torque) {
+		if (this.type != BodyType.DYNAMIC) {
+			return;
+		}
+		if (this.isSleeping) {
+			setIsSleeping(false);
+		}
+		this.externalTorque.add(_torque);
+	}
+	
+	/**
+	 *  Set the variable to know if the gravity is applied to this rigid body
+	 * @param _isEnabled True if you want the gravity to be applied to this body
+	 */
+	public void enableGravity(final boolean _isEnabled) {
+		this.isGravityEnabled = _isEnabled;
+	}
+	
+	/**
+	 *  Get the angular velocity damping factor
+	 * @return The angular damping factor of this body
+	 */
+	public float getAngularDamping() {
+		return this.angularDamping;
+	}
+	
+	/**
+		 *  Get the angular velocity of the body
+		 * @return The angular velocity vector of the body
+		 */
+	public Vector3f getAngularVelocity() {
+		return this.angularVelocity;
+	}
+	
+	/**
+	 *  Get the inverse of the inertia tensor in world coordinates.
+	 * The inertia tensor I_w in world coordinates is computed with the
+	 * local inverse inertia tensor I_b^-1 in body coordinates
+	 * by I_w = R * I_b^-1 * R^T
+	 * where R is the rotation matrix (and R^T its transpose) of the
+	 * current orientation quaternion of the body
+	 * @return The 3x3 inverse inertia tensor matrix of the body in world-space coordinates
+	 */
+	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
+		//Log.error("}}}    this.transform=" + this.transform);
+		//Log.error("}}}    this.inertiaTensorLocalInverse=" + this.inertiaTensorLocalInverse);
+		//Log.error("}}}    this.transform.getOrientation().getMatrix().transposeNew()=" + this.transform.getOrientation().getMatrix().transposeNew());
+		//Log.error("}}}    this.transform.getOrientation().getMatrix()=" + this.transform.getOrientation().getMatrix());
+		final Matrix3f tmp = this.transform.getOrientation().getMatrix().multiplyNew(this.inertiaTensorLocalInverse).multiply(this.transform.getOrientation().getMatrix().transposeNew());
+		//Log.error("}}}    tmp=" + tmp);
+		return tmp;
+	}
+	
+	/**
+	 *  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;
+	}
+	
+	/**
+	 *  Get the inertia tensor in world coordinates.
+	 * The inertia tensor I_w in world coordinates is computed
+	 * with the local inertia tensor I_b in body coordinates
+	 * by I_w = R * I_b * R^T
+	 * where R is the rotation matrix (and R^T its transpose) of
+	 * the current orientation quaternion of the body
+	 * @return The 3x3 inertia tensor matrix of the body in world-space coordinates
+	 */
+	public Matrix3f getInertiaTensorWorld() {
+		// Compute and return the inertia tensor in world coordinates
+		return this.transform.getOrientation().getMatrix().multiplyNew(this.inertiaTensorLocal).multiply(this.transform.getOrientation().getMatrix().transposeNew());
+	}
+	
+	/**
+	 *  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;
+	}
+	
+	/**
+	 *  Get the linear velocity damping factor
+	 * @return The linear damping factor of this body
+	 */
+	public float getLinearDamping() {
+		return this.linearDamping;
+	}
+	
+	/**
+	 *  Get the linear velocity
+	 * @return The linear velocity vector of the body
+	 */
+	public Vector3f getLinearVelocity() {
+		return this.linearVelocity;
+	}
+	
+	/**
+	 *  Get the mass of the body
+	 * @return The mass (in kilograms) of the body
+	 */
+	public float getMass() {
+		return this.initMass;
+	}
+	
+	/**
+	 *  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;
+	}
+	
+	public boolean isAngularReactionEnable() {
+		return this.angularReactionEnable;
+	}
+	
+	/**
+	 *  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;
+	}
+	
+	/**
+	 *  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 == BodyType.STATIC || this.type == BodyType.KINEMATIC) {
+			this.centerOfMassWorld = this.transform.getPosition();
+			return;
+		}
+		assert (this.type == BodyType.DYNAMIC);
+		// Compute the total mass of the body
+		for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) {
+			this.initMass += shape.getMass();
+			this.centerOfMassLocal.add(shape.getLocalToBodyTransform().getPosition().multiplyNew(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
+		final Vector3f oldCenterOfMass = this.centerOfMassWorld;
+		this.centerOfMassLocal.multiply(this.massInverse);
+		this.centerOfMassWorld = this.transform.multiplyNew(this.centerOfMassLocal);
+		// Compute the total mass and inertia tensor using all the collision shapes
+		for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) {
+			// Get the inertia tensor of the collision shape in its local-space
+			Matrix3f inertiaTensor = new Matrix3f();
+			shape.getCollisionShape().computeLocalInertiaTensor(inertiaTensor, shape.getMass());
+			// Convert the collision shape inertia tensor into the local-space of the body
+			final Transform3D shapeTransform = shape.getLocalToBodyTransform();
+			final Matrix3f rotationMatrix = shapeTransform.getOrientation().getMatrix();
+			inertiaTensor = rotationMatrix.multiplyNew(inertiaTensor).multiply(rotationMatrix.transposeNew());
+			// 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.
+			final Vector3f offset = shapeTransform.getPosition().lessNew(this.centerOfMassLocal);
+			final float offsetSquare = offset.length2();
+			final Vector3f off1 = offset.multiplyNew(-offset.x);
+			final Vector3f off2 = offset.multiplyNew(-offset.y);
+			final Vector3f off3 = offset.multiplyNew(-offset.z);
+			final Matrix3f offsetMatrix = new Matrix3f(off1.x + offsetSquare, off1.y, off1.z, off2.x, off2.y + offsetSquare, off2.z, off3.x, off3.y, off3.z + offsetSquare);
+			offsetMatrix.multiply(shape.getMass());
+			this.inertiaTensorLocal.add(inertiaTensor.addNew(offsetMatrix));
+		}
+		// Compute the local inverse inertia tensor
+		this.inertiaTensorLocalInverse = this.inertiaTensorLocal.inverseNew();
+		// Update the linear velocity of the center of mass
+		this.linearVelocity.add(this.angularVelocity.cross(this.centerOfMassWorld.lessNew(oldCenterOfMass)));
+	}
+	
+	@Override
+	public void removeCollisionShape(final ProxyShape _proxyShape) {
+		super.removeCollisionShape(_proxyShape);
+		recomputeMassInformation();
+	}
+	
+	/**
+	 *  Remove a joint from the joints list
+	 */
+	public void removeJointFromjointsList(final 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;
+			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;
+					elementToRemove = null;
+					break;
+				}
+				currentElement = currentElement.next;
+			}
+		}
+	}
+	
+	/**
+	 *  Set the angular damping factor. This is the ratio of the angular velocity that the body will lose at every seconds of simulation.
+	 * @param _angularDamping The angular damping factor of this body
+	 */
+	public void setAngularDamping(final float _angularDamping) {
+		assert (_angularDamping >= 0.0f);
+		this.angularDamping = _angularDamping;
+	}
+	
+	public void setAngularReactionEnable(final boolean angularReactionEnable) {
+		this.angularReactionEnable = angularReactionEnable;
+	}
+	
+	/**
+	*  Set the angular velocity.
+	* @param _angularVelocity The angular velocity vector of the body
+	*/
+	public void setAngularVelocity(final Vector3f _angularVelocity) {
+		if (this.type == BodyType.STATIC) {
+			return;
+		}
+		this.angularVelocity = _angularVelocity;
+		if (this.angularVelocity.length2() > 0.0f) {
+			setIsSleeping(false);
+		}
+	}
+	
+	/**
+	 *  Set the local center of mass of the body (in local-space coordinates)
+	 * @param _centerOfMassLocal The center of mass of the body in local-space coordinates
+	 */
+	public void setCenterOfMassLocal(final Vector3f centerOfMassLocal) {
+		if (this.type != BodyType.DYNAMIC) {
+			return;
+		}
+		final Vector3f oldCenterOfMass = this.centerOfMassWorld;
+		this.centerOfMassLocal = centerOfMassLocal;
+		this.centerOfMassWorld = this.transform.multiplyNew(this.centerOfMassLocal);
+		this.linearVelocity.add(this.angularVelocity.cross(this.centerOfMassWorld.lessNew(oldCenterOfMass)));
+	}
+	
+	/**
+	 *  Set the local inertia tensor of the body (in local-space coordinates)
+	 * @param _inertiaTensorLocal The 3x3 inertia tensor matrix of the body in local-space coordinates
+	 */
+	public void setInertiaTensorLocal(final Matrix3f inertiaTensorLocal) {
+		if (this.type != BodyType.DYNAMIC) {
+			return;
+		}
+		this.inertiaTensorLocal = inertiaTensorLocal;
+		this.inertiaTensorLocalInverse = this.inertiaTensorLocal.inverseNew();
+	}
+	
+	/**
+	 *  Set the variable to know whether or not the body is sleeping
+	 * @param _isSleeping New sleeping state of the body
+	 */
+	@Override
+	public void setIsSleeping(final boolean _isSleeping) {
+		if (_isSleeping) {
+			this.linearVelocity.setZero();
+			this.angularVelocity.setZero();
+			this.externalForce.setZero();
+			this.externalTorque.setZero();
+		}
+		super.setIsSleeping(_isSleeping);
+	}
+	
+	/**
+	 *  Set the linear damping factor. This is the ratio of the linear velocity that the body will lose every at seconds of simulation.
+	 * @param _linearDamping The linear damping factor of this body
+	 */
+	public void setLinearDamping(final float _linearDamping) {
+		assert (_linearDamping >= 0.0f);
+		this.linearDamping = _linearDamping;
+	}
+	
+	/**
+	 *  Set the linear velocity of the rigid body.
+	 * @param _linearVelocity Linear velocity vector of the body
+	 */
+	public void setLinearVelocity(final Vector3f _linearVelocity) {
+		if (this.type == BodyType.STATIC) {
+			return;
+		}
+		this.linearVelocity = _linearVelocity;
+		if (this.linearVelocity.length2() > 0.0f) {
+			setIsSleeping(false);
+		}
+	}
+	
+	/**
+	 *  Set the mass of the rigid body
+	 * @param _mass The mass (in kilograms) of the body
+	 */
+	public void setMass(final float _mass) {
+		if (this.type != BodyType.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;
+		}
+	}
+	
+	/**
+	 *  Set a new material for this rigid body
+	 * @param _material The material you want to set to the body
+	 */
+	public void setMaterial(final Material _material) {
+		this.material = _material;
+	}
+	
+	/**
+	 *  Set the current position and orientation
+	 * @param _transform The transformation of the body that transforms the local-space of the body into world-space
+	 */
+	@Override
+	public void setTransform(final Transform3D _transform) {
+		//Log.info("         set transform: " + this.transform + " ==> " + _transform);
+		if (_transform.getPosition().x == Float.NaN) {
+			Log.critical("         set transform: " + this.transform + " ==> " + _transform);
+		}
+		if (Float.isInfinite(_transform.getOrientation().z)) {
+			Log.critical("         set transform: " + this.transform + " ==> " + _transform);
+		}
+		this.transform = _transform;
+		final Vector3f oldCenterOfMass = this.centerOfMassWorld;
+		// Compute the new center of mass in world-space coordinates
+		this.centerOfMassWorld = this.transform.multiplyNew(this.centerOfMassLocal);
+		// Update the linear velocity of the center of mass
+		this.linearVelocity.add(this.angularVelocity.cross(this.centerOfMassWorld.lessNew(oldCenterOfMass)));
+		updateBroadPhaseState();
+	}
+	
+	@Override
+	public void setType(final BodyType _type) {
+		if (this.type == _type) {
+			return;
+		}
+		super.setType(_type);
+		recomputeMassInformation();
+		if (this.type == BodyType.STATIC) {
+			// Reset the velocity to zero
+			this.linearVelocity.setZero();
+			this.angularVelocity.setZero();
+		}
+		if (this.type == BodyType.STATIC || this.type == BodyType.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.inverseNew();
+		}
+		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();
+	}
+	
+	@Override
+	public void updateBroadPhaseState() {
+		final DynamicsWorld world = (DynamicsWorld) this.world;
+		final Vector3f displacement = this.linearVelocity.multiplyNew(world.timeStep);
+		
+		// For all the proxy collision shapes of the body
+		for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) {
+			// Recompute the world-space AABB of the collision shape
+			final AABB aabb = new AABB();
+			//Log.info("         : " + aabb.getMin() + " " + aabb.getMax());
+			//Log.info("         this.transform: " + this.transform);
+			shape.getCollisionShape().computeAABB(aabb, this.transform.multiplyNew(shape.getLocalToBodyTransform()));
+			//Log.info("         : " + aabb.getMin() + " " + aabb.getMax());
+			// Update the broad-phase state for the proxy collision shape
+			//Log.warning("            ==> updateProxyCollisionShape");
+			world.collisionDetection.updateProxyCollisionShape(shape, aabb, displacement);
+		}
+	}
+	
+	/**
+	 *  Update the transform of the body after a change of the center of mass
+	 */
+	public void updateTransformWithCenterOfMass() {
+		// Translate the body according to the translation of the center of mass position
+		this.transform.setPosition(this.centerOfMassWorld.lessNew(this.transform.getOrientation().multiply(this.centerOfMassLocal)));
+		if (Float.isNaN(this.transform.getPosition().x) == true) {
+			Log.critical("updateTransformWithCenterOfMass: " + this.transform);
+		}
+		if (Float.isInfinite(this.transform.getOrientation().z) == true) {
+			Log.critical("         set transform: " + this.transform);
+		}
+	}
+	
+}

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

@@ -0,0 +1,497 @@
+package org.atriasoft.ephysics.collision;
+
+import java.util.Iterator;
+import java.util.Map;
+import java.util.TreeMap;
+
+import org.atriasoft.ephysics.RaycastCallback;
+import org.atriasoft.ephysics.RaycastTest;
+import org.atriasoft.ephysics.body.BodyType;
+import org.atriasoft.ephysics.body.CollisionBody;
+import org.atriasoft.ephysics.collision.broadphase.BroadPhaseAlgorithm;
+import org.atriasoft.ephysics.collision.broadphase.DTree;
+import org.atriasoft.ephysics.collision.broadphase.PairDTree;
+import org.atriasoft.ephysics.collision.narrowphase.CollisionDispatch;
+import org.atriasoft.ephysics.collision.narrowphase.DefaultCollisionDispatch;
+import org.atriasoft.ephysics.collision.narrowphase.NarrowPhaseAlgorithm;
+import org.atriasoft.ephysics.collision.narrowphase.NarrowPhaseCallback;
+import org.atriasoft.ephysics.collision.narrowphase.GJK.GJKAlgorithm;
+import org.atriasoft.ephysics.collision.shapes.AABB;
+import org.atriasoft.ephysics.collision.shapes.CollisionShape;
+import org.atriasoft.ephysics.collision.shapes.CollisionShapeType;
+import org.atriasoft.ephysics.constraint.ContactPoint;
+import org.atriasoft.ephysics.constraint.ContactPointInfo;
+import org.atriasoft.ephysics.engine.CollisionCallback;
+import org.atriasoft.ephysics.engine.CollisionWorld;
+import org.atriasoft.ephysics.engine.EventListener;
+import org.atriasoft.ephysics.engine.OverlappingPair;
+import org.atriasoft.ephysics.internal.Log;
+import org.atriasoft.ephysics.mathematics.PairInt;
+import org.atriasoft.ephysics.mathematics.Ray;
+import org.atriasoft.ephysics.mathematics.Set;
+import org.atriasoft.ephysics.mathematics.SetMultiple;
+import org.atriasoft.etk.math.Vector3f;
+
+/*
+ * @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.
+ */
+public class CollisionDetection implements NarrowPhaseCallback {
+	/// Reference on the physics world
+	private final CollisionWorld world;
+	/// Broad-phase algorithm
+	private final BroadPhaseAlgorithm broadPhaseAlgorithm;
+	/// Collision Detection Dispatch configuration
+	private CollisionDispatch collisionDispatch;
+	private final DefaultCollisionDispatch defaultCollisionDispatch = new DefaultCollisionDispatch();
+	/// Collision detection matrix (algorithms to use)
+	private final NarrowPhaseAlgorithm[][] collisionMatrix = new NarrowPhaseAlgorithm[CollisionShapeType.NB_COLLISION_SHAPE_TYPES][CollisionShapeType.NB_COLLISION_SHAPE_TYPES];
+	public Map<PairDTree, OverlappingPair> overlappingPairs = new TreeMap<>(Set.getPairDTreeCoparator()); //!< Broad-phase overlapping pairs
+	private final Map<PairDTree, OverlappingPair> contactOverlappingPair = new TreeMap<>(Set.getPairDTreeCoparator()); //!< Overlapping pairs in contact (during the current Narrow-phase collision detection)
+	// TODO : Delete this
+	private final GJKAlgorithm narrowPhaseGJKAlgorithm; //!< Narrow-phase GJK algorithm
+	private final SetMultiple<PairInt> noCollisionPairs = new SetMultiple<PairInt>(Set.getPairIntCoparator()); //!< Set of pair of bodies that cannot collide between each other
+	private boolean isCollisionShapesAdded; //!< True if some collision shapes have been added previously
+	
+	/// Constructor
+	public CollisionDetection(final CollisionWorld _world) {
+		this.world = _world;
+		this.broadPhaseAlgorithm = new BroadPhaseAlgorithm(this);
+		this.isCollisionShapesAdded = false;
+		this.narrowPhaseGJKAlgorithm = new GJKAlgorithm(this);
+		// Set the default collision dispatch configuration
+		setCollisionDispatch(this.defaultCollisionDispatch);
+		// Fill-in the collision detection matrix with algorithms
+		fillInCollisionMatrix();
+	}
+	
+	/// Add all the contact manifold of colliding pairs to their bodies
+	private void addAllContactManifoldsToBodies() {
+		// For each overlapping pairs in contact during the narrow-phase
+		for (final OverlappingPair it : this.contactOverlappingPair.values()) {
+			// Add all the contact manifolds of the pair int32_to the list of contact manifolds
+			// of the two bodies involved in the contact
+			addContactManifoldToBody(it);
+		}
+	}
+	
+	/// Add a contact manifold to the linked list of contact manifolds of the two bodies
+	/// involed in the corresponding contact.
+	private void addContactManifoldToBody(final OverlappingPair _pair) {
+		assert (_pair != null);
+		final CollisionBody body1 = _pair.getShape1().getBody();
+		final CollisionBody body2 = _pair.getShape2().getBody();
+		final ContactManifoldSet manifoldSet = _pair.getContactManifoldSet();
+		// For each contact manifold in the set of manifolds in the pair
+		for (int i = 0; i < manifoldSet.getNbContactManifolds(); i++) {
+			final 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.contactManifoldsList = new ContactManifoldListElement(contactManifold, body1.contactManifoldsList);
+			
+			// Add the contact manifold at the beginning of the linked
+			// list of the contact manifolds of the second body
+			body2.contactManifoldsList = new ContactManifoldListElement(contactManifold, body2.contactManifoldsList);
+			
+		}
+	}
+	
+	/// Add a pair of bodies that cannot collide with each other
+	public void addNoCollisionPair(final CollisionBody _body1, final CollisionBody _body2) {
+		this.noCollisionPairs.add(OverlappingPair.computeBodiesIndexPair(_body1, _body2));
+	}
+	
+	/// Add a proxy collision shape to the collision detection
+	public void addProxyCollisionShape(final ProxyShape _proxyShape, final AABB _aabb) {
+		// Add the body to the broad-phase
+		this.broadPhaseAlgorithm.addProxyCollisionShape(_proxyShape, _aabb);
+		this.isCollisionShapesAdded = true;
+	}
+	
+	// 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.
+	public void askForBroadPhaseCollisionCheck(final ProxyShape _shape) {
+		this.broadPhaseAlgorithm.addMovedCollisionShape(_shape.broadPhaseID);
+	}
+	
+	/// Allow the broadphase to notify the collision detection about an overlapping pair.
+	/// This method is called by the broad-phase collision detection algorithm
+	public void broadPhaseNotifyOverlappingPair(final ProxyShape _shape1, final ProxyShape _shape2) {
+		assert (_shape1.broadPhaseID != _shape2.broadPhaseID);
+		// If the two proxy collision shapes are from the same body, skip it
+		if (_shape1.getBody().getID() == _shape2.getBody().getID()) {
+			return;
+		}
+		//Log.info(" check collision is allowed: " + _shape1.getBody().getID() + "  " + _shape2.getBody().getID());
+		// Check if the collision filtering allows collision between the two shapes
+		if ((_shape1.getCollideWithMaskBits() & _shape2.getCollisionCategoryBits()) == 0 || (_shape1.getCollisionCategoryBits() & _shape2.getCollideWithMaskBits()) == 0) {
+			Log.info("    ==> not permited ...");
+			return;
+		}
+		// Compute the overlapping pair ID
+		final PairDTree pairID = OverlappingPair.computeID(_shape1, _shape2);
+		// Check if the overlapping pair already exists
+		if (this.overlappingPairs.get(pairID) != null) {
+			return;
+		}
+		// Compute the maximum number of contact manifolds for this pair
+		final int nbMaxManifolds = CollisionShape.computeNbMaxContactManifolds(_shape1.getCollisionShape().getType(), _shape2.getCollisionShape().getType());
+		// Create the overlapping pair and add it int32_to the set of overlapping pairs
+		final OverlappingPair newPair = new OverlappingPair(_shape1, _shape2, nbMaxManifolds);
+		assert (newPair != null);
+		this.overlappingPairs.putIfAbsent(pairID, newPair);
+		// Wake up the two bodies
+		_shape1.getBody().setIsSleeping(false);
+		_shape2.getBody().setIsSleeping(false);
+	}
+	
+	/// Delete all the contact points in the currently overlapping pairs
+	private void clearContactPoints() {
+		// For each overlapping pair
+		this.overlappingPairs.forEach((key, value) -> value.clearContactPoints());
+	}
+	
+	/// Compute the broad-phase collision detection
+	private void 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();
+		}
+	}
+	
+	/// Compute the collision detection
+	public void computeCollisionDetection() {
+		//Log.info("computeBroadPhase();");
+		// Compute the broad-phase collision detection
+		computeBroadPhase();
+		//Log.info("computeNarrowPhase();");
+		// Compute the narrow-phase collision detection
+		computeNarrowPhase();
+	}
+	
+	/// Compute the narrow-phase collision detection
+	private void computeNarrowPhase() {
+		// Clear the set of overlapping pairs in narrow-phase contact
+		this.contactOverlappingPair.clear();
+		{
+			//Log.info("list elements:");
+			final Iterator<Map.Entry<PairDTree, OverlappingPair>> ittt = this.overlappingPairs.entrySet().iterator();
+			while (ittt.hasNext()) {
+				final Map.Entry<PairDTree, OverlappingPair> entry = ittt.next();
+				//Log.info("    " + entry.getKey() + "  " + entry.getValue());
+				
+			}
+		}
+		// For each possible collision pair of bodies
+		final Iterator<Map.Entry<PairDTree, OverlappingPair>> it = this.overlappingPairs.entrySet().iterator();
+		
+		while (it.hasNext()) {
+			final Map.Entry<PairDTree, OverlappingPair> entry = it.next();
+			final OverlappingPair pair = entry.getValue();
+			final ProxyShape shape1 = pair.getShape1();
+			final ProxyShape shape2 = pair.getShape2();
+			assert (shape1.broadPhaseID != shape2.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
+				it.remove();
+				continue;
+			}
+			final CollisionBody body1 = shape1.getBody();
+			final 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
+			final boolean isBody1Active = !body1.isSleeping() && body1.getType() != BodyType.STATIC;
+			final boolean isBody2Active = !body2.isSleeping() && body2.getType() != BodyType.STATIC;
+			if (!isBody1Active && !isBody2Active) {
+				continue;
+			}
+			// Check if the bodies are in the set of bodies that cannot collide between each other
+			final PairInt 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
+			final CollisionShapeType shape1Type = shape1.getCollisionShape().getType();
+			final CollisionShapeType shape2Type = shape2.getCollisionShape().getType();
+			final NarrowPhaseAlgorithm narrowPhaseAlgorithm = this.collisionMatrix[shape1Type.value][shape2Type.value];
+			// 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
+			final CollisionShapeInfo shape1Info = new CollisionShapeInfo(shape1, shape1.getCollisionShape(), shape1.getLocalToWorldTransform(), pair, shape1.getCachedCollisionData());
+			
+			final CollisionShapeInfo shape2Info = new CollisionShapeInfo(shape2, shape2.getCollisionShape(), shape2.getLocalToWorldTransform(), pair, shape2.getCachedCollisionData());
+			
+			// Use the narrow-phase collision detection algorithm to check
+			// if there really is a collision. If a collision occurs, the
+			// notifyContact() callback method will be called.
+			narrowPhaseAlgorithm.testCollision(shape1Info, shape2Info, this);
+		}
+		
+		// Add all the contact manifolds (between colliding bodies) to the bodies
+		addAllContactManifoldsToBodies();
+		
+	}
+	
+	/// Compute the narrow-phase collision detection
+	public void computeNarrowPhaseBetweenShapes(final CollisionCallback _callback, final Set<DTree> _shapes1, final Set<DTree> _shapes2) {
+		this.contactOverlappingPair.clear();
+		// For each possible collision pair of bodies
+		final Iterator<Map.Entry<PairDTree, OverlappingPair>> it = this.overlappingPairs.entrySet().iterator();
+		while (it.hasNext()) {
+			final Map.Entry<PairDTree, OverlappingPair> entry = it.next();
+			final OverlappingPair pair = entry.getValue();
+			final ProxyShape shape1 = pair.getShape1();
+			final ProxyShape shape2 = pair.getShape2();
+			assert (shape1.broadPhaseID != shape2.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.isEmpty() && !_shapes2.isEmpty() && (_shapes1.count(shape1.broadPhaseID) == 0 || _shapes2.count(shape2.broadPhaseID) == 0)
+					&& (_shapes1.count(shape2.broadPhaseID) == 0 || _shapes2.count(shape1.broadPhaseID) == 0)) {
+				continue;
+			}
+			if (!_shapes1.isEmpty() && _shapes2.isEmpty() && _shapes1.count(shape1.broadPhaseID) == 0 && _shapes1.count(shape2.broadPhaseID) == 0) {
+				continue;
+			}
+			if (!_shapes2.isEmpty() && _shapes1.isEmpty() && _shapes2.count(shape1.broadPhaseID) == 0 && _shapes2.count(shape2.broadPhaseID) == 0) {
+				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
+				it.remove();
+				continue;
+			}
+			final CollisionBody body1 = shape1.getBody();
+			final 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() != BodyType.DYNAMIC && body2.getType() != BodyType.DYNAMIC) {
+				continue;
+			}
+			final PairInt 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
+			final CollisionShapeType shape1Type = shape1.getCollisionShape().getType();
+			final CollisionShapeType shape2Type = shape2.getCollisionShape().getType();
+			final NarrowPhaseAlgorithm narrowPhaseAlgorithm = this.collisionMatrix[shape1Type.value][shape2Type.value];
+			// 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
+			final CollisionShapeInfo shape1Info = new CollisionShapeInfo(shape1, shape1.getCollisionShape(), shape1.getLocalToWorldTransform(), pair, shape1.getCachedCollisionData());
+			
+			final CollisionShapeInfo shape2Info = new CollisionShapeInfo(shape2, shape2.getCollisionShape(), shape2.getLocalToWorldTransform(), pair, shape2.getCachedCollisionData());
+			
+			final TestCollisionBetweenShapesCallback narrowPhaseCallback = new TestCollisionBetweenShapesCallback(_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 createContact(final OverlappingPair _overlappingPair, final ContactPointInfo _contactInfo) {
+		// Create a new contact
+		final ContactPoint contact = new ContactPoint(_contactInfo);
+		// Add the contact to the contact manifold set of the corresponding overlapping pair
+		_overlappingPair.addContact(contact);
+		// Add the overlapping pair int32_to the set of pairs in contact during narrow-phase
+		final PairDTree pairId = OverlappingPair.computeID(_overlappingPair.getShape1(), _overlappingPair.getShape2());
+		this.contactOverlappingPair.put(pairId, _overlappingPair);
+	}
+	
+	/// Fill-in the collision detection matrix
+	private void fillInCollisionMatrix() {
+		// For each possible type of collision shape
+		for (int i = 0; i < CollisionShapeType.NB_COLLISION_SHAPE_TYPES; i++) {
+			for (int j = 0; j < CollisionShapeType.NB_COLLISION_SHAPE_TYPES; j++) {
+				this.collisionMatrix[i][j] = this.collisionDispatch.selectAlgorithm(CollisionShapeType.getType(i), CollisionShapeType.getType(j));
+			}
+		}
+	}
+	
+	/// Return the Narrow-phase collision detection algorithm to use between two types of shapes
+	public NarrowPhaseAlgorithm getCollisionAlgorithm(final CollisionShapeType _shape1Type, final CollisionShapeType _shape2Type) {
+		return this.collisionMatrix[_shape1Type.value][_shape2Type.value];
+	}
+	
+	public GJKAlgorithm getNarrowPhaseGJKAlgorithm() {
+		return this.narrowPhaseGJKAlgorithm;
+	}
+	
+	/// Return a pointer to the world
+	public CollisionWorld getWorld() {
+		return this.world;
+	}
+	
+	/// Return the world event listener
+	public EventListener getWorldEventListener() {
+		return this.world.eventListener;
+	}
+	
+	/// Called by a narrow-phase collision algorithm when a new contact has been found
+	@Override
+	public void notifyContact(final OverlappingPair _overlappingPair, final ContactPointInfo _contactInfo) {
+		Log.error("Notify Contact ...     --------------------");
+		// If it is the first contact since the pairs are overlapping
+		if (_overlappingPair.getNbContactPoints() == 0) {
+			// Trigger a callback event
+			if (this.world.eventListener != null) {
+				this.world.eventListener.beginContact(_contactInfo);
+			}
+		}
+		// Create a new contact
+		createContact(_overlappingPair, _contactInfo);
+		// Trigger a callback event for the new contact
+		if (this.world.eventListener != null) {
+			this.world.eventListener.newContact(_contactInfo);
+		}
+	}
+	
+	/// Ray casting method
+	public void raycast(final RaycastCallback _raycastCallback, final Ray _ray, final int _raycastWithCategoryMaskBits) {
+		final RaycastTest rayCastTest = new 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);
+	}
+	
+	/// Remove a pair of bodies that cannot collide with each other
+	public void removeNoCollisionPair(final CollisionBody _body1, final CollisionBody _body2) {
+		this.noCollisionPairs.erase(this.noCollisionPairs.find(OverlappingPair.computeBodiesIndexPair(_body1, _body2)));
+	}
+	
+	/// Remove a proxy collision shape from the collision detection
+	public void removeProxyCollisionShape(final ProxyShape _proxyShape) {
+		// Remove all the overlapping pairs involving this proxy shape
+		final Iterator<Map.Entry<PairDTree, OverlappingPair>> it = this.overlappingPairs.entrySet().iterator();
+		while (it.hasNext()) {
+			final Map.Entry<PairDTree, OverlappingPair> entry = it.next();
+			final OverlappingPair pair = entry.getValue();
+			if (pair.getShape1().broadPhaseID == _proxyShape.broadPhaseID || pair.getShape2().broadPhaseID == _proxyShape.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
+				it.remove();
+			}
+		}
+		// Remove the body from the broad-phase
+		this.broadPhaseAlgorithm.removeProxyCollisionShape(_proxyShape);
+	}
+	
+	/// Report collision between two sets of shapes
+	public void reportCollisionBetweenShapes(final CollisionCallback _callback, final Set<DTree> _shapes1, final Set<DTree> _shapes2) {
+		
+		// For each possible collision pair of bodies
+		final Iterator<Map.Entry<PairDTree, OverlappingPair>> it = this.overlappingPairs.entrySet().iterator();
+		while (it.hasNext()) {
+			final Map.Entry<PairDTree, OverlappingPair> entry = it.next();
+			final OverlappingPair pair = entry.getValue();
+			final ProxyShape shape1 = pair.getShape1();
+			final ProxyShape shape2 = pair.getShape2();
+			assert (shape1.broadPhaseID != shape2.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.isEmpty() && !_shapes2.isEmpty() && (_shapes1.count(shape1.broadPhaseID) == 0 || _shapes2.count(shape2.broadPhaseID) == 0)
+					&& (_shapes1.count(shape2.broadPhaseID) == 0 || _shapes2.count(shape1.broadPhaseID) == 0)) {
+				continue;
+			}
+			if (!_shapes1.isEmpty() && _shapes2.isEmpty() && _shapes1.count(shape1.broadPhaseID) == 0 && _shapes1.count(shape2.broadPhaseID) == 0) {
+				continue;
+			}
+			if (!_shapes2.isEmpty() && _shapes1.isEmpty() && _shapes2.count(shape1.broadPhaseID) == 0 && _shapes2.count(shape2.broadPhaseID) == 0) {
+				continue;
+			}
+			// For each contact manifold set of the overlapping pair
+			final ContactManifoldSet manifoldSet = pair.getContactManifoldSet();
+			for (int j = 0; j < manifoldSet.getNbContactManifolds(); j++) {
+				final ContactManifold manifold = manifoldSet.getContactManifold(j);
+				// For each contact manifold of the manifold set
+				for (int i = 0; i < manifold.getNbContactPoints(); i++) {
+					final ContactPoint contactPoint = manifold.getContactPoint(i);
+					// Create the contact info object for the contact
+					final ContactPointInfo contactInfo = new ContactPointInfo(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);
+					}
+				}
+				
+			}
+		}
+	}
+	
+	/// Set the collision dispatch configuration
+	public void setCollisionDispatch(final CollisionDispatch _collisionDispatch) {
+		this.collisionDispatch = _collisionDispatch;
+		this.collisionDispatch.init(this);
+		// Fill-in the collision matrix with the new algorithms to use
+		fillInCollisionMatrix();
+	}
+	
+	/// Test if the AABBs of two proxy shapes overlap
+	public boolean testAABBOverlap(final ProxyShape _shape1, final 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);
+	}
+	
+	/// Compute the collision detection
+	public void testCollisionBetweenShapes(final CollisionCallback _callback, final Set<DTree> _shapes1, final Set<DTree> _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);
+	}
+	
+	/// Update a proxy collision shape (that has moved for instance)
+	public void updateProxyCollisionShape(final ProxyShape _shape, final AABB _aabb) {
+		updateProxyCollisionShape(_shape, _aabb, new Vector3f(0, 0, 0), false);
+	}
+	
+	public void updateProxyCollisionShape(final ProxyShape _shape, final AABB _aabb, final Vector3f _displacement) {
+		updateProxyCollisionShape(_shape, _aabb, _displacement, false);
+	}
+	
+	public void updateProxyCollisionShape(final ProxyShape _shape, final AABB _aabb, final Vector3f _displacement, final boolean _forceReinsert) {
+		this.broadPhaseAlgorithm.updateProxyCollisionShape(_shape, _aabb, _displacement);
+	}
+}

src/org/atriasoft/ephysics/collision/CollisionShapeInfo.java

@@ -0,0 +1,30 @@
+package org.atriasoft.ephysics.collision;
+
+import org.atriasoft.etk.math.Transform3D;
+
+import org.atriasoft.ephysics.collision.shapes.CollisionShape;
+import org.atriasoft.ephysics.engine.OverlappingPair;
+
+/**
+ *  It regroups different things about a collision shape. This is
+ * used to pass information about a collision shape to a collision algorithm.
+ */
+public class CollisionShapeInfo {
+	
+	public final OverlappingPair overlappingPair; //!< Broadphase overlapping pair
+	public final ProxyShape proxyShape; //!< Proxy shape
+	public final CollisionShape collisionShape; //!< Pointer to the collision shape
+	public final Transform3D shapeToWorldTransform; //!< Transform3D that maps from collision shape local-space to world-space
+	public final Object cachedCollisionData; //!< Cached collision data of the proxy shape
+	/// Constructor
+	
+	public CollisionShapeInfo(final ProxyShape _proxyCollisionShape, final CollisionShape _shape, final Transform3D _shapeLocalToWorldTransform, final OverlappingPair _pair,
+			final Object _cachedData) {
+		this.overlappingPair = _pair;
+		this.proxyShape = _proxyCollisionShape;
+		this.collisionShape = _shape;
+		this.shapeToWorldTransform = _shapeLocalToWorldTransform;
+		this.cachedCollisionData = _cachedData;
+		
+	}
+}

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

@@ -0,0 +1,374 @@
+/** @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)
+ */
+
+package org.atriasoft.ephysics.collision;
+
+import org.atriasoft.ephysics.Configuration;
+import org.atriasoft.ephysics.body.CollisionBody;
+import org.atriasoft.ephysics.constraint.ContactPoint;
+import org.atriasoft.etk.math.Transform3D;
+import org.atriasoft.etk.math.Vector3f;
+
+/**
+ *  This class represents the set of contact points between two bodies.
+ * The contact manifold is implemented in a way to cache the contact
+ * points among the frames for better stability following the
+ * "Contact Generation" presentation of Erwin Coumans at GDC 2010
+ * conference (bullet.googlecode.com/files/GDC10_Coumans_Erwin_Contact.pdf).
+ * Some code of this class is based on the implementation of the
+ * btPersistentManifold class from Bullet physics engine (www.http://bulletphysics.org).
+ * The contacts between two bodies are added one after the other in the cache.
+ * When the cache is full, we have to remove one point. The idea is to keep
+ * the point with the deepest penetration depth and also to keep the
+ * points producing the larger area (for a more stable contact manifold).
+ * The new added point is always kept.
+ */
+public class ContactManifold {
+	
+	//!< Maximum number of contacts in the manifold
+	public static final int MAX_CONTACT_POINTS_IN_MANIFOLD = 4;;
+	
+	private final ProxyShape shape1; //!< Pointer to the first proxy shape of the contact
+	
+	private final ProxyShape shape2; //!< Pointer to the second proxy shape of the contact
+	private final ContactPoint[] contactPoints = new ContactPoint[MAX_CONTACT_POINTS_IN_MANIFOLD]; //!< Contact points in the manifold
+	private final int normalDirectionId; //!< Normal direction Id (Unique Id representing the normal direction)
+	private int nbContactPoints; //!< Number of contacts in the cache
+	private Vector3f frictionVector1 = new Vector3f(); //!< First friction vector of the contact manifold
+	private Vector3f frictionvec2 = new Vector3f(); //!< Second friction vector of the contact manifold
+	private float frictionImpulse1; //!< First friction raint accumulated impulse
+	
+	private float frictionImpulse2; //!< Second friction raint accumulated impulse
+	private float frictionTwistImpulse; //!< Twist friction raint accumulated impulse
+	private Vector3f rollingResistanceImpulse; //!< Accumulated rolling resistance impulse
+	public boolean isAlreadyInIsland; //!< True if the contact manifold has already been added longo an island
+	/// Return the index of maximum area
+	/// Constructor
+	
+	public ContactManifold(final ProxyShape _shape1, final ProxyShape _shape2, final int _normalDirectionId) {
+		this.shape1 = _shape1;
+		this.shape2 = _shape2;
+		this.normalDirectionId = _normalDirectionId;
+		this.nbContactPoints = 0;
+		this.frictionImpulse1 = 0.0f;
+		this.frictionImpulse2 = 0.0f;
+		this.frictionTwistImpulse = 0.0f;
+		this.isAlreadyInIsland = false;
+		
+	}
+	
+	/// Add a contact point to the manifold
+	public void addContactPoint(final ContactPoint contact) {
+		// For contact already in the manifold
+		for (int iii = 0; iii < this.nbContactPoints; iii++) {
+			// Check if the new point point does not correspond to a same contact point
+			// already in the manifold.
+			final float distance = (this.contactPoints[iii].getWorldPointOnBody1().lessNew(contact.getWorldPointOnBody1()).length2());
+			if (distance <= Configuration.PERSISTENT_CONTACT_DIST_THRESHOLD * Configuration.PERSISTENT_CONTACT_DIST_THRESHOLD) {
+				assert (this.nbContactPoints > 0);
+				return;
+			}
+		}
+		// If the contact manifold is full
+		if (this.nbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) {
+			final int indexMaxPenetration = getIndexOfDeepestPenetration(contact);
+			final 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);
+	}
+	
+	/// Clear the contact manifold
+	public void clear() {
+		for (int iii = 0; iii < this.nbContactPoints; ++iii) {
+			this.contactPoints[iii] = null;
+		}
+		this.nbContactPoints = 0;
+	}
+	
+	/// Return the normalized averaged normal vector
+	public Vector3f getAverageContactNormal() {
+		if (this.nbContactPoints == 0) {
+			return new Vector3f(0, 0, 1);
+		}
+		final Vector3f averageNormal = new Vector3f();
+		for (int iii = 0; iii < this.nbContactPoints; iii++) {
+			averageNormal.add(this.contactPoints[iii].getNormal());
+		}
+		return averageNormal.safeNormalizeNew();
+	}
+	
+	/// Return a pointer to the first body of the contact manifold
+	public CollisionBody getBody1() {
+		return this.shape1.getBody();
+	}
+	
+	/// Return a pointer to the second body of the contact manifold
+	public CollisionBody getBody2() {
+		return this.shape2.getBody();
+	}
+	
+	/// Return a contact point of the manifold
+	public ContactPoint getContactPoint(final int index) {
+		assert (index < this.nbContactPoints);
+		return this.contactPoints[index];
+	}
+	
+	/// Return the first friction accumulated impulse
+	public float getFrictionImpulse1() {
+		return this.frictionImpulse1;
+	}
+	
+	/// Return the second friction accumulated impulse
+	public float getFrictionImpulse2() {
+		return this.frictionImpulse2;
+	}
+	
+	/// Return the friction twist accumulated impulse
+	public float getFrictionTwistImpulse() {
+		return this.frictionTwistImpulse;
+	}
+	
+	/// Return the second friction vector at the center of the contact manifold
+	public Vector3f getFrictionvec2() {
+		return this.frictionvec2;
+	}
+	
+	/// Return the first friction vector at the center of the contact manifold
+	public Vector3f getFrictionVector1() {
+		return this.frictionVector1;
+	}
+	
+	/**
+	 *  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(final ContactPoint newContact) {
+		assert (this.nbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
+		int indexMaxPenetrationDepth = -1;
+		float maxPenetrationDepth = newContact.getPenetrationDepth();
+		// For each contact in the cache
+		for (int iii = 0; iii < this.nbContactPoints; iii++) {
+			// If the current contact has a larger penetration depth
+			if (this.contactPoints[iii].getPenetrationDepth() > maxPenetrationDepth) {
+				maxPenetrationDepth = this.contactPoints[iii].getPenetrationDepth();
+				indexMaxPenetrationDepth = iii;
+			}
+		}
+		// Return the index of largest penetration depth
+		return indexMaxPenetrationDepth;
+	}
+	
+	/**
+	 *  Return the index that will be removed.
+	 * The index of the contact point with the larger penetration
+	 * depth is given as a parameter. This contact won't be removed. Given this contact, we compute
+	 * the different area and we want to keep the contacts with the largest area. The new point is also
+	 * kept. In order to compute the area of a quadrilateral, we use the formula :
+	 * Area = 0.5 * | AC x BD | where AC and BD form the diagonals of the quadrilateral. Note that
+	 * when we compute this area, we do not calculate it exactly but we
+	 * only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore,
+	 * this is only a guess that is faster to compute. This idea comes from the Bullet Physics library
+	 * by Erwin Coumans (http://wwww.bulletphysics.org).
+	 */
+	int getIndexToRemove(final int indexMaxPenetration, final Vector3f newPoint) {
+		assert (this.nbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
+		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
+			final Vector3f vector1 = newPoint.lessNew(this.contactPoints[1].getLocalPointOnBody1());
+			final Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1().lessNew(this.contactPoints[2].getLocalPointOnBody1());
+			final Vector3f crossProduct = vector1.cross(vector2);
+			area0 = crossProduct.length2();
+		}
+		if (indexMaxPenetration != 1) {
+			// Compute the area
+			final Vector3f vector1 = newPoint.lessNew(this.contactPoints[0].getLocalPointOnBody1());
+			final Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1().lessNew(this.contactPoints[2].getLocalPointOnBody1());
+			final Vector3f crossProduct = vector1.cross(vector2);
+			area1 = crossProduct.length2();
+		}
+		if (indexMaxPenetration != 2) {
+			// Compute the area
+			final Vector3f vector1 = newPoint.lessNew(this.contactPoints[0].getLocalPointOnBody1());
+			final Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1().lessNew(this.contactPoints[1].getLocalPointOnBody1());
+			final Vector3f crossProduct = vector1.cross(vector2);
+			area2 = crossProduct.length2();
+		}
+		if (indexMaxPenetration != 3) {
+			// Compute the area
+			final Vector3f vector1 = newPoint.lessNew(this.contactPoints[0].getLocalPointOnBody1());
+			final Vector3f vector2 = this.contactPoints[2].getLocalPointOnBody1().lessNew(this.contactPoints[1].getLocalPointOnBody1());
+			final Vector3f crossProduct = vector1.cross(vector2);
+			area3 = crossProduct.length2();
+		}
+		// Return the index of the contact to remove
+		return getMaxArea(area0, area1, area2, area3);
+	}
+	
+	/// Return the largest depth of all the contact points
+	public float getLargestContactDepth() {
+		float largestDepth = 0.0f;
+		for (int iii = 0; iii < this.nbContactPoints; iii++) {
+			final float depth = this.contactPoints[iii].getPenetrationDepth();
+			if (depth > largestDepth) {
+				largestDepth = depth;
+			}
+		}
+		return largestDepth;
+	}
+	
+	private int getMaxArea(final float area0, final float area1, final float area2, final 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;
+		}
+	}
+	
+	/// Return the number of contact points in the manifold
+	public int getNbContactPoints() {
+		return this.nbContactPoints;
+	}
+	
+	/// Return the normal direction Id
+	public int getNormalDirectionId() {
+		return this.normalDirectionId;
+	}
+	
+	public Vector3f getRollingResistanceImpulse() {
+		return this.rollingResistanceImpulse;
+	}
+	
+	/// Return a pointer to the first proxy shape of the contact
+	public ProxyShape getShape1() {
+		return this.shape1;
+	}
+	
+	/// Return a pointer to the second proxy shape of the contact
+	public ProxyShape getShape2() {
+		return this.shape2;
+	}
+	
+	/// Return true if the contact manifold has already been added longo an island
+	public boolean isAlreadyInIsland() {
+		return this.isAlreadyInIsland;
+	}
+	
+	/// Remove a contact point from the manifold
+	public void removeContactPoint(final int index) {
+		assert (index < this.nbContactPoints);
+		assert (this.nbContactPoints > 0);
+		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--;
+	}
+	
+	/// Set the first friction accumulated impulse
+	public void setFrictionImpulse1(final float frictionImpulse1) {
+		this.frictionImpulse1 = frictionImpulse1;
+	}
+	
+	/// Set the second friction accumulated impulse
+	public void setFrictionImpulse2(final float frictionImpulse2) {
+		this.frictionImpulse2 = frictionImpulse2;
+	}
+	
+	/// Set the friction twist accumulated impulse
+	public void setFrictionTwistImpulse(final float frictionTwistImpulse) {
+		this.frictionTwistImpulse = frictionTwistImpulse;
+	}
+	
+	/// set the second friction vector at the center of the contact manifold
+	public void setFrictionvec2(final Vector3f frictionvec2) {
+		this.frictionvec2 = frictionvec2;
+	}
+	
+	/// set the first friction vector at the center of the contact manifold
+	public void setFrictionVector1(final Vector3f frictionVector1) {
+		this.frictionVector1 = frictionVector1;
+	}
+	
+	/// Set the accumulated rolling resistance impulse
+	public void setRollingResistanceImpulse(final Vector3f rollingResistanceImpulse) {
+		this.rollingResistanceImpulse = rollingResistanceImpulse;
+	}
+	
+	/**
+	 *  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.
+	 */
+	public void update(final Transform3D transform1, final Transform3D transform2) {
+		if (this.nbContactPoints == 0) {
+			return;
+		}
+		// Update the world coordinates and penetration depth of the contact points in the manifold
+		for (int iii = 0; iii < this.nbContactPoints; iii++) {
+			this.contactPoints[iii].setWorldPointOnBody1(transform1.multiplyNew(this.contactPoints[iii].getLocalPointOnBody1()));
+			this.contactPoints[iii].setWorldPointOnBody2(transform2.multiplyNew(this.contactPoints[iii].getLocalPointOnBody2()));
+			this.contactPoints[iii]
+					.setPenetrationDepth((this.contactPoints[iii].getWorldPointOnBody1().lessNew(this.contactPoints[iii].getWorldPointOnBody2())).dot(this.contactPoints[iii].getNormal()));
+		}
+		final float squarePersistentContactThreshold = Configuration.PERSISTENT_CONTACT_DIST_THRESHOLD * Configuration.PERSISTENT_CONTACT_DIST_THRESHOLD;
+		// Remove the contact points that don't represent very well the contact manifold
+		for (int iii = (this.nbContactPoints) - 1; iii >= 0; iii--) {
+			assert (iii < (this.nbContactPoints));
+			// Compute the distance between contact points in the normal direction
+			final float distanceNormal = -this.contactPoints[iii].getPenetrationDepth();
+			// If the contacts points are too far from each other in the normal direction
+			if (distanceNormal > squarePersistentContactThreshold) {
+				removeContactPoint(iii);
+			} else {
+				// Compute the distance of the two contact points in the plane
+				// orthogonal to the contact normal
+				final Vector3f projOfPoint1 = this.contactPoints[iii].getNormal().multiplyNew(distanceNormal).add(this.contactPoints[iii].getWorldPointOnBody1());
+				final Vector3f projDifference = this.contactPoints[iii].getWorldPointOnBody2().lessNew(projOfPoint1);
+				// If the orthogonal distance is larger than the valid distance
+				// threshold, we remove the contact
+				if (projDifference.length2() > squarePersistentContactThreshold) {
+					removeContactPoint(iii);
+				}
+			}
+		}
+	}
+}

src/org/atriasoft/ephysics/collision/ContactManifoldListElement.java

@@ -0,0 +1,14 @@
+package org.atriasoft.ephysics.collision;
+
+/**
+ *  This structure represents a single element of a linked list of contact manifolds
+ */
+public class ContactManifoldListElement {
+	public ContactManifold contactManifold; //!< Pointer to the actual contact manifold
+	public ContactManifoldListElement next; //!< Next element of the list
+	
+	public ContactManifoldListElement(final ContactManifold _initContactManifold, final ContactManifoldListElement _initNext) {
+		this.contactManifold = _initContactManifold;
+		this.next = _initNext;
+	}
+}

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

@@ -0,0 +1,217 @@
+package org.atriasoft.ephysics.collision;
+
+import org.atriasoft.ephysics.constraint.ContactPoint;
+import org.atriasoft.etk.math.Constant;
+import org.atriasoft.etk.math.FMath;
+import org.atriasoft.etk.math.Vector3f;
+
+/**
+ *  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.
+ */
+public class ContactManifoldSet {
+	
+	private static final int MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET = 3; // Maximum number of contact manifolds in the set
+	private static final int CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS = 3; // N Number for the N x N subdivisions of the cubemap
+	
+	private final int nbMaxManifolds; //!< Maximum number of contact manifolds in the set
+	private int nbManifolds; //!< Current number of contact manifolds in the set
+	private final ProxyShape shape1; //!< Pointer to the first proxy shape of the contact
+	private final ProxyShape shape2; //!< Pointer to the second proxy shape of the contact
+	
+	private final ContactManifold[] manifolds = new ContactManifold[MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET]; //!< Contact manifolds of the set
+	/// Create a new contact manifold and add it to the set
+	
+	/// Constructor
+	public ContactManifoldSet(final ProxyShape _shape1, final ProxyShape _shape2, final int _nbMaxManifolds) {
+		this.nbMaxManifolds = _nbMaxManifolds;
+		this.nbManifolds = 0;
+		this.shape1 = _shape1;
+		this.shape2 = _shape2;
+		assert (_nbMaxManifolds >= 1);
+	}
+	
+	/// Add a contact point to the manifold set
+	public void addContactPoint(final ContactPoint contact) {
+		// Compute an Id corresponding to the normal direction (using a cubemap)
+		final int 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 iii = 0; iii < this.nbManifolds; iii++) {
+				assert (this.manifolds[iii].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 iii = 0; iii < this.nbManifolds; iii++) {
+				assert (this.manifolds[iii].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 iii = 0; iii < this.nbManifolds; iii++) {
+			final float depth = this.manifolds[iii].getLargestContactDepth();
+			if (depth < minDepth) {
+				minDepth = depth;
+				smallestDepthIndex = iii;
+			}
+		}
+		// If we do not want to keep to new manifold (not created yet) with the
+		// new contact point
+		if (smallestDepthIndex == -1) {
+			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 iii = 0; iii < this.nbManifolds; iii++) {
+			assert (this.manifolds[iii].getNbContactPoints() > 0);
+		}
+		return;
+	}
+	
+	/// Clear the contact manifold set
+	public void clear() {
+		for (int iii = this.nbManifolds - 1; iii >= 0; iii--) {
+			removeManifold(iii);
+		}
+		assert (this.nbManifolds == 0);
+	}
+	
+	// Map the normal vector longo a cubemap face bucket (a face contains 4x4 buckets)
+	// Each face of the cube is divided longo 4x4 buckets. This method maps the
+	// normal vector longo of the of the bucket and returns a unique Id for the bucket
+	private int computeCubemapNormalId(final Vector3f normal) {
+		assert (normal.length2() > Constant.FLOAT_EPSILON);
+		int faceNo;
+		float u, v;
+		final float max = FMath.max(FMath.abs(normal.x), FMath.abs(normal.y), FMath.abs(normal.z));
+		final Vector3f normalScaled = normal.divideNew(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 = FMath.floor(((u + 1) / 2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
+		int indexV = FMath.floor(((v + 1) / 2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
+		if (indexU == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) {
+			indexU--;
+		}
+		if (indexV == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) {
+			indexV--;
+		}
+		final int nbSubDivInFace = CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS;
+		return faceNo * 200 + indexU * nbSubDivInFace + indexV;
+	}
+	
+	private void createManifold(final int normalDirectionId) {
+		assert (this.nbManifolds < this.nbMaxManifolds);
+		this.manifolds[this.nbManifolds] = new ContactManifold(this.shape1, this.shape2, normalDirectionId);
+		this.nbManifolds++;
+	}
+	
+	/// Return a given contact manifold
+	public ContactManifold getContactManifold(final int index) {
+		assert (index >= 0 && index < this.nbManifolds);
+		return this.manifolds[index];
+	}
+	
+	/// Return the number of manifolds in the set
+	public long getNbContactManifolds() {
+		return this.nbManifolds;
+	}
+	
+	/// Return the first proxy shape
+	public ProxyShape getShape1() {
+		return this.shape1;
+	}
+	
+	/// Return the second proxy shape
+	public ProxyShape getShape2() {
+		return this.shape2;
+	}
+	
+	/// Return the total number of contact points in the set of manifolds
+	public int getTotalNbContactPoints() {
+		int nbPoints = 0;
+		for (int iii = 0; iii < this.nbManifolds; iii++) {
+			nbPoints += this.manifolds[iii].getNbContactPoints();
+		}
+		return nbPoints;
+	}
+	
+	/// Remove a contact manifold from the set
+	private void removeManifold(final int index) {
+		assert (this.nbManifolds > 0);
+		assert (index >= 0 && index < this.nbManifolds);
+		// Delete the new contact
+		this.manifolds[index] = null;
+		for (int iii = index; (iii + 1) < this.nbManifolds; iii++) {
+			this.manifolds[iii] = this.manifolds[iii + 1];
+		}
+		this.nbManifolds--;
+	}
+	
+	// Return the index of the contact manifold with a similar average normal.
+	private int selectManifoldWithSimilarNormal(final int normalDirectionId) {
+		// Return the Id of the manifold with the same normal direction id (if exists)
+		for (int iii = 0; iii < this.nbManifolds; iii++) {
+			if (normalDirectionId == this.manifolds[iii].getNormalDirectionId()) {
+				return iii;
+			}
+		}
+		return -1;
+	}
+	
+	/// Update the contact manifolds
+	public void update() {
+		for (int iii = this.nbManifolds - 1; iii >= 0; iii--) {
+			// Update the contact manifold
+			this.manifolds[iii].update(this.shape1.getBody().getTransform().multiplyNew(this.shape1.getLocalToBodyTransform()),
+					this.shape2.getBody().getTransform().multiplyNew(this.shape2.getLocalToBodyTransform()));
+			// Remove the contact manifold if has no contact points anymore
+			if (this.manifolds[iii].getNbContactPoints() == 0) {
+				removeManifold(iii);
+			}
+		}
+	}
+	
+}

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

@@ -0,0 +1,201 @@
+package org.atriasoft.ephysics.collision;
+
+import org.atriasoft.ephysics.RaycastInfo;
+import org.atriasoft.ephysics.body.CollisionBody;
+import org.atriasoft.ephysics.collision.broadphase.DTree;
+import org.atriasoft.ephysics.collision.shapes.CollisionShape;
+import org.atriasoft.ephysics.mathematics.Ray;
+import org.atriasoft.etk.math.Transform3D;
+import org.atriasoft.etk.math.Vector3f;
+
+public class ProxyShape {
+	protected CollisionBody body; //!< Pointer to the parent body
+	protected CollisionShape collisionShape; //!< Internal collision shape
+	public Transform3D localToBodyTransform; //!< Local-space to parent body-space transform (does not change over time)
+	protected float mass; //!< Mass (in kilogramms) of the corresponding collision shape
+	public ProxyShape next = null; //!< Pointer to the next proxy shape of the body (linked list)
+	public DTree broadPhaseID = null; //!< Broad-phase ID (node ID in the dynamic AABB tree)
+	
+	protected Object cachedCollisionData = null; //!< Cached collision data
+	
+	protected Object userData = null; //!< 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.
+	 */
+	protected int collisionCategoryBits = 0x0001;
+	/**
+	 * @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.
+	 */
+	protected int collideWithMaskBits = 0xFFFF;
+	
+	/**
+	 * @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)
+	 */
+	public ProxyShape(final CollisionBody body, final CollisionShape shape, final Transform3D transform, final float mass) {
+		this.body = body;
+		this.collisionShape = shape;
+		this.localToBodyTransform = transform;
+		this.mass = mass;
+	}
+	
+	/**
+	 * @return Pointer to the parent body
+	 */
+	public CollisionBody getBody() {
+		return this.body;
+	}
+	
+	public DTree getBroadPhaseID() {
+		return this.broadPhaseID;
+	}
+	
+	/// Return the pointer to the cached collision data
+	public Object getCachedCollisionData() {
+		return this.cachedCollisionData;
+	}
+	
+	/// Return the collision bits mask
+	public int getCollideWithMaskBits() {
+		return this.collideWithMaskBits;
+	}
+	
+	/// Return the collision category bits
+	public int getCollisionCategoryBits() {
+		return this.collisionCategoryBits;
+	}
+	
+	/// Return the collision shape
+	public CollisionShape getCollisionShape() {
+		return this.collisionShape;
+	}
+	
+	/// Return the local scaling vector of the collision shape
+	public Vector3f getLocalScaling() {
+		return this.collisionShape.getScaling();
+	}
+	
+	/**
+	 * 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
+	 */
+	public Transform3D getLocalToBodyTransform() {
+		return this.localToBodyTransform;
+	}
+	
+	/// Return the local to world transform
+	public Transform3D getLocalToWorldTransform() {
+		return this.body.getTransform().multiplyNew(this.localToBodyTransform);
+	}
+	
+	/// Return the mass of the collision shape
+	public float getMass() {
+		return this.mass;
+	}
+	
+	/// Return the next proxy shape in the linked list of proxy shapes
+	public ProxyShape getNext() {
+		return this.next;
+	}
+	
+	/// Return a pointer to the user data attached to this body
+	public Object getUserData() {
+		return this.userData;
+	}
+	
+	/**
+	 * @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
+	 */
+	public boolean raycast(final Ray ray, final RaycastInfo raycastInfo) {
+		
+		// If the corresponding body is not active, it cannot be hit by rays
+		if (!this.body.isActive()) {
+			return false;
+		}
+		
+		// Convert the ray longo the local-space of the collision shape
+		final Transform3D localToWorldTransform = getLocalToWorldTransform();
+		final Transform3D worldToLocalTransform = localToWorldTransform.inverseNew();
+		
+		final Ray rayLocal = new Ray(worldToLocalTransform.multiplyNew(ray.point1), worldToLocalTransform.multiplyNew(ray.point2), ray.maxFraction);
+		
+		final boolean isHit = this.collisionShape.raycast(rayLocal, raycastInfo, this);
+		if (isHit == true) {
+			// Convert the raycast info longo world-space
+			raycastInfo.worldPoint = localToWorldTransform.multiplyNew(raycastInfo.worldPoint);
+			raycastInfo.worldNormal = localToWorldTransform.getOrientation().multiply(raycastInfo.worldNormal);
+			raycastInfo.worldNormal.normalize();
+		}
+		return isHit;
+	}
+	
+	public void setBroadPhaseID(final DTree broadPhase) {
+		this.broadPhaseID = broadPhase;
+	}
+	
+	/// Set the collision bits mask
+	public void setCollideWithMaskBits(final int collideWithMaskBits) {
+		this.collideWithMaskBits = collideWithMaskBits;
+	}
+	
+	/// Set the collision category bits
+	public void setCollisionCategoryBits(final int collisionCategoryBits) {
+		this.collisionCategoryBits = collisionCategoryBits;
+	}
+	
+	/**
+	 * Set the local scaling vector of the collision shape
+	 * @param scaling The new local scaling vector
+	 */
+	public void setLocalScaling(final 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);
+	}
+	
+	/// Set the local to parent body transform
+	public void setLocalToBodyTransform(final 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);
+	}
+	
+	/// Attach user data to this body
+	public void setUserData(final Object userData) {
+		this.userData = userData;
+	}
+	
+	/**
+	 * @param worldPoint Point to test in world-space coordinates
+	 * @return True if the point is inside the collision shape
+	 */
+	public boolean testPointInside(final Vector3f worldPoint) {
+		final Transform3D localToWorld = this.body.getTransform().multiplyNew(this.localToBodyTransform);
+		final Vector3f localPoint = localToWorld.inverseNew().multiply(worldPoint);
+		return this.collisionShape.testPointInside(localPoint, this);
+	}
+	
+}

src/org/atriasoft/ephysics/collision/TestCollisionBetweenShapesCallback.java

@@ -0,0 +1,22 @@
+package org.atriasoft.ephysics.collision;
+
+import org.atriasoft.ephysics.collision.narrowphase.NarrowPhaseCallback;
+import org.atriasoft.ephysics.constraint.ContactPointInfo;
+import org.atriasoft.ephysics.engine.CollisionCallback;
+import org.atriasoft.ephysics.engine.OverlappingPair;
+
+public class TestCollisionBetweenShapesCallback implements NarrowPhaseCallback {
+	
+	private final CollisionCallback collisionCallback;
+	
+	// Constructor
+	public TestCollisionBetweenShapesCallback(final CollisionCallback _callback) {
+		this.collisionCallback = _callback;
+		
+	}
+	
+	// Called by a narrow-phase collision algorithm when a new contact has been found
+	public void notifyContact(final OverlappingPair _overlappingPair, final ContactPointInfo _contactInfo) {
+		this.collisionCallback.notifyContact(_contactInfo);
+	}
+}

src/org/atriasoft/ephysics/collision/Triangle.java

@@ -0,0 +1,12 @@
+package org.atriasoft.ephysics.collision;
+
+import org.atriasoft.etk.math.Vector3f;
+
+public class Triangle {
+	public final Vector3f[] value = new Vector3f[3];
+	
+	public Vector3f get(final int _id) {
+		return this.value[_id];
+	}
+	
+}

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

@@ -0,0 +1,36 @@
+package org.atriasoft.ephysics.collision;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class TriangleMesh {
+	/// All the triangle arrays of the mesh (one triangle array per part)
+	protected List<TriangleVertexArray> triangleArrays = new ArrayList<>();
+	
+	/**
+	 *  Constructor
+	 */
+	public TriangleMesh() {}
+	
+	/**
+	 *  Add a subpart of the mesh
+	 */
+	public void addSubpart(final TriangleVertexArray _triangleVertexArray) {
+		this.triangleArrays.add(_triangleVertexArray);
+	}
+	
+	/**
+	 *  Get the number of subparts of the mesh
+	 */
+	public int getNbSubparts() {
+		return this.triangleArrays.size();
+	}
+	
+	/**
+	 *  Get a pointer to a given subpart (triangle vertex array) of the mesh
+	 */
+	public TriangleVertexArray getSubpart(final int _indexSubpart) {
+		assert (_indexSubpart < this.triangleArrays.size());
+		return this.triangleArrays.get(_indexSubpart);
+	}
+}

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

@@ -0,0 +1,84 @@
+package org.atriasoft.ephysics.collision;
+
+import java.util.List;
+
+import org.atriasoft.etk.math.Vector3f;
+
+/**
+ * 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
+ * longo 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.
+ */
+public class TriangleVertexArray {
+	/// Vertice list
+	protected final Vector3f[] vertices;
+	/// List of triangle (3 pos for each triangle)
+	protected final int[] triangles;
+	
+	public TriangleVertexArray(final List<Vector3f> _vertices, final List<Integer> _triangles) {
+		this.vertices = _vertices.toArray(new Vector3f[0]);
+		
+		this.triangles = new int[_triangles.size()];
+		for (int iii = 0; iii < this.triangles.length; iii++) {
+			this.triangles[iii] = _triangles.get(iii);
+		}
+	}
+	
+	/**
+	 * Constructor
+	 * @param _vertices List Of all vertices
+	 * @param _triangles List of all linked points
+	 */
+	public TriangleVertexArray(final Vector3f[] _vertices, final int[] _triangles) {
+		this.vertices = _vertices;
+		this.triangles = _triangles;
+	}
+	
+	/**
+	* Get The table of the triangle indice
+	* @return reference on the triangle indice
+	*/
+	public int[] getIndices() {
+		return this.triangles;
+	}
+	
+	/**
+	 * Get the number of triangle
+	 * @return Number of triangles
+	 */
+	public int getNbTriangles() {
+		return this.triangles.length / 3;
+	}
+	
+	/**
+	 * Get the number of vertices
+	 * @return Number of vertices
+	 */
+	public int getNbVertices() {
+		return this.vertices.length;
+	}
+	
+	/**
+	 * Get a triangle at the specific ID
+	 * @return Buffer of 3 points
+	 */
+	public Triangle getTriangle(final int _id) {
+		final Triangle out = new Triangle();
+		out.value[0] = this.vertices[this.triangles[_id * 3]];
+		out.value[1] = this.vertices[this.triangles[_id * 3 + 1]];
+		out.value[2] = this.vertices[this.triangles[_id * 3 + 2]];
+		return out;
+	}
+	
+	/**
+	* Get The table of the vertices
+	* @return reference on the vertices
+	*/
+	public Vector3f[] getVertices() {
+		return this.vertices;
+	}
+};

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

@@ -0,0 +1,256 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.collision.broadphase;
+
+import java.util.ArrayList;
+import java.util.Comparator;
+import java.util.Iterator;
+import java.util.List;
+
+import org.atriasoft.ephysics.Configuration;
+import org.atriasoft.ephysics.RaycastTest;
+import org.atriasoft.ephysics.collision.CollisionDetection;
+import org.atriasoft.ephysics.collision.ProxyShape;
+import org.atriasoft.ephysics.collision.shapes.AABB;
+import org.atriasoft.ephysics.mathematics.Ray;
+import org.atriasoft.etk.math.Vector3f;
+
+/**
+ *  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.
+ */
+public class BroadPhaseAlgorithm {
+	/**
+	 * Callback called when the AABB of a leaf node is hit by a ray the
+	 * broad-phase Dynamic AABB Tree.
+	 */
+	public class BroadPhaseRaycastCallback implements CallbackRaycast {
+		private final DynamicAABBTree dynamicAABBTree;
+		private final int raycastWithCategoryMaskBits;
+		private final RaycastTest raycastTest;
+		
+		// Constructor
+		public BroadPhaseRaycastCallback(final DynamicAABBTree _dynamicAABBTree, final int _raycastWithCategoryMaskBits, final RaycastTest _raycastTest) {
+			this.dynamicAABBTree = _dynamicAABBTree;
+			this.raycastWithCategoryMaskBits = _raycastWithCategoryMaskBits;
+			this.raycastTest = _raycastTest;
+			
+		}
+		
+		@Override
+		public float callback(final DTree _node, final Ray _ray) {
+			float hitFraction = -1.0f;
+			// Get the proxy shape from the node
+			final ProxyShape proxyShape = (ProxyShape) this.dynamicAABBTree.getNodeDataPointer(_node);
+			// 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;
+		}
+	};
+	
+	/// A tree of data that is separated by a specific distance in AABB model.
+	protected DynamicAABBTree dynamicAABBTree;
+	/** 
+	 * 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. ==> and re-index in the overlapping tree
+	 */
+	protected List<DTree> movedShapes = new ArrayList<>();
+	/**
+	 * Temporary array of potential overlapping pairs (with potential duplicates)
+	 */
+	protected List<PairDTree> potentialPairs = new ArrayList<>();
+	/**
+	 * Reference to the collision detection object
+	 */
+	protected CollisionDetection collisionDetection; // TODO revoked with generic interface for the callback...
+	
+	public BroadPhaseAlgorithm(final CollisionDetection _collisionDetection) {
+		this.dynamicAABBTree = new DynamicAABBTree(Configuration.DYNAMIC_TREE_AABB_GAP);
+		this.collisionDetection = _collisionDetection;
+	}
+	
+	/// 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.
+	public void addMovedCollisionShape(final DTree _broadPhaseID) {
+		//Log.info("  ** Element that has moved ... = " + _broadPhaseID);
+		this.movedShapes.add(_broadPhaseID);
+	}
+	
+	/// Add a proxy collision shape longo the broad-phase collision detection
+	public void addProxyCollisionShape(final ProxyShape _proxyShape, final AABB _aabb) {
+		// Add the collision shape longo the dynamic AABB tree and get its broad-phase ID
+		final DTree nodeId = this.dynamicAABBTree.addObject(_aabb, _proxyShape);
+		// Set the broad-phase ID of the proxy shape
+		_proxyShape.setBroadPhaseID(nodeId);
+		// Add the collision shape longo the array of bodies that have moved (or have been created)
+		// during the last simulation step
+		addMovedCollisionShape(_proxyShape.getBroadPhaseID());
+	}
+	
+	/// Compute all the overlapping pairs of collision shapes
+	public void computeOverlappingPairs() {
+		this.potentialPairs.clear();
+		// For all collision shapes that have moved (or have been created) during the
+		// last simulation step
+		//Log.info("moved shape = " + this.movedShapes.size());
+		/*
+		for (final DTree it : this.movedShapes) {
+			Log.info("    - " + it);
+		}
+		*/
+		for (final DTree it : this.movedShapes) {
+			// Get the AABB of the shape
+			final AABB shapeAABB = this.dynamicAABBTree.getFatAABB(it);
+			final BroadPhaseAlgorithm self = this;
+			// 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, new CallbackOverlapping() {
+				@Override
+				public void callback(final DTree _nodeId) {
+					// TODO Auto-generated method stub// If both the nodes are the same, we do not create store the overlapping pair
+					if (it == _nodeId) {
+						return;
+					}
+					// Add the new potential pair longo the array of potential overlapping pairs
+					self.potentialPairs.add(new PairDTree(it, _nodeId));
+				}
+			});
+		}
+		//Log.print("Find potential pair : " + this.potentialPairs.size());
+		// 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
+		this.potentialPairs.sort(new Comparator<PairDTree>() {
+			@Override
+			public int compare(final PairDTree _pair1, final PairDTree _pair2) {
+				if (_pair1.first.uid < _pair2.first.uid) {
+					return -1;
+				}
+				if (_pair1.first.uid == _pair2.first.uid) {
+					if (_pair1.second.uid < _pair2.second.uid) {
+						return -1;
+					}
+					if (_pair1.second.uid == _pair2.second.uid) {
+						return 0;
+					}
+					return +1;
+				}
+				return +1;
+			}
+		});
+		// 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
+			final PairDTree pair = this.potentialPairs.get(iii);
+			++iii;
+			// Get the two collision shapes of the pair
+			final ProxyShape shape1 = (ProxyShape) (this.dynamicAABBTree.getNodeDataPointer(pair.first));
+			final ProxyShape shape2 = (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
+				final PairDTree nextPair = this.potentialPairs.get(iii);
+				// If the next pair is different from the previous one, we stop skipping pairs
+				// TODO check if it work without uid ...
+				if (nextPair.first.uid != pair.first.uid || nextPair.second.uid != pair.second.uid) {
+					break;
+				}
+				++iii;
+			}
+		}
+	}
+	
+	/// Ray casting method
+	public void raycast(final Ray _ray, final RaycastTest _raycastTest, final int _raycastWithCategoryMaskBits) {
+		final BroadPhaseRaycastCallback broadPhaseRaycastCallback = new BroadPhaseRaycastCallback(this.dynamicAABBTree, _raycastWithCategoryMaskBits, _raycastTest);
+		this.dynamicAABBTree.raycast(_ray, broadPhaseRaycastCallback);
+	}
+	
+	/// 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.
+	public void removeMovedCollisionShape(final DTree _broadPhaseID) {
+		final Iterator<DTree> it = this.movedShapes.iterator();
+		while (it.hasNext()) {
+			final DTree elem = it.next();
+			if (elem == _broadPhaseID) {
+				it.remove();
+			}
+		}
+	}
+	
+	/// Remove a proxy collision shape from the broad-phase collision detection
+	public void removeProxyCollisionShape(final ProxyShape _proxyShape) {
+		final DTree broadPhaseID = _proxyShape.getBroadPhaseID();
+		// Remove the collision shape from the dynamic AABB tree
+		this.dynamicAABBTree.removeObject(broadPhaseID);
+		// Remove the collision shape longo the array of shapes that have moved (or have been created)
+		// during the last simulation step
+		removeMovedCollisionShape(broadPhaseID);
+	}
+	
+	/// Return true if the two broad-phase collision shapes are overlapping
+	public boolean testOverlappingShapes(final ProxyShape _shape1, final ProxyShape _shape2) {
+		if (_shape1 == _shape2) {
+			return false;
+		}
+		// Get the two AABBs of the collision shapes
+		final AABB aabb1 = this.dynamicAABBTree.getFatAABB(_shape1.getBroadPhaseID());
+		final AABB aabb2 = this.dynamicAABBTree.getFatAABB(_shape2.getBroadPhaseID());
+		// Check if the two AABBs are overlapping
+		return aabb1.testCollision(aabb2);
+	}
+	
+	/// Notify the broad-phase that a collision shape has moved and need to be updated
+	public void updateProxyCollisionShape(final ProxyShape _proxyShape, final AABB _aabb, final Vector3f _displacement) {
+		updateProxyCollisionShape(_proxyShape, _aabb, _displacement, false);
+	}
+	
+	public void updateProxyCollisionShape(final ProxyShape _proxyShape, final AABB _aabb, final Vector3f _displacement, final boolean _forceReinsert) {
+		final DTree broadPhaseID = _proxyShape.getBroadPhaseID();
+		// Update the dynamic AABB tree according to the movement of the collision shape
+		final 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
+		//Log.error("                             ==> hasBeenReInserted = " + hasBeenReInserted);
+		// longo the tree).
+		if (hasBeenReInserted) {
+			// Add the collision shape longo the array of shapes that have moved (or have been created)
+			// during the last simulation step
+			addMovedCollisionShape(broadPhaseID);
+		}
+	}
+};

src/org/atriasoft/ephysics/collision/broadphase/CallbackOverlapping.java

@@ -0,0 +1,5 @@
+package org.atriasoft.ephysics.collision.broadphase;
+
+public interface CallbackOverlapping {
+	public void callback(DTree _nodeId);
+};

src/org/atriasoft/ephysics/collision/broadphase/CallbackRaycast.java

@@ -0,0 +1,7 @@
+package org.atriasoft.ephysics.collision.broadphase;
+
+import org.atriasoft.ephysics.mathematics.Ray;
+
+public interface CallbackRaycast {
+	public float callback(DTree _node, Ray _ray);
+}

src/org/atriasoft/ephysics/collision/broadphase/DTree.java

@@ -0,0 +1,28 @@
+package org.atriasoft.ephysics.collision.broadphase;
+
+import java.lang.ref.WeakReference;
+
+import org.atriasoft.ephysics.collision.shapes.AABB;
+
+/**
+ *  It represents a node of the dynamic AABB tree.
+ */
+public class DTree {
+	private static int simpleCounter = 0;
+	public final int uid;
+	/**
+	 *  A node is either in the tree (has a parent) or in the free nodes list (has a next node)
+	 */
+	WeakReference<DTree> parent = null; //!< Parent node ID (0 is ROOT)
+	int height = -1; //!< Height of the node in the tree TODO check the need...
+	AABB aabb = new AABB(); //!< Fat axis aligned bounding box (AABB) corresponding to the node
+	
+	public DTree() {
+		this.uid = simpleCounter++;
+	}
+	
+	boolean isLeaf() {
+		return false;
+	}
+	
+}

src/org/atriasoft/ephysics/collision/broadphase/DTreeLeafData.java

@@ -0,0 +1,15 @@
+package org.atriasoft.ephysics.collision.broadphase;
+
+class DTreeLeafData extends DTree {
+	Object dataPointer = null;
+	
+	public DTreeLeafData(final Object dataPointer) {
+		super();
+		this.dataPointer = dataPointer;
+	}
+	
+	@Override
+	boolean isLeaf() {
+		return true;
+	}
+}

src/org/atriasoft/ephysics/collision/broadphase/DTreeLeafInt.java

@@ -0,0 +1,17 @@
+package org.atriasoft.ephysics.collision.broadphase;
+
+class DTreeLeafInt extends DTree {
+	int dataInt_0 = 0;
+	int dataInt_1 = 0;
+	
+	public DTreeLeafInt(final int dataInt_0, final int dataInt_1) {
+		super();
+		this.dataInt_0 = dataInt_0;
+		this.dataInt_1 = dataInt_1;
+	}
+	
+	@Override
+	boolean isLeaf() {
+		return true;
+	}
+}

src/org/atriasoft/ephysics/collision/broadphase/DTreeNode.java

@@ -0,0 +1,6 @@
+package org.atriasoft.ephysics.collision.broadphase;
+
+class DTreeNode extends DTree {
+	DTree children_left = null; //!< Left child of the node
+	DTree children_right = null; //!< Right child of the node
+}

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

@@ -0,0 +1,576 @@
+package org.atriasoft.ephysics.collision.broadphase;
+
+import java.lang.ref.WeakReference;
+import java.util.Stack;
+
+import org.atriasoft.ephysics.Configuration;
+import org.atriasoft.ephysics.collision.shapes.AABB;
+import org.atriasoft.ephysics.internal.Log;
+import org.atriasoft.ephysics.mathematics.Ray;
+import org.atriasoft.etk.math.FMath;
+import org.atriasoft.etk.math.Vector3f;
+
+/**
+ *  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.
+ */
+public class DynamicAABBTree {
+	
+	private DTree rootNode; //!< Pointer to the memory location of the nodes of the tree
+	
+	private final float 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
+	
+	/// Constructor
+	public DynamicAABBTree() {
+		this(0.0f);
+	}
+	
+	public DynamicAABBTree(final float extraAABBGap) {
+		this.extraAABBGap = extraAABBGap;
+		init();
+	}
+	
+	/// Add an object into the tree (where node data are two integers)
+	public DTree addObject(final AABB aabb, final int data1, final int data2) {
+		final DTreeLeafInt node = new DTreeLeafInt(data1, data2);
+		addObjectInternal(aabb, node);
+		return node;
+	}
+	
+	/// Add an object into the tree (where node data is a pointer)
+	public DTree addObject(final AABB aabb, final Object data) {
+		final DTreeLeafData node = new DTreeLeafData(data);
+		addObjectInternal(aabb, node);
+		return node;
+	}
+	
+	/// Internally add an object into the tree
+	private void addObjectInternal(final AABB aabb, final DTree leafNode) {
+		// Create the fat aabb to use in the tree
+		leafNode.aabb.setMin(aabb.getMin().lessNew(this.extraAABBGap));
+		leafNode.aabb.setMax(aabb.getMax().addNew(this.extraAABBGap));
+		// Set the height of the node in the tree
+		leafNode.height = 0;
+		// Insert the new leaf node in the tree
+		insertLeafNode(leafNode);
+		assert (leafNode.isLeaf());
+	}
+	
+	/// Balance the sub-tree of a given node using left or right rotations.
+	private DTree balanceSubTreeAtNode(final DTree _node) {
+		assert (_node != null);
+		// If the node is a leaf or the height of A's sub-tree is less than 2
+		if (_node.isLeaf() || _node.height < 2) {
+			// Do not perform any rotation
+			return _node;
+		}
+		final DTreeNode nodeA = (DTreeNode) _node;
+		// Get the two children nodes
+		final DTree nodeB = nodeA.children_left;
+		final DTree nodeC = nodeA.children_right;
+		// Compute the factor of the left and right sub-trees
+		final int balanceFactor = nodeC.height - nodeB.height;
+		// If the right node C is 2 higher than left node B
+		if (balanceFactor > 1) {
+			assert (!nodeC.isLeaf());
+			final DTreeNode nodeCTree = (DTreeNode) nodeC;
+			final DTree nodeF = nodeCTree.children_left;
+			final DTree nodeG = nodeCTree.children_right;
+			nodeCTree.children_left = _node;
+			nodeCTree.parent = nodeA.parent;
+			nodeA.parent = new WeakReference<DTree>(nodeC);
+			if (nodeC.parent != null) {
+				final DTreeNode nodeCParent = (DTreeNode) nodeC.parent.get();
+				if (nodeCParent.children_left == _node) {
+					nodeCParent.children_left = nodeC;
+				} else {
+					assert (nodeCParent.children_right == _node);
+					nodeCParent.children_right = nodeC;
+				}
+			} else {
+				this.rootNode = nodeC;
+			}
+			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) {
+				nodeCTree.children_right = nodeF;
+				nodeA.children_right = nodeG;
+				nodeG.parent = new WeakReference<DTree>(_node);
+				// 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 = FMath.max(nodeB.height, nodeG.height) + 1;
+				nodeC.height = FMath.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
+				nodeCTree.children_right = nodeG;
+				nodeA.children_right = nodeF;
+				nodeF.parent = new WeakReference<DTree>(_node);
+				// 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 = FMath.max(nodeB.height, nodeF.height) + 1;
+				nodeC.height = FMath.max(nodeA.height, nodeG.height) + 1;
+				assert (nodeA.height > 0);
+				assert (nodeC.height > 0);
+			}
+			// Return the new root of the sub-tree
+			return nodeC;
+		}
+		// If the left node B is 2 higher than right node C
+		if (balanceFactor < -1) {
+			assert (!nodeB.isLeaf());
+			final DTreeNode nodeBTree = (DTreeNode) nodeB;
+			final DTree nodeF = nodeBTree.children_left;
+			final DTree nodeG = nodeBTree.children_right;
+			nodeBTree.children_left = _node;
+			nodeB.parent = nodeA.parent;
+			nodeA.parent = new WeakReference<DTree>(nodeB);
+			if (nodeB.parent != null) {
+				final DTreeNode nodeBParent = (DTreeNode) nodeB.parent.get();
+				if (nodeBParent.children_left == _node) {
+					nodeBParent.children_left = nodeB;
+				} else {
+					assert (nodeBParent.children_right == _node);
+					nodeBParent.children_right = nodeB;
+				}
+			} else {
+				this.rootNode = nodeB;
+			}
+			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) {
+				nodeBTree.children_right = nodeF;
+				nodeA.children_left = nodeG;
+				nodeG.parent = new WeakReference<DTree>(_node);
+				// 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 = FMath.max(nodeC.height, nodeG.height) + 1;
+				nodeB.height = FMath.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
+				nodeBTree.children_right = nodeG;
+				nodeA.children_left = nodeF;
+				nodeF.parent = new WeakReference<DTree>(_node);
+				// 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 = FMath.max(nodeC.height, nodeF.height) + 1;
+				nodeB.height = FMath.max(nodeA.height, nodeG.height) + 1;
+				assert (nodeA.height > 0);
+				assert (nodeB.height > 0);
+			}
+			// Return the new root of the sub-tree
+			return nodeB;
+		}
+		// If the sub-tree is balanced, return the current root node
+		return _node;
+	}
+	
+	/// Compute the height of the tree
+	public int computeHeight() {
+		return computeHeight(this.rootNode);
+	}
+	
+	/// Compute the height of a given node in the tree
+	private int computeHeight(final DTree _node) {
+		// If the node is a leaf, its height is zero
+		if (_node.isLeaf()) {
+			return 0;
+		}
+		final DTreeNode nodeTree = (DTreeNode) _node;
+		
+		// Compute the height of the left and right sub-tree
+		final int leftHeight = computeHeight(nodeTree.children_left);
+		final int rightHeight = computeHeight(nodeTree.children_right);
+		// Return the height of the node
+		return 1 + Math.max(leftHeight, rightHeight);
+	}
+	
+	/// Return the fat AABB corresponding to a given node ID
+	public AABB getFatAABB(final DTree node) {
+		return node.aabb;
+	}
+	
+	public int getNodeDataInt_0(final DTree node) {
+		assert (node.isLeaf());
+		return ((DTreeLeafInt) node).dataInt_0;
+	}
+	
+	public int getNodeDataInt_1(final DTree node) {
+		assert (node.isLeaf());
+		return ((DTreeLeafInt) node).dataInt_1;
+	}
+	
+	/// Return the data pointer of a given leaf node of the tree
+	public Object getNodeDataPointer(final DTree node) {
+		assert (node.isLeaf());
+		return ((DTreeLeafData) node).dataPointer;
+	}
+	
+	/// Return the root AABB of the tree
+	public AABB getRootAABB() {
+		return getFatAABB(this.rootNode);
+	}
+	
+	/// Initialize the tree
+	private void init() {
+		this.rootNode = null;
+	}
+	
+	/// Insert a leaf node in the tree
+	private void insertLeafNode(final DTree _node) {
+		// If the tree is empty
+		if (this.rootNode == null) {
+			this.rootNode = _node;
+			return;
+		}
+		// Find the best sibling node for the new node
+		final AABB newNodeAABB = _node.aabb;
+		DTree currentNode = this.rootNode;
+		while (!currentNode.isLeaf()) {
+			final DTreeNode node = (DTreeNode) currentNode;
+			final DTree leftChild = node.children_left;
+			final DTree rightChild = node.children_right;
+			// Compute the merged AABB
+			final float volumeAABB = currentNode.aabb.getVolume();
+			final AABB mergedAABBs = new AABB();
+			mergedAABBs.mergeTwoAABBs(currentNode.aabb, newNodeAABB);
+			final float mergedVolume = mergedAABBs.getVolume();
+			// Compute the cost of making the current node the sibbling of the new node
+			final float costS = 2.0f * mergedVolume;
+			// Compute the minimum cost of pushing the new node further down the tree (inheritance cost)
+			final float costI = 2.0f * (mergedVolume - volumeAABB);
+			// Compute the cost of descending into the left child
+			float costLeft;
+			final AABB currentAndLeftAABB = new AABB();
+			currentAndLeftAABB.mergeTwoAABBs(newNodeAABB, leftChild.aabb);
+			if (leftChild.isLeaf()) { // If the left child is a leaf
+				costLeft = currentAndLeftAABB.getVolume() + costI;
+			} else {
+				final float leftChildVolume = leftChild.aabb.getVolume();
+				costLeft = costI + currentAndLeftAABB.getVolume() - leftChildVolume;
+			}
+			// Compute the cost of descending into the right child
+			float costRight;
+			final AABB currentAndRightAABB = new AABB();
+			currentAndRightAABB.mergeTwoAABBs(newNodeAABB, rightChild.aabb);
+			if (rightChild.isLeaf()) { // If the right child is a leaf
+				costRight = currentAndRightAABB.getVolume() + costI;
+			} else {
+				final float rightChildVolume = 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) {
+				currentNode = leftChild;
+			} else {
+				currentNode = rightChild;
+			}
+		}
+		final DTree siblingNode = currentNode;
+		// Create a new parent for the new node and the sibling node
+		final WeakReference<DTree> oldParentNode = siblingNode.parent;
+		final DTreeNode newParentNode = new DTreeNode();
+		newParentNode.parent = currentNode.parent;
+		newParentNode.aabb.mergeTwoAABBs(currentNode.aabb, newNodeAABB);
+		newParentNode.height = currentNode.height + 1;
+		// If the sibling node was not the root node
+		if (oldParentNode != null) {
+			// replace in parent the child with the new child
+			final DTreeNode parentNode = (DTreeNode) oldParentNode.get();
+			if (parentNode.children_left == siblingNode) {
+				parentNode.children_left = newParentNode;
+			} else {
+				parentNode.children_right = newParentNode;
+			}
+		} else {
+			// If the sibling node was the root node
+			this.rootNode = newParentNode;
+		}
+		// setup the children
+		newParentNode.children_left = siblingNode;
+		newParentNode.children_right = _node;
+		siblingNode.parent = new WeakReference<DTree>(newParentNode);
+		_node.parent = new WeakReference<DTree>(newParentNode);
+		
+		// Move up in the tree to change the AABBs that have changed
+		currentNode = newParentNode;
+		assert (!currentNode.isLeaf());
+		while (currentNode != null) {
+			// Balance the sub-tree of the current node if it is not balanced
+			currentNode = balanceSubTreeAtNode(currentNode);
+			assert (_node.isLeaf());
+			assert (!currentNode.isLeaf());
+			final DTreeNode nodeDouble = (DTreeNode) currentNode;
+			final DTree leftChild = nodeDouble.children_left;
+			final DTree rightChild = nodeDouble.children_right;
+			assert (leftChild != null);
+			assert (rightChild != null);
+			// Recompute the height of the node in the tree
+			currentNode.height = FMath.max(leftChild.height, rightChild.height) + 1;
+			assert (currentNode.height > 0);
+			// Recompute the AABB of the node
+			currentNode.aabb.mergeTwoAABBs(leftChild.aabb, rightChild.aabb);
+			if (currentNode.parent != null) {
+				currentNode = currentNode.parent.get();
+			} else {
+				currentNode = null;
+			}
+		}
+		assert (_node.isLeaf());
+	}
+	
+	/// Ray casting method
+	public void raycast(final Ray _ray, final CallbackRaycast _callback) {
+		if (_callback == null) {
+			Log.error("call with null callback");
+			return;
+		}
+		float maxFraction = _ray.maxFraction;
+		// 128 max
+		final Stack<DTree> stack = new Stack<DTree>();
+		stack.push(this.rootNode);
+		// Walk through the tree from the root looking for proxy shapes
+		// that overlap with the ray AABB
+		while (stack.size() > 0) {
+			// Get the next node in the stack
+			final DTree node = stack.pop();
+			// If it is a null node, skip it
+			if (node == null) {
+				continue;
+			}
+			final Ray rayTemp = new Ray(_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
+				final float hitFraction = _callback.callback(node, 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
+				final DTreeNode tmpNode = (DTreeNode) node;
+				// Push its children in the stack of nodes to explore
+				stack.push(tmpNode.children_left);
+				stack.push(tmpNode.children_right);
+			}
+		}
+		
+	}
+	
+	/// Release a node
+	private void releaseNode(final DTree _node) {
+		//this.numberNodes--;
+	}
+	
+	/// Remove a leaf node from the tree
+	private void removeLeafNode(final DTree _node) {
+		assert (_node.isLeaf());
+		// If we are removing the root node (root node is a leaf in this case)
+		if (this.rootNode == _node) {
+			this.rootNode = null;
+			return;
+		}
+		// parent can not be null.
+		final DTreeNode parentNode = (DTreeNode) _node.parent.get();
+		final WeakReference<DTree> grandParentNodeWeak = parentNode.parent;
+		DTree siblingNode;
+		if (parentNode.children_left == _node) {
+			siblingNode = parentNode.children_right;
+		} else {
+			siblingNode = parentNode.children_left;
+		}
+		// If the parent of the node to remove is not the root node
+		if (grandParentNodeWeak == null) {
+			// If the parent of the node to remove is the root node
+			// The sibling node becomes the new root node
+			this.rootNode = siblingNode;
+			siblingNode.parent = null;
+			releaseNode(parentNode);
+		} else {
+			final DTreeNode grandParentNode = (DTreeNode) grandParentNodeWeak.get();
+			// Destroy the parent node
+			if (grandParentNode.children_left == parentNode) {
+				grandParentNode.children_left = siblingNode;
+			} else {
+				grandParentNode.children_right = siblingNode;
+			}
+			siblingNode.parent = parentNode.parent;
+			releaseNode(parentNode);
+			// 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
+			DTree currentNode = grandParentNode;
+			while (currentNode != null) {
+				// Balance the current sub-tree if necessary
+				currentNode = balanceSubTreeAtNode(currentNode);
+				assert (!currentNode.isLeaf());
+				final DTreeNode currentTreeNode = (DTreeNode) currentNode;
+				// Get the two children of the current node
+				final DTree leftChild = currentTreeNode.children_left;
+				final DTree rightChild = currentTreeNode.children_right;
+				// Recompute the AABB and the height of the current node
+				currentNode.aabb.mergeTwoAABBs(leftChild.aabb, rightChild.aabb);
+				currentNode.height = FMath.max(leftChild.height, rightChild.height) + 1;
+				assert (currentNode.height > 0);
+				if (currentNode.parent == null) {
+					currentNode = null;
+				} else {
+					currentNode = currentNode.parent.get();
+				}
+			}
+		}
+	}
+	
+	/// Remove an object from the tree
+	public void removeObject(final DTree _node) {
+		assert (_node.isLeaf());
+		// Remove the node from the tree
+		removeLeafNode(_node);
+		releaseNode(_node);
+	}
+	
+	/// Report all shapes overlapping with the AABB given in parameter.
+	public void reportAllShapesOverlappingWithAABB(final AABB _aabb, final CallbackOverlapping _callback) {
+		if (_callback == null) {
+			Log.error("call with null callback");
+			return;
+		}
+		//Log.error("reportAllShapesOverlappingWithAABB");
+		// Create a stack with the nodes to visit
+		final Stack<DTree> stack = new Stack<DTree>();
+		// 64 max
+		stack.push(this.rootNode);
+		//Log.error("    add stack: " + this.rootNode);
+		// While there are still nodes to visit
+		while (stack.size() > 0) {
+			// Get the next node ID to visit
+			final DTree nodeIDToVisit = stack.pop();
+			// Skip it if it is a null node
+			if (nodeIDToVisit == null) {
+				continue;
+			}
+			// Get the corresponding node
+			final DTree nodeToVisit = nodeIDToVisit;
+			//Log.error("      check colision: " + 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()) {
+					/*
+					if (_aabb != nodeToVisit.aabb) {
+						Log.error("           ======> Real collision ...");
+					}
+					*/
+					// Notify the broad-phase about a new potential overlapping pair
+					_callback.callback(nodeIDToVisit);
+				} else {
+					final DTreeNode tmp = (DTreeNode) nodeToVisit;
+					// If the node is not a leaf
+					// We need to visit its children
+					stack.push(tmp.children_left);
+					stack.push(tmp.children_right);
+					//Log.error("    add stack: " + tmp.children_left);
+					//Log.error("    add stack: " + tmp.children_right);
+				}
+			}
+		}
+	}
+	
+	/// Clear all the nodes and reset the tree
+	public void reset() {
+		// Initialize the tree
+		init();
+	}
+	
+	/// 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).
+	public boolean updateObject(final DTree _node, final AABB _newAABB, final Vector3f _displacement) {
+		return updateObject(_node, _newAABB, _displacement, false);
+	}
+	
+	public boolean updateObject(final DTree _node, final AABB _newAABB, final Vector3f _displacement, final boolean _forceReinsert) {
+		assert (_node.isLeaf());
+		assert (_node.height >= 0);
+		//Log.verbose(" compare : " + _node.aabb.getMin() + " " + _node.aabb.getMax());
+		//Log.verbose("         : " + _newAABB.getMin() + " " + _newAABB.getMax());
+		// If the new AABB is still inside the fat AABB of the node
+		if (_forceReinsert == false && _node.aabb.contains(_newAABB)) {
+			return false;
+		}
+		// If the new AABB is outside the fat AABB, we remove the corresponding node
+		removeLeafNode(_node);
+		// Compute the fat AABB by inflating the AABB with a ant gap
+		_node.aabb = _newAABB;
+		
+		final Vector3f gap = new Vector3f(this.extraAABBGap, this.extraAABBGap, this.extraAABBGap);
+		_node.aabb.getMin().less(gap);
+		_node.aabb.getMax().add(gap);
+		// Inflate the fat AABB in direction of the linear motion of the AABB
+		if (_displacement.x < 0.0f) {
+			_node.aabb.getMin().setX(_node.aabb.getMin().x + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * _displacement.x);
+		} else {
+			_node.aabb.getMax().setX(_node.aabb.getMax().x + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * _displacement.x);
+		}
+		if (_displacement.y < 0.0f) {
+			_node.aabb.getMin().setY(_node.aabb.getMin().y + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * _displacement.y);
+		} else {
+			_node.aabb.getMax().setY(_node.aabb.getMax().y + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * _displacement.y);
+		}
+		if (_displacement.z < 0.0f) {
+			_node.aabb.getMin().setZ(_node.aabb.getMin().z + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * _displacement.z);
+		} else {
+			_node.aabb.getMax().setZ(_node.aabb.getMax().z + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * _displacement.z);
+		}
+		//Log.error(" compare : " + _node.aabb.getMin() + " " + _node.aabb.getMax());
+		//Log.error("         : " + _newAABB.getMin() + " " + _newAABB.getMax());
+		if (_node.aabb.contains(_newAABB) == false) {
+			//Log.critical("ERROR");
+		}
+		assert (_node.aabb.contains(_newAABB));
+		// Reinsert the node into the tree
+		insertLeafNode(_node);
+		return true;
+	}
+	
+}

src/org/atriasoft/ephysics/collision/broadphase/PairDTree.java

@@ -0,0 +1,73 @@
+package org.atriasoft.ephysics.collision.broadphase;
+
+import java.util.Set;
+
+import javafx.collections.transformation.SortedList;
+
+public class PairDTree {
+	public static int countInSet(final Set<PairDTree> values, final PairDTree sample) {
+		int count = 0;
+		for (final PairDTree elem : values) {
+			if (elem.first != sample.first) {
+				continue;
+			}
+			if (elem.second != sample.second) {
+				continue;
+			}
+			count++;
+		}
+		return count;
+	}
+	
+	public static int countInSet(final SortedList<PairDTree> values, final PairDTree sample) {
+		int count = 0;
+		for (final PairDTree elem : values) {
+			if (elem.first != sample.first) {
+				continue;
+			}
+			if (elem.second != sample.second) {
+				continue;
+			}
+			count++;
+		}
+		return count;
+	}
+	
+	public final DTree first;
+	
+	public final DTree second;
+	
+	public PairDTree(final DTree first, final DTree second) {
+		if (first.uid < second.uid) {
+			this.first = first;
+			this.second = second;
+		} else {
+			this.first = second;
+			this.second = first;
+			
+		}
+	}
+	
+	@Override
+	public boolean equals(final Object obj) {
+		if (this == obj) {
+			return true;
+		}
+		if (obj == null || getClass() != obj.getClass()) {
+			return false;
+		}
+		final PairDTree tmp = (PairDTree) obj;
+		if (this.first != tmp.first) {
+			return false;
+		}
+		if (this.second != tmp.second) {
+			return false;
+		}
+		return true;
+	}
+	
+	@Override
+	public String toString() {
+		return "PairDTree [first=" + this.first.uid + ", second=" + this.second.uid + "]";
+	}
+}

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

@@ -0,0 +1,21 @@
+package org.atriasoft.ephysics.collision.narrowphase;
+
+import org.atriasoft.ephysics.collision.CollisionDetection;
+import org.atriasoft.ephysics.collision.shapes.CollisionShapeType;
+
+/**
+ * @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.
+ */
+public abstract class CollisionDispatch {
+	
+	/// Initialize the collision dispatch configuration
+	public void init(final CollisionDetection _collisionDetection) {
+		// Nothing to do ...
+	}
+	
+	/// Select and return the narrow-phase collision detection algorithm to
+	/// use between two types of collision shapes.
+	public abstract NarrowPhaseAlgorithm selectAlgorithm(CollisionShapeType _shape1Type, CollisionShapeType _shape2Type);
+}

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

@@ -0,0 +1,355 @@
+package org.atriasoft.ephysics.collision.narrowphase;
+
+import java.util.ArrayList;
+import java.util.Comparator;
+import java.util.List;
+
+import org.atriasoft.etk.math.Transform3D;
+import org.atriasoft.etk.math.Vector3f;
+
+import org.atriasoft.ephysics.collision.CollisionDetection;
+import org.atriasoft.ephysics.collision.CollisionShapeInfo;
+import org.atriasoft.ephysics.collision.ProxyShape;
+import org.atriasoft.ephysics.collision.shapes.AABB;
+import org.atriasoft.ephysics.collision.shapes.CollisionShapeType;
+import org.atriasoft.ephysics.collision.shapes.ConcaveShape;
+import org.atriasoft.ephysics.collision.shapes.ConvexShape;
+import org.atriasoft.ephysics.collision.shapes.TriangleShape;
+import org.atriasoft.ephysics.constraint.ContactPointInfo;
+import org.atriasoft.ephysics.engine.OverlappingPair;
+import org.atriasoft.ephysics.mathematics.Mathematics;
+import org.atriasoft.ephysics.mathematics.PairIntVector3f;
+
+//static boolean sortFunction(SmoothMeshContactInfo&_contact1,SmoothMeshContactInfo&_contact2){return _contact1.contactInfo.penetrationDepth<=_contact2.contactInfo.penetrationDepth;}
+
+/**
+ *  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.
+ */
+public class ConcaveVsConvexAlgorithm extends NarrowPhaseAlgorithm {
+	/**
+	 *  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 implements ConcaveShape.TriangleCallback {
+		
+		protected CollisionDetection collisionDetection; //!< Pointer to the collision detection object
+		protected NarrowPhaseCallback narrowPhaseCallback; //!< Narrow-phase collision callback
+		protected ConvexShape convexShape; //!< Convex collision shape to test collision with
+		protected ConcaveShape concaveShape; //!< Concave collision shape
+		protected ProxyShape convexProxyShape; //!< Proxy shape of the convex collision shape
+		protected ProxyShape concaveProxyShape; //!< Proxy shape of the concave collision shape
+		protected OverlappingPair overlappingPair; //!< Broadphase overlapping pair
+		
+		//  protected static boolean contactsDepthCompare(ContactPointInfo _contact1, ContactPointInfo _contact2);
+		
+		/// Set the collision detection pointer
+		public void setCollisionDetection(final CollisionDetection _collisionDetection) {
+			this.collisionDetection = _collisionDetection;
+		}
+		
+		/// Set the concave collision shape
+		public void setConcaveShape(final ConcaveShape _concaveShape) {
+			this.concaveShape = _concaveShape;
+		}
+		
+		/// Set the convex collision shape to test collision with
+		public void setConvexShape(final ConvexShape _convexShape) {
+			this.convexShape = _convexShape;
+		}
+		
+		/// Set the narrow-phase collision callback
+		public void setNarrowPhaseCallback(final NarrowPhaseCallback _callback) {
+			this.narrowPhaseCallback = _callback;
+		}
+		
+		/// Set the broadphase overlapping pair
+		public void setOverlappingPair(final OverlappingPair _overlappingPair) {
+			this.overlappingPair = _overlappingPair;
+		}
+		
+		/// Set the proxy shapes of the two collision shapes
+		public void setProxyShapes(final ProxyShape _convexProxyShape, final ProxyShape _concaveProxyShape) {
+			this.convexProxyShape = _convexProxyShape;
+			this.concaveProxyShape = _concaveProxyShape;
+		}
+		
+		/// Test collision between a triangle and the convex mesh shape
+		@Override
+		public void testTriangle(final Vector3f[] _trianglePoints) {
+			// Create a triangle collision shape
+			final float margin = this.concaveShape.getTriangleMargin();
+			
+			final TriangleShape triangleShape = new TriangleShape(_trianglePoints[0], _trianglePoints[1], _trianglePoints[2], margin);
+			// Select the collision algorithm to use between the triangle and the convex shape
+			final 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
+			final CollisionShapeInfo shapeConvexInfo = new CollisionShapeInfo(this.convexProxyShape, this.convexShape, this.convexProxyShape.getLocalToWorldTransform(), this.overlappingPair,
+					this.convexProxyShape.getCachedCollisionData());
+			
+			final CollisionShapeInfo shapeConcaveInfo = new CollisionShapeInfo(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);
+		}
+		
+	}
+	
+	/**
+	 *  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 implements NarrowPhaseCallback {
+		private final List<SmoothMeshContactInfo> contactPoints;
+		
+		// Constructor
+		public SmoothCollisionNarrowPhaseCallback(final List<SmoothMeshContactInfo> _contactPoints) {
+			this.contactPoints = new ArrayList<>(_contactPoints);
+		}
+		
+		/// Called by a narrow-phase collision algorithm when a new contact has been found
+		@Override
+		public void notifyContact(final OverlappingPair _overlappingPair, final ContactPointInfo _contactInfo) {
+			final Vector3f[] triangleVertices = new Vector3f[3];
+			boolean isFirstShapeTriangle;
+			// If the collision shape 1 is the triangle
+			if (_contactInfo.collisionShape1.getType() == CollisionShapeType.TRIANGLE) {
+				assert (_contactInfo.collisionShape2.getType() != CollisionShapeType.TRIANGLE);
+				final TriangleShape triangleShape = (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() == CollisionShapeType.TRIANGLE);
+				final TriangleShape triangleShape = (TriangleShape) _contactInfo.collisionShape2;
+				triangleVertices[0] = triangleShape.getVertex(0);
+				triangleVertices[1] = triangleShape.getVertex(1);
+				triangleVertices[2] = triangleShape.getVertex(2);
+				isFirstShapeTriangle = false;
+			}
+			
+			final SmoothMeshContactInfo smoothContactInfo = new SmoothMeshContactInfo(_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.add(smoothContactInfo);
+		}
+		
+	}
+	
+	/**
+	 *  This class is used to store data about a contact with a triangle for the smooth
+	 * mesh algorithm.
+	 */
+	class SmoothMeshContactInfo {
+		public ContactPointInfo contactInfo;
+		public boolean isFirstShapeTriangle;
+		
+		public Vector3f[] triangleVertices = new Vector3f[3];
+		
+		public SmoothMeshContactInfo() {
+			// TODO: add it for List
+		}
+		
+		/// Constructor
+		public SmoothMeshContactInfo(final ContactPointInfo _contact, final boolean _firstShapeTriangle, final Vector3f _trianglePoint1, final Vector3f _trianglePoint2,
+				final Vector3f _trianglePoint3) {
+			this.contactInfo = new ContactPointInfo(_contact);
+			this.isFirstShapeTriangle = _firstShapeTriangle;
+			this.triangleVertices[0] = _trianglePoint1;
+			this.triangleVertices[1] = _trianglePoint2;
+			this.triangleVertices[2] = _trianglePoint3;
+		}
+	}
+	
+	/// Constructor
+	public ConcaveVsConvexAlgorithm(final CollisionDetection collisionDetection) {
+		super(collisionDetection);
+	}
+	
+	/// Add a triangle vertex into the set of processed triangles
+	protected void addProcessedVertex(final List<PairIntVector3f> _processTriangleVertices, final Vector3f _vertex) {
+		_processTriangleVertices.add(new PairIntVector3f((int) (_vertex.x * _vertex.y * _vertex.z), _vertex));
+	}
+	
+	/// Return true if the vertex is in the set of already processed vertices
+	protected boolean hasVertexBeenProcessed(final List<PairIntVector3f> _processTriangleVertices, final Vector3f _vertex) {
+		/* TODO : List<etk::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.equal_range(key);
+		for (auto it = range.first; it != range.second; ++it) {
+			if (    _vertex.x == it.second.x
+			     && _vertex.y == it.second.y
+			     && _vertex.z == it.second.z) {
+				return true;
+			}
+		}
+		return false;
+		*/
+		// TODO : This is not really the same ...
+		for (final PairIntVector3f it : _processTriangleVertices) {
+			if (_vertex.x == it.second.x && _vertex.y == it.second.y && _vertex.z == it.second.z) {
+				return true;
+			}
+		}
+		return false;
+	}
+	
+	/// Process the concave triangle mesh collision using the smooth mesh collision algorithm
+	protected void processSmoothMeshCollision(final OverlappingPair _overlappingPair, final List<SmoothMeshContactInfo> _contactPoints, final NarrowPhaseCallback _callback) {
+		// Set with the triangle vertices already processed to void further contacts with same triangle
+		final List<PairIntVector3f> processTriangleVertices = new ArrayList<>();
+		// Sort the list of narrow-phase contacts according to their penetration depth
+		_contactPoints.sort(new Comparator<SmoothMeshContactInfo>() {
+			@Override
+			public int compare(final SmoothMeshContactInfo _pair1, final SmoothMeshContactInfo _pair2) {
+				if (_pair1.contactInfo.penetrationDepth < _pair2.contactInfo.penetrationDepth) {
+					return -1;
+				}
+				if (_pair1.contactInfo.penetrationDepth == _pair2.contactInfo.penetrationDepth) {
+					return 0;
+				}
+				return +1;
+			}
+		});
+		
+		// For each contact point (from smaller penetration depth to larger)
+		for (final SmoothMeshContactInfo info : _contactPoints) {
+			final Vector3f contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
+			// Compute the barycentric coordinates of the point in the triangle
+			final Float u = 0.0f, v = 0.0f, w = 0.0f;
+			Mathematics.computeBarycentricCoordinatesInTriangle(info.triangleVertices[0], info.triangleVertices[1], info.triangleVertices[2], contactPoint, u, v, w);
+			int nbZeros = 0;
+			final boolean isUZero = Mathematics.ApproxEqual(u, 0.0f, 0.0001f);
+			final boolean isVZero = Mathematics.ApproxEqual(v, 0.0f, 0.0001f);
+			final boolean isWZero = Mathematics.ApproxEqual(w, 0.0f, 0.0001f);
+			if (isUZero) {
+				nbZeros++;
+			}
+			if (isVZero) {
+				nbZeros++;
+			}
+			if (isWZero) {
+				nbZeros++;
+			}
+			// If it is a vertex contact
+			if (nbZeros == 2) {
+				final 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
+				final Vector3f contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]);
+				final 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
+				final ContactPointInfo newContactInfo = new ContactPointInfo(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
+				final Vector3f a = info.triangleVertices[1].lessNew(info.triangleVertices[0]);
+				final Vector3f b = info.triangleVertices[2].lessNew(info.triangleVertices[0]);
+				final Vector3f localNormal = a.cross(b);
+				newContactInfo.normal = firstShape.getLocalToWorldTransform().getOrientation().multiply(localNormal);
+				final Vector3f firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
+				final Vector3f firstWorldPoint = firstShape.getLocalToWorldTransform().multiplyNew(firstLocalPoint);
+				newContactInfo.normal.normalize();
+				if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) {
+					newContactInfo.normal.multiply(-1);
+				}
+				// 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
+				final Transform3D worldToLocalSecondPoint = secondShape.getLocalToWorldTransform().inverseNew();
+				if (info.isFirstShapeTriangle) {
+					final Vector3f newSecondWorldPoint = firstWorldPoint.addNew(newContactInfo.normal);
+					newContactInfo.localPoint2 = worldToLocalSecondPoint.multiplyNew(newSecondWorldPoint);
+				} else {
+					final Vector3f newSecondWorldPoint = firstWorldPoint.lessNew(newContactInfo.normal);
+					newContactInfo.localPoint1 = worldToLocalSecondPoint.multiplyNew(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]);
+		}
+		
+	}
+	
+	/// Compute a contact info if the two bounding volume collide
+	@Override
+	public void testCollision(final CollisionShapeInfo _shape1Info, final CollisionShapeInfo _shape2Info, final 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 = (ConvexShape) _shape1Info.collisionShape;
+			concaveProxyShape = _shape2Info.proxyShape;
+			concaveShape = (ConcaveShape) _shape2Info.collisionShape;
+		} else {
+			// Collision shape 2 is convex, collision shape 1 is concave
+			convexProxyShape = _shape2Info.proxyShape;
+			convexShape = (ConvexShape) _shape2Info.collisionShape;
+			concaveProxyShape = _shape1Info.proxyShape;
+			concaveShape = (ConcaveShape) _shape1Info.collisionShape;
+		}
+		// Set the parameters of the callback object
+		final ConvexVsTriangleCallback convexVsTriangleCallback = new 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
+		final AABB aabb = new AABB();
+		convexShape.computeAABB(aabb, convexProxyShape.getLocalToWorldTransform());
+		// If smooth mesh collision is enabled for the concave mesh
+		if (concaveShape.getIsSmoothMeshCollisionEnabled()) {
+			final List<SmoothMeshContactInfo> contactPoints = new ArrayList<>();
+			
+			final SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback = new SmoothCollisionNarrowPhaseCallback(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);
+		}
+	}
+}

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

@@ -0,0 +1,46 @@
+package org.atriasoft.ephysics.collision.narrowphase;
+
+import org.atriasoft.ephysics.collision.CollisionDetection;
+import org.atriasoft.ephysics.collision.narrowphase.GJK.GJKAlgorithm;
+import org.atriasoft.ephysics.collision.shapes.CollisionShape;
+import org.atriasoft.ephysics.collision.shapes.CollisionShapeType;
+
+/**
+ *  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.
+ */
+public class DefaultCollisionDispatch extends CollisionDispatch {
+	
+	//!< Sphere vs Sphere collision algorithm
+	protected SphereVsSphereAlgorithm sphereVsSphereAlgorithm;
+	//!< Concave vs Convex collision algorithm
+	protected ConcaveVsConvexAlgorithm concaveVsConvexAlgorithm;
+	//!< GJK Algorithm
+	protected GJKAlgorithm GJKAlgorithm;
+	
+	@Override
+	public void init(final CollisionDetection _collisionDetection) {
+		// Initialize the collision algorithms
+		this.sphereVsSphereAlgorithm = new SphereVsSphereAlgorithm(_collisionDetection);
+		this.GJKAlgorithm = new GJKAlgorithm(_collisionDetection);
+		this.concaveVsConvexAlgorithm = new ConcaveVsConvexAlgorithm(_collisionDetection);
+	}
+	
+	@Override
+	public NarrowPhaseAlgorithm selectAlgorithm(final CollisionShapeType _type1, final CollisionShapeType _type2) {
+		// Sphere vs Sphere algorithm
+		if (_type1 == CollisionShapeType.SPHERE && _type2 == CollisionShapeType.SPHERE) {
+			return this.sphereVsSphereAlgorithm;
+		} else if ((!CollisionShape.isConvex(_type1) && CollisionShape.isConvex(_type2)) || (!CollisionShape.isConvex(_type2) && CollisionShape.isConvex(_type1))) {
+			// Concave vs Convex algorithm
+			return this.concaveVsConvexAlgorithm;
+		} else if (CollisionShape.isConvex(_type1) && CollisionShape.isConvex(_type2)) {
+			// Convex vs Convex algorithm (GJK algorithm)
+			return this.GJKAlgorithm;
+		} else {
+			return null;
+		}
+	}
+	
+}

src/org/atriasoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.java

@@ -0,0 +1,423 @@
+package org.atriasoft.ephysics.collision.narrowphase.EPA;
+
+import java.util.Comparator;
+import java.util.SortedSet;
+import java.util.TreeSet;
+
+import org.atriasoft.ephysics.collision.CollisionShapeInfo;
+import org.atriasoft.ephysics.collision.narrowphase.NarrowPhaseCallback;
+import org.atriasoft.ephysics.collision.narrowphase.GJK.GJKAlgorithm;
+import org.atriasoft.ephysics.collision.narrowphase.GJK.Simplex;
+import org.atriasoft.ephysics.collision.shapes.CacheData;
+import org.atriasoft.ephysics.collision.shapes.ConvexShape;
+import org.atriasoft.ephysics.constraint.ContactPointInfo;
+import org.atriasoft.etk.math.Constant;
+import org.atriasoft.etk.math.FMath;
+import org.atriasoft.etk.math.Quaternion;
+import org.atriasoft.etk.math.Transform3D;
+import org.atriasoft.etk.math.Vector3f;
+
+/**
+ *  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".
+ */
+public class EPAAlgorithm {
+	/// Add a triangle face in the candidate triangle heap
+	private void addFaceCandidate(final TriangleEPA _triangle, final SortedSet<TriangleEPA> _heap, final float _upperBoundSquarePenDepth) {
+		////Log.info("addFaceCandidate: " + _triangle.get(0) + ", " + _triangle.get(1) + ", " + _triangle.get(2) + "    " + _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
+		////Log.info("    _triangle.isClosestPointInternalToTriangle(): " + _triangle.isClosestPointInternalToTriangle());
+		////Log.info("    _triangle.getDistSquare(): " + _triangle.getDistSquare());
+		if (_triangle.isClosestPointInternalToTriangle() && _triangle.getDistSquare() <= _upperBoundSquarePenDepth) {
+			// Add the triangle face to the list of candidates
+			_heap.add(_triangle);
+			////Log.info("add in heap:");
+			//int iii = 0;
+			//for (final TriangleEPA elem : _heap) {
+			//	////Log.info("    [" + iii + "] " + elem.getDistSquare());
+			//	++iii;
+			//}
+		}
+	}
+	
+	// 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
+	public void computePenetrationDepthAndContactPoints(final Simplex _simplex, final CollisionShapeInfo _shape1Info, final Transform3D _transform1, final CollisionShapeInfo _shape2Info,
+			final Transform3D _transform2, final Vector3f _vector, final NarrowPhaseCallback _narrowPhaseCallback) {
+		////Log.info("computePenetrationDepthAndContactPoints()");
+		assert (_shape1Info.collisionShape.isConvex());
+		assert (_shape2Info.collisionShape.isConvex());
+		final ConvexShape shape1 = (ConvexShape) (_shape1Info.collisionShape);
+		final ConvexShape shape2 = (ConvexShape) (_shape2Info.collisionShape);
+		final CacheData shape1CachedCollisionData = (CacheData) _shape1Info.cachedCollisionData;
+		final CacheData shape2CachedCollisionData = (CacheData) _shape2Info.cachedCollisionData;
+		final Vector3f suppPointsA[] = new Vector3f[EdgeEPA.MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates
+		final Vector3f suppPointsB[] = new Vector3f[EdgeEPA.MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates
+		final Vector3f points[] = new Vector3f[EdgeEPA.MAX_SUPPORT_POINTS]; // Current points
+		final TrianglesStore triangleStore = new TrianglesStore(); // Store the triangles
+		
+		//https://docs.oracle.com/en/java/javase/11/docs/api/java.base/java/util/SortedSet.html
+		//	https://stackoverflow.com/questions/38066291/how-to-define-comparator-on-sortedset-like-treeset
+		final SortedSet<TriangleEPA> triangleHeap = new TreeSet<>(new Comparator<TriangleEPA>() {
+			@Override
+			public int compare(final TriangleEPA _face1, final TriangleEPA _face2) {
+				final float d1 = _face1.getDistSquare();
+				final float d2 = _face2.getDistSquare();
+				if (d1 < d2) {
+					return -1;
+				}
+				if (d1 > d2) {
+					return 1;
+				}
+				return 0;
+			}
+		});
+		// 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)
+		final Transform3D body2Tobody1 = _transform1.inverseNew().multiplyNew(_transform2);
+		// Matrix that transform a direction from local
+		// space of body 1 into local space of body 2
+		final Quaternion rotateToBody2 = _transform2.getOrientation().inverseNew().multiplyNew(_transform1.getOrientation());
+		// Get the simplex computed previously by the GJK algorithm
+		int nbVertices = _simplex.getSimplex(suppPointsA, suppPointsB, points);
+		// Compute the tolerance
+		final float tolerance = Constant.FLOAT_EPSILON * _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.
+		////Log.info(">>>>>>>>>>>>>>>>>> *** " + nbVertices);
+		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
+				final Vector3f d = points[1].lessNew(points[0]).safeNormalizeNew();
+				// Choose the coordinate axis from the minimal absolute component of the vector d
+				final int minAxis = d.abs().getMinAxis();
+				// Compute sin(60)
+				final float sin60 = FMath.sqrt(3.0f) * 0.5f;
+				// Create a rotation quaternion to rotate the vector v1 to get the vectors
+				// v2 and v3
+				final Quaternion rotationQuat = new Quaternion(d.x * sin60, d.y * sin60, d.z * sin60, 0.5f);
+				// Compute the vector v1, v2, v3
+				final Vector3f v1 = d.cross(new Vector3f(minAxis == 0 ? 1.0f : 0.0f, minAxis == 1 ? 1.0f : 0.0f, minAxis == 2 ? 1.0f : 0.0f));
+				final Vector3f v2 = rotationQuat.multiply(v1);
+				final Vector3f v3 = rotationQuat.multiply(v2);
+				// Compute the support point in the direction of v1
+				suppPointsA[2] = shape1.getLocalSupportPointWithMargin(v1, shape1CachedCollisionData);
+				suppPointsB[2] = body2Tobody1.multiplyNew(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(v1.multiplyNew(-1)), shape2CachedCollisionData));
+				points[2] = suppPointsA[2].lessNew(suppPointsB[2]);
+				// Compute the support point in the direction of v2
+				suppPointsA[3] = shape1.getLocalSupportPointWithMargin(v2, shape1CachedCollisionData);
+				suppPointsB[3] = body2Tobody1.multiplyNew(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(v2.multiplyNew(-1)), shape2CachedCollisionData));
+				points[3] = suppPointsA[3].lessNew(suppPointsB[3]);
+				// Compute the support point in the direction of v3
+				suppPointsA[4] = shape1.getLocalSupportPointWithMargin(v3, shape1CachedCollisionData);
+				suppPointsB[4] = body2Tobody1.multiplyNew(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(v3.multiplyNew(-1)), shape2CachedCollisionData));
+				points[4] = suppPointsA[4].lessNew(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)
+				final 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
+					////Log.error("befor call: (points, 0, 1, 2)");
+					final TriangleEPA face0 = triangleStore.newTriangle(points, 0, 1, 2);
+					////Log.error("befor call: (points, 0, 3, 1)");
+					final TriangleEPA face1 = triangleStore.newTriangle(points, 0, 3, 1);
+					////Log.error("befor call: (points, 0, 2, 3)");
+					final TriangleEPA face2 = triangleStore.newTriangle(points, 0, 2, 3);
+					////Log.error("befor call: (points, 1, 3, 2)");
+					final TriangleEPA face3 = triangleStore.newTriangle(points, 1, 3, 2);
+					// If the ructed tetrahedron is not correct
+					if (!((face0 != null) && (face1 != null) && (face2 != null) && (face3 != null) && face0.getDistSquare() > 0.0f && face1.getDistSquare() > 0.0 && face2.getDistSquare() > 0.0f
+							&& face3.getDistSquare() > 0.0)) {
+						return;
+					}
+					// Associate the edges of neighbouring triangle faces
+					TriangleEPA.link(new EdgeEPA(face0, 0), new EdgeEPA(face1, 2));
+					TriangleEPA.link(new EdgeEPA(face0, 1), new EdgeEPA(face3, 2));
+					TriangleEPA.link(new EdgeEPA(face0, 2), new EdgeEPA(face2, 0));
+					TriangleEPA.link(new EdgeEPA(face1, 0), new EdgeEPA(face2, 2));
+					TriangleEPA.link(new EdgeEPA(face1, 1), new EdgeEPA(face3, 0));
+					TriangleEPA.link(new EdgeEPA(face2, 1), new EdgeEPA(face3, 1));
+					// Add the triangle faces in the candidate heap
+					addFaceCandidate(face0, triangleHeap, Float.MAX_VALUE);
+					addFaceCandidate(face1, triangleHeap, Float.MAX_VALUE);
+					addFaceCandidate(face2, triangleHeap, Float.MAX_VALUE);
+					addFaceCandidate(face3, triangleHeap, Float.MAX_VALUE);
+					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
+				final Vector3f v1 = points[1].lessNew(points[0]);
+				final Vector3f v2 = points[2].lessNew(points[0]);
+				final Vector3f n = v1.cross(v2);
+				////Log.info(">>>>>>>>>>>>>>>>>>");
+				////Log.info("    v1 = " + v1);
+				////Log.info("    v2 = " + v2);
+				////Log.info("    n = " + n);
+				// Compute the two new vertices to obtain a hexahedron
+				suppPointsA[3] = shape1.getLocalSupportPointWithMargin(n, shape1CachedCollisionData);
+				suppPointsB[3] = body2Tobody1.multiplyNew(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(n.multiplyNew(-1)), shape2CachedCollisionData));
+				points[3] = suppPointsA[3].lessNew(suppPointsB[3]);
+				////Log.info("    suppPointsA[3]= " + suppPointsA[3]);
+				////Log.info("    suppPointsB[3]= " + suppPointsB[3]);
+				////Log.info("    points[3] = " + points[3]);
+				suppPointsA[4] = shape1.getLocalSupportPointWithMargin(n.multiplyNew(-1), shape1CachedCollisionData);
+				suppPointsB[4] = body2Tobody1.multiplyNew(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(n), shape2CachedCollisionData));
+				points[4] = suppPointsA[4].lessNew(suppPointsB[4]);
+				////Log.info("    suppPointsA[4]= " + suppPointsA[4]);
+				////Log.info("    suppPointsB[4]= " + suppPointsB[4]);
+				////Log.info("    points[4]= " + points[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
+					////Log.error("befor call: (points, 0, 1, 2)");
+					face0 = triangleStore.newTriangle(points, 0, 1, 2);
+					////Log.error("befor call: (points, 0, 2, 1)");
+					face1 = triangleStore.newTriangle(points, 0, 3, 1);
+					////Log.error("befor call: (points, 0, 2, 3)");
+					face2 = triangleStore.newTriangle(points, 0, 2, 3);
+					////Log.error("befor call: (points, 1, 3, 2)");
+					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
+					////Log.error("befor call: (points, 0, 1, 2)");
+					face0 = triangleStore.newTriangle(points, 0, 1, 2);
+					////Log.error("befor call: (points, 0, 4, 1)");
+					face1 = triangleStore.newTriangle(points, 0, 4, 1);
+					////Log.error("befor call: (points, 0, 2, 4)");
+					face2 = triangleStore.newTriangle(points, 0, 2, 4);
+					////Log.error("befor call: (points, 1, 4, 2)");
+					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.0f && face1.getDistSquare() > 0.0f && face2.getDistSquare() > 0.0f
+						&& face3.getDistSquare() > 0.0f)) {
+					return;
+				}
+				// Associate the edges of neighbouring triangle faces
+				TriangleEPA.link(new EdgeEPA(face0, 0), new EdgeEPA(face1, 2));
+				TriangleEPA.link(new EdgeEPA(face0, 1), new EdgeEPA(face3, 2));
+				TriangleEPA.link(new EdgeEPA(face0, 2), new EdgeEPA(face2, 0));
+				TriangleEPA.link(new EdgeEPA(face1, 0), new EdgeEPA(face2, 2));
+				TriangleEPA.link(new EdgeEPA(face1, 1), new EdgeEPA(face3, 0));
+				TriangleEPA.link(new EdgeEPA(face2, 1), new EdgeEPA(face3, 1));
+				// Add the triangle faces in the candidate heap
+				addFaceCandidate(face0, triangleHeap, Float.MAX_VALUE);
+				addFaceCandidate(face1, triangleHeap, Float.MAX_VALUE);
+				addFaceCandidate(face2, triangleHeap, Float.MAX_VALUE);
+				addFaceCandidate(face3, triangleHeap, Float.MAX_VALUE);
+				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 = null;
+		float upperBoundSquarePenDepth = Float.MAX_VALUE;
+		do {
+			triangle = triangleHeap.first();
+			triangleHeap.remove(triangle);
+			////Log.info("rm from heap:");
+			int iii = 0;
+			for (final TriangleEPA elem : triangleHeap) {
+				////Log.info("    [" + iii + "] " + elem.getDistSquare());
+				++iii;
+			}
+			// 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 == EdgeEPA.MAX_SUPPORT_POINTS) {
+					assert (false);
+					break;
+				}
+				// Compute the support point of the Minkowski
+				// difference (A-B) in the closest point direction
+				suppPointsA[nbVertices] = shape1.getLocalSupportPointWithMargin(triangle.getClosestPoint(), shape1CachedCollisionData);
+				suppPointsB[nbVertices] = body2Tobody1
+						.multiplyNew(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(triangle.getClosestPoint().multiplyNew(-1)), shape2CachedCollisionData));
+				points[nbVertices] = suppPointsA[nbVertices].lessNew(suppPointsB[nbVertices]);
+				final int indexNewVertex = nbVertices;
+				nbVertices++;
+				// Update the upper bound of the penetration depth
+				final float wDotv = points[indexNewVertex].dot(triangle.getClosestPoint());
+				////Log.info("      point=" + points[indexNewVertex]);
+				////Log.info("close point=" + triangle.getClosestPoint());
+				////Log.info("         ==>" + wDotv);
+				if (wDotv < 0.0f) {
+					////Log.error("depth penetration error " + wDotv);
+					continue;
+				}
+				assert (wDotv >= 0.0f);
+				final float wDotVSquare = wDotv * wDotv / triangle.getDistSquare();
+				if (wDotVSquare < upperBoundSquarePenDepth) {
+					upperBoundSquarePenDepth = wDotVSquare;
+				}
+				// Compute the error
+				final float error = wDotv - triangle.getDistSquare();
+				if (error <= FMath.max(tolerance, GJKAlgorithm.REL_ERROR_SQUARE * wDotv) || points[indexNewVertex].isEqual(points[triangle.get(0)])
+						|| points[indexNewVertex].isEqual(points[triangle.get(1)]) || points[indexNewVertex].isEqual(points[triangle.get(2)])) {
+					break;
+				}
+				// Now, we compute the silhouette cast by the new vertex. The current triangle
+				// face will not be in the convex hull. We start the local recursive silhouette
+				// algorithm from the current triangle face.
+				int i = triangleStore.getNbTriangles();
+				if (!triangle.computeSilhouette(points, indexNewVertex, triangleStore)) {
+					break;
+				}
+				// Add all the new triangle faces computed with the silhouette algorithm
+				// to the candidates list of faces of the current polytope
+				while (i != triangleStore.getNbTriangles()) {
+					final TriangleEPA newTriangle = triangleStore.get(i);
+					addFaceCandidate(newTriangle, triangleHeap, upperBoundSquarePenDepth);
+					i++;
+				}
+			}
+		} while (triangleHeap.size() > 0 && triangleHeap.first().getDistSquare() <= upperBoundSquarePenDepth);
+		// Compute the contact info
+		final Vector3f tmp = _transform1.getOrientation().multiply(triangle.getClosestPoint());
+		_vector.set(tmp);
+		final Vector3f pALocal = triangle.computeClosestPointOfObject(suppPointsA);
+		final Vector3f pBLocal = body2Tobody1.inverseNew().multiply(triangle.computeClosestPointOfObject(suppPointsB));
+		final Vector3f normal = _vector.safeNormalizeNew();
+		final float penetrationDepth = _vector.length();
+		assert (penetrationDepth >= 0.0f);
+		if (normal.length2() < Constant.FLOAT_EPSILON) {
+			return;
+		}
+		// Create the contact info object
+		final ContactPointInfo contactInfo = new ContactPointInfo(_shape1Info.proxyShape, _shape2Info.proxyShape, _shape1Info.collisionShape, _shape2Info.collisionShape, normal, penetrationDepth,
+				pALocal, pBLocal);
+		_narrowPhaseCallback.notifyContact(_shape1Info.overlappingPair, contactInfo);
+	}
+	
+	/// Initalize the algorithm
+	public void init() {
+		
+	}
+	
+	// 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
+	private int isOriginInTetrahedron(final Vector3f _p1, final Vector3f _p2, final Vector3f _p3, final Vector3f _p4) {
+		////Log.error("isOriginInTetrahedron(" + _p1 + ", " + _p2 + ", " + _p3 + ", " + _p4 + ")");
+		// Check vertex 1
+		final Vector3f normal1 = _p2.lessNew(_p1).cross(_p3.lessNew(_p1));
+		if ((normal1.dot(_p1) > 0.0f) == (normal1.dot(_p4) > 0.0)) {
+			////Log.error("    ==> 4");
+			return 4;
+		}
+		// Check vertex 2
+		final Vector3f normal2 = _p4.lessNew(_p2).cross(_p3.lessNew(_p2));
+		if ((normal2.dot(_p2) > 0.0f) == (normal2.dot(_p1) > 0.0)) {
+			////Log.error("    ==> 1");
+			return 1;
+		}
+		// Check vertex 3
+		final Vector3f normal3 = _p4.lessNew(_p3).cross(_p1.lessNew(_p3));
+		if ((normal3.dot(_p3) > 0.0f) == (normal3.dot(_p2) > 0.0)) {
+			////Log.error("    ==> 2");
+			return 2;
+		}
+		// Check vertex 4
+		final Vector3f normal4 = _p2.lessNew(_p4).cross(_p1.lessNew(_p4));
+		if ((normal4.dot(_p4) > 0.0f) == (normal4.dot(_p3) > 0.0)) {
+			////Log.error("    ==> 3");
+			return 3;
+		}
+		////Log.error("    ==> 0");
+		// The origin is in the tetrahedron, we return 0
+		return 0;
+	}
+	
+}

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

@@ -0,0 +1,138 @@
+package org.atriasoft.ephysics.collision.narrowphase.EPA;
+
+import org.atriasoft.etk.math.Vector3f;
+
+/**
+ *  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".
+ */
+public class EdgeEPA implements Cloneable {
+	/// Maximum number of support points of the polytope
+	static public final int MAX_SUPPORT_POINTS = 100;
+	/// Maximum number of facets of the polytope
+	static public final int MAX_FACETS = 200;
+	
+	// Return the index of the next counter-clockwise edge of the ownver triangle
+	static public int indexOfNextCounterClockwiseEdge(final int iii) {
+		return (iii + 1) % 3;
+	}
+	
+	// Return the index of the previous counter-clockwise edge of the ownver triangle
+	static public int indexOfPreviousCounterClockwiseEdge(final int iii) {
+		return (iii + 2) % 3;
+	}
+	
+	/// Pointer to the triangle that contains this edge
+	private TriangleEPA 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)
+	private int index;
+	
+	/// Constructor
+	public EdgeEPA() {
+		this.ownerTriangle = null;
+	};
+	
+	/// Copy-ructor
+	public EdgeEPA(final EdgeEPA obj) {
+		this.ownerTriangle = obj.ownerTriangle;
+		this.index = obj.index;
+	}
+	
+	/// Constructor
+	public EdgeEPA(final TriangleEPA ownerTriangle, final int index) {
+		this.ownerTriangle = ownerTriangle;
+		this.index = index;
+		assert (index >= 0 && index < 3);
+	}
+	
+	@Override
+	protected EdgeEPA clone() throws CloneNotSupportedException {
+		return new EdgeEPA(this);
+	}
+	
+	/// Execute the recursive silhouette algorithm from this edge
+	public boolean computeSilhouette(final Vector3f[] vertices, final int indexNewVertex, final TrianglesStore triangleStore) {
+		//Log.error("EdgeEPA computeSilhouette ...");
+		// 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)) {
+				//Log.error("EdgeEPA 1 befor call: " + indexNewVertex + ", " + getTargetVertexIndex() + ", " + getSourceVertexIndex());
+				final TriangleEPA triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex());
+				// If the triangle has been created
+				if (triangle != null) {
+					TriangleEPA.halfLink(new EdgeEPA(triangle, 1), this);
+					return true;
+				}
+				return false;
+			} else {
+				// The current triangle is visible and therefore obsolete
+				this.ownerTriangle.setIsObsolete(true);
+				final int backup = triangleStore.getNbTriangles();
+				if (!this.ownerTriangle.getAdjacentEdge(indexOfNextCounterClockwiseEdge(this.index)).computeSilhouette(vertices, indexNewVertex, triangleStore)) {
+					this.ownerTriangle.setIsObsolete(false);
+					//Log.error("EdgeEPA 2 befor call: " + indexNewVertex + ", " + getTargetVertexIndex() + ", " + getSourceVertexIndex());
+					final TriangleEPA triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex());
+					// If the triangle has been created
+					if (triangle != null) {
+						TriangleEPA.halfLink(new EdgeEPA(triangle, 1), this);
+						return true;
+					}
+					return false;
+				} else if (!this.ownerTriangle.getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(this.index)).computeSilhouette(vertices, indexNewVertex, triangleStore)) {
+					this.ownerTriangle.setIsObsolete(false);
+					triangleStore.resize(backup);
+					//Log.error("EdgeEPA 3 befor call: " + indexNewVertex + ", " + getTargetVertexIndex() + ", " + getSourceVertexIndex());
+					final TriangleEPA triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex());
+					if (triangle != null) {
+						TriangleEPA.halfLink(new EdgeEPA(triangle, 1), this);
+						return true;
+					}
+					return false;
+				}
+			}
+		}
+		return true;
+	}
+	
+	/// Return the index of the edge in the triangle
+	public int getIndex() {
+		return this.index;
+	}
+	
+	/// Return the pointer to the owner triangle
+	public TriangleEPA getOwnerTriangle() {
+		return this.ownerTriangle;
+	}
+	
+	/// Return index of the source vertex of the edge
+	public int getSourceVertexIndex() {
+		//return this.ownerTriangle[this.index];
+		return this.ownerTriangle.get(this.index);
+	}
+	
+	/// Return the index of the target vertex of the edge
+	public int getTargetVertexIndex() {
+		return this.ownerTriangle.get(indexOfNextCounterClockwiseEdge(this.index));
+		//return this.ownerTriangle[indexOfNextCounterClockwiseEdge(this.index)];
+	}
+	
+	/// Assignment
+	public EdgeEPA set(final EdgeEPA obj) {
+		this.ownerTriangle = obj.ownerTriangle;
+		this.index = obj.index;
+		return this;
+	}
+	
+}

src/org/atriasoft/ephysics/collision/narrowphase/EPA/TriangleEPA.java

@@ -0,0 +1,223 @@
+package org.atriasoft.ephysics.collision.narrowphase.EPA;
+
+import org.atriasoft.etk.math.Vector3f;
+
+/**
+ *  Class TriangleEPA
+ * This class represents a triangle face of the current polytope in the EPA algorithm.
+ */
+public class TriangleEPA {
+	/// 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.
+	public static void halfLink(final EdgeEPA _edge0, final EdgeEPA _edge1) {
+		assert (_edge0.getSourceVertexIndex() == _edge1.getTargetVertexIndex() && _edge0.getTargetVertexIndex() == _edge1.getSourceVertexIndex());
+		_edge0.getOwnerTriangle().adjacentEdges[_edge0.getIndex()] = _edge1;
+	}
+	
+	public static boolean link(final EdgeEPA _edge0, final EdgeEPA _edge1) {
+		if (_edge0.getSourceVertexIndex() == _edge1.getTargetVertexIndex() && _edge0.getTargetVertexIndex() == _edge1.getSourceVertexIndex()) {
+			_edge0.getOwnerTriangle().adjacentEdges[_edge0.getIndex()] = _edge1;
+			_edge1.getOwnerTriangle().adjacentEdges[_edge1.getIndex()] = _edge0;
+			return true;
+		}
+		return false;
+	}
+	
+	private final int[] indicesVertices = new int[3]; //!< Indices of the vertices y_i of the triangle
+	private final EdgeEPA[] adjacentEdges = new EdgeEPA[3]; //!< Three adjacent edges of the triangle (edges of other triangles)
+	private boolean isObsolete; //!< True if the triangle face is visible from the new support point
+	private float determinant; //!< Determinant
+	private Vector3f closestPoint; //!< Point v closest to the origin on the affine hull of the triangle
+	private float lambda1; //!< Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
+	
+	private float lambda2; //!< Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
+	
+	private float distSquare; //!< Square distance of the point closest point v to the origin
+	
+	/// Constructor
+	/*
+	public TriangleEPA() {
+		this.adjacentEdges[0] = new EdgeEPA(this, 0);
+		this.adjacentEdges[1] = new EdgeEPA(this, 1);
+		this.adjacentEdges[2] = new EdgeEPA(this, 2);
+		this.closestPoint = new Vector3f();
+	}
+	*/
+	
+	/// Constructor
+	public TriangleEPA(final int _indexVertex1, final int _indexVertex2, final int _indexVertex3) {
+		this.isObsolete = false;
+		this.indicesVertices[0] = _indexVertex1;
+		this.indicesVertices[1] = _indexVertex2;
+		this.indicesVertices[2] = _indexVertex3;
+		this.adjacentEdges[0] = new EdgeEPA();
+		this.adjacentEdges[1] = new EdgeEPA();
+		this.adjacentEdges[2] = new EdgeEPA();
+		this.closestPoint = new Vector3f();
+	}
+	
+	/// Private copy-ructor
+	/*
+	public TriangleEPA(final 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;
+	}
+	*/
+	
+	/// Compute the point v closest to the origin of this triangle
+	public boolean computeClosestPoint(final Vector3f[] _vertices) {
+		final Vector3f p0 = _vertices[this.indicesVertices[0]];
+		final Vector3f v1 = _vertices[this.indicesVertices[1]].lessNew(p0);
+		final Vector3f v2 = _vertices[this.indicesVertices[2]].lessNew(p0);
+		final float v1Dotv1 = v1.dot(v1);
+		final float v1Dotv2 = v1.dot(v2);
+		final float v2Dotv2 = v2.dot(v2);
+		final float p0Dotv1 = p0.dot(v1);
+		final 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.0f) {
+			// Compute the closest point v
+			this.closestPoint = v1.multiplyNew(this.lambda1).add(v2.multiplyNew(this.lambda2)).multiply(1.0f / this.determinant).add(p0);
+			// Compute the square distance of closest point to the origin
+			this.distSquare = this.closestPoint.dot(this.closestPoint);
+			return true;
+		}
+		return false;
+	}
+	
+	/// Compute the point of an object closest to the origin
+	public Vector3f computeClosestPointOfObject(final Vector3f[] _supportPointsOfObject) {
+		final Vector3f p0 = _supportPointsOfObject[this.indicesVertices[0]].clone();
+		final Vector3f tmp1 = _supportPointsOfObject[this.indicesVertices[1]].lessNew(p0).multiply(this.lambda1);
+		final Vector3f tmp2 = _supportPointsOfObject[this.indicesVertices[2]].lessNew(p0).multiply(this.lambda2);
+		return p0.add(tmp1.add(tmp2).multiply(1.0f / this.determinant));
+	}
+	
+	// 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 idered as being a candidate face in the future.
+	public boolean computeSilhouette(final Vector3f[] _vertices, final int _indexNewVertex, final TrianglesStore _triangleStore) {
+		final 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
+		final 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++) {
+				final TriangleEPA triangle = _triangleStore.get(i);
+				halfLink(triangle.getAdjacentEdge(1), new EdgeEPA(triangle, 1));
+				if (!link(new EdgeEPA(triangle, 0), new EdgeEPA(_triangleStore.get(j), 2))) {
+					return false;
+				}
+			}
+		}
+		return result;
+	}
+	
+	/// Access operator
+	public int get(final 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 aint both edges).
+	
+	/// Return an adjacent edge of the triangle
+	public EdgeEPA getAdjacentEdge(final int _index) {
+		assert (_index >= 0 && _index < 3);
+		return this.adjacentEdges[_index];
+	}
+	
+	/// Return the point closest to the origin
+	public Vector3f getClosestPoint() {
+		return this.closestPoint;
+	}
+	
+	/// Return the square distance of the closest point to origin
+	public float getDistSquare() {
+		return this.distSquare;
+	}
+	
+	/// Return true if the triangle face is obsolete
+	public boolean getIsObsolete() {
+		return this.isObsolete;
+	}
+	
+	// Return true if the closest point on affine hull is inside the triangle
+	public boolean isClosestPointInternalToTriangle() {
+		return (this.lambda1 >= 0.0f && this.lambda2 >= 0.0 && (this.lambda1 + this.lambda2) <= this.determinant);
+	}
+	
+	/// Return true if the triangle is visible from a given vertex
+	public boolean isVisibleFromVertex(final Vector3f[] _vertices, final int _index) {
+		final Vector3f closestToVert = _vertices[_index].lessNew(this.closestPoint);
+		return (this.closestPoint.dot(closestToVert) > 0.0f);
+	}
+	
+	/// Constructor
+	public void set(final int _indexVertex1, final int _indexVertex2, final int _indexVertex3) {
+		this.isObsolete = false;
+		this.indicesVertices[0] = _indexVertex1;
+		this.indicesVertices[1] = _indexVertex2;
+		this.indicesVertices[2] = _indexVertex3;
+	}
+	
+	/// Private assignment operator
+	public TriangleEPA set(final 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;
+	}
+	
+	/// Set an adjacent edge of the triangle
+	public void setAdjacentEdge(final int _index, final EdgeEPA _edge) {
+		assert (_index >= 0 && _index < 3);
+		this.adjacentEdges[_index] = _edge;
+	}
+	
+	/// Set the isObsolete value
+	public void setIsObsolete(final boolean _isObsolete) {
+		this.isObsolete = _isObsolete;
+	}
+	
+}

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

@@ -0,0 +1,70 @@
+package org.atriasoft.ephysics.collision.narrowphase.EPA;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import org.atriasoft.ephysics.internal.Log;
+import org.atriasoft.etk.math.Vector3f;
+
+public class TrianglesStore {
+	private static int MAX_TRIANGLES = 200;
+	
+	public static void shrinkTo(final List list, final int newSize) {
+		final int size = list.size();
+		if (newSize >= size) {
+			return;
+		}
+		for (int i = newSize; i < size; i++) {
+			list.remove(list.size() - 1);
+		}
+	}
+	
+	private final List<TriangleEPA> triangles = new ArrayList<>();
+	
+	/// Clear all the storage
+	public void clear() {
+		this.triangles.clear();
+	}
+	
+	/// Access operator
+	public TriangleEPA get(final int _id) {
+		return this.triangles.get(_id);
+	}
+	
+	/// Return the number of triangles
+	public int getNbTriangles() {
+		return this.triangles.size();
+	}
+	
+	/// Return the last triangle
+	public TriangleEPA last() {
+		return this.triangles.get(this.triangles.size() - 1);
+	}
+	
+	/// Create a new triangle
+	public TriangleEPA newTriangle(final Vector3f[] _vertices, final int _v0, final int _v1, final int _v2) {
+		//Log.info("newTriangle: " + _v0 + ", " + _v1 + ", " + _v2);
+		// If we have not reached the maximum number of triangles
+		if (this.triangles.size() < MAX_TRIANGLES) {
+			
+			final TriangleEPA tmp = new TriangleEPA(_v0, _v1, _v2);
+			if (!tmp.computeClosestPoint(_vertices)) {
+				return null;
+			}
+			this.triangles.add(tmp);
+			//Log.info(" ==> retrurn Triangle: " + tmp.get(0) + ", " + tmp.get(1) + ", " + tmp.get(2));
+			return tmp;
+		}
+		// We are at the limit (internal)
+		return null;
+		
+	}
+	
+	/// Set the number of triangles
+	public void resize(final int _backup) {
+		if (_backup > this.triangles.size()) {
+			Log.error("RESIZE BIGGER : " + _backup + " > " + this.triangles.size());
+		}
+		shrinkTo(this.triangles, _backup);
+	}
+}

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

@@ -0,0 +1,435 @@
+package org.atriasoft.ephysics.collision.narrowphase.GJK;
+
+import org.atriasoft.ephysics.RaycastInfo;
+import org.atriasoft.ephysics.collision.CollisionDetection;
+import org.atriasoft.ephysics.collision.CollisionShapeInfo;
+import org.atriasoft.ephysics.collision.ProxyShape;
+import org.atriasoft.ephysics.collision.narrowphase.NarrowPhaseAlgorithm;
+import org.atriasoft.ephysics.collision.narrowphase.NarrowPhaseCallback;
+import org.atriasoft.ephysics.collision.narrowphase.EPA.EPAAlgorithm;
+import org.atriasoft.ephysics.collision.shapes.CacheData;
+import org.atriasoft.ephysics.collision.shapes.ConvexShape;
+import org.atriasoft.ephysics.constraint.ContactPointInfo;
+import org.atriasoft.ephysics.mathematics.Ray;
+import org.atriasoft.etk.math.Constant;
+import org.atriasoft.etk.math.FMath;
+import org.atriasoft.etk.math.Matrix3f;
+import org.atriasoft.etk.math.Transform3D;
+import org.atriasoft.etk.math.Vector3f;
+
+/**
+ *  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.
+ */
+public class GJKAlgorithm extends NarrowPhaseAlgorithm {
+	public static final float REL_ERROR = 0.001f;
+	
+	public static final float REL_ERROR_SQUARE = REL_ERROR * REL_ERROR;
+	public static final int MAX_ITERATIONS_GJK_RAYCAST = 32;
+	private final EPAAlgorithm algoEPA; //!< EPA 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.
+	
+	public GJKAlgorithm(final CollisionDetection collisionDetection) {
+		super(collisionDetection);
+		this.algoEPA = new EPAAlgorithm();
+		this.algoEPA.init();
+	}
+	
+	private void computePenetrationDepthForEnlargedObjects(final CollisionShapeInfo shape1Info, final Transform3D transform1, final CollisionShapeInfo shape2Info, final Transform3D transform2,
+			final NarrowPhaseCallback narrowPhaseCallback, final Vector3f v) {
+		//Log.info("computePenetrationDepthForEnlargedObjects...");
+		final Simplex simplex = new Simplex();
+		float distSquare = Float.MAX_VALUE;
+		float prevDistSquare;
+		assert (shape1Info.collisionShape.isConvex());
+		assert (shape2Info.collisionShape.isConvex());
+		final ConvexShape shape1 = (ConvexShape) (shape1Info.collisionShape);
+		final ConvexShape shape2 = (ConvexShape) (shape2Info.collisionShape);
+		final Object shape1CachedCollisionData = shape1Info.cachedCollisionData;
+		final Object shape2CachedCollisionData = shape2Info.cachedCollisionData;
+		//Log.info("    transform1=" + transform1);
+		//Log.info("    transform2=" + transform2);
+		
+		// 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)
+		final Transform3D body2ToBody1 = transform1.inverseNew().multiplyNew(transform2);
+		// Matrix that transform a direction from local space of body 1 into local space of body 2
+		final Matrix3f rotateToBody2 = transform2.getOrientation().getMatrix().transposeNew().multiply(transform1.getOrientation().getMatrix());
+		//Log.info("    body2ToBody1=" + body2ToBody1);
+		//Log.info("    rotateToBody2=" + rotateToBody2);
+		//Log.info("    v=" + v);
+		do {
+			// Compute the support points for the enlarged object A and B
+			final Vector3f suppA = shape1.getLocalSupportPointWithMargin(v.multiplyNew(-1), (CacheData) shape1CachedCollisionData);
+			//Log.info("    suppA=" + suppA);
+			final Vector3f suppB = body2ToBody1.multiplyNew(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiplyNew(v), (CacheData) shape2CachedCollisionData));
+			//Log.info("    suppB=" + suppB);
+			// Compute the support point for the Minkowski difference A-B
+			final Vector3f w = suppA.lessNew(suppB);
+			final float vDotw = v.dot(w);
+			//Log.info("    vDotw=" + vDotw);
+			// If the enlarge objects do not intersect
+			if (vDotw > 0.0f) {
+				//Log.info("        ==> ret 1");
+				// No intersection, we return
+				return;
+			}
+			// Add the new support point to the simplex
+			simplex.addPoint(w, suppA, suppB);
+			if (simplex.isAffinelyDependent()) {
+				//Log.info("        ==> ret 2");
+				return;
+			}
+			if (!simplex.computeClosestPoint(v)) {
+				//Log.info("        ==> ret 3");
+				return;
+			}
+			// Store and update the square distance
+			prevDistSquare = distSquare;
+			//Log.info("    distSquare=" + distSquare);
+			distSquare = v.length2();
+			//Log.info("    distSquare=" + distSquare);
+			if (prevDistSquare - distSquare <= Constant.FLOAT_EPSILON * prevDistSquare) {
+				//Log.info("        ==> ret 4");
+				return;
+			}
+		} while (!simplex.isFull() && distSquare > Constant.FLOAT_EPSILON * simplex.getMaxLengthSquareOfAPoint());
+		// Give the simplex computed with GJK algorithm to the EPA algorithm
+		// which will compute the correct penetration depth and contact points
+		// between the two enlarged objects
+		//Log.info("        ==> ret 5");
+		this.algoEPA.computePenetrationDepthAndContactPoints(simplex, shape1Info, transform1, shape2Info, transform2, v, narrowPhaseCallback);
+	}
+	
+	/// 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".
+	public boolean raycast(final Ray ray, final ProxyShape proxyShape, final RaycastInfo raycastInfo) {
+		assert (proxyShape.getCollisionShape().isConvex());
+		final ConvexShape shape = (ConvexShape) (proxyShape.getCollisionShape());
+		final Object 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
+		final float machineEpsilonSquare = Constant.FLOAT_EPSILON * Constant.FLOAT_EPSILON;
+		final float epsilon = 0.0001f;
+		// Convert the ray origin and direction into the local-space of the collision shape
+		final Vector3f rayDirection = ray.point2.lessNew(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
+		final Simplex simplex = new Simplex();
+		
+		Vector3f n = new Vector3f(0.0f, 0.0f, 0.0f);
+		float lambda = 0.0f;
+		suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin)
+		suppB = shape.getLocalSupportPointWithoutMargin(rayDirection, (CacheData) shapeCachedCollisionData);
+		final Vector3f v = suppA.lessNew(suppB);
+		float vDotW, vDotR;
+		float distSquare = v.length2();
+		int nbIterations = 0;
+		// GJK Algorithm loop
+		while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) {
+			// Compute the support points
+			suppB = shape.getLocalSupportPointWithoutMargin(v, (CacheData) shapeCachedCollisionData);
+			w = suppA.lessNew(suppB);
+			vDotW = v.dot(w);
+			if (vDotW > 0.0f) {
+				vDotR = v.dot(rayDirection);
+				if (vDotR >= -machineEpsilonSquare) {
+					return false;
+				} else {
+					// We have found a better lower bound for the hit point aint the ray
+					lambda = lambda - vDotW / vDotR;
+					suppA = rayDirection.multiplyNew(lambda).add(ray.point1);
+					w = suppA.lessNew(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 < Constant.FLOAT_EPSILON) {
+			return false;
+		}
+		// Compute the closet points of both objects (without the margins)
+		final Vector3f pointA = new Vector3f();
+		final Vector3f pointB = new Vector3f();
+		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 = new Vector3f(0.0f, 0.0f, 0.0f);
+		}
+		return true;
+	}
+	
+	@Override
+	public void testCollision(final CollisionShapeInfo shape1Info, final CollisionShapeInfo shape2Info, final NarrowPhaseCallback callback) {
+		//Log.error("=================================================");
+		//Log.error(" shape1Info=" + shape1Info.shapeToWorldTransform);
+		//Log.error(" shape2Info=" + shape2Info.shapeToWorldTransform);
+		Vector3f suppA = new Vector3f(); // Support point of object A
+		Vector3f suppB = new Vector3f(); // Support point of object B
+		Vector3f w = new Vector3f(); // Support point of Minkowski difference A-B
+		Vector3f pA = new Vector3f(); // Closest point of object A
+		Vector3f pB = new Vector3f(); // Closest point of object B
+		float vDotw;
+		float prevDistSquare;
+		assert (shape1Info.collisionShape.isConvex());
+		assert (shape2Info.collisionShape.isConvex());
+		final ConvexShape shape1 = (ConvexShape) (shape1Info.collisionShape);
+		final ConvexShape shape2 = (ConvexShape) (shape2Info.collisionShape);
+		final CacheData shape1CachedCollisionData = (CacheData) shape1Info.cachedCollisionData;
+		final CacheData shape2CachedCollisionData = (CacheData) shape2Info.cachedCollisionData;
+		// Get the local-space to world-space transforms
+		final Transform3D transform1 = shape1Info.shapeToWorldTransform.clone();
+		final Transform3D transform2 = shape2Info.shapeToWorldTransform.clone();
+		// 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)
+		final Transform3D body2Tobody1 = transform1.inverseNew().multiplyNew(transform2);
+		// Matrix that transform a direction from local
+		// space of body 1 into local space of body 2
+		final Matrix3f rotateToBody2 = transform2.getOrientation().getMatrix().transposeNew().multiplyNew(transform1.getOrientation().getMatrix());
+		// Initialize the margin (sum of margins of both objects)
+		final float margin = shape1.getMargin() + shape2.getMargin();
+		final float marginSquare = margin * margin;
+		assert (margin > 0.0f);
+		// Create a simplex set
+		final Simplex simplex = new Simplex();
+		// Get the previous point V (last cached separating axis)
+		final Vector3f cacheSeparatingAxis = this.currentOverlappingPair.getCachedSeparatingAxis().clone();
+		// Initialize the upper bound for the square distance
+		float distSquare = Float.MAX_VALUE;
+		
+		//Log.error(" T1=" + transform1 + "       T2=" + transform2);
+		//Log.error(" BT1=" + body2Tobody1 + "    RT2=" + rotateToBody2);
+		//Log.error(" M=" + FMath.floatToString(margin) + "             M2=" + FMath.floatToString(marginSquare));
+		//Log.error(" v=" + cacheSeparatingAxis);
+		
+		do {
+			//Log.error("------------------");
+			// Compute the support points for original objects (without margins) A and B
+			suppA = shape1.getLocalSupportPointWithoutMargin(cacheSeparatingAxis.multiplyNew(-1.0f), shape1CachedCollisionData);
+			suppB = body2Tobody1.multiplyNew(shape2.getLocalSupportPointWithoutMargin(rotateToBody2.multiplyNew(cacheSeparatingAxis), shape2CachedCollisionData));
+			// Compute the support point for the Minkowski difference A-B
+			w = suppA.lessNew(suppB);
+			vDotw = cacheSeparatingAxis.dot(w);
+			//Log.error(" suppA=" + suppA);
+			//Log.error(" suppB=" + suppB);
+			//Log.error(" w=" + w);
+			// If the enlarge objects (with margins) do not intersect
+			if (vDotw > 0.0f && vDotw * vDotw > distSquare * marginSquare) {
+				// Cache the current separating axis for frame coherence
+				this.currentOverlappingPair.setCachedSeparatingAxis(cacheSeparatingAxis);
+				// No intersection, we return
+				return;
+			}
+			// If the objects intersect only in the margins
+			if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) {
+				//Log.error("11111111 ");
+				// 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	
+				final float dist = FMath.sqrt(distSquare);
+				assert (dist > 0.0f);
+				pA = pA.lessNew(cacheSeparatingAxis.multiplyNew(shape1.getMargin() / dist));
+				pB = body2Tobody1.inverseNew().multiplyNew(cacheSeparatingAxis.multiplyNew(shape2.getMargin() / dist).add(pB));
+				// Compute the contact info
+				final Vector3f normal = transform1.getOrientation().multiply(cacheSeparatingAxis.safeNormalizeNew().multiply(-1));
+				final float penetrationDepth = margin - dist;
+				// Reject the contact if the penetration depth is negative (due too numerical errors)
+				if (penetrationDepth <= 0.0f) {
+					return;
+				}
+				// Create the contact info object
+				final ContactPointInfo contactInfo = new ContactPointInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth,
+						pA, pB);
+				callback.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()) {
+				//Log.error("222222 ");
+				// 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
+				final float dist = FMath.sqrt(distSquare);
+				assert (dist > 0.0f);
+				pA = pA.lessNew(cacheSeparatingAxis.multiplyNew(shape1.getMargin() / dist));
+				pB = body2Tobody1.inverseNew().multiply(cacheSeparatingAxis.multiplyNew(shape2.getMargin() / dist).add(pB));
+				// Compute the contact info
+				final Vector3f normal = transform1.getOrientation().multiply(cacheSeparatingAxis.safeNormalizeNew().multiply(-1));
+				final float penetrationDepth = margin - dist;
+				
+				// Reject the contact if the penetration depth is negative (due too numerical errors)
+				if (penetrationDepth <= 0.0f) {
+					//Log.info("penetration depth " + penetrationDepth);
+					return;
+				}
+				
+				// Create the contact info object
+				final ContactPointInfo contactInfo = new ContactPointInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth,
+						pA, pB);
+				callback.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(cacheSeparatingAxis)) {
+				//Log.error("3333333333 ");
+				// 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
+				final float dist = FMath.sqrt(distSquare);
+				assert (dist > 0.0f);
+				pA = pA.lessNew(cacheSeparatingAxis.multiplyNew(shape1.getMargin() / dist));
+				pB = body2Tobody1.inverseNew().multiply(cacheSeparatingAxis.multiplyNew(shape2.getMargin() / dist).add(pB));
+				// Compute the contact info
+				final Vector3f normal = transform1.getOrientation().multiply(cacheSeparatingAxis.safeNormalizeNew().multiply(-1));
+				final float penetrationDepth = margin - dist;
+				
+				// Reject the contact if the penetration depth is negative (due too numerical errors)
+				if (penetrationDepth <= 0.0f) {
+					return;
+				}
+				
+				// Create the contact info object
+				final ContactPointInfo contactInfo = new ContactPointInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth,
+						pA, pB);
+				callback.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 = cacheSeparatingAxis.length2();
+			// If the distance to the closest point doesn't improve a lot
+			if (prevDistSquare - distSquare <= Constant.FLOAT_EPSILON * prevDistSquare) {
+				//Log.error("444444444 ");
+				simplex.backupClosestPointInSimplex(cacheSeparatingAxis);
+				
+				// Get the new squared distance
+				distSquare = cacheSeparatingAxis.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
+				final float dist = FMath.sqrt(distSquare);
+				assert (dist > 0.0f);
+				pA = pA.lessNew(cacheSeparatingAxis.multiplyNew(shape1.getMargin() / dist));
+				pB = body2Tobody1.inverseNew().multiply(cacheSeparatingAxis.multiplyNew(shape2.getMargin() / dist).add(pB));
+				// Compute the contact info
+				final Vector3f normal = transform1.getOrientation().multiply(cacheSeparatingAxis.safeNormalizeNew().multiply(-1));
+				final float penetrationDepth = margin - dist;
+				
+				// Reject the contact if the penetration depth is negative (due too numerical errors)
+				if (penetrationDepth <= 0.0f) {
+					return;
+				}
+				
+				// Create the contact info object
+				final ContactPointInfo contactInfo = new ContactPointInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth,
+						pA, pB);
+				callback.notifyContact(shape1Info.overlappingPair, contactInfo);
+				// There is an intersection, therefore we return
+				return;
+			}
+		} while (!simplex.isFull() && distSquare > Constant.FLOAT_EPSILON * simplex.getMaxLengthSquareOfAPoint());
+		// The objects (without margins) intersect. Therefore, we run the GJK algorithm
+		// again but on the enlarged objects to compute a simplex polytope that contains
+		// the origin. Then, we give that simplex polytope to the EPA algorithm to compute
+		// the correct penetration depth and contact points between the enlarged objects.
+		computePenetrationDepthForEnlargedObjects(shape1Info, transform1, shape2Info, transform2, callback, cacheSeparatingAxis);
+	}
+	
+	/// Use the GJK Algorithm to find if a point is inside a convex collision shape
+	public boolean testPointInside(final Vector3f localPoint, final ProxyShape proxyShape) {
+		Vector3f suppA = new Vector3f(); // Support point of object A
+		Vector3f w = new Vector3f(); // Support point of Minkowski difference A-B
+		float prevDistSquare;
+		assert (proxyShape.getCollisionShape().isConvex());
+		final ConvexShape shape = (ConvexShape) (proxyShape.getCollisionShape());
+		final CacheData shapeCachedCollisionData = (CacheData) proxyShape.getCachedCollisionData();
+		// Support point of object B (object B is a single point)
+		final Vector3f suppB = new Vector3f(localPoint);
+		// Create a simplex set
+		final Simplex simplex = new Simplex();
+		
+		// Initial supporting direction
+		final Vector3f v = new Vector3f(1, 1, 1);
+		// Initialize the upper bound for the square distance
+		float distSquare = Float.MAX_VALUE;
+		do {
+			// Compute the support points for original objects (without margins) A and B
+			suppA = shape.getLocalSupportPointWithoutMargin(v.multiplyNew(-1), shapeCachedCollisionData);
+			// Compute the support point for the Minkowski difference A-B
+			w = suppA.lessNew(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 <= Constant.FLOAT_EPSILON * prevDistSquare) {
+				return false;
+			}
+		} while (!simplex.isFull() && distSquare > Constant.FLOAT_EPSILON * simplex.getMaxLengthSquareOfAPoint());
+		// The point is inside the collision shape
+		return true;
+	}
+}

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

@@ -0,0 +1,444 @@
+package org.atriasoft.ephysics.collision.narrowphase.GJK;
+
+import org.atriasoft.etk.math.Vector3f;
+
+/**
+ * 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.
+ */
+public class Simplex {
+	
+	private class Array2f {
+		private final float[] data;
+		private final int sizeX;
+		private final int sizeY;
+		
+		public Array2f(final int sizeX, final int sizeY) {
+			this.sizeX = sizeX;
+			this.sizeY = sizeY;
+			this.data = new float[sizeX * sizeY];
+		}
+		
+		public float get(final int xxx, final int yyy) {
+			return this.data[yyy * this.sizeX + xxx];
+		}
+		
+		public int getSizeX() {
+			return this.sizeX;
+		}
+		
+		public int getSizeY() {
+			return this.sizeY;
+		}
+		
+		public void set(final int xxx, final int yyy, final float data) {
+			this.data[yyy * this.sizeX + xxx] = data;
+		}
+	}
+	
+	private class Array2Vector3f {
+		private final Vector3f[] data;
+		private final int sizeX;
+		private final int sizeY;
+		
+		public Array2Vector3f(final int sizeX, final int sizeY) {
+			this.sizeX = sizeX;
+			this.sizeY = sizeY;
+			this.data = new Vector3f[sizeX * sizeY];
+		}
+		
+		public Vector3f get(final int xxx, final int yyy) {
+			return this.data[yyy * this.sizeX + xxx];
+		}
+		
+		public int getSizeX() {
+			return this.sizeX;
+		}
+		
+		public int getSizeY() {
+			return this.sizeY;
+		}
+		
+		public void set(final int xxx, final int yyy, final Vector3f data) {
+			this.data[yyy * this.sizeX + xxx] = data;
+		}
+	}
+	
+	/// Current points
+	private final Vector3f[] points = new Vector3f[4];
+	
+	/// pointsLengthSquare[i] = (points[i].length)^2
+	private final float[] pointsLengthSquare = new float[4];
+	
+	/// Maximum length of pointsLengthSquare[i]
+	private float maxLengthSquare;
+	
+	/// Support points of object A in local coordinates
+	private final Vector3f[] suppPointsA = new Vector3f[4];
+	
+	/// Support points of object B in local coordinates
+	private final Vector3f[] suppPointsB = new Vector3f[4];
+	
+	/// diff[i][j] contains points[i] - points[j]
+	private final Array2Vector3f diffLength = new Array2Vector3f(4, 4);
+	
+	/// Cached determinant values
+	private final Array2f det = new Array2f(16, 4);
+	
+	/// norm[i][j] = (diff[i][j].length())^2
+	private final Array2f normalSquare = new Array2f(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
+	private int bitsCurrentSimplex = 0;
+	
+	/// Number between 1 and 4 that identify the last found support point
+	private int lastFound = 0;
+	
+	/// Position of the last found support point (lastFoundBit = 0x1 << lastFound)
+	private int lastFoundBit = 0;
+	
+	/// allint = bitsCurrentSimplex | lastFoundBit;
+	private int allBits = 0;
+	
+	// -------------------- Methods -------------------- //
+	
+	/// Constructor
+	public Simplex() {
+		
+	}
+	
+	/// Add a new support point of (A-B) into the simplex.
+	/// suppPointA : support point of object A in a direction -v
+	/// suppPointB : support point of object B in a direction v
+	/// point	  : support point of object (A-B) => point = suppPointA - suppPointB
+	public void addPoint(final Vector3f point, final Vector3f suppPointA, final Vector3f suppPointB) {
+		assert (!isFull());
+		//Log.warning("simplex: addPoint(" + point + ", " + suppPointA + ", " + suppPointA + ")");
+		this.lastFound = 0;
+		this.lastFoundBit = 0x1;
+		
+		// Look for the bit corresponding to one of the four point that is not in
+		// the current simplex
+		while (overlap(this.bitsCurrentSimplex, this.lastFoundBit)) {
+			this.lastFound++;
+			this.lastFoundBit <<= 1;
+		}
+		//Log.warning("     this.lastFound " + this.lastFound);
+		//Log.warning("     this.lastFoundBit " + this.lastFoundBit);
+		
+		assert (this.lastFound < 4);
+		
+		// Add the point into the simplex
+		this.points[this.lastFound] = point;
+		this.pointsLengthSquare[this.lastFound] = point.dot(point);
+		this.allBits = this.bitsCurrentSimplex | this.lastFoundBit;
+		//Log.warning("     this.allBits " + this.allBits);
+		
+		// Update the cached values
+		updateCache();
+		
+		// Compute the cached determinant values
+		computeDeterminants();
+		
+		// Add the support points of objects A and B
+		this.suppPointsA[this.lastFound] = suppPointA;
+		this.suppPointsB[this.lastFound] = suppPointB;
+	}
+	
+	/// Backup the closest point
+	public void backupClosestPointInSimplex(final Vector3f point) {
+		float minDistSquare = Float.MAX_VALUE;
+		for (int bit = this.allBits; bit != 0x0; bit--) {
+			if (isSubset(bit, this.allBits) && isProperSubset(bit)) {
+				final Vector3f u = computeClosestPointForSubset(bit);
+				final float distSquare = u.dot(u);
+				if (distSquare < minDistSquare) {
+					minDistSquare = distSquare;
+					this.bitsCurrentSimplex = bit;
+					point.set(u);
+				}
+			}
+		}
+	}
+	
+	/// Compute the closest point 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.
+	public boolean computeClosestPoint(final Vector3f vvv) {
+		// For each possible simplex set
+		for (int subset = this.bitsCurrentSimplex; 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, this.bitsCurrentSimplex) && isValidSubset(subset | this.lastFoundBit)) {
+				this.bitsCurrentSimplex = subset | this.lastFoundBit; // Add the last added point to the current simplex
+				vvv.set(computeClosestPointForSubset(this.bitsCurrentSimplex)); // 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(this.lastFoundBit)) {
+			this.bitsCurrentSimplex = this.lastFoundBit; // Set the current simplex to the set that contains only the last added point
+			this.maxLengthSquare = this.pointsLengthSquare[this.lastFound]; // Update the maximum square length
+			vvv.set(this.points[this.lastFound]); // The closest point of the simplex "v" is the last added point
+			return true;
+		}
+		
+		// The algorithm failed to found a point
+		return false;
+	}
+	
+	/// Return the closest point "v" in the convex hull of a subset of points
+	private Vector3f computeClosestPointForSubset(final int subset) {
+		final Vector3f vvv = new Vector3f(0.0f, 0.0f, 0.0f); // Closet point v = sum(lambda_i * points[i])
+		this.maxLengthSquare = 0.0f;
+		float deltaX = 0.0f; // deltaX = sum of all det[subset][i]
+		// For each four point in the possible simplex set
+		for (int iii = 0, bit = 0x1; iii < 4; iii++, bit <<= 1) {
+			// If the current point is in the subset
+			if (overlap(subset, bit)) {
+				// deltaX = sum of all det[subset][i]
+				deltaX += this.det.get(subset, iii);
+				if (this.maxLengthSquare < this.pointsLengthSquare[iii]) {
+					this.maxLengthSquare = this.pointsLengthSquare[iii];
+				}
+				// Closest point v = sum(lambda_i * points[i])
+				vvv.add(this.points[iii].multiplyNew(this.det.get(subset, iii)));
+			}
+		}
+		assert (deltaX > 0.0f);
+		// Return the closet point "v" in the convex hull for the given subset
+		return vvv.multiply(1.0f / deltaX);
+	}
+	
+	/// Compute the closest points "pA" and "pB" of object A and B.
+	/// The points are computed as follows :
+	///	  pA = sum(lambda_i * a_i)	where "a_i" are the support points of object A
+	///	  pB = sum(lambda_i * b_i)	where "b_i" are the support points of object B
+	///	  with lambda_i = deltaX_i / deltaX
+	public void computeClosestPointsOfAandB(final Vector3f pA, final Vector3f pB) {
+		float deltaX = 0.0f;
+		pA.set(0.0f, 0.0f, 0.0f);
+		pB.set(0.0f, 0.0f, 0.0f);
+		// For each four points in the possible simplex set
+		for (int iii = 0, bit = 0x1; iii < 4; iii++, bit <<= 1) {
+			// If the current point is part of the simplex
+			if (overlap(this.bitsCurrentSimplex, bit)) {
+				deltaX += this.det.get(this.bitsCurrentSimplex, iii);
+				pA.add(this.suppPointsA[iii].multiplyNew(this.det.get(this.bitsCurrentSimplex, iii)));
+				pB.add(this.suppPointsB[iii].multiplyNew(this.det.get(this.bitsCurrentSimplex, iii)));
+			}
+		}
+		
+		assert (deltaX > 0.0f);
+		final float factor = 1.0f / deltaX;
+		pA.multiply(factor);
+		pB.multiply(factor);
+	}
+	
+	/// Compute the cached determinant values
+	private void computeDeterminants() {
+		this.det.set(this.lastFoundBit, this.lastFound, 1.0f);
+		//Log.warning("simplex:     computeDeterminants() " + this.det.get(this.lastFoundBit, this.lastFound));
+		// If the current simplex is not empty
+		if (!isEmpty()) {
+			// For each possible four points in the simplex set
+			for (int iii = 0, bitI = 0x1; iii < 4; iii++, bitI <<= 1) {
+				// If the current point is in the simplex
+				if (overlap(this.bitsCurrentSimplex, bitI)) {
+					final int bit2 = bitI | this.lastFoundBit;
+					float tmpp = this.diffLength.get(this.lastFound, iii).dot(this.points[this.lastFound]);
+					this.det.set(bit2, iii, tmpp);
+					tmpp = this.diffLength.get(iii, this.lastFound).dot(this.points[iii]);
+					this.det.set(bit2, this.lastFound, tmpp);
+					for (int jjj = 0, bitJ = 0x1; jjj < iii; jjj++, bitJ <<= 1) {
+						if (overlap(this.bitsCurrentSimplex, bitJ)) {
+							final int bit3 = bitJ | bit2;
+							int kkk = this.normalSquare.get(iii, jjj) < this.normalSquare.get(this.lastFound, jjj) ? iii : this.lastFound;
+							float tmp2 = this.det.get(bit2, iii) * this.diffLength.get(kkk, jjj).dot(this.points[iii])
+									+ this.det.get(bit2, this.lastFound) * this.diffLength.get(kkk, jjj).dot(this.points[this.lastFound]);
+							this.det.set(bit3, jjj, tmp2);
+							kkk = this.normalSquare.get(jjj, iii) < this.normalSquare.get(this.lastFound, iii) ? jjj : this.lastFound;
+							tmp2 = this.det.get(bitJ | this.lastFoundBit, jjj) * this.diffLength.get(kkk, iii).dot(this.points[jjj])
+									+ this.det.get(bitJ | this.lastFoundBit, this.lastFound) * this.diffLength.get(kkk, iii).dot(this.points[this.lastFound]);
+							this.det.set(bit3, iii, tmp2);
+							kkk = this.normalSquare.get(iii, this.lastFound) < this.normalSquare.get(jjj, this.lastFound) ? iii : jjj;
+							tmp2 = this.det.get(bitJ | bitI, jjj) * this.diffLength.get(kkk, this.lastFound).dot(this.points[jjj])
+									+ this.det.get(bitJ | bitI, iii) * this.diffLength.get(kkk, this.lastFound).dot(this.points[iii]);
+							this.det.set(bit3, this.lastFound, tmp2);
+						}
+					}
+				}
+			}
+			
+			if (this.allBits == 0xf) {
+				int kkk;
+				
+				kkk = this.normalSquare.get(1, 0) < this.normalSquare.get(2, 0) ? (this.normalSquare.get(1, 0) < this.normalSquare.get(3, 0) ? 1 : 3)
+						: (this.normalSquare.get(2, 0) < this.normalSquare.get(3, 0) ? 2 : 3);
+				float tmp3 = this.det.get(0xe, 1) * this.diffLength.get(kkk, 0).dot(this.points[1]) + this.det.get(0xe, 2) * this.diffLength.get(kkk, 0).dot(this.points[2])
+						+ this.det.get(0xe, 3) * this.diffLength.get(kkk, 0).dot(this.points[3]);
+				this.det.set(0xf, 0, tmp3);
+				
+				kkk = this.normalSquare.get(0, 1) < this.normalSquare.get(2, 1) ? (this.normalSquare.get(0, 1) < this.normalSquare.get(3, 1) ? 0 : 3)
+						: (this.normalSquare.get(2, 1) < this.normalSquare.get(3, 1) ? 2 : 3);
+				tmp3 = this.det.get(0xd, 0) * this.diffLength.get(kkk, 1).dot(this.points[0]) + this.det.get(0xd, 2) * this.diffLength.get(kkk, 1).dot(this.points[2])
+						+ this.det.get(0xd, 3) * this.diffLength.get(kkk, 1).dot(this.points[3]);
+				this.det.set(0xf, 1, tmp3);
+				
+				kkk = this.normalSquare.get(0, 2) < this.normalSquare.get(1, 2) ? (this.normalSquare.get(0, 2) < this.normalSquare.get(3, 2) ? 0 : 3)
+						: (this.normalSquare.get(1, 2) < this.normalSquare.get(3, 2) ? 1 : 3);
+				tmp3 = this.det.get(0xb, 0) * this.diffLength.get(kkk, 2).dot(this.points[0]) + this.det.get(0xb, 1) * this.diffLength.get(kkk, 2).dot(this.points[1])
+						+ this.det.get(0xb, 3) * this.diffLength.get(kkk, 2).dot(this.points[3]);
+				this.det.set(0xf, 2, tmp3);
+				
+				kkk = this.normalSquare.get(0, 3) < this.normalSquare.get(1, 3) ? (this.normalSquare.get(0, 3) < this.normalSquare.get(2, 3) ? 0 : 2)
+						: (this.normalSquare.get(1, 3) < this.normalSquare.get(2, 3) ? 1 : 2);
+				tmp3 = this.det.get(0x7, 0) * this.diffLength.get(kkk, 3).dot(this.points[0]) + this.det.get(0x7, 1) * this.diffLength.get(kkk, 3).dot(this.points[1])
+						+ this.det.get(0x7, 2) * this.diffLength.get(kkk, 3).dot(this.points[2]);
+				this.det.set(0xf, 3, tmp3);
+			}
+		}
+	}
+	
+	/// Return the maximum squared length of a point
+	public float getMaxLengthSquareOfAPoint() {
+		return this.maxLengthSquare;
+	}
+	
+	/// Return the points of the simplex
+	public int getSimplex(final Vector3f[] suppPointsA, final Vector3f[] suppPointsB, final Vector3f[] points) {
+		int nbVertices = 0;
+		// For each four point in the possible simplex
+		for (int iii = 0, bit = 0x1; iii < 4; iii++, bit <<= 1) {
+			// If the current point is in the simplex
+			if (overlap(this.bitsCurrentSimplex, bit)) {
+				// Store the points
+				suppPointsA[nbVertices] = this.suppPointsA[nbVertices].clone();
+				suppPointsB[nbVertices] = this.suppPointsB[nbVertices].clone();
+				points[nbVertices] = this.points[nbVertices].clone();
+				nbVertices++;
+			}
+		}
+		// Return the number of points in the simplex
+		return nbVertices;
+	}
+	
+	/// 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
+	public boolean isAffinelyDependent() {
+		float sum = 0.0f;
+		// For each four point of the possible simplex set
+		for (int iii = 0, bit = 0x1; iii < 4; iii++, bit <<= 1) {
+			if (overlap(this.allBits, bit)) {
+				sum += this.det.get(this.allBits, iii);
+			}
+		}
+		return (sum <= 0.0f);
+	}
+	
+	/// Return true if the simplex is empty
+	public boolean isEmpty() {
+		return (this.bitsCurrentSimplex == 0x0);
+	}
+	
+	/// Return true if the simplex contains 4 points
+	public boolean isFull() {
+		return (this.bitsCurrentSimplex == 0xf);
+	}
+	
+	/// Return true if the point is in the simplex
+	public boolean isPointInSimplex(final Vector3f point) {
+		// For each four possible points in the simplex
+		for (int iii = 0, bit = 0x1; iii < 4; iii++, bit <<= 1) {
+			// Check if the current point is in the simplex
+			if (overlap(this.allBits, bit) && point == this.points[iii]) {
+				return true;
+			}
+		}
+		return false;
+	}
+	
+	/// Return true if the subset is a proper subset.
+	/// A proper subset X is a subset where for all point "y_i" in X, we have
+	/// detX_i value bigger than zero
+	private boolean isProperSubset(final int subset) {
+		// For each four point of the possible simplex set
+		for (int iii = 0, bit = 0x1; iii < 4; iii++, bit <<= 1) {
+			if (overlap(subset, bit) && this.det.get(subset, iii) <= 0.0f) {
+				return false;
+			}
+		}
+		return true;
+	}
+	
+	/// Return true if the bits of "b" is a subset of the bits of "a"
+	private boolean isSubset(final int a, final int b) {
+		return ((a & b) == a);
+	}
+	
+	/// Return true if the subset is a valid one for the closest point computation.
+	/// A subset X is valid if :
+	///	1. delta(X)_i > 0 for each "i" in I_x and
+	///	2. delta(X U {y_j})_j <= 0 for each "j" not in I_x_
+	private boolean isValidSubset(final int subset) {
+		// For each four point in the possible simplex set
+		for (int iii = 0, bit = 0x1; iii < 4; iii++, bit <<= 1) {
+			if (overlap(this.allBits, 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 (this.det.get(subset, iii) <= 0.0f) {
+						// The subset is not valid
+						return false;
+					}
+				}
+				// If the point is not in the subset and the value delta(X U {y_j})_j
+				// is bigger than zero
+				else if (this.det.get(subset | bit, iii) > 0.0f) {
+					// The subset is not valid
+					return false;
+				}
+			}
+		}
+		return true;
+	}
+	
+	/// Return true if some bits of "a" overlap with bits of "b"
+	private boolean overlap(final int a, final int b) {
+		return ((a & b) != 0x0);
+	}
+	
+	/// Update the cached values used during the GJK algorithm
+	private void updateCache() {
+		//Log.warning("simplex:     update Cache()");
+		// For each of the four possible points of the simplex
+		for (int iii = 0, bit = 0x1; iii < 4; iii++, bit <<= 1) {
+			//Log.warning("simplex:         iii=" + iii);
+			//Log.warning("simplex:         bit=" + bit);
+			
+			// If the current points is in the simplex
+			if (overlap(this.bitsCurrentSimplex, bit)) {
+				//Log.warning("simplex:         ==> overlap");
+				// Compute the distance between two points in the possible simplex set
+				final Vector3f tmp = this.points[iii].lessNew(this.points[this.lastFound]);
+				this.diffLength.set(iii, this.lastFound, tmp);
+				final Vector3f tmp2 = tmp.multiplyNew(-1);
+				this.diffLength.set(this.lastFound, iii, tmp2);
+				// Compute the squared length of the vector
+				// distances from points in the possible simplex set
+				final float normal = tmp.dot(tmp);
+				this.normalSquare.set(iii, this.lastFound, normal);
+				this.normalSquare.set(this.lastFound, iii, normal);
+			}
+		}
+	}
+	
+}

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

@@ -0,0 +1,32 @@
+package org.atriasoft.ephysics.collision.narrowphase;
+
+import org.atriasoft.ephysics.collision.CollisionDetection;
+import org.atriasoft.ephysics.collision.CollisionShapeInfo;
+import org.atriasoft.ephysics.engine.OverlappingPair;
+
+/**
+ * @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.
+ */
+public abstract class NarrowPhaseAlgorithm {
+	
+	protected CollisionDetection collisionDetection; //!< Pointer to the collision detection object
+	protected OverlappingPair currentOverlappingPair; //!< Overlapping pair of the bodies currently tested for collision
+	
+	/// Constructor
+	public NarrowPhaseAlgorithm(final CollisionDetection collisionDetection) {
+		this.currentOverlappingPair = null;
+		this.collisionDetection = null;
+		this.collisionDetection = collisionDetection;
+	}
+	
+	/// Set the current overlapping pair of bodies
+	public void setCurrentOverlappingPair(final OverlappingPair overlappingPair) {
+		this.currentOverlappingPair = overlappingPair;
+	}
+	
+	/// Compute a contact info if the two bounding volume collide
+	public abstract void testCollision(final CollisionShapeInfo _shape1Info, CollisionShapeInfo _shape2Info, NarrowPhaseCallback _narrowPhaseCallback);
+	
+}

src/org/atriasoft/ephysics/collision/narrowphase/NarrowPhaseCallback.java

@@ -0,0 +1,14 @@
+package org.atriasoft.ephysics.collision.narrowphase;
+
+import org.atriasoft.ephysics.constraint.ContactPointInfo;
+import org.atriasoft.ephysics.engine.OverlappingPair;
+
+/**
+ * It is the base class for a narrow-phase collision
+ * callback class.
+ */
+public interface NarrowPhaseCallback {
+	/// Called by a narrow-phase collision algorithm when a new contact has been found
+	void notifyContact(OverlappingPair _overlappingPair, ContactPointInfo _contactInfo);
+	
+};

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

@@ -0,0 +1,50 @@
+package org.atriasoft.ephysics.collision.narrowphase;
+
+import org.atriasoft.etk.math.FMath;
+import org.atriasoft.etk.math.Transform3D;
+import org.atriasoft.etk.math.Vector3f;
+
+import org.atriasoft.ephysics.collision.CollisionDetection;
+import org.atriasoft.ephysics.collision.CollisionShapeInfo;
+import org.atriasoft.ephysics.collision.shapes.SphereShape;
+import org.atriasoft.ephysics.constraint.ContactPointInfo;
+
+/**
+ *  It is used to compute the narrow-phase collision detection
+ * between two sphere collision shapes.
+ */
+public class SphereVsSphereAlgorithm extends NarrowPhaseAlgorithm {
+	
+	public SphereVsSphereAlgorithm(final CollisionDetection collisionDetection) {
+		super(collisionDetection);
+	}
+	
+	@Override
+	public void testCollision(final CollisionShapeInfo _shape1Info, final CollisionShapeInfo _shape2Info, final NarrowPhaseCallback _narrowPhaseCallback) {
+		// Get the sphere collision shapes
+		final SphereShape sphereShape1 = (SphereShape) _shape1Info.collisionShape;
+		final SphereShape sphereShape2 = (SphereShape) _shape2Info.collisionShape;
+		// Get the local-space to world-space transforms
+		final Transform3D transform1 = _shape1Info.shapeToWorldTransform;
+		final Transform3D transform2 = _shape2Info.shapeToWorldTransform;
+		// Compute the distance between the centers
+		final Vector3f vectorBetweenCenters = transform2.getPosition().lessNew(transform1.getPosition());
+		final float squaredDistanceBetweenCenters = vectorBetweenCenters.length2();
+		// Compute the sum of the radius
+		final float sumRadius = sphereShape1.getRadius() + sphereShape2.getRadius();
+		// If the sphere collision shapes intersect
+		if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) {
+			final Vector3f centerSphere2InBody1LocalSpace = transform1.inverseNew().multiply(transform2.getPosition());
+			final Vector3f centerSphere1InBody2LocalSpace = transform2.inverseNew().multiply(transform1.getPosition());
+			final Vector3f intersectionOnBody1 = centerSphere2InBody1LocalSpace.safeNormalizeNew().multiply(sphereShape1.getRadius());
+			final Vector3f intersectionOnBody2 = centerSphere1InBody2LocalSpace.safeNormalizeNew().multiply(sphereShape2.getRadius());
+			final float penetrationDepth = sumRadius - FMath.sqrt(squaredDistanceBetweenCenters);
+			
+			// Create the contact info object
+			final ContactPointInfo contactInfo = new ContactPointInfo(_shape1Info.proxyShape, _shape2Info.proxyShape, _shape1Info.collisionShape, _shape2Info.collisionShape,
+					vectorBetweenCenters.safeNormalizeNew(), penetrationDepth, intersectionOnBody1, intersectionOnBody2);
+			// Notify about the new contact
+			_narrowPhaseCallback.notifyContact(_shape1Info.overlappingPair, contactInfo);
+		}
+	}
+}

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

@@ -0,0 +1,322 @@
+/** @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-now, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+package org.atriasoft.ephysics.collision.shapes;
+
+import org.atriasoft.ephysics.mathematics.Ray;
+import org.atriasoft.etk.math.Constant;
+import org.atriasoft.etk.math.FMath;
+import org.atriasoft.etk.math.Vector3f;
+
+/**
+ * This class represents a bounding volume of type "Axis Aligned Bounding Box". It's a box where all the edges are
+ * always aligned with the world coordinate system. The AABB is defined by the  this.inimum and  this.aximum world coordinates of
+ * the three axis.
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class AABB {
+	
+	/**
+	 * @brief Create and return an AABB for a triangle
+	 * @param[in] _trianglePoints List of 3 point od a triangle
+	 * @return An AABB box
+	 */
+	public static AABB createAABBForTriangle(final Vector3f[] _trianglePoints) {
+		final Vector3f minCoords = new Vector3f(_trianglePoints[0].x, _trianglePoints[0].y, _trianglePoints[0].z);
+		final Vector3f maxCoords = new Vector3f(_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 new AABB(minCoords, maxCoords);
+	}
+	
+	// Maximum world coordinates of the AABB on the x,y and z axis
+	private final Vector3f maxCoordinates;
+	
+	// Minimum world coordinates of the AABB on the x,y and z axis
+	private final Vector3f minCoordinates;
+	
+	/**
+	 * @brief default contructor
+	 */
+	public AABB() {
+		this.maxCoordinates = new Vector3f();
+		this.minCoordinates = new Vector3f();
+	}
+	
+	/**
+	 * @brief contructor Whit sizes
+	 * @param[in] _minCoordinates Minimum coordinates
+	 * @param[in] _maxCoordinates Maximum coordinates
+	 */
+	public AABB(final Vector3f minCoordinates, final Vector3f maxCoordinates) {
+		this.maxCoordinates = maxCoordinates;
+		this.minCoordinates = minCoordinates;
+	}
+	
+	/**
+	 * @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
+	 */
+	public boolean contains(final 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;
+	}
+	
+	/**
+	 * @brief Return true if a point is inside the AABB
+	 * @param[in] _point Point to check.
+	 * @return true The point in contained inside
+	 */
+	public boolean contains(final Vector3f _point) {
+		return _point.x >= this.minCoordinates.x - Constant.FLOAT_EPSILON && _point.x <= this.maxCoordinates.x + Constant.FLOAT_EPSILON && _point.y >= this.minCoordinates.y - Constant.FLOAT_EPSILON
+				&& _point.y <= this.maxCoordinates.y + Constant.FLOAT_EPSILON && _point.z >= this.minCoordinates.z - Constant.FLOAT_EPSILON
+				&& _point.z <= this.maxCoordinates.z + Constant.FLOAT_EPSILON;
+	}
+	
+	/**
+	 * @brief Get the center point of the AABB box
+	 * @return The 3D position of the center
+	 */
+	public Vector3f getCenter() {
+		return new Vector3f(this.minCoordinates).add(this.maxCoordinates).multiply(0.5f);
+	}
+	
+	/**
+	 * @brief Get the size of the AABB in the three dimension x, y and z
+	 * @return the AABB 3D size
+	 */
+	public Vector3f getExtent() {
+		return this.maxCoordinates.lessNew(this.minCoordinates);
+	}
+	
+	/**
+	 * @brief Return the maximum coordinates of the AABB
+	 * @return The 3d maximum coordonates
+	 */
+	public Vector3f getMax() {
+		return this.maxCoordinates;
+	}
+	
+	/**
+	 * @brief Get the minimum coordinates of the AABB
+	 * @return The 3d minimum coordonates
+	 */
+	public Vector3f getMin() {
+		return this.minCoordinates;
+	}
+	
+	/**
+	 * @brief Get the volume of the AABB
+	 * @return The 3D volume.
+	 */
+	public float getVolume() {
+		final Vector3f diff = this.maxCoordinates.lessNew(this.minCoordinates);
+		return (diff.x * diff.y * diff.z);
+	}
+	
+	/**
+	 * @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
+	 */
+	public void inflate(final float _dx, final float _dy, final float _dz) {
+		this.maxCoordinates.add(new Vector3f(_dx, _dy, _dz));
+		this.minCoordinates.less(new Vector3f(_dx, _dy, _dz));
+	}
+	
+	/**
+	 * @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.
+	 */
+	public void mergeTwoAABBs(final AABB _aabb1, final AABB _aabb2) {
+		this.minCoordinates.setX(FMath.min(_aabb1.minCoordinates.x, _aabb2.minCoordinates.x));
+		this.minCoordinates.setY(FMath.min(_aabb1.minCoordinates.y, _aabb2.minCoordinates.y));
+		this.minCoordinates.setZ(FMath.min(_aabb1.minCoordinates.z, _aabb2.minCoordinates.z));
+		this.maxCoordinates.setX(FMath.max(_aabb1.maxCoordinates.x, _aabb2.maxCoordinates.x));
+		this.maxCoordinates.setY(FMath.max(_aabb1.maxCoordinates.y, _aabb2.maxCoordinates.y));
+		this.maxCoordinates.setZ(FMath.max(_aabb1.maxCoordinates.z, _aabb2.maxCoordinates.z));
+	}
+	
+	/**
+	 * @brief Merge the AABB in parameter with the current one
+	 * @param[in] _aabb Other AABB box to merge.
+	 */
+	public void mergeWithAABB(final AABB _aabb) {
+		this.minCoordinates.setX(FMath.min(this.minCoordinates.x, _aabb.minCoordinates.x));
+		this.minCoordinates.setY(FMath.min(this.minCoordinates.y, _aabb.minCoordinates.y));
+		this.minCoordinates.setZ(FMath.min(this.minCoordinates.z, _aabb.minCoordinates.z));
+		this.maxCoordinates.setX(FMath.max(this.maxCoordinates.x, _aabb.maxCoordinates.x));
+		this.maxCoordinates.setY(FMath.max(this.maxCoordinates.y, _aabb.maxCoordinates.y));
+		this.maxCoordinates.setZ(FMath.max(this.maxCoordinates.z, _aabb.maxCoordinates.z));
+	}
+	
+	/**
+	 * @brief Set the maximum coordinates of the AABB
+	 * @param[in] _max The 3d maximum coordonates
+	 */
+	public void setMax(final Vector3f max) {
+		this.maxCoordinates.set(max);
+	}
+	
+	/**
+	 * @brief Set the minimum coordinates of the AABB
+	 * @param[in] _min The 3d minimum coordonates
+	 */
+	public void setMin(final Vector3f min) {
+		this.minCoordinates.set(min);
+	}
+	
+	/**
+	 * @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
+	 */
+	public boolean testCollision(final AABB aabb) {
+		if (this == aabb) {
+			///Log.info("test : AABB    ==> same object ");
+			return true;
+		}
+		//Log.info("test : " + this + " && " + aabb);
+		if (this.maxCoordinates.getX() < aabb.minCoordinates.getX() || aabb.maxCoordinates.getX() < this.minCoordinates.getX()) {
+			return false;
+		}
+		if (this.maxCoordinates.getZ() < aabb.minCoordinates.getZ() || aabb.maxCoordinates.getZ() < this.minCoordinates.getZ()) {
+			return false;
+		}
+		if (this.maxCoordinates.getY() < aabb.minCoordinates.getY() || aabb.maxCoordinates.getY() < this.minCoordinates.getY()) {
+			return false;
+		}
+		//Log.info("detect collision ");
+		return true;
+	}
+	
+	/**
+	 * @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
+	 */
+	public boolean testCollisionTriangleAABB(final Vector3f[] _trianglePoints) {
+		if (FMath.min(_trianglePoints[0].x, _trianglePoints[1].x, _trianglePoints[2].x) > this.maxCoordinates.x) {
+			return false;
+		}
+		if (FMath.min(_trianglePoints[0].y, _trianglePoints[1].y, _trianglePoints[2].y) > this.maxCoordinates.y) {
+			return false;
+		}
+		if (FMath.min(_trianglePoints[0].z, _trianglePoints[1].z, _trianglePoints[2].z) > this.maxCoordinates.z) {
+			return false;
+		}
+		if (FMath.max(_trianglePoints[0].x, _trianglePoints[1].x, _trianglePoints[2].x) < this.minCoordinates.x) {
+			return false;
+		}
+		if (FMath.max(_trianglePoints[0].y, _trianglePoints[1].y, _trianglePoints[2].y) < this.minCoordinates.y) {
+			return false;
+		}
+		if (FMath.max(_trianglePoints[0].z, _trianglePoints[1].z, _trianglePoints[2].z) < this.minCoordinates.z) {
+			return false;
+		}
+		return true;
+	}
+	
+	/*
+	 * @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
+	 */
+	public boolean testRayIntersect(final Ray ray) {
+		final Vector3f point2 = ray.point2.lessNew(ray.point1).multiply(ray.maxFraction).add(ray.point1);
+		final Vector3f e = this.maxCoordinates.lessNew(this.minCoordinates);
+		final Vector3f d = point2.lessNew(ray.point1);
+		final Vector3f m = ray.point1.addNew(point2).less(this.minCoordinates).less(this.maxCoordinates);
+		// Test if the AABB face normals are separating axis
+		float adx = FMath.abs(d.x);
+		if (FMath.abs(m.x) > e.x + adx) {
+			return false;
+		}
+		float ady = FMath.abs(d.y);
+		if (FMath.abs(m.y) > e.y + ady) {
+			return false;
+		}
+		float adz = FMath.abs(d.z);
+		if (FMath.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)
+		final float epsilon = 0.00001f;
+		adx += epsilon;
+		ady += epsilon;
+		adz += epsilon;
+		// Test if the cross products between face normals and ray direction are
+		// separating axis
+		if (FMath.abs(m.y * d.z - m.z * d.y) > e.y * adz + e.z * ady) {
+			return false;
+		}
+		if (FMath.abs(m.z * d.x - m.x * d.z) > e.x * adz + e.z * adx) {
+			return false;
+		}
+		if (FMath.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;
+	}
+	
+	@Override
+	public String toString() {
+		return "AABB [min=" + this.minCoordinates + ", max=" + this.maxCoordinates + "]";
+	}
+	
+}

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

@@ -0,0 +1,170 @@
+/** @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-now, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+package org.atriasoft.ephysics.collision.shapes;
+
+import org.atriasoft.ephysics.RaycastInfo;
+import org.atriasoft.ephysics.collision.ProxyShape;
+import org.atriasoft.ephysics.configuration.Defaults;
+import org.atriasoft.ephysics.mathematics.Ray;
+import org.atriasoft.etk.math.Constant;
+import org.atriasoft.etk.math.FMath;
+import org.atriasoft.etk.math.Matrix3f;
+import org.atriasoft.etk.math.Vector3f;
+
+/**
+ * This class represents a 3D box shape. Those axis are unit length. The three extents are half-widths of the box along
+ * the three axis x, y, z local axis. The "transform" of the corresponding rigid body will give an orientation and a
+ * position to the box. This collision shape uses an extra margin distance around it for collision detection purpose.
+ * The default margin is 4cm (if your units are meters, which is recommended). In case, you want to simulate small
+ * objects (smaller than the margin distance), you might want to reduce the margin by specifying your own margin
+ * distance using the "margin" parameter in the constructor of the box shape. Otherwise, it is recommended to use the
+ * default margin distance by not using the "margin" parameter in the constructor.
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class BoxShape extends ConvexShape {
+	
+	// Extent sizes of the box in the x, y and z direction
+	private final Vector3f extent;
+	
+	// Copy-constructor
+	public BoxShape(final BoxShape shape) {
+		super(CollisionShapeType.BOX, shape.margin);
+		this.extent = shape.extent.clone();
+	}
+	
+	// Constructor
+	public BoxShape(final Vector3f extent) {
+		this(extent, Defaults.OBJECT_MARGIN);
+	}
+	
+	public BoxShape(final Vector3f extent, final float margin) {
+		super(CollisionShapeType.BOX, margin);
+		assert (extent.getX() > 0.0f && extent.getX() > margin);
+		assert (extent.getY() > 0.0f && extent.getY() > margin);
+		assert (extent.getZ() > 0.0f && extent.getZ() > margin);
+		this.extent = extent.lessNew(margin);
+	}
+	
+	@Override
+	public BoxShape clone() {
+		return new BoxShape(this);
+	}
+	
+	@Override
+	public void computeLocalInertiaTensor(final Matrix3f tensor, final float _mass) {
+		final float factor = (1.0f / 3.0f) * _mass;
+		final Vector3f realExtent = this.extent.addNew(new Vector3f(this.margin, this.margin, this.margin));
+		final float xSquare = realExtent.x * realExtent.x;
+		final float ySquare = realExtent.y * realExtent.y;
+		final float zSquare = realExtent.z * realExtent.z;
+		tensor.set(factor * (ySquare + zSquare), 0.0f, 0.0f, 0.0f, factor * (xSquare + zSquare), 0.0f, 0.0f, 0.0f, factor * (xSquare + ySquare));
+	}
+	
+	public Vector3f getExtent() {
+		return (new Vector3f(this.margin, this.margin, this.margin)).add(this.extent);
+	}
+	
+	@Override
+	public void getLocalBounds(final Vector3f _min, final Vector3f _max) {
+		// Maximum bounds
+		_max.set(this.extent.x + this.margin, this.extent.y + this.margin, this.extent.z + this.margin);
+		// Minimum bounds
+		_min.set(-_max.x, -_max.y, -_max.z);
+	}
+	
+	/*
+	protected size_t getSizeInBytes() {
+		return sizeof(BoxShape);
+	}
+	 */
+	@Override
+	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) {
+		//Log.error("getLocalSupportPointWithoutMargin(" + _direction);
+		//Log.error("    extends = " + this.extent);
+		return new 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);
+	}
+	
+	@Override
+	public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) {
+		final Vector3f rayDirection = _ray.point2.less(_ray.point1);
+		float tMin = Float.MIN_VALUE;
+		float tMax = Float.MAX_VALUE;
+		Vector3f normalDirection = new Vector3f(0, 0, 0);
+		Vector3f currentNormal = new Vector3f(0, 0, 0);
+		// For each of the three slabs
+		for (int iii = 0; iii < 3; ++iii) {
+			// If ray is parallel to the slab
+			if (FMath.abs(rayDirection.get(iii)) < Constant.FLOAT_EPSILON) {
+				// If the ray's origin is not inside the slab, there is no hit
+				if (_ray.point1.get(iii) > this.extent.get(iii) || _ray.point1.get(iii) < -this.extent.get(iii)) {
+					return false;
+				}
+			} else {
+				// Compute the intersection of the ray with the near and far plane of the slab
+				final float oneOverD = 1.0f / rayDirection.get(iii);
+				float t1 = (-this.extent.get(iii) - _ray.point1.get(iii)) * oneOverD;
+				float t2 = (this.extent.get(iii) - _ray.point1.get(iii)) * oneOverD;
+				currentNormal.x = (iii == 0) ? -this.extent.get(iii) : 0.0f;
+				currentNormal.y = (iii == 1) ? -this.extent.get(iii) : 0.0f;
+				currentNormal.z = (iii == 2) ? -this.extent.get(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) {
+					final float ttt = t2;
+					t2 = t1;
+					t1 = ttt;
+					currentNormal = currentNormal.multiply(-1);
+				}
+				// Compute the intersection of the of slab intersection interval with previous slabs
+				if (t1 > tMin) {
+					tMin = t1;
+					normalDirection = currentNormal;
+				}
+				tMax = FMath.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.isZero()) {
+			return false;
+		}
+		// The ray longersects the three slabs, we compute the hit point
+		final Vector3f localHitPoint = rayDirection.multiplyNew(tMin).add(_ray.point1);
+		_raycastInfo.body = _proxyShape.getBody();
+		_raycastInfo.proxyShape = _proxyShape;
+		_raycastInfo.hitFraction = tMin;
+		_raycastInfo.worldPoint = localHitPoint;
+		_raycastInfo.worldNormal = normalDirection;
+		return true;
+	}
+	
+	@Override
+	public void setLocalScaling(final Vector3f _scaling) {
+		this.extent.divide(this.scaling).multiply(_scaling);
+		super.setLocalScaling(_scaling);
+	}
+	
+	@Override
+	public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) {
+		return (_localPoint.x < this.extent.x && _localPoint.x > -this.extent.x && _localPoint.y < this.extent.y && _localPoint.y > -this.extent.y && _localPoint.z < this.extent.z
+				&& _localPoint.z > -this.extent.z);
+	}
+	
+}

src/org/atriasoft/ephysics/collision/shapes/CacheData.java

@@ -0,0 +1,10 @@
+/** @file
+ * @author Edouard DUPIN
+ * @copyright 2020-now, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+package org.atriasoft.ephysics.collision.shapes;
+
+public class CacheData {
+	public Object data = null;
+}

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

@@ -0,0 +1,284 @@
+/** @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-now, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+package org.atriasoft.ephysics.collision.shapes;
+
+import org.atriasoft.ephysics.RaycastInfo;
+import org.atriasoft.ephysics.collision.ProxyShape;
+import org.atriasoft.ephysics.mathematics.Ray;
+import org.atriasoft.etk.math.Constant;
+import org.atriasoft.etk.math.FMath;
+import org.atriasoft.etk.math.Matrix3f;
+import org.atriasoft.etk.math.Vector3f;
+
+/**
+ * This class represents a capsule collision shape that is defined around the Y axis. A capsule shape can be seen as the
+ * convex hull of two spheres. The capsule shape is defined by its radius (radius of the two spheres of the capsule) and
+ * its height (distance between the centers of the two spheres). This collision shape does not have an explicit object
+ * margin distance. The margin is implicitly the radius and height of the shape. Therefore, no need to specify an object
+ * margin for a capsule shape.
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class CapsuleShape extends ConvexShape {
+	// Half height of the capsule (height = distance between the centers of the two spheres)
+	private float halfHeight;
+	
+	// Copy-constructor
+	public CapsuleShape(final CapsuleShape shape) {
+		super(CollisionShapeType.CAPSULE, shape.margin);
+		this.halfHeight = shape.halfHeight;
+	}
+	
+	// Constructor
+	public CapsuleShape(final float radius, final float height) {
+		// TODO: Should radius really be the margin for a capsule? Seems like a bug.
+		super(CollisionShapeType.CAPSULE, radius);
+		this.halfHeight = height * 0.5f;
+	}
+	
+	@Override
+	public CapsuleShape clone() {
+		return new CapsuleShape(this);
+	}
+	
+	@Override
+	public void computeLocalInertiaTensor(final Matrix3f _tensor, final float _mass) {
+		// The inertia tensor formula for a capsule can be found in : Game Engine Gems, Volume 1
+		final float height = this.halfHeight + this.halfHeight;
+		final float radiusSquare = this.margin * this.margin;
+		final float heightSquare = height * height;
+		final float radiusSquareDouble = radiusSquare + radiusSquare;
+		final float factor1 = 2.0f * this.margin / (4.0f * this.margin + 3.0f * height);
+		final float factor2 = 3.0f * height / (4.0f * this.margin + 3.0f * height);
+		final float sum1 = 0.4f * radiusSquareDouble;
+		final float sum2 = 0.75f * height * this.margin + 0.5f * heightSquare;
+		final float sum3 = 0.25f * radiusSquare + 1.0f / 12.0f * heightSquare;
+		final float IxxAndzz = factor1 * _mass * (sum1 + sum2) + factor2 * _mass * sum3;
+		final float Iyy = factor1 * _mass * sum1 + factor2 * _mass * 0.25f * radiusSquareDouble;
+		_tensor.set(IxxAndzz, 0.0f, 0.0f, 0.0f, Iyy, 0.0f, 0.0f, 0.0f, IxxAndzz);
+	}
+	
+	// Return the height of the capsule
+	public float getHeight() {
+		return this.halfHeight + this.halfHeight;
+	}
+	
+	@Override
+	public void getLocalBounds(final Vector3f min, final Vector3f max) {
+		// Maximum bounds
+		max.setX(this.margin);
+		max.setY(this.halfHeight + this.margin);
+		max.setZ(this.margin);
+		// Minimum bounds
+		min.setX(-max.x);
+		min.setY(-max.y);
+		min.setZ(-max.z);
+	}
+	
+	@Override
+	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) {
+		// Support point top sphere
+		final float dotProductTop = this.halfHeight * _direction.y;
+		// Support point bottom sphere
+		final float dotProductBottom = -this.halfHeight * _direction.y;
+		// Return the point with the maximum dot product
+		if (dotProductTop > dotProductBottom) {
+			return new Vector3f(0, this.halfHeight, 0);
+		}
+		return new Vector3f(0, -this.halfHeight, 0);
+	}
+	
+	// Get the radius of the capsule
+	public float getRadius() {
+		return this.margin;
+	}
+	
+	@Override
+	public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) {
+		final Vector3f n = _ray.point2.lessNew(_ray.point1);
+		final float epsilon = 0.01f;
+		final Vector3f p = new Vector3f(0.0f, -this.halfHeight, 0.0f);
+		final Vector3f q = new Vector3f(0.0f, this.halfHeight, 0.0f);
+		final Vector3f d = q.lessNew(p);
+		final Vector3f m = _ray.point1.lessNew(p);
+		float t;
+		final float mDotD = m.dot(d);
+		final float nDotD = n.dot(d);
+		final float dDotD = d.dot(d);
+		// Test if the segment is outside the cylinder
+		final float vec1DotD = _ray.point1.lessNew(new Vector3f(0.0f, -this.halfHeight - this.margin, 0.0f)).dot(d);
+		if (vec1DotD < 0.0f && vec1DotD + nDotD < 0.0f) {
+			return false;
+		}
+		final float ddotDExtraCaps = 2.0f * this.margin * d.y;
+		if (vec1DotD > dDotD + ddotDExtraCaps && vec1DotD + nDotD > dDotD + ddotDExtraCaps) {
+			return false;
+		}
+		final float nDotN = n.dot(n);
+		final float mDotN = m.dot(n);
+		final float a = dDotD * nDotN - nDotD * nDotD;
+		final float k = m.dot(m) - this.margin * this.margin;
+		final float c = dDotD * k - mDotD * mDotD;
+		// If the ray is parallel to the capsule axis
+		if (FMath.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 longersect an endcap of the capsule
+			// If the ray longersects with the "p" endcap of the capsule
+			if (mDotD < 0.0f) {
+				// Check longersection between the ray and the "p" sphere endcap of the capsule
+				final Vector3f hitLocalPoint = new Vector3f();
+				final Float hitFraction = 0.0f;
+				if (raycastWithSphereEndCap(_ray.point1, _ray.point2, p, _ray.maxFraction, hitLocalPoint, hitFraction)) {
+					_raycastInfo.body = _proxyShape.getBody();
+					_raycastInfo.proxyShape = _proxyShape;
+					_raycastInfo.hitFraction = hitFraction;
+					_raycastInfo.worldPoint = hitLocalPoint;
+					final Vector3f normalDirection = hitLocalPoint.lessNew(p);
+					_raycastInfo.worldNormal = normalDirection;
+					return true;
+				}
+				return false;
+			} else if (mDotD > dDotD) { // If the ray longersects with the "q" endcap of the cylinder
+				// Check longersection between the ray and the "q" sphere endcap of the capsule
+				final Vector3f hitLocalPoint = new Vector3f();
+				final Float hitFraction = 0.0f;
+				if (raycastWithSphereEndCap(_ray.point1, _ray.point2, q, _ray.maxFraction, hitLocalPoint, hitFraction)) {
+					_raycastInfo.body = _proxyShape.getBody();
+					_raycastInfo.proxyShape = _proxyShape;
+					_raycastInfo.hitFraction = hitFraction;
+					_raycastInfo.worldPoint = hitLocalPoint;
+					final Vector3f normalDirection = hitLocalPoint.lessNew(q);
+					_raycastInfo.worldNormal = normalDirection;
+					return true;
+				}
+				return false;
+			} else {
+				// If the origin is inside the cylinder, we return no hit
+				return false;
+			}
+		}
+		final float b = dDotD * mDotN - nDotD * mDotD;
+		final 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 longersection along the ray)
+		final float t0 = t = (-b - FMath.sqrt(discriminant)) / a;
+		// If the longersection is outside the finite cylinder of the capsule on "p" endcap side
+		final float value = mDotD + t * nDotD;
+		if (value < 0.0f) {
+			// Check longersection between the ray and the "p" sphere endcap of the capsule
+			final Vector3f hitLocalPoint = new Vector3f();
+			final Float hitFraction = 0.0f;
+			if (raycastWithSphereEndCap(_ray.point1, _ray.point2, p, _ray.maxFraction, hitLocalPoint, hitFraction)) {
+				_raycastInfo.body = _proxyShape.getBody();
+				_raycastInfo.proxyShape = _proxyShape;
+				_raycastInfo.hitFraction = hitFraction;
+				_raycastInfo.worldPoint = hitLocalPoint;
+				final Vector3f normalDirection = hitLocalPoint.lessNew(p);
+				_raycastInfo.worldNormal = normalDirection;
+				return true;
+			}
+			return false;
+		} else if (value > dDotD) { // If the longersection is outside the finite cylinder on the "q" side
+			// Check longersection between the ray and the "q" sphere endcap of the capsule
+			final Vector3f hitLocalPoint = new Vector3f();
+			final Float hitFraction = 0.0f;
+			if (raycastWithSphereEndCap(_ray.point1, _ray.point2, q, _ray.maxFraction, hitLocalPoint, hitFraction)) {
+				_raycastInfo.body = _proxyShape.getBody();
+				_raycastInfo.proxyShape = _proxyShape;
+				_raycastInfo.hitFraction = hitFraction;
+				_raycastInfo.worldPoint = hitLocalPoint;
+				final Vector3f normalDirection = hitLocalPoint.lessNew(q);
+				_raycastInfo.worldNormal = normalDirection;
+				return true;
+			}
+			return false;
+		}
+		t = t0;
+		// If the longersection 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
+		final Vector3f localHitPoint = n.multiplyNew(t).add(_ray.point1);
+		_raycastInfo.body = _proxyShape.getBody();
+		_raycastInfo.proxyShape = _proxyShape;
+		_raycastInfo.hitFraction = t;
+		_raycastInfo.worldPoint = localHitPoint;
+		final Vector3f v = localHitPoint.lessNew(p);
+		final Vector3f w = d.multiplyNew(v.dot(d) / d.length2());
+		final Vector3f normalDirection = localHitPoint.lessNew(p.addNew(w)).safeNormalize();
+		_raycastInfo.worldNormal = normalDirection;
+		return true;
+	}
+	
+	/**
+	 * @brief Raycasting method between a ray one of the two spheres end cap of the capsule
+	 */
+	protected boolean raycastWithSphereEndCap(final Vector3f _point1, final Vector3f _point2, final Vector3f _sphereCenter, final float _maxFraction, Vector3f _hitLocalPoint, Float _hitFraction) {
+		final Vector3f m = _point1.lessNew(_sphereCenter);
+		final float c = m.dot(m) - this.margin * this.margin;
+		// If the origin of the ray is inside the sphere, we return no longersection
+		if (c < 0.0f) {
+			return false;
+		}
+		final Vector3f rayDirection = _point2.lessNew(_point1);
+		final 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 longersection
+		if (b > 0.0f) {
+			return false;
+		}
+		final float raySquareLength = rayDirection.length2();
+		// Compute the discriminant of the quadratic equation
+		final float discriminant = b * b - raySquareLength * c;
+		// If the discriminant is negative or the ray length is very small, there is no longersection
+		if (discriminant < 0.0f || raySquareLength < Constant.FLOAT_EPSILON) {
+			return false;
+		}
+		// Compute the solution "t" closest to the origin
+		float t = -b - FMath.sqrt(discriminant);
+		assert (t >= 0.0f);
+		// If the hit point is withing the segment ray fraction
+		if (t < _maxFraction * raySquareLength) {
+			// Compute the longersection information
+			t /= raySquareLength;
+			_hitFraction = t;
+			_hitLocalPoint = rayDirection.multiplyNew(t).add(_point1);
+			return true;
+		}
+		return false;
+	}
+	
+	@Override
+	public void setLocalScaling(final Vector3f _scaling) {
+		this.halfHeight = (this.halfHeight / this.scaling.y) * _scaling.y;
+		this.margin = (this.margin / this.scaling.x) * _scaling.x;
+		super.setLocalScaling(_scaling);
+	}
+	
+	@Override
+	public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) {
+		final float diffYCenterSphere1 = _localPoint.y - this.halfHeight;
+		final float diffYCenterSphere2 = _localPoint.y + this.halfHeight;
+		final float xSquare = _localPoint.x * _localPoint.x;
+		final float zSquare = _localPoint.z * _localPoint.z;
+		final 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;
+	}
+	
+}

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

@@ -0,0 +1,126 @@
+/** @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-now, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+package org.atriasoft.ephysics.collision.shapes;
+
+import org.atriasoft.etk.math.Matrix3f;
+import org.atriasoft.etk.math.Transform3D;
+import org.atriasoft.etk.math.Vector3f;
+
+import org.atriasoft.ephysics.RaycastInfo;
+import org.atriasoft.ephysics.collision.ProxyShape;
+import org.atriasoft.ephysics.configuration.Defaults;
+import org.atriasoft.ephysics.mathematics.Ray;
+
+/**
+ * This abstract class represents the collision shape associated with a
+ * body that is used during the narrow-phase collision detection.
+ */
+public abstract class CollisionShape {
+	/**
+	 * @brief Get the maximum number of contact
+	 * @return The maximum number of contact manifolds in an overlapping pair given two shape types
+	 */
+	public static int computeNbMaxContactManifolds(final CollisionShapeType _shapeType1, final CollisionShapeType _shapeType2) {
+		// If both shapes are convex
+		if (isConvex(_shapeType1) && isConvex(_shapeType2)) {
+			return Defaults.NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE;
+		}
+		// If there is at least one concave shape
+		return Defaults.NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE;
+	}
+	
+	/**
+	 * @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
+	 */
+	public static boolean isConvex(final CollisionShapeType _shapeType) {
+		return _shapeType != CollisionShapeType.CONCAVE_MESH && _shapeType != CollisionShapeType.HEIGHTFIELD;
+	}
+	
+	protected CollisionShapeType type; //!< Type of the collision shape
+	protected Vector3f scaling; //!< Scaling vector of the collision shape
+	/// Constructor
+	
+	public CollisionShape(final CollisionShapeType type) {
+		this.type = type;
+		this.scaling = new Vector3f(1.0f, 1.0f, 1.0f);
+	}
+	
+	/**
+	 * @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
+	 */
+	public void computeAABB(final AABB _aabb, final Transform3D _transform) {
+		// Get the local bounds in x,y and z direction
+		final Vector3f minBounds = new Vector3f(0, 0, 0);
+		final Vector3f maxBounds = new Vector3f(0, 0, 0);
+		getLocalBounds(minBounds, maxBounds);
+		// Rotate the local bounds according to the orientation of the body
+		final Matrix3f worldAxis = _transform.getOrientation().getMatrix().getAbsolute();
+		final Vector3f worldMinBounds = new Vector3f(worldAxis.getColumn(0).dot(minBounds), worldAxis.getColumn(1).dot(minBounds), worldAxis.getColumn(2).dot(minBounds));
+		final Vector3f worldMaxBounds = new Vector3f(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
+		final Vector3f minCoordinates = _transform.getPosition().addNew(worldMinBounds);
+		final Vector3f maxCoordinates = _transform.getPosition().addNew(worldMaxBounds);
+		// Update the AABB with the new minimum and maximum coordinates
+		_aabb.setMin(minCoordinates);
+		_aabb.setMax(maxCoordinates);
+	}
+	
+	/**
+	 * @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
+	 */
+	public abstract void computeLocalInertiaTensor(Matrix3f _tensor, float _mass);
+	
+	/**
+	 * @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
+	 */
+	public abstract void getLocalBounds(Vector3f _min, Vector3f _max);
+	
+	/// Return the scaling vector of the collision shape
+	public Vector3f getScaling() {
+		return this.scaling;
+	}
+	
+	/**
+	 * @brief Get the type of the collision shapes
+	 * @return The type of the collision shape (box, sphere, cylinder, ...)
+	 */
+	public 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
+	 */
+	public abstract boolean isConvex();
+	
+	/// Raycast method with feedback information
+	public abstract boolean raycast(Ray ray, RaycastInfo raycastInfo, ProxyShape proxyShape);
+	
+	/**
+	 * @brief Set the scaling vector of the collision shape
+	 */
+	public void setLocalScaling(final Vector3f _scaling) {
+		this.scaling = _scaling;
+	}
+	
+	/// Return true if a point is inside the collision shape
+	public abstract boolean testPointInside(Vector3f worldPoint, ProxyShape proxyShape);
+};

src/org/atriasoft/ephysics/collision/shapes/CollisionShapeType.java

@@ -0,0 +1,58 @@
+/** @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-now, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+package org.atriasoft.ephysics.collision.shapes;
+
+/**
+ * Type of the collision shape
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public enum CollisionShapeType {
+	TRIANGLE(0),
+	BOX(1),
+	SPHERE(2),
+	CONE(3),
+	CYLINDER(4),
+	CAPSULE(5),
+	CONVEX_MESH(6),
+	CONCAVE_MESH(7),
+	HEIGHTFIELD(8);
+	
+	public static final int NB_COLLISION_SHAPE_TYPES = 9;
+	
+	public static CollisionShapeType getType(final int value) {
+		switch (value) {
+			case 0:
+				return TRIANGLE;
+			case 1:
+				return BOX;
+			case 2:
+				return SPHERE;
+			case 3:
+				return CONE;
+			case 4:
+				return CYLINDER;
+			case 5:
+				return CAPSULE;
+			case 6:
+				return CONVEX_MESH;
+			case 7:
+				return CONCAVE_MESH;
+			case 8:
+				return HEIGHTFIELD;
+		}
+		return null;
+	}
+	
+	public final int value;
+	
+	private CollisionShapeType(final int value) {
+		this.value = value;
+	}
+}

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

@@ -0,0 +1,210 @@
+package org.atriasoft.ephysics.collision.shapes;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import org.atriasoft.ephysics.RaycastInfo;
+import org.atriasoft.ephysics.collision.ProxyShape;
+import org.atriasoft.ephysics.collision.Triangle;
+import org.atriasoft.ephysics.collision.TriangleMesh;
+import org.atriasoft.ephysics.collision.TriangleVertexArray;
+import org.atriasoft.ephysics.collision.broadphase.CallbackOverlapping;
+import org.atriasoft.ephysics.collision.broadphase.CallbackRaycast;
+import org.atriasoft.ephysics.collision.broadphase.DTree;
+import org.atriasoft.ephysics.collision.broadphase.DynamicAABBTree;
+import org.atriasoft.ephysics.collision.shapes.TriangleShape.TriangleRaycastSide;
+import org.atriasoft.ephysics.internal.Log;
+import org.atriasoft.ephysics.mathematics.Ray;
+import org.atriasoft.etk.math.Matrix3f;
+import org.atriasoft.etk.math.Vector3f;
+
+/**
+ *  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.
+ */
+public class ConcaveMeshShape extends ConcaveShape {
+	class ConcaveMeshRaycastCallback {
+		private final List<DTree> hitAABBNodes = new ArrayList<>();
+		private final DynamicAABBTree dynamicAABBTree;
+		private final ConcaveMeshShape concaveMeshShape;
+		private final ProxyShape proxyShape;
+		private final RaycastInfo raycastInfo;
+		private final Ray ray;
+		private boolean isHit;
+		
+		// Constructor
+		ConcaveMeshRaycastCallback(final DynamicAABBTree _dynamicAABBTree, final ConcaveMeshShape _concaveMeshShape, final ProxyShape _proxyShape, final RaycastInfo _raycastInfo, final Ray _ray) {
+			this.dynamicAABBTree = _dynamicAABBTree;
+			this.concaveMeshShape = _concaveMeshShape;
+			this.proxyShape = _proxyShape;
+			this.raycastInfo = _raycastInfo;
+			this.ray = _ray;
+			this.isHit = false;
+			
+		}
+		
+		/// Return true if a raycast hit has been found
+		public boolean getIsHit() {
+			return this.isHit;
+		}
+		
+		/// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree
+		public float operator__parenthese(final DTree _node, final Ray _ray) {
+			// Add the id of the hit AABB node longo
+			this.hitAABBNodes.add(_node);
+			return _ray.maxFraction;
+		}
+		
+		/// Raycast all collision shapes that have been collected
+		public void raycastTriangles() {
+			float smallestHitFraction = this.ray.maxFraction;
+			for (final DTree it : this.hitAABBNodes) {
+				// Get the node data (triangle index and mesh subpart index)
+				final int data_0 = this.dynamicAABBTree.getNodeDataInt_0(it);
+				final int data_1 = this.dynamicAABBTree.getNodeDataInt_1(it);
+				// Get the triangle vertices for this node from the concave mesh shape
+				final Vector3f[] trianglePoints = new Vector3f[3];
+				this.concaveMeshShape.getTriangleVerticesWithIndexPointer(data_0, data_1, trianglePoints);
+				// Create a triangle collision shape
+				final float margin = this.concaveMeshShape.getTriangleMargin();
+				
+				final TriangleShape triangleShape = new TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
+				triangleShape.setRaycastTestType(this.concaveMeshShape.getRaycastTestType());
+				// Ray casting test against the collision shape
+				final RaycastInfo raycastInfo = new RaycastInfo();
+				final 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;
+				}
+			}
+		}
+		
+	};
+	
+	protected TriangleMesh triangleMesh; //!< Triangle mesh
+	
+	protected DynamicAABBTree dynamicAABBTree; //!< Dynamic AABB tree to accelerate collision with the triangles
+	
+	public ConcaveMeshShape(final TriangleMesh _triangleMesh) {
+		super(CollisionShapeType.CONCAVE_MESH);
+		this.triangleMesh = _triangleMesh;
+		this.raycastTestType = TriangleRaycastSide.FRONT;
+		initBVHTree();
+	}
+	
+	@Override
+	public void computeLocalInertiaTensor(final Matrix3f _tensor, final 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.set(_mass, 0.0f, 0.0f, 0.0f, _mass, 0.0f, 0.0f, 0.0f, _mass);
+	}
+	
+	@Override
+	public void getLocalBounds(final Vector3f _min, final Vector3f _max) {
+		// Get the AABB of the whole tree
+		final AABB treeAABB = this.dynamicAABBTree.getRootAABB();
+		_min.set(treeAABB.getMin());
+		_max.set(treeAABB.getMax());
+	}
+	
+	/// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
+	/// given the start vertex index pointer of the triangle.
+	protected void getTriangleVerticesWithIndexPointer(final int _subPart, final int _triangleIndex, final Vector3f[] _outTriangleVertices) {
+		assert (_outTriangleVertices != null);
+		// Get the triangle vertex array of the current sub-part
+		final TriangleVertexArray triangleVertexArray = this.triangleMesh.getSubpart(_subPart);
+		if (triangleVertexArray == null) {
+			Log.error("get null ...");
+		}
+		final Triangle trianglePoints = triangleVertexArray.getTriangle(_triangleIndex);
+		_outTriangleVertices[0] = trianglePoints.get(0).multiplyNew(this.scaling);
+		_outTriangleVertices[1] = trianglePoints.get(1).multiplyNew(this.scaling);
+		_outTriangleVertices[2] = trianglePoints.get(2).multiplyNew(this.scaling);
+	}
+	
+	/// Insert all the triangles longo the dynamic AABB tree
+	protected void 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
+			final TriangleVertexArray triangleVertexArray = this.triangleMesh.getSubpart(subPart);
+			// For each triangle of the concave mesh
+			for (int iii = 0; iii < triangleVertexArray.getNbTriangles(); ++iii) {
+				final Triangle trianglePoints = triangleVertexArray.getTriangle(iii);
+				final Vector3f[] trianglePoints2 = new Vector3f[3];
+				trianglePoints2[0] = trianglePoints.get(0);
+				trianglePoints2[1] = trianglePoints.get(1);
+				trianglePoints2[2] = trianglePoints.get(2);
+				// Create the AABB for the triangle
+				final AABB aabb = AABB.createAABBForTriangle(trianglePoints2);
+				aabb.inflate(this.triangleMargin, this.triangleMargin, this.triangleMargin);
+				// Add the AABB with the index of the triangle longo the dynamic AABB tree
+				this.dynamicAABBTree.addObject(aabb, subPart, iii);
+			}
+		}
+	}
+	
+	@Override
+	public boolean isConvex() {
+		return false;
+	}
+	
+	@Override
+	public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) {
+		// Create the callback object that will compute ray casting against triangles
+		final ConcaveMeshRaycastCallback raycastCallback = new ConcaveMeshRaycastCallback(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, new CallbackRaycast() {
+			
+			@Override
+			public float callback(final DTree _node, final Ray _ray) {
+				return raycastCallback.operator__parenthese(_node, _ray);
+			}
+		});
+		raycastCallback.raycastTriangles();
+		return raycastCallback.getIsHit();
+	}
+	
+	@Override
+	public void setLocalScaling(final Vector3f _scaling) {
+		super.setLocalScaling(_scaling);
+		this.dynamicAABBTree.reset();
+		initBVHTree();
+	}
+	
+	@Override
+	public void testAllTriangles(final ConcaveShape.TriangleCallback _callback, final AABB _localAABB) {
+		// Ask the Dynamic AABB Tree to report all the triangles that are overlapping
+		// with the AABB of the convex shape.
+		final ConcaveMeshShape self = this;
+		this.dynamicAABBTree.reportAllShapesOverlappingWithAABB(_localAABB, new CallbackOverlapping() {
+			@Override
+			public void callback(final DTree _node) {
+				// Get the node data (triangle index and mesh subpart index)
+				final int data_0 = self.dynamicAABBTree.getNodeDataInt_0(_node);
+				final int data_1 = self.dynamicAABBTree.getNodeDataInt_1(_node);
+				// Get the triangle vertices for this node from the concave mesh shape
+				final Vector3f[] trianglePoints = new Vector3f[3];
+				getTriangleVerticesWithIndexPointer(data_0, data_1, trianglePoints);
+				// Call the callback to test narrow-phase collision with this triangle
+				_callback.testTriangle(trianglePoints);
+			}
+		});
+	}
+}

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

@@ -0,0 +1,88 @@
+/** @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-now, Edouard DUPIN
+ * @license MPL v2.0 (see license file)
+ */
+package org.atriasoft.ephysics.collision.shapes;
+
+import org.atriasoft.etk.math.Vector3f;
+
+import org.atriasoft.ephysics.collision.ProxyShape;
+import org.atriasoft.ephysics.collision.shapes.TriangleShape.TriangleRaycastSide;
+
+/**
+ *  This abstract class represents a concave collision shape associated with a
+ * body that is used during the narrow-phase collision detection.
+ */
+public abstract class ConcaveShape extends CollisionShape {
+	
+	/**
+	 *  It is used to encapsulate a callback method for
+	 * a single triangle of a ConcaveMesh.
+	 */
+	public interface TriangleCallback {
+		/// Report a triangle
+		public void testTriangle(Vector3f[] _trianglePoints);
+	}
+	
+	boolean isSmoothMeshCollisionEnabled; //!< True if the smooth mesh collision algorithm is enabled
+	
+	protected float triangleMargin; //!< Margin use for collision detection for each triangle
+	
+	protected TriangleRaycastSide raycastTestType; //!< Raycast test type for the triangle (front, back, front-back)
+	
+	/// Return true if the collision shape is convex, false if it is concave
+	public boolean isConvex;
+	
+	public ConcaveShape(final CollisionShapeType _type) {
+		super(_type);
+		this.isSmoothMeshCollisionEnabled = false;
+		this.triangleMargin = 0;
+		this.raycastTestType = TriangleRaycastSide.FRONT;
+	}
+	
+	/// Return true if the smooth mesh collision is enabled
+	public boolean getIsSmoothMeshCollisionEnabled() {
+		return this.isSmoothMeshCollisionEnabled;
+	}
+	
+	/// Return the raycast test type (front, back, front-back)
+	public TriangleRaycastSide getRaycastTestType() {
+		return this.raycastTestType;
+	}
+	
+	/// Return the triangle margin
+	public float getTriangleMargin() {
+		return this.triangleMargin;
+	}
+	
+	/**
+	 *  Enable/disable the smooth mesh collision algorithm
+	 *
+	 * Smooth mesh collision is used to avoid collisions against some longernal edges of the triangle mesh.
+	 * If it is enabled, collsions with the mesh will be smoother but collisions computation is a bit more expensive.
+	 */
+	public void setIsSmoothMeshCollisionEnabled(final boolean _isEnabled) {
+		this.isSmoothMeshCollisionEnabled = _isEnabled;
+	}
+	
+	/**
+	 *  Set the raycast test type (front, back, front-back)
+	 * @param testType Raycast test type for the triangle (front, back, front-back)
+	 */
+	public void setRaycastTestType(final TriangleRaycastSide _testType) {
+		this.raycastTestType = _testType;
+	}
+	
+	/// Use a callback method on all triangles of the concave shape inside a given AABB
+	public abstract void testAllTriangles(TriangleCallback _callback, AABB _localAABB);
+	
+	@Override
+	public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) {
+		return false;
+	}
+	
+};

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

@@ -0,0 +1,255 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.collision.shapes;
+
+import org.atriasoft.ephysics.RaycastInfo;
+import org.atriasoft.ephysics.collision.ProxyShape;
+import org.atriasoft.ephysics.configuration.Defaults;
+import org.atriasoft.ephysics.mathematics.Ray;
+import org.atriasoft.etk.math.Constant;
+import org.atriasoft.etk.math.FMath;
+import org.atriasoft.etk.math.Matrix3f;
+import org.atriasoft.etk.math.Vector3f;
+
+/**
+ *  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.
+ */
+public class ConeShape extends ConvexShape {
+	protected float radius; //!< Radius of the base
+	protected float halfHeight; //!< Half height of the cone
+	
+	protected float sinTheta; //!< sine of the semi angle at the apex point
+	
+	/**
+	 *  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
+	 */
+	public ConeShape(final float _radius, final float _height) {
+		this(_radius, _height, Defaults.OBJECT_MARGIN);
+	}
+	
+	public ConeShape(final float _radius, final float _height, final float _margin) {
+		super(CollisionShapeType.CONE, _margin);
+		this.radius = _radius;
+		this.halfHeight = _height * 0.5f;
+		// Compute the sine of the semi-angle at the apex point
+		this.sinTheta = this.radius / (FMath.sqrt(this.radius * this.radius + _height * _height));
+	}
+	
+	@Override
+	public void computeLocalInertiaTensor(final Matrix3f _tensor, final float _mass) {
+		final float rSquare = this.radius * this.radius;
+		final float diagXZ = 0.15f * _mass * (rSquare + this.halfHeight);
+		_tensor.set(diagXZ, 0.0f, 0.0f, 0.0f, 0.3f * _mass * rSquare, 0.0f, 0.0f, 0.0f, diagXZ);
+	}
+	
+	/**
+	 *  Return the height
+	 * @return Height of the cone (in meters)
+	 */
+	public float getHeight() {
+		return 2.0f * this.halfHeight;
+	}
+	
+	@Override
+	public void getLocalBounds(final Vector3f min, final Vector3f max) {
+		// Maximum bounds
+		max.setX(this.radius + this.margin);
+		max.setY(this.halfHeight + this.margin);
+		max.setZ(this.radius + this.margin);
+		// Minimum bounds
+		min.setX(-max.x);
+		min.setY(-max.y);
+		min.setZ(-max.z);
+	}
+	
+	@Override
+	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) {
+		final Vector3f v = _direction;
+		final float sinThetaTimesLengthV = this.sinTheta * v.length();
+		Vector3f supportPoint;
+		if (v.y > sinThetaTimesLengthV) {
+			supportPoint = new Vector3f(0.0f, this.halfHeight, 0.0f);
+		} else {
+			final float projectedLength = FMath.sqrt(v.x * v.x + v.z * v.z);
+			if (projectedLength > Constant.FLOAT_EPSILON) {
+				final float d = this.radius / projectedLength;
+				supportPoint = new Vector3f(v.x * d, -this.halfHeight, v.z * d);
+			} else {
+				supportPoint = new Vector3f(0.0f, -this.halfHeight, 0.0f);
+			}
+		}
+		return supportPoint;
+	}
+	
+	/**
+	 *  Return the radius
+	 * @return Radius of the cone (in meters)
+	 */
+	public float getRadius() {
+		return this.radius;
+	}
+	
+	@Override
+	public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) {
+		final Vector3f r = _ray.point2.lessNew(_ray.point1);
+		final float epsilon = 0.00001f;
+		final Vector3f V = new Vector3f(0, this.halfHeight, 0);
+		final Vector3f centerBase = new Vector3f(0, -this.halfHeight, 0);
+		final Vector3f axis = new Vector3f(0, -1.0f, 0);
+		final float heightSquare = 4.0f * this.halfHeight * this.halfHeight;
+		final float cosThetaSquare = heightSquare / (heightSquare + this.radius * this.radius);
+		final float factor = 1.0f - cosThetaSquare;
+		final Vector3f delta = _ray.point1.lessNew(V);
+		final float c0 = -cosThetaSquare * delta.x * delta.x + factor * delta.y * delta.y - cosThetaSquare * delta.z * delta.z;
+		final float c1 = -cosThetaSquare * delta.x * r.x + factor * delta.y * r.y - cosThetaSquare * delta.z * r.z;
+		final float c2 = -cosThetaSquare * r.x * r.x + factor * r.y * r.y - cosThetaSquare * r.z * r.z;
+		final float tHit[] = { -1.0f, -1.0f, -1.0f };
+		final Vector3f[] localHitPoint = new Vector3f[3];
+		final Vector3f[] localNormal = new Vector3f[3];
+		// If c2 is different from zero
+		if (FMath.abs(c2) > Constant.FLOAT_EPSILON) {
+			final 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 longersections
+				final float sqrRoot = FMath.sqrt(gamma);
+				tHit[0] = (-c1 - sqrRoot) / c2;
+				tHit[1] = (-c1 + sqrRoot) / c2;
+			} else { // If the equation has a single real root
+				// Compute the longersection
+				tHit[0] = -c1 / c2;
+			}
+		} else // If c2 == 0
+		if (FMath.abs(c1) > Constant.FLOAT_EPSILON) {
+			// If c2 = 0 and c1 != 0
+			tHit[0] = -c0 / (2.0f * 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] = r.multiplyNew(tHit[0]).add(_ray.point1);
+		localHitPoint[1] = r.multiplyNew(tHit[1]).add(_ray.point1);
+		// Only keep hit points in one side of the double cone (the cone we are longerested in)
+		if (axis.dot(localHitPoint[0].lessNew(V)) < 0.0f) {
+			tHit[0] = -1.0f;
+		}
+		if (axis.dot(localHitPoint[1].lessNew(V)) < 0.0f) {
+			tHit[1] = -1.0f;
+		}
+		// Only keep hit points that are within the correct height of the cone
+		if (localHitPoint[0].y < -this.halfHeight) {
+			tHit[0] = -1.0f;
+		}
+		if (localHitPoint[1].y < -this.halfHeight) {
+			tHit[1] = -1.0f;
+		}
+		// If the ray is in direction of the base plane of the cone
+		if (r.y > epsilon) {
+			// Compute the longersection with the base plane of the cone
+			tHit[2] = (-_ray.point1.y - this.halfHeight) / (r.y);
+			// Only keep this longersection if it is inside the cone radius
+			localHitPoint[2] = r.multiplyNew(tHit[2]).add(_ray.point1);
+			if ((localHitPoint[2].lessNew(centerBase)).length2() > this.radius * this.radius) {
+				tHit[2] = -1.0f;
+			}
+			// Compute the normal direction
+			localNormal[2] = axis;
+		}
+		// Find the smallest positive t value
+		int hitIndex = -1;
+		float t = Float.MAX_VALUE;
+		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) {
+			final float h = 2.0f * this.halfHeight;
+			final float value1 = (localHitPoint[hitIndex].x * localHitPoint[hitIndex].x + localHitPoint[hitIndex].z * localHitPoint[hitIndex].z);
+			final float rOverH = this.radius / h;
+			final float value2 = 1.0f + rOverH * rOverH;
+			final float factor22 = 1.0f / FMath.sqrt(value1 * value2);
+			final float x = localHitPoint[hitIndex].x * factor22;
+			final float z = localHitPoint[hitIndex].z * factor22;
+			localNormal[hitIndex].setX(x);
+			localNormal[hitIndex].setY(FMath.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;
+	}
+	
+	@Override
+	public void setLocalScaling(final Vector3f _scaling) {
+		this.halfHeight = (this.halfHeight / this.scaling.y) * _scaling.y;
+		this.radius = (this.radius / this.scaling.x) * _scaling.x;
+		super.setLocalScaling(_scaling);
+	}
+	
+	@Override
+	public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) {
+		final float radiusHeight = this.radius * (-_localPoint.y + this.halfHeight) / (this.halfHeight * 2.0f);
+		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/ConvexMeshShape.java

@@ -0,0 +1,343 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.collision.shapes;
+
+import java.util.ArrayList;
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
+
+import org.atriasoft.etk.math.Matrix3f;
+import org.atriasoft.etk.math.Vector3f;
+
+import org.atriasoft.ephysics.RaycastInfo;
+import org.atriasoft.ephysics.collision.ProxyShape;
+import org.atriasoft.ephysics.collision.TriangleVertexArray;
+import org.atriasoft.ephysics.configuration.Defaults;
+import org.atriasoft.ephysics.mathematics.Ray;
+import org.atriasoft.ephysics.mathematics.SetInteger;
+
+/**
+ *  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.
+ */
+public class ConvexMeshShape extends ConvexShape {
+	protected List<Vector3f> vertices = new ArrayList<>(); //!< Array with the vertices of the mesh
+	protected int numberVertices = 0; //!< Number of vertices in the mesh
+	protected Vector3f minBounds = new Vector3f(); //!< Mesh minimum bounds in the three local x, y and z directions
+	protected Vector3f maxBounds = new Vector3f(); //!< Mesh maximum bounds in the three local x, y and z directions
+	protected boolean isEdgesInformationUsed; //!< True if the shape contains the edges of the convex mesh in order to make the collision detection faster
+	
+	protected Map<Integer, SetInteger> edgesAdjacencyList = new HashMap<>(); //!< Adjacency list representing the edges of the mesh
+	
+	/**
+	 *  Constructor.
+	 * If you use this ructor, you will need to set the vertices manually one by one using the addVertex method.
+	 */
+	public ConvexMeshShape() {
+		this(Defaults.OBJECT_MARGIN);
+	}
+	
+	public ConvexMeshShape(final float _margin) {
+		super(CollisionShapeType.CONVEX_MESH, _margin);
+		this.minBounds = new Vector3f(0, 0, 0);
+		this.maxBounds = new Vector3f(0, 0, 0);
+		this.numberVertices = 0;
+		this.isEdgesInformationUsed = false;
+	}
+	
+	/**
+	 *  Constructor to initialize with an array of 3D vertices.
+	 * This method creates an longernal copy of the input vertices.
+	 * @param _arrayVertices Array with the vertices of the convex mesh
+	 * @param _nbVertices Number of vertices in the convex mesh
+	 * @param _stride Stride between the beginning of two elements in the vertices array
+	 * @param _margin Collision margin (in meters) around the collision shape
+	 */
+	public ConvexMeshShape(final float[] _arrayVertices, final int _nbVertices, final int _stride) {
+		this(_arrayVertices, _nbVertices, _stride, Defaults.OBJECT_MARGIN);
+	}
+	
+	public ConvexMeshShape(final float[] _arrayVertices, final int _nbVertices, final int _stride, final float _margin) {
+		super(CollisionShapeType.CONVEX_MESH, _margin);
+		this.numberVertices = _nbVertices;
+		this.minBounds = new Vector3f(0, 0, 0);
+		this.maxBounds = new Vector3f(0, 0, 0);
+		this.isEdgesInformationUsed = false;
+		int offset = 0;
+		// Copy all the vertices longo the longernal array
+		for (long iii = 0; iii < this.numberVertices; iii++) {
+			this.vertices.add(new Vector3f(_arrayVertices[offset], _arrayVertices[offset + 1], _arrayVertices[offset + 2]));
+			offset += _stride;
+		}
+		// Recalculate the bounds of the mesh
+		recalculateBounds();
+	}
+	
+	/**
+	 *  Constructor to initialize with a triangle mesh
+	 * This method creates an internal copy of the input vertices.
+	 * @param _triangleVertexArray Array with the vertices and indices of the vertices and triangles of the mesh
+	 * @param _isEdgesInformationUsed True if you want to use edges information for collision detection (faster but requires more memory)
+	 * @param _margin Collision margin (in meters) around the collision shape
+	 */
+	public ConvexMeshShape(final TriangleVertexArray _triangleVertexArray) {
+		this(_triangleVertexArray, true);
+	}
+	
+	public ConvexMeshShape(final TriangleVertexArray _triangleVertexArray, final boolean _isEdgesInformationUsed) {
+		this(_triangleVertexArray, true, Defaults.OBJECT_MARGIN);
+	}
+	
+	public ConvexMeshShape(final TriangleVertexArray _triangleVertexArray, final boolean _isEdgesInformationUsed, final float _margin) {
+		super(CollisionShapeType.CONVEX_MESH, _margin);
+		this.minBounds = new Vector3f(0, 0, 0);
+		this.maxBounds = new Vector3f(0, 0, 0);
+		this.isEdgesInformationUsed = _isEdgesInformationUsed;
+		// For each vertex of the mesh
+		for (final Vector3f it : _triangleVertexArray.getVertices()) {
+			this.vertices.add(it.multiplyNew(this.scaling));
+		}
+		// If we need to use the edges information of the mesh
+		if (this.isEdgesInformationUsed) {
+			// For each triangle of the mesh
+			for (int iii = 0; iii < _triangleVertexArray.getNbTriangles(); iii++) {
+				final int vertexIndex[] = { 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();
+	}
+	
+	/**
+	 *  Add an edge longo 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 longo the convex mesh.
+	 * @param _v1 Index of the first vertex of the edge to add
+	 * @param _v2 Index of the second vertex of the edge to add
+	 */
+	public void addEdge(final int _v1, final int _v2) {
+		// If the entry for vertex v1 does not exist in the adjacency list
+		if (!this.edgesAdjacencyList.containsKey(_v1)) {
+			this.edgesAdjacencyList.put(_v1, new SetInteger());
+		}
+		// If the entry for vertex v2 does not exist in the adjacency list
+		if (!this.edgesAdjacencyList.containsKey(_v2)) {
+			this.edgesAdjacencyList.put(_v2, new SetInteger());
+		}
+		// Add the edge in the adjacency list
+		this.edgesAdjacencyList.get(_v1).add(_v2);
+		this.edgesAdjacencyList.get(_v2).add(_v1);
+	}
+	
+	/**
+	 *  Add a vertex longo the convex mesh
+	 * @param vertex Vertex to be added
+	 */
+	public void addVertex(final Vector3f _vertex) {
+		// Add the vertex in to vertices array
+		this.vertices.add(_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);
+		}
+	}
+	
+	@Override
+	public void computeLocalInertiaTensor(final Matrix3f _tensor, final float _mass) {
+		final float factor = (1.0f / 3.0f) * _mass;
+		final Vector3f realExtent = this.maxBounds.lessNew(this.minBounds).multiply(0.5f);
+		assert (realExtent.x > 0 && realExtent.y > 0 && realExtent.z > 0);
+		final float xSquare = realExtent.x * realExtent.x;
+		final float ySquare = realExtent.y * realExtent.y;
+		final float zSquare = realExtent.z * realExtent.z;
+		_tensor.set(factor * (ySquare + zSquare), 0.0f, 0.0f, 0.0f, factor * (xSquare + zSquare), 0.0f, 0.0f, 0.0f, factor * (xSquare + ySquare));
+	}
+	
+	@Override
+	public void getLocalBounds(final Vector3f _min, final Vector3f _max) {
+		_min.set(this.minBounds);
+		_max.set(this.maxBounds);
+	}
+	
+	@Override
+	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) {
+		assert (this.numberVertices == this.vertices.size());
+		assert (_cachedCollisionData != null);
+		// Allocate memory for the cached collision data if not allocated yet
+		if (_cachedCollisionData.data == null) {
+			// TODO the data is nort set outside ==> find how ...  !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+			_cachedCollisionData.data = 0;
+		}
+		// If the edges information is used to speed up the collision detection
+		if (this.isEdgesInformationUsed) {
+			assert (this.edgesAdjacencyList.size() == this.numberVertices);
+			int maxVertex = (Integer) (_cachedCollisionData.data);
+			float maxDotProduct = _direction.dot(this.vertices.get(maxVertex));
+			boolean isOptimal;
+			// Perform hill-climbing (local search)
+			do {
+				isOptimal = true;
+				assert (this.edgesAdjacencyList.get(maxVertex).size() > 0);
+				// For all neighbors of the current vertex
+				for (final Integer it : this.edgesAdjacencyList.get(maxVertex).getRaw()) {
+					// Compute the dot product
+					final float dotProduct = _direction.dot(this.vertices.get(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
+			_cachedCollisionData.data = maxVertex;
+			// Return the support vertex
+			return this.vertices.get(maxVertex).multiplyNew(this.scaling);
+		} else {
+			// If the edges information is not used
+			double maxDotProduct = Float.MIN_VALUE;
+			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
+				final double dotProduct = _direction.dot(this.vertices.get(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.get(indexMaxDotProduct).multiplyNew(this.scaling);
+		}
+	}
+	
+	/**
+	 *  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
+	 */
+	public boolean isEdgesInformationUsed() {
+		return this.isEdgesInformationUsed;
+	}
+	
+	@Override
+	public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) {
+		return _proxyShape.getBody().getWorld().getCollisionDetection().getNarrowPhaseGJKAlgorithm().raycast(_ray, _proxyShape, _raycastInfo);
+	}
+	
+	/// Recompute the bounds of the mesh
+	protected void 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.get(i).x > this.maxBounds.x) {
+				this.maxBounds.setX(this.vertices.get(i).x);
+			}
+			if (this.vertices.get(i).x < this.minBounds.x) {
+				this.minBounds.setX(this.vertices.get(i).x);
+			}
+			if (this.vertices.get(i).y > this.maxBounds.y) {
+				this.maxBounds.setY(this.vertices.get(i).y);
+			}
+			if (this.vertices.get(i).y < this.minBounds.y) {
+				this.minBounds.setY(this.vertices.get(i).y);
+			}
+			if (this.vertices.get(i).z > this.maxBounds.z) {
+				this.maxBounds.setZ(this.vertices.get(i).z);
+			}
+			if (this.vertices.get(i).z < this.minBounds.z) {
+				this.minBounds.setZ(this.vertices.get(i).z);
+			}
+		}
+		// Apply the local scaling factor
+		this.maxBounds.multiply(this.scaling);
+		this.minBounds.multiply(this.scaling);
+		// Add the object margin to the bounds
+		this.maxBounds.add(this.margin);
+		this.minBounds.less(this.margin);
+	}
+	
+	/**
+	 *  Set the variable to know if the edges information is used to speed up the
+	 * collision detection
+	 * @param isEdgesUsed True if you want to use the edges information to speed up the collision detection with the convex mesh shape
+	 */
+	public void setIsEdgesInformationUsed(final boolean _isEdgesUsed) {
+		this.isEdgesInformationUsed = _isEdgesUsed;
+	}
+	
+	@Override
+	public void setLocalScaling(final Vector3f _scaling) {
+		super.setLocalScaling(_scaling);
+		recalculateBounds();
+	}
+	
+	@Override
+	public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) {
+		// Use the GJK algorithm to test if the point is inside the convex mesh
+		return _proxyShape.getBody().getWorld().getCollisionDetection().getNarrowPhaseGJKAlgorithm().testPointInside(_localPoint, _proxyShape);
+	}
+};

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

@@ -0,0 +1,56 @@
+package org.atriasoft.ephysics.collision.shapes;
+
+import org.atriasoft.ephysics.collision.ProxyShape;
+import org.atriasoft.etk.math.Constant;
+import org.atriasoft.etk.math.Vector3f;
+
+public abstract class ConvexShape extends CollisionShape {
+	protected float margin; //!< Margin used for the GJK collision detection algorithm
+	/// Constructor
+	
+	public ConvexShape(final CollisionShapeType _type, final float _margin) {
+		super(_type);
+		this.margin = _margin;
+	}
+	
+	// Return a local support point in a given direction with the object margin
+	public Vector3f getLocalSupportPointWithMargin(final Vector3f _direction, final CacheData _cachedCollisionData) {
+		////Log.error(" ->  getLocalSupportPointWithMargin(" + _direction);
+		// Get the support point without margin
+		final Vector3f supportPoint = getLocalSupportPointWithoutMargin(_direction, _cachedCollisionData);
+		////Log.error(" ->  supportPoint = " + supportPoint);
+		////Log.error(" ->  margin = " + FMath.floatToString(this.margin));
+		if (this.margin != 0.0f) {
+			// Add the margin to the support point
+			Vector3f unitVec = new Vector3f(0.0f, -1.0f, 0.0f);
+			////Log.error(" ->      _direction.length2()=" + FMath.floatToString(_direction.length2()));
+			////Log.error(" ->      Constant.FLOAT_EPSILON=" + FMath.floatToString(Constant.FLOAT_EPSILON));
+			if (_direction.length2() > Constant.FLOAT_EPSILON * Constant.FLOAT_EPSILON) {
+				unitVec = _direction.safeNormalizeNew();
+				////Log.error(" ->          unitVec= " + unitVec);
+			}
+			supportPoint.add(unitVec.multiplyNew(this.margin));
+		}
+		////Log.error(" ->      ==> supportPoint = " + supportPoint);
+		return supportPoint;
+	}
+	
+	/// Return a local support point in a given direction without the object margin
+	public abstract Vector3f getLocalSupportPointWithoutMargin(Vector3f _direction, CacheData _cachedCollisionData);
+	
+	/**
+	 * @brief Get the current object margin
+	 * @return The margin (in meters) around the collision shape
+	 */
+	public float getMargin() {
+		return this.margin;
+	}
+	
+	@Override
+	public boolean isConvex() {
+		return true;
+	}
+	
+	@Override
+	public abstract boolean testPointInside(Vector3f _worldPoint, ProxyShape _proxyShape);
+}

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

@@ -0,0 +1,297 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.collision.shapes;
+
+import org.atriasoft.ephysics.RaycastInfo;
+import org.atriasoft.ephysics.collision.ProxyShape;
+import org.atriasoft.ephysics.configuration.Defaults;
+import org.atriasoft.ephysics.mathematics.Ray;
+import org.atriasoft.etk.math.Constant;
+import org.atriasoft.etk.math.FMath;
+import org.atriasoft.etk.math.Matrix3f;
+import org.atriasoft.etk.math.Vector3f;
+
+/**
+ * This class represents a cylinder collision shape around the Y axis and centered at the origin. The cylinder is
+ * defined by its height and the radius of its base. The "transform" of the corresponding rigid body gives an
+ * orientation and a position to the cylinder. This collision shape uses an extra margin distance around it for
+ * collision detection purpose. The default margin is 4cm (if your units are meters, which is recommended). In case, you
+ * want to simulate small objects (smaller than the margin distance), you might want to reduce the margin by specifying
+ * your own margin distance using the "margin" parameter in the constructor of the cylinder shape. Otherwise, it is
+ * recommended to use the default margin distance by not using the "margin" parameter in the constructor.
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+/**
+ *  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.
+ */
+public class CylinderShape extends ConvexShape {
+	protected float radius; //!< Radius of the base
+	protected float halfHeight; //!< Half height of the cylinder
+	
+	/**
+	 * 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
+	 */
+	public CylinderShape(final float _radius, final float _height) {
+		this(_radius, _height, Defaults.OBJECT_MARGIN);
+	}
+	
+	public CylinderShape(final float _radius, final float _height, final float _margin) {
+		super(CollisionShapeType.CYLINDER, _margin);
+		this.radius = _radius;
+		this.halfHeight = _height / 2.0f;
+	}
+	
+	@Override
+	public void computeLocalInertiaTensor(final Matrix3f _tensor, final float _mass) {
+		final float height = 2.0f * this.halfHeight;
+		final float diag = (1.0f / 12.0f) * _mass * (3.0f * this.radius * this.radius + height * height);
+		_tensor.set(diag, 0.0f, 0.0f, 0.0f, 0.5f * _mass * this.radius * this.radius, 0.0f, 0.0f, 0.0f, diag);
+	}
+	
+	/**
+	 * Get the Shape height
+	 * @return Height of the cylinder (in meters)
+	 */
+	public float getHeight() {
+		return this.halfHeight + this.halfHeight;
+	}
+	
+	@Override
+	public void getLocalBounds(final Vector3f min, final Vector3f max) {
+		// Maximum bounds
+		max.setX(this.radius + this.margin);
+		max.setY(this.halfHeight + this.margin);
+		max.setZ(this.radius + this.margin);
+		// Minimum bounds
+		min.setX(-max.x);
+		min.setY(-max.y);
+		min.setZ(-max.z);
+	}
+	
+	@Override
+	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) {
+		final Vector3f supportPoint = new Vector3f(0.0f, 0.0f, 0.0f);
+		final float uDotv = _direction.y;
+		final Vector3f w = new Vector3f(_direction.x, 0.0f, _direction.z);
+		final float lengthW = FMath.sqrt(_direction.x * _direction.x + _direction.z * _direction.z);
+		if (lengthW > Constant.FLOAT_EPSILON) {
+			if (uDotv < 0.0f) {
+				supportPoint.setY(-this.halfHeight);
+			} else {
+				supportPoint.setY(this.halfHeight);
+			}
+			supportPoint.add(w.multiply(this.radius / lengthW));
+		} else if (uDotv < 0.0f) {
+			supportPoint.setY(-this.halfHeight);
+		} else {
+			supportPoint.setY(this.halfHeight);
+		}
+		return supportPoint;
+	}
+	
+	/**
+	 * Get the Shape radius
+	 * @return Radius of the cylinder (in meters)
+	 */
+	public float getRadius() {
+		return this.radius;
+	}
+	
+	@Override
+	public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) {
+		final Vector3f n = _ray.point2.lessNew(_ray.point1);
+		final float epsilon = 0.01f;
+		final Vector3f p = new Vector3f(0.0f, -this.halfHeight, 0.0f);
+		final Vector3f q = new Vector3f(0.0f, this.halfHeight, 0.0f);
+		final Vector3f d = q.lessNew(p);
+		final Vector3f m = _ray.point1.lessNew(p);
+		float t;
+		final float mDotD = m.dot(d);
+		final float nDotD = n.dot(d);
+		final float dDotD = d.dot(d);
+		// Test if the segment is outside the cylinder
+		if (mDotD < 0.0f && mDotD + nDotD < 0.0f) {
+			return false;
+		}
+		
+		if (mDotD > dDotD && mDotD + nDotD > dDotD) {
+			return false;
+		}
+		final float nDotN = n.dot(n);
+		final float mDotN = m.dot(n);
+		final float a = dDotD * nDotN - nDotD * nDotD;
+		final float k = m.dot(m) - this.radius * this.radius;
+		final float c = dDotD * k - mDotD * mDotD;
+		// If the ray is parallel to the cylinder axis
+		if (FMath.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 longersect an endcap of the cylinder
+			// If the ray longersects with the "p" endcap of the cylinder
+			if (mDotD < 0.0f) {
+				t = -mDotN / nDotN;
+				// If the longersection 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
+				final Vector3f localHitPoint = n.multiplyNew(t).add(_ray.point1);
+				_raycastInfo.body = _proxyShape.getBody();
+				_raycastInfo.proxyShape = _proxyShape;
+				_raycastInfo.hitFraction = t;
+				_raycastInfo.worldPoint = localHitPoint;
+				final Vector3f normalDirection = new Vector3f(0.0f, -1.0f, 0.0f);
+				_raycastInfo.worldNormal = normalDirection;
+				return true;
+			}
+			// If the ray longersects with the "q" endcap of the cylinder
+			if (mDotD > dDotD) {
+				t = (nDotD - mDotN) / nDotN;
+				// If the longersection 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
+				final Vector3f localHitPoint = n.multiplyNew(t).add(_ray.point1);
+				_raycastInfo.body = _proxyShape.getBody();
+				_raycastInfo.proxyShape = _proxyShape;
+				_raycastInfo.hitFraction = t;
+				_raycastInfo.worldPoint = localHitPoint;
+				_raycastInfo.worldNormal = new Vector3f(0, 1.0f, 0);
+				return true;
+			}
+			// If the origin is inside the cylinder, we return no hit
+			return false;
+		}
+		final float b = dDotD * mDotN - nDotD * mDotD;
+		final 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 longersection along the ray)
+		final float t0 = t = (-b - FMath.sqrt(discriminant)) / a;
+		// If the longersection is outside the cylinder on "p" endcap side
+		final 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 longersection against the "p" endcap (longersection agains whole plane)
+			t = -mDotD / nDotD;
+			// Keep the longersection if the it is inside the cylinder radius
+			if (k + t * (2.0f * mDotN + t) > 0.0f) {
+				return false;
+			}
+			// If the longersection 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
+			final Vector3f localHitPoint = n.multiplyNew(t).add(_ray.point1);
+			_raycastInfo.body = _proxyShape.getBody();
+			_raycastInfo.proxyShape = _proxyShape;
+			_raycastInfo.hitFraction = t;
+			_raycastInfo.worldPoint = localHitPoint;
+			_raycastInfo.worldNormal = new Vector3f(0, -1.0f, 0);
+			return true;
+		}
+		// If the longersection 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 longersection against the "q" endcap (longersection against whole plane)
+			t = (dDotD - mDotD) / nDotD;
+			// Keep the longersection if it is inside the cylinder radius
+			if (k + dDotD - 2.0f * mDotD + t * (2.0f * (mDotN - nDotD) + t) > 0.0f) {
+				return false;
+			}
+			// If the longersection 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
+			final Vector3f localHitPoint = n.multiplyNew(t).add(_ray.point1);
+			_raycastInfo.body = _proxyShape.getBody();
+			_raycastInfo.proxyShape = _proxyShape;
+			_raycastInfo.hitFraction = t;
+			_raycastInfo.worldPoint = localHitPoint;
+			_raycastInfo.worldNormal = new Vector3f(0, 1.0f, 0);
+			return true;
+		}
+		t = t0;
+		// If the longersection 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
+		final Vector3f localHitPoint = n.multiplyNew(t).add(_ray.point1);
+		_raycastInfo.body = _proxyShape.getBody();
+		_raycastInfo.proxyShape = _proxyShape;
+		_raycastInfo.hitFraction = t;
+		_raycastInfo.worldPoint = localHitPoint;
+		final Vector3f v = localHitPoint.lessNew(p);
+		final Vector3f w = d.multiplyNew(v.dot(d) / d.length2());
+		final Vector3f normalDirection = localHitPoint.lessNew(p.addNew(w));
+		_raycastInfo.worldNormal = normalDirection;
+		return true;
+	}
+	
+	@Override
+	public void setLocalScaling(final Vector3f _scaling) {
+		this.halfHeight = (this.halfHeight / this.scaling.y) * _scaling.y;
+		this.radius = (this.radius / this.scaling.x) * _scaling.x;
+		super.setLocalScaling(_scaling);
+	}
+	
+	@Override
+	public boolean testPointInside(final Vector3f _localPoint, final 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/HeightFieldShape.java

@@ -0,0 +1,305 @@
+package org.atriasoft.ephysics.collision.shapes;
+
+import org.atriasoft.etk.math.FMath;
+import org.atriasoft.etk.math.Matrix3f;
+import org.atriasoft.etk.math.Vector3f;
+
+import org.atriasoft.ephysics.RaycastInfo;
+import org.atriasoft.ephysics.collision.ProxyShape;
+import org.atriasoft.ephysics.mathematics.Ray;
+
+/**
+ *  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 longo the shape
+ * but are shared instead. The height values can be of type longeger, 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.
+ */
+public class HeightFieldShape extends ConcaveShape {
+	
+	/**
+	 *  This class is used for testing AABB and triangle overlap for raycasting
+	 */
+	public class TriangleOverlapCallback implements TriangleCallback {
+		protected Ray ray;
+		protected ProxyShape proxyShape;
+		protected RaycastInfo raycastInfo;
+		protected boolean isHit;
+		protected float smallestHitFraction;
+		protected HeightFieldShape heightFieldShape;
+		
+		public TriangleOverlapCallback(final Ray _ray, final ProxyShape _proxyShape, final RaycastInfo _raycastInfo, final HeightFieldShape _heightFieldShape) {
+			this.ray = _ray;
+			this.proxyShape = _proxyShape;
+			this.raycastInfo = _raycastInfo;
+			this.heightFieldShape = _heightFieldShape;
+			this.isHit = false;
+			this.smallestHitFraction = this.ray.maxFraction;
+		}
+		
+		public boolean getIsHit() {
+			return this.isHit;
+		}
+		
+		/// Raycast test between a ray and a triangle of the heightfield
+		@Override
+		public void testTriangle(final Vector3f[] _trianglePoints) {
+			// Create a triangle collision shape
+			final float margin = this.heightFieldShape.getTriangleMargin();
+			final TriangleShape triangleShape = new TriangleShape(_trianglePoints[0], _trianglePoints[1], _trianglePoints[2], margin);
+			triangleShape.setRaycastTestType(this.heightFieldShape.getRaycastTestType());
+			// Ray casting test against the collision shape
+			final RaycastInfo raycastInfo = new RaycastInfo();
+			final 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;
+			}
+		}
+	};
+	
+	protected int numberColumns; //!< Number of columns in the grid of the height field
+	protected int numberRows; //!< Number of rows in the grid of the height field
+	
+	protected float width; //!< Height field width
+	protected float length; //!< Height field length
+	protected float minHeight; //!< Minimum height of the height field
+	protected float maxHeight; //!< Maximum height of the height field
+	protected int upAxis; //!< Up axis direction (0 => x, 1 => y, 2 => z)
+	protected float[] heightFieldData; //!< Array of data with all the height values of the height field
+	protected AABB AABB; //!< Local AABB of the height field (without scaling)
+	
+	/**
+		 *  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 (long, float, double)
+		 * @param upAxis Integer representing the up axis direction (0 for x, 1 for y and 2 for z)
+		 * @param longegerHeightScale Scaling factor used to scale the height values (only when height values type is longeger)
+		 */
+	public HeightFieldShape(final int _nbGridColumns, final int _nbGridRows, final float _minHeight, final float _maxHeight, final float[] _heightFieldData) {
+		this(_nbGridColumns, _nbGridRows, _minHeight, _maxHeight, _heightFieldData, 1);
+	}
+	
+	/// Insert all the triangles longo the dynamic AABB tree
+	//protected void initBVHTree();
+	
+	public HeightFieldShape(final int _nbGridColumns, final int _nbGridRows, final float _minHeight, final float _maxHeight, final float[] _heightFieldData, final int _upAxis) {
+		super(CollisionShapeType.HEIGHTFIELD);
+		this.numberColumns = _nbGridColumns;
+		this.numberRows = _nbGridRows;
+		this.width = _nbGridColumns - 1;
+		this.length = _nbGridRows - 1;
+		this.minHeight = _minHeight;
+		this.maxHeight = _maxHeight;
+		this.upAxis = _upAxis;
+		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;
+		final 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(new Vector3f(-halfHeight, -this.width * 0.5f, -this.length * 0.5f));
+			this.AABB.setMax(new Vector3f(halfHeight, this.width * 0.5f, this.length * 0.5f));
+		} else if (this.upAxis == 1) {
+			this.AABB.setMin(new Vector3f(-this.width * 0.5f, -halfHeight, -this.length * 0.5f));
+			this.AABB.setMax(new Vector3f(this.width * 0.5f, halfHeight, this.length * 0.5f));
+		} else if (this.upAxis == 2) {
+			this.AABB.setMin(new Vector3f(-this.width * 0.5f, -this.length * 0.5f, -halfHeight));
+			this.AABB.setMax(new Vector3f(this.width * 0.5f, this.length * 0.5f, halfHeight));
+		}
+	}
+	
+	/// Return the closest inside longeger grid value of a given floating grid value
+	protected int computeIntegerGridValue(final float _value) {
+		return (int) ((_value < 0.0f) ? _value - 0.5f : _value + 0.5f);
+	}
+	
+	@Override
+	public void computeLocalInertiaTensor(final Matrix3f _tensor, final 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.set(_mass, 0.0f, 0.0f, 0.0f, _mass, 0.0f, 0.0f, 0.0f, _mass);
+	}
+	
+	/// Compute the min/max grid coords corresponding to the longersection of the AABB of the height field and the AABB to collide
+	protected void computeMinMaxGridCoordinates(final Vector3f _minCoords, final Vector3f _maxCoords, final AABB _aabbToCollide) {
+		// Clamp the min/max coords of the AABB to collide inside the height field AABB
+		Vector3f minPoint = FMath.max(_aabbToCollide.getMin(), this.AABB.getMin());
+		minPoint = FMath.min(minPoint, this.AABB.getMax());
+		Vector3f maxPoint = FMath.min(_aabbToCollide.getMax(), this.AABB.getMax());
+		maxPoint = FMath.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]
+		final Vector3f translateVec = this.AABB.getExtent().multiplyNew(0.5f);
+		minPoint.add(translateVec);
+		maxPoint.add(translateVec);
+		// Convert the floating min/max coords of the AABB longo closest longeger
+		// grid values (note that we use the closest grid coordinate that is out
+		// of the AABB)
+		_minCoords.set(computeIntegerGridValue(minPoint.x) - 1, computeIntegerGridValue(minPoint.y) - 1, computeIntegerGridValue(minPoint.z) - 1);
+		_maxCoords.set(computeIntegerGridValue(maxPoint.x) + 1, computeIntegerGridValue(maxPoint.y) + 1, computeIntegerGridValue(maxPoint.z) + 1);
+	}
+	
+	/// Return the height of a given (x,y) point in the height field
+	protected float getHeightAt(final int _xxx, final int _yyy) {
+		return this.heightFieldData[_yyy * this.numberColumns + _xxx];
+	}
+	
+	@Override
+	public void getLocalBounds(final Vector3f _min, final Vector3f _max) {
+		_min.set(this.AABB.getMin().multiplyNew(this.scaling));
+		_max.set(this.AABB.getMax().multiplyNew(this.scaling));
+	}
+	
+	/// Return the number of columns in the height field
+	public long getNbColumns() {
+		return this.numberColumns;
+	}
+	
+	/// Return the number of rows in the height field
+	public long getNbRows() {
+		return this.numberRows;
+	}
+	
+	/// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
+	/// given the start vertex index pointer of the triangle.
+	/*
+	protected		void getTriangleVerticesWithIndexPointer(final long _subPart,
+		                                         final long _triangleIndex,
+		                                         Vector3f* _outTriangleVertices) ;
+		                                         */
+	/// Return the vertex (local-coordinates) of the height field at a given (x,y) position
+	protected Vector3f getVertexAt(final int _xxx, final int _yyy) {
+		// Get the height value
+		final float height = getHeightAt(_xxx, _yyy);
+		// Height values origin
+		final float heightOrigin = -(this.maxHeight - this.minHeight) * 0.5f - this.minHeight;
+		Vector3f vertex = null;
+		switch (this.upAxis) {
+			case 0:
+				vertex = new Vector3f(heightOrigin + height, -this.width * 0.5f + _xxx, -this.length * 0.5f + _yyy);
+				break;
+			case 1:
+				vertex = new Vector3f(-this.width * 0.5f + _xxx, heightOrigin + height, -this.length * 0.5f + _yyy);
+				break;
+			case 2:
+				vertex = new Vector3f(-this.width * 0.5f + _xxx, -this.length * 0.5f + _yyy, heightOrigin + height);
+				break;
+			default:
+				assert (false);
+		}
+		assert (this.AABB.contains(vertex));
+		return vertex.multiply(this.scaling);
+	}
+	
+	@Override
+	public boolean isConvex() {
+		return false;
+	}
+	
+	@Override
+	public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) {
+		// TODO : Implement raycasting without using an AABB for the ray
+		//		but using a dynamic AABB tree or octree instead
+		
+		final TriangleOverlapCallback triangleCallback = new TriangleOverlapCallback(_ray, _proxyShape, _raycastInfo, this);
+		// Compute the AABB for the ray
+		final Vector3f rayEnd = _ray.point2.lessNew(_ray.point1).multiply(_ray.maxFraction).add(_ray.point1);
+		
+		final AABB rayAABB = new AABB(FMath.min(_ray.point1, rayEnd), FMath.max(_ray.point1, rayEnd));
+		testAllTriangles(triangleCallback, rayAABB);
+		return triangleCallback.getIsHit();
+	}
+	
+	@Override
+	public void setLocalScaling(final Vector3f _scaling) {
+		super.setLocalScaling(_scaling);
+	}
+	
+	@Override
+	public void testAllTriangles(final TriangleCallback _callback, final AABB _localAABB) {
+		
+		// Compute the non-scaled AABB
+		final Vector3f inverseScaling = new Vector3f(1.0f / this.scaling.x, 1.0f / this.scaling.y, 1.0f / this.scaling.z);
+		
+		final AABB aabb = new AABB(_localAABB.getMin().multiplyNew(inverseScaling), _localAABB.getMax().multiplyNew(inverseScaling));
+		// Compute the longeger grid coordinates inside the area we need to test for collision
+		final Vector3f minGridCoords = new Vector3f();
+		final Vector3f maxGridCoords = new Vector3f();
+		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 = FMath.clamp((int) minGridCoords.y, 0, this.numberColumns - 1);
+				iMax = FMath.clamp((int) maxGridCoords.y, 0, this.numberColumns - 1);
+				jMin = FMath.clamp((int) minGridCoords.z, 0, this.numberRows - 1);
+				jMax = FMath.clamp((int) maxGridCoords.z, 0, this.numberRows - 1);
+				break;
+			case 1:
+				iMin = FMath.clamp((int) minGridCoords.x, 0, this.numberColumns - 1);
+				iMax = FMath.clamp((int) maxGridCoords.x, 0, this.numberColumns - 1);
+				jMin = FMath.clamp((int) minGridCoords.z, 0, this.numberRows - 1);
+				jMax = FMath.clamp((int) maxGridCoords.z, 0, this.numberRows - 1);
+				break;
+			case 2:
+				iMin = FMath.clamp((int) minGridCoords.x, 0, this.numberColumns - 1);
+				iMax = FMath.clamp((int) maxGridCoords.x, 0, this.numberColumns - 1);
+				jMin = FMath.clamp((int) minGridCoords.y, 0, this.numberRows - 1);
+				jMax = FMath.clamp((int) maxGridCoords.y, 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
+				final Vector3f p1 = getVertexAt(i, j);
+				final Vector3f p2 = getVertexAt(i, j + 1);
+				final Vector3f p3 = getVertexAt(i + 1, j);
+				final Vector3f p4 = getVertexAt(i + 1, j + 1);
+				// Generate the first triangle for the current grid rectangle
+				final Vector3f[] trianglePoints = { 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);
+			}
+		}
+	}
+	
+}

src/org/atriasoft/ephysics/collision/shapes/SphereShape.java

@@ -0,0 +1,142 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.collision.shapes;
+
+import org.atriasoft.ephysics.RaycastInfo;
+import org.atriasoft.ephysics.collision.ProxyShape;
+import org.atriasoft.ephysics.mathematics.Ray;
+import org.atriasoft.etk.math.Constant;
+import org.atriasoft.etk.math.FMath;
+import org.atriasoft.etk.math.Matrix3f;
+import org.atriasoft.etk.math.Transform3D;
+import org.atriasoft.etk.math.Vector3f;
+
+/**
+ * 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.
+ */
+public class SphereShape extends ConvexShape {
+	/**
+		 *  Constructor
+		 * @param radius Radius of the sphere (in meters)
+		 */
+	public SphereShape(final float _radius) {
+		super(CollisionShapeType.SPHERE, _radius);
+	}
+	
+	@Override
+	public void computeAABB(final AABB _aabb, final Transform3D _transform) {
+		
+		// Get the local extents in x,y and z direction
+		final Vector3f extents = new Vector3f(this.margin, this.margin, this.margin);
+		// Update the AABB with the new minimum and maximum coordinates
+		_aabb.setMin(_transform.getPosition().lessNew(extents));
+		_aabb.setMax(_transform.getPosition().addNew(extents));
+	}
+	
+	@Override
+	public void computeLocalInertiaTensor(final Matrix3f _tensor, final float _mass) {
+		final float diag = 0.4f * _mass * this.margin * this.margin;
+		_tensor.set(diag, 0.0f, 0.0f, 0.0f, diag, 0.0f, 0.0f, 0.0f, diag);
+	}
+	
+	@Override
+	public void getLocalBounds(final Vector3f min, final Vector3f max) {
+		// Maximum bounds
+		max.setX(this.margin);
+		max.setY(this.margin);
+		max.setZ(this.margin);
+		// Minimum bounds
+		min.setX(-max.x);
+		min.setY(-max.y);
+		min.setZ(-max.z);
+	}
+	
+	@Override
+	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) {
+		return new Vector3f(0.0f, 0.0f, 0.0f);
+	}
+	
+	/**
+	 *  Get the radius of the sphere
+	 * @return Radius of the sphere (in meters)
+	 */
+	public float getRadius() {
+		return this.margin;
+	}
+	
+	@Override
+	public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) {
+		final Vector3f m = _ray.point1;
+		final float c = m.dot(m) - this.margin * this.margin;
+		// If the origin of the ray is inside the sphere, we return no longersection
+		if (c < 0.0f) {
+			return false;
+		}
+		final Vector3f rayDirection = _ray.point2.lessNew(_ray.point1);
+		final 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 longersection
+		if (b > 0.0f) {
+			return false;
+		}
+		final float raySquareLength = rayDirection.length2();
+		// Compute the discriminant of the quadratic equation
+		final float discriminant = b * b - raySquareLength * c;
+		// If the discriminant is negative or the ray length is very small, there is no longersection
+		if (discriminant < 0.0f || raySquareLength < Constant.FLOAT_EPSILON) {
+			return false;
+		}
+		// Compute the solution "t" closest to the origin
+		float t = -b - FMath.sqrt(discriminant);
+		assert (t >= 0.0f);
+		// If the hit point is withing the segment ray fraction
+		if (t < _ray.maxFraction * raySquareLength) {
+			// Compute the longersection information
+			t /= raySquareLength;
+			_raycastInfo.body = _proxyShape.getBody();
+			_raycastInfo.proxyShape = _proxyShape;
+			_raycastInfo.hitFraction = t;
+			_raycastInfo.worldPoint = rayDirection.multiplyNew(t).add(_ray.point1);
+			_raycastInfo.worldNormal = _raycastInfo.worldPoint;
+			return true;
+		}
+		return false;
+	}
+	
+	@Override
+	public void setLocalScaling(final Vector3f _scaling) {
+		this.margin = (this.margin / this.scaling.x) * _scaling.x;
+		super.setLocalScaling(_scaling);
+	}
+	
+	@Override
+	public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) {
+		return (_localPoint.length2() < this.margin * this.margin);
+	}
+};

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

@@ -0,0 +1,195 @@
+package org.atriasoft.ephysics.collision.shapes;
+
+import org.atriasoft.etk.math.FMath;
+import org.atriasoft.etk.math.Matrix3f;
+import org.atriasoft.etk.math.Transform3D;
+import org.atriasoft.etk.math.Vector3f;
+
+import org.atriasoft.ephysics.RaycastInfo;
+import org.atriasoft.ephysics.collision.ProxyShape;
+import org.atriasoft.ephysics.configuration.Defaults;
+import org.atriasoft.ephysics.mathematics.Ray;
+
+/**
+ * This class represents a triangle collision shape that is centered
+ * at the origin and defined three points.
+ */
+public class TriangleShape extends ConvexShape {
+	/**
+	 *  Raycast test side for the triangle
+	 */
+	public enum TriangleRaycastSide {
+		FRONT, //!< Raycast against front triangle
+		BACK, //!< Raycast against back triangle
+		FRONT_AND_BACK //!< Raycast against front and back triangle
+	};
+	
+	protected Vector3f[] points = new Vector3f[3]; //!< Three points of the triangle
+	protected TriangleRaycastSide raycastTestType; //!< Raycast test type for the triangle (front, back, front-back)
+	
+	public TriangleShape(final Vector3f _point1, final Vector3f _point2, final Vector3f _point3) {
+		super(CollisionShapeType.TRIANGLE, Defaults.OBJECT_MARGIN);
+		this.points[0] = _point1;
+		this.points[1] = _point2;
+		this.points[2] = _point3;
+		this.raycastTestType = TriangleRaycastSide.FRONT;
+	}
+	
+	/**
+		 *  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
+		 */
+	public TriangleShape(final Vector3f _point1, final Vector3f _point2, final Vector3f _point3, final float _margin) {
+		super(CollisionShapeType.TRIANGLE, _margin);
+		this.points[0] = _point1;
+		this.points[1] = _point2;
+		this.points[2] = _point3;
+		this.raycastTestType = TriangleRaycastSide.FRONT;
+	}
+	
+	@Override
+	public void computeAABB(final AABB aabb, final Transform3D _transform) {
+		final Vector3f worldPoint1 = _transform.multiplyNew(this.points[0]);
+		final Vector3f worldPoint2 = _transform.multiplyNew(this.points[1]);
+		final Vector3f worldPoint3 = _transform.multiplyNew(this.points[2]);
+		final Vector3f xAxis = new Vector3f(worldPoint1.x, worldPoint2.x, worldPoint3.x);
+		final Vector3f yAxis = new Vector3f(worldPoint1.y, worldPoint2.y, worldPoint3.y);
+		final Vector3f zAxis = new Vector3f(worldPoint1.z, worldPoint2.z, worldPoint3.z);
+		aabb.setMin(new Vector3f(xAxis.getMin(), yAxis.getMin(), zAxis.getMin()));
+		aabb.setMax(new Vector3f(xAxis.getMax(), yAxis.getMax(), zAxis.getMax()));
+	}
+	
+	@Override
+	public void computeLocalInertiaTensor(final Matrix3f _tensor, final float _mass) {
+		_tensor.setZero();
+	}
+	
+	@Override
+	public void getLocalBounds(final Vector3f min, final Vector3f max) {
+		final Vector3f xAxis = new Vector3f(this.points[0].x, this.points[1].x, this.points[2].x);
+		final Vector3f yAxis = new Vector3f(this.points[0].y, this.points[1].y, this.points[2].y);
+		final Vector3f zAxis = new Vector3f(this.points[0].z, this.points[1].z, this.points[2].z);
+		min.setValue(xAxis.getMin() - this.margin, yAxis.getMin() - this.margin, zAxis.getMin() - this.margin);
+		max.setValue(xAxis.getMax() + this.margin, yAxis.getMax() + this.margin, zAxis.getMax() + this.margin);
+	}
+	
+	@Override
+	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) {
+		final Vector3f dotProducts = new Vector3f(_direction.dot(this.points[0]), _direction.dot(this.points[1]), _direction.dot(this.points[2]));
+		return this.points[dotProducts.getMaxAxis()];
+	}
+	
+	/// Return the raycast test type (front, back, front-back)
+	public TriangleRaycastSide getRaycastTestType() {
+		return this.raycastTestType;
+	}
+	
+	/**
+	 *  Return the coordinates of a given vertex of the triangle
+	 * @param _index Index (0 to 2) of a vertex of the triangle
+	 */
+	public Vector3f getVertex(final int _index) {
+		assert (_index >= 0 && _index < 3);
+		return this.points[_index];
+	}
+	
+	@Override
+	public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) {
+		final Vector3f pq = _ray.point2.lessNew(_ray.point1);
+		final Vector3f pa = this.points[0].lessNew(_ray.point1);
+		final Vector3f pb = this.points[1].lessNew(_ray.point1);
+		final Vector3f pc = this.points[2].lessNew(_ray.point1);
+		// Test if the line PQ is inside the eges BC, CA and AB. We use the triple
+		// product for this test.
+		final Vector3f m = pq.cross(pc);
+		float u = pb.dot(m);
+		if (this.raycastTestType == TriangleRaycastSide.FRONT) {
+			if (u < 0.0f) {
+				return false;
+			}
+		} else if (this.raycastTestType == TriangleRaycastSide.BACK) {
+			if (u > 0.0f) {
+				return false;
+			}
+		}
+		float v = -pa.dot(m);
+		if (this.raycastTestType == TriangleRaycastSide.FRONT) {
+			if (v < 0.0f) {
+				return false;
+			}
+		} else if (this.raycastTestType == TriangleRaycastSide.BACK) {
+			if (v > 0.0f) {
+				return false;
+			}
+		} else if (this.raycastTestType == TriangleRaycastSide.FRONT_AND_BACK) {
+			if (!FMath.sameSign(u, v)) {
+				return false;
+			}
+		}
+		float w = pa.dot(pq.cross(pb));
+		if (this.raycastTestType == TriangleRaycastSide.FRONT) {
+			if (w < 0.0f) {
+				return false;
+			}
+		} else if (this.raycastTestType == TriangleRaycastSide.BACK) {
+			if (w > 0.0f) {
+				return false;
+			}
+		} else if (this.raycastTestType == TriangleRaycastSide.FRONT_AND_BACK) {
+			if (!FMath.sameSign(u, w)) {
+				return false;
+			}
+		}
+		// If the line PQ is in the triangle plane (case where u=v=w=0)
+		if (FMath.approxEqual(u, 0.0f) && FMath.approxEqual(v, 0) && FMath.approxEqual(w, 0)) {
+			return false;
+		}
+		// Compute the barycentric coordinates (u, v, w) to determine the
+		// longersection point R, R = u * a + v * b + w * c
+		final float denom = 1.0f / (u + v + w);
+		u *= denom;
+		v *= denom;
+		w *= denom;
+		// Compute the local hit point using the barycentric coordinates
+		final Vector3f localHitPoint = this.points[0].multiplyNew(u).add(this.points[1].multiplyNew(v)).add(this.points[2].multiplyNew(w));
+		final float hitFraction = localHitPoint.lessNew(_ray.point1).length() / pq.length();
+		if (hitFraction < 0.0f || hitFraction > _ray.maxFraction) {
+			return false;
+		}
+		final Vector3f localHitNormal = this.points[1].lessNew(this.points[0]).cross(this.points[2].lessNew(this.points[0]));
+		if (localHitNormal.dot(pq) > 0.0f) {
+			localHitNormal.less(localHitNormal);
+		}
+		_raycastInfo.body = _proxyShape.getBody();
+		_raycastInfo.proxyShape = _proxyShape;
+		_raycastInfo.worldPoint = localHitPoint;
+		_raycastInfo.hitFraction = hitFraction;
+		_raycastInfo.worldNormal = localHitNormal;
+		return true;
+	}
+	
+	@Override
+	public void setLocalScaling(final Vector3f _scaling) {
+		this.points[0].divide(this.scaling).multiply(_scaling);
+		this.points[1].divide(this.scaling).multiply(_scaling);
+		this.points[2].divide(this.scaling).multiply(_scaling);
+		super.setLocalScaling(_scaling);
+	}
+	
+	/**
+	 *  Set the raycast test type (front, back, front-back)
+	 * @param _testType Raycast test type for the triangle (front, back, front-back)
+	 */
+	public void setRaycastTestType(final TriangleRaycastSide _testType) {
+		this.raycastTestType = _testType;
+	}
+	
+	@Override
+	public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) {
+		return false;
+	}
+	
+};

src/org/atriasoft/ephysics/configuration/Defaults.java

@@ -0,0 +1,86 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.configuration;
+
+import org.atriasoft.etk.math.Constant;
+
+/**
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class Defaults {
+	
+	// Smallest decimal value (negative)
+	public static final float DECIMAL_SMALLEST = Float.MIN_VALUE;
+	
+	// Maximum decimal value
+	public static final float DECIMAL_LARGEST = Float.MAX_VALUE;
+	
+	// Default internal constant timestep in seconds
+	public static final float DEFAULT_TIMESTEP = 1.0f / 60.0f;
+	
+	// Default friction coefficient for a rigid body
+	public static final float DEFAULT_FRICTION_COEFFICIENT = 0.3f;
+	
+	// Default bounciness factor for a rigid body
+	public static final float DEFAULT_BOUNCINESS = 0.5f;
+	
+	// True if the spleeping technique is enabled
+	public static final boolean SPLEEPING_ENABLED = true;
+	
+	// Object margin for collision detection in meters (for the GJK-EPA Algorithm)
+	public static final float OBJECT_MARGIN = 0.04f;
+	
+	// Distance threshold for two contact points for a valid persistent contact (in meters)
+	public static final float PERSISTENT_CONTACT_DIST_THRESHOLD = 0.03f;
+	
+	// Velocity threshold for contact velocity restitution
+	public static final float RESTITUTION_VELOCITY_THRESHOLD = 1.0f;
+	
+	// Number of iterations when solving the velocity constraints of the Sequential Impulse technique
+	public static final int DEFAULT_VELOCITY_SOLVER_NUM_ITERATIONS = 10;
+	
+	// Number of iterations when solving the position constraints of the Sequential Impulse technique
+	public static final int DEFAULT_POSITION_SOLVER_NUM_ITERATIONS = 5;
+	
+	// Time (in seconds) that a body must stay still to be considered sleeping
+	public static final float DEFAULT_TIME_BEFORE_SLEEP = 1.0f;
+	
+	// A body with a linear velocity smaller than the sleep linear velocity (in m/s)
+	// might enter sleeping mode.
+	public static final float DEFAULT_SLEEP_LINEAR_VELOCITY = 0.02f;
+	
+	// A body with angular velocity smaller than the sleep angular velocity (in rad/s)
+	// might enter sleeping mode
+	public static final float DEFAULT_SLEEP_ANGULAR_VELOCITY = 3.0f * (Constant.PI / 180.0f);
+	
+	/// Maximum number of contact manifolds in an overlapping pair that involves two
+	/// convex collision shapes.
+	public static final int NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE = 1;
+	
+	/// Maximum number of contact manifolds in an overlapping pair that involves at
+	/// least one concave collision shape.
+	public static final int NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3;
+}

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

@@ -0,0 +1,215 @@
+package org.atriasoft.ephysics.constraint;
+
+import org.atriasoft.etk.math.Matrix3f;
+import org.atriasoft.etk.math.Quaternion;
+import org.atriasoft.etk.math.Vector3f;
+
+import org.atriasoft.ephysics.body.BodyType;
+import org.atriasoft.ephysics.engine.ConstraintSolverData;
+
+public class BallAndSocketJoint extends Joint {
+	
+	private static float BETA = 0.2f; //!< Beta value for the bias factor of position correction
+	private Vector3f localAnchorPointBody1 = new Vector3f(0, 0, 0); //!< Anchor point of body 1 (in local-space coordinates of body 1)
+	private Vector3f localAnchorPointBody2 = new Vector3f(0, 0, 0); //!< Anchor point of body 2 (in local-space coordinates of body 2)
+	private Vector3f r1World = new Vector3f(0, 0, 0); //!< Vector from center of body 2 to anchor point in world-space
+	private Vector3f r2World = new Vector3f(0, 0, 0); //!< Vector from center of body 2 to anchor point in world-space
+	private Matrix3f i1 = new Matrix3f(); //!< Inertia tensor of body 1 (in world-space coordinates)
+	private Matrix3f i2 = new Matrix3f(); //!< Inertia tensor of body 2 (in world-space coordinates)
+	private Vector3f biasVector = new Vector3f(0, 0, 0); //!< Bias vector for the raint
+	private Matrix3f inverseMassMatrix = new Matrix3f(); //!< Inverse mass matrix K=JM^-1J^-t of the raint
+	private final Vector3f impulse = new Vector3f(0, 0, 0); //!< Accumulated impulse
+	/// Constructor
+	
+	public BallAndSocketJoint(final BallAndSocketJointInfo jointInfo) {
+		super(jointInfo);
+		// Compute the local-space anchor point for each body
+		this.localAnchorPointBody1 = this.body1.getTransform().inverseNew().multiply(jointInfo.anchorPointWorldSpace);
+		this.localAnchorPointBody2 = this.body2.getTransform().inverseNew().multiply(jointInfo.anchorPointWorldSpace);
+	}
+	
+	@Override
+	public void initBeforeSolve(final ConstraintSolverData raintSolverData) {
+		
+		// Initialize the bodies index in the velocity array
+		this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.get(this.body1);
+		this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.get(this.body2);
+		
+		// Get the bodies center of mass and orientations
+		final Vector3f x1 = this.body1.centerOfMassWorld;
+		final Vector3f x2 = this.body2.centerOfMassWorld;
+		final Quaternion orientationBody1 = this.body1.getTransform().getOrientation();
+		final 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.multiply(this.localAnchorPointBody1);
+		this.r2World = orientationBody2.multiply(this.localAnchorPointBody2);
+		
+		// Compute the corresponding skew-symmetric matrices
+		final Matrix3f skewSymmetricMatrixU1 = Matrix3f.computeSkewSymmetricMatrixForCrossProduct(this.r1World);
+		final Matrix3f skewSymmetricMatrixU2 = Matrix3f.computeSkewSymmetricMatrixForCrossProduct(this.r2World);
+		
+		// Compute the matrix K=JM^-1J^t (3x3 matrix)
+		final float inverseMassBodies = this.body1.massInverse + this.body2.massInverse;
+		final Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies);
+		massMatrix.add(skewSymmetricMatrixU1.multiplyNew(this.i1).multiply(skewSymmetricMatrixU1.transposeNew()));
+		massMatrix.add(skewSymmetricMatrixU2.multiplyNew(this.i2).multiply(skewSymmetricMatrixU2.transposeNew()));
+		
+		// Compute the inverse mass matrix K^-1
+		this.inverseMassMatrix.setZero();
+		if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) {
+			this.inverseMassMatrix = massMatrix.inverseNew();
+		}
+		
+		// Compute the bias "b" of the raint
+		this.biasVector.setZero();
+		if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) {
+			final float biasFactor = (BETA / raintSolverData.timeStep);
+			this.biasVector = (x2.addNew(this.r2World).less(x1).less(this.r1World)).multiply(biasFactor);
+		}
+		
+		// If warm-starting is not enabled
+		if (!raintSolverData.isWarmStartingActive) {
+			
+			// Reset the accumulated impulse
+			this.impulse.setZero();
+		}
+	}
+	
+	@Override
+	public void solvePositionConstraint(final ConstraintSolverData raintSolverData) {
+		
+		// Get the velocities
+		final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+		final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+		final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+		final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+		
+		// Compute J*v
+		final Vector3f Jv = v2.addNew(w2.cross(this.r2World)).less(v1).less(w1.cross(this.r1World));
+		
+		// Compute the Lagrange multiplier lambda
+		final Vector3f deltaLambda = this.inverseMassMatrix.multiplyNew(Jv.multiplyNew(-1).less(this.biasVector));
+		this.impulse.add(deltaLambda);
+		
+		// Compute the impulse P=J^T * lambda for the body 1
+		final Vector3f linearImpulseBody1 = deltaLambda.multiplyNew(-1);
+		final Vector3f angularImpulseBody1 = deltaLambda.cross(this.r1World);
+		
+		// Apply the impulse to the body 1
+		v1.add(linearImpulseBody1.multiplyNew(this.body1.massInverse));
+		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		
+		// Compute the impulse P=J^T * lambda for the body 2
+		final Vector3f angularImpulseBody2 = deltaLambda.cross(this.r2World).multiplyNew(-1);
+		
+		// Apply the impulse to the body 2
+		v2.add(deltaLambda.multiplyNew(this.body2.massInverse));
+		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+	}
+	
+	@Override
+	public void solveVelocityConstraint(final 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 != JointsPositionCorrectionTechnique.NON_LINEAR_GAUSS_SEIDEL) {
+			return;
+		}
+		
+		// Get the bodies center of mass and orientations
+		final Vector3f x1 = raintSolverData.positions[this.indexBody1];
+		final Vector3f x2 = raintSolverData.positions[this.indexBody2];
+		final Quaternion q1 = raintSolverData.orientations[this.indexBody1];
+		final Quaternion q2 = raintSolverData.orientations[this.indexBody2];
+		
+		// Get the inverse mass and inverse inertia tensors of the bodies
+		final float inverseMassBody1 = this.body1.massInverse;
+		final float inverseMassBody2 = this.body2.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.multiply(this.localAnchorPointBody1);
+		this.r2World = q2.multiply(this.localAnchorPointBody2);
+		
+		// Compute the corresponding skew-symmetric matrices
+		final Matrix3f skewSymmetricMatrixU1 = Matrix3f.computeSkewSymmetricMatrixForCrossProduct(this.r1World);
+		final Matrix3f skewSymmetricMatrixU2 = Matrix3f.computeSkewSymmetricMatrixForCrossProduct(this.r2World);
+		
+		// Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation raints
+		final float inverseMassBodies = inverseMassBody1 + inverseMassBody2;
+		final Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies);
+		massMatrix.add(skewSymmetricMatrixU1.multiplyNew(this.i1).multiplyNew(skewSymmetricMatrixU1.transposeNew()));
+		massMatrix.add(skewSymmetricMatrixU2.multiplyNew(this.i2).multiplyNew(skewSymmetricMatrixU2.transposeNew()));
+		this.inverseMassMatrix.setZero();
+		if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) {
+			this.inverseMassMatrix = massMatrix.inverseNew();
+		}
+		
+		// Compute the raint error (value of the C(x) function)
+		final Vector3f raintError = x2.addNew(this.r2World).less(x1).less(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.
+		final Vector3f lambda = this.inverseMassMatrix.multiplyNew(raintError.multiplyNew(-1));
+		
+		// Compute the impulse of body 1
+		final Vector3f linearImpulseBody1 = lambda.multiplyNew(-1);
+		final Vector3f angularImpulseBody1 = lambda.cross(this.r1World);
+		
+		// Compute the pseudo velocity of body 1
+		final Vector3f v1 = linearImpulseBody1.multiplyNew(inverseMassBody1);
+		final Vector3f w1 = this.i1.multiplyNew(angularImpulseBody1);
+		
+		// Update the body center of mass and orientation of body 1
+		x1.add(v1);
+		q1.add((new Quaternion(0, w1)).multiplyNew(q1).multiplyNew(0.5f));
+		q1.normalize();
+		
+		// Compute the impulse of body 2
+		final Vector3f angularImpulseBody2 = lambda.cross(this.r2World).multiplyNew(-1);
+		
+		// Compute the pseudo velocity of body 2
+		final Vector3f v2 = lambda.multiplyNew(inverseMassBody2);
+		final Vector3f w2 = this.i2.multiplyNew(angularImpulseBody2);
+		
+		// Update the body position/orientation of body 2
+		x2.add(v2);
+		q2.add((new Quaternion(0, w2)).multiplyNew(q2).multiply(0.5f));
+		q2.normalize();
+	}
+	
+	@Override
+	public void warmstart(final ConstraintSolverData raintSolverData) {
+		
+		// Get the velocities
+		final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+		final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+		final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+		final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+		
+		// Compute the impulse P=J^T * lambda for the body 1
+		final Vector3f linearImpulseBody1 = this.impulse.multiplyNew(-1);
+		final Vector3f angularImpulseBody1 = this.impulse.cross(this.r1World);
+		
+		// Apply the impulse to the body 1
+		v1.add(linearImpulseBody1.multiplyNew(this.body1.massInverse));
+		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		
+		// Compute the impulse P=J^T * lambda for the body 2
+		final Vector3f angularImpulseBody2 = this.impulse.cross(this.r2World).multiplyNew(-1);
+		
+		// Apply the impulse to the body to the body 2
+		v2.add(this.impulse.multiplyNew(this.body2.massInverse));
+		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+	}
+	
+}

src/org/atriasoft/ephysics/constraint/BallAndSocketJointInfo.java

@@ -0,0 +1,26 @@
+package org.atriasoft.ephysics.constraint;
+
+import org.atriasoft.etk.math.Vector3f;
+
+import org.atriasoft.ephysics.body.RigidBody;
+
+/**
+ *  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.
+ */
+public class BallAndSocketJointInfo extends JointInfo {
+	
+	public Vector3f anchorPointWorldSpace; //!< Anchor point (in world-space coordinates)
+	
+	/**
+	 * 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
+	 */
+	public BallAndSocketJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace) {
+		super(_rigidBody1, _rigidBody2, JointType.BALLSOCKETJOINT);
+		this.anchorPointWorldSpace = _initAnchorPointWorldSpace;
+		
+	}
+}

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

@@ -0,0 +1,174 @@
+package org.atriasoft.ephysics.constraint;
+
+import org.atriasoft.etk.math.Vector3f;
+
+import org.atriasoft.ephysics.body.CollisionBody;
+
+/**
+ *  This class represents a collision contact point between two
+ * bodies in the physics engine.
+ */
+public class ContactPoint {
+	
+	private final CollisionBody body1; //!< First rigid body of the contact
+	private final CollisionBody body2; //!< Second rigid body of the contact
+	private final Vector3f normal; //!< Normalized normal vector of the contact (from body1 toward body2) in world space
+	
+	private float penetrationDepth; //!< Penetration depth
+	private final Vector3f localPointOnBody1; //!< Contact point on body 1 in local space of body 1
+	private final Vector3f localPointOnBody2; //!< Contact point on body 2 in local space of body 2
+	private Vector3f worldPointOnBody1; //!< Contact point on body 1 in world space
+	private Vector3f worldPointOnBody2; //!< Contact point on body 2 in world space
+	private boolean isRestingContact; //!< True if the contact is a resting contact (exists for more than one time step)
+	private Vector3f frictionVectors_1; //!< Two orthogonal vectors that span the tangential friction plane
+	private Vector3f frictionVectors_2; //!< Two orthogonal vectors that span the tangential friction plane
+	private float penetrationImpulse; //!< Cached penetration impulse
+	private float frictionImpulse1; //!< Cached first friction impulse
+	private float frictionImpulse2; //!< Cached second friction impulse
+	private Vector3f rollingResistanceImpulse; //!< Cached rolling resistance impulse
+	/// Constructor
+	
+	public ContactPoint(final ContactPointInfo contactInfo) {
+		this.body1 = contactInfo.shape1.getBody();
+		this.body2 = contactInfo.shape2.getBody();
+		this.normal = contactInfo.normal.clone();
+		this.penetrationDepth = contactInfo.penetrationDepth;
+		this.localPointOnBody1 = contactInfo.localPoint1.clone();
+		this.localPointOnBody2 = contactInfo.localPoint2.clone();
+		this.worldPointOnBody1 = contactInfo.shape1.getBody().getTransform().multiplyNew(contactInfo.shape1.getLocalToBodyTransform().multiply(contactInfo.localPoint1));
+		this.worldPointOnBody2 = contactInfo.shape2.getBody().getTransform().multiplyNew(contactInfo.shape2.getLocalToBodyTransform().multiply(contactInfo.localPoint2));
+		this.isRestingContact = false;
+		
+		this.frictionVectors_1 = new Vector3f(0, 0, 0);
+		this.frictionVectors_2 = new Vector3f(0, 0, 0);
+		
+		assert (this.penetrationDepth >= 0.0f);
+		
+	}
+	
+	/// Return the reference to the body 1
+	public CollisionBody getBody1() {
+		return this.body1;
+	}
+	
+	/// Return the reference to the body 2
+	public CollisionBody getBody2() {
+		return this.body2;
+	}
+	
+	/// Return the cached first friction impulse
+	public float getFrictionImpulse1() {
+		return this.frictionImpulse1;
+	}
+	
+	/// Return the cached second friction impulse
+	public float getFrictionImpulse2() {
+		return this.frictionImpulse2;
+	}
+	
+	/// Get the second friction vector
+	public Vector3f getFrictionvec2() {
+		return this.frictionVectors_2;
+	}
+	
+	/// Get the first friction vector
+	public Vector3f getFrictionVector1() {
+		return this.frictionVectors_1;
+	}
+	
+	/// Return true if the contact is a resting contact
+	public boolean getIsRestingContact() {
+		return this.isRestingContact;
+	}
+	
+	/// Return the contact local point on body 1
+	public Vector3f getLocalPointOnBody1() {
+		return this.localPointOnBody1;
+	}
+	
+	/// Return the contact local point on body 2
+	public Vector3f getLocalPointOnBody2() {
+		return this.localPointOnBody2;
+	}
+	
+	/// Return the normal vector of the contact
+	public Vector3f getNormal() {
+		return this.normal;
+	}
+	
+	/// Return the penetration depth
+	public float getPenetrationDepth() {
+		return this.penetrationDepth;
+	}
+	
+	/// Return the cached penetration impulse
+	public float getPenetrationImpulse() {
+		return this.penetrationImpulse;
+	}
+	
+	/// Return the cached rolling resistance impulse
+	public Vector3f getRollingResistanceImpulse() {
+		return this.rollingResistanceImpulse;
+	}
+	
+	/// Return the contact world point on body 1
+	public Vector3f getWorldPointOnBody1() {
+		return this.worldPointOnBody1;
+	}
+	
+	/// Return the contact world point on body 2
+	public Vector3f getWorldPointOnBody2() {
+		return this.worldPointOnBody2;
+	}
+	
+	/// Set the first cached friction impulse
+	public void setFrictionImpulse1(final float impulse) {
+		this.frictionImpulse1 = impulse;
+	}
+	
+	/// Set the second cached friction impulse
+	public void setFrictionImpulse2(final float impulse) {
+		this.frictionImpulse2 = impulse;
+	}
+	
+	/// Set the second friction vector
+	public void setFrictionvec2(final Vector3f frictionvec2) {
+		this.frictionVectors_2 = frictionvec2;
+	}
+	
+	/// Set the first friction vector
+	public void setFrictionVector1(final Vector3f frictionVector1) {
+		this.frictionVectors_1 = frictionVector1;
+	}
+	
+	/// Set the this.isRestingContact variable
+	public void setIsRestingContact(final boolean isRestingContact) {
+		this.isRestingContact = isRestingContact;
+	}
+	
+	/// Set the penetration depth of the contact
+	public void setPenetrationDepth(final float penetrationDepth) {
+		this.penetrationDepth = penetrationDepth;
+	}
+	
+	/// Set the cached penetration impulse
+	public void setPenetrationImpulse(final float impulse) {
+		this.penetrationImpulse = impulse;
+	}
+	
+	/// Set the cached rolling resistance impulse
+	public void setRollingResistanceImpulse(final Vector3f impulse) {
+		this.rollingResistanceImpulse = impulse;
+	}
+	
+	/// Set the contact world point on body 1
+	public void setWorldPointOnBody1(final Vector3f worldPoint) {
+		this.worldPointOnBody1 = worldPoint;
+	}
+	
+	/// Set the contact world point on body 2
+	public void setWorldPointOnBody2(final Vector3f worldPoint) {
+		this.worldPointOnBody2 = worldPoint;
+	}
+	
+}

src/org/atriasoft/ephysics/constraint/ContactPointInfo.java

@@ -0,0 +1,54 @@
+package org.atriasoft.ephysics.constraint;
+
+import org.atriasoft.etk.math.Vector3f;
+
+import org.atriasoft.ephysics.collision.ProxyShape;
+import org.atriasoft.ephysics.collision.shapes.CollisionShape;
+
+/**
+ *  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.
+ */
+public class ContactPointInfo {
+	public ProxyShape shape1; //!< First proxy shape of the contact
+	public ProxyShape shape2; //!< Second proxy shape of the contact
+	public CollisionShape collisionShape1; //!< First collision shape
+	public CollisionShape collisionShape2; //!< Second collision shape
+	public Vector3f normal; //!< Normalized normal vector of the collision contact in world space
+	public float penetrationDepth; //!< Penetration depth of the contact
+	public Vector3f localPoint1; //!< Contact point of body 1 in local space of body 1
+	public Vector3f localPoint2; //!< Contact point of body 2 in local space of body 2
+	
+	public ContactPointInfo() {
+		this.shape1 = null;
+		this.shape2 = null;
+		this.collisionShape1 = null;
+		this.collisionShape2 = null;
+	}
+	
+	public ContactPointInfo(final ContactPointInfo obj) {
+		this.shape1 = obj.shape1;
+		this.shape2 = obj.shape2;
+		this.collisionShape1 = obj.collisionShape1;
+		this.collisionShape2 = obj.collisionShape2;
+		this.normal = obj.normal;
+		this.penetrationDepth = obj.penetrationDepth;
+		this.localPoint1 = obj.localPoint1;
+		this.localPoint2 = obj.localPoint2;
+	}
+	
+	public ContactPointInfo(final ProxyShape _proxyShape1, final ProxyShape _proxyShape2, final CollisionShape _collShape1, final CollisionShape _collShape2, final Vector3f _normal,
+			final float _penetrationDepth, final Vector3f _localPoint1, final Vector3f _localPoint2) {
+		this.shape1 = _proxyShape1;
+		this.shape2 = _proxyShape2;
+		this.collisionShape1 = _collShape1;
+		this.collisionShape2 = _collShape2;
+		this.normal = _normal;
+		this.penetrationDepth = _penetrationDepth;
+		this.localPoint1 = _localPoint1;
+		this.localPoint2 = _localPoint2;
+		
+	}
+};

src/org/atriasoft/ephysics/constraint/ContactsPositionCorrectionTechnique.java

@@ -0,0 +1,12 @@
+package org.atriasoft.ephysics.constraint;
+
+/// Position correction technique used in the contact solver (for contacts)
+/// BAUMGARTE_CONTACTS : Faster but can be innacurate and can lead to unexpected bounciness
+///                      in some situations (due to error correction factor being added to
+///                      the bodies momentum).
+/// SPLIT_IMPULSES : A bit slower but the error correction factor is not added to the
+///                 bodies momentum. This is the option used by default.
+public enum ContactsPositionCorrectionTechnique {
+	BAUMGARTE_CONTACTS,
+	SPLIT_IMPULSES
+}

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

@@ -0,0 +1,313 @@
+package org.atriasoft.ephysics.constraint;
+
+import org.atriasoft.etk.math.Matrix3f;
+import org.atriasoft.etk.math.Quaternion;
+import org.atriasoft.etk.math.Transform3D;
+import org.atriasoft.etk.math.Vector3f;
+
+import org.atriasoft.ephysics.body.BodyType;
+import org.atriasoft.ephysics.engine.ConstraintSolverData;
+
+public class FixedJoint extends Joint {
+	
+	protected static float BETA = 0.2f; //!< Beta value for the bias factor of position correction
+	protected Vector3f localAnchorPointBody1 = new Vector3f(0, 0, 0); //!< Anchor point of body 1 (in local-space coordinates of body 1)
+	protected Vector3f localAnchorPointBody2 = new Vector3f(0, 0, 0); //!< Anchor point of body 2 (in local-space coordinates of body 2)
+	protected Vector3f r1World = new Vector3f(0, 0, 0); //!< Vector from center of body 2 to anchor point in world-space
+	protected Vector3f r2World = new Vector3f(0, 0, 0); //!< Vector from center of body 2 to anchor point in world-space
+	protected Matrix3f i1 = new Matrix3f(); //!< Inertia tensor of body 1 (in world-space coordinates)
+	protected Matrix3f i2 = new Matrix3f(); //!< Inertia tensor of body 2 (in world-space coordinates)
+	protected Vector3f impulseTranslation = new Vector3f(0, 0, 0); //!< Accumulated impulse for the 3 translation raints
+	protected Vector3f impulseRotation = new Vector3f(0, 0, 0); //!< Accumulate impulse for the 3 rotation raints
+	protected Matrix3f inverseMassMatrixTranslation = new Matrix3f(); //!< Inverse mass matrix K=JM^-1J^-t of the 3 translation raints (3x3 matrix)
+	protected Matrix3f inverseMassMatrixRotation = new Matrix3f(); //!< Inverse mass matrix K=JM^-1J^-t of the 3 rotation raints (3x3 matrix)
+	protected Vector3f biasTranslation = new Vector3f(0, 0, 0); //!< Bias vector for the 3 translation raints
+	protected Vector3f biasRotation = new Vector3f(0, 0, 0); //!< Bias vector for the 3 rotation raints
+	protected Quaternion initOrientationDifferenceInv = new Quaternion(); //!< Inverse of the initial orientation difference between the two bodies
+	
+	/// Constructor
+	public FixedJoint(final FixedJointInfo jointInfo) {
+		super(jointInfo);
+		// Compute the local-space anchor point for each body
+		final Transform3D transform1 = this.body1.getTransform();
+		final Transform3D transform2 = this.body2.getTransform();
+		this.localAnchorPointBody1 = transform1.inverseNew().multiply(jointInfo.anchorPointWorldSpace);
+		this.localAnchorPointBody2 = transform2.inverseNew().multiply(jointInfo.anchorPointWorldSpace);
+		
+		// Compute the inverse of the initial orientation difference between the two bodies
+		this.initOrientationDifferenceInv = transform2.getOrientation().multiplyNew(transform1.getOrientation().inverseNew());
+		this.initOrientationDifferenceInv.normalize();
+		this.initOrientationDifferenceInv.inverse();
+	}
+	
+	@Override
+	public void initBeforeSolve(final ConstraintSolverData raintSolverData) {
+		
+		// Initialize the bodies index in the velocity array
+		this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.get(this.body1);
+		this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.get(this.body2);
+		
+		// Get the bodies positions and orientations
+		final Vector3f x1 = this.body1.centerOfMassWorld;
+		final Vector3f x2 = this.body2.centerOfMassWorld;
+		final Quaternion orientationBody1 = this.body1.getTransform().getOrientation();
+		final 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.multiply(this.localAnchorPointBody1);
+		this.r2World = orientationBody2.multiply(this.localAnchorPointBody2);
+		
+		// Compute the corresponding skew-symmetric matrices
+		final Matrix3f skewSymmetricMatrixU1 = Matrix3f.computeSkewSymmetricMatrixForCrossProduct(this.r1World);
+		final Matrix3f skewSymmetricMatrixU2 = Matrix3f.computeSkewSymmetricMatrixForCrossProduct(this.r2World);
+		
+		// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints
+		final float inverseMassBodies = this.body1.massInverse + this.body2.massInverse;
+		final Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies);
+		massMatrix.add(skewSymmetricMatrixU1.multiplyNew(this.i1).multiply(skewSymmetricMatrixU1.transposeNew()));
+		massMatrix.add(skewSymmetricMatrixU2.multiplyNew(this.i2).multiply(skewSymmetricMatrixU2.transposeNew()));
+		
+		// Compute the inverse mass matrix K^-1 for the 3 translation raints
+		this.inverseMassMatrixTranslation.setZero();
+		if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) {
+			this.inverseMassMatrixTranslation = massMatrix.inverseNew();
+		}
+		
+		// Compute the bias "b" of the raint for the 3 translation raints
+		final float biasFactor = (BETA / raintSolverData.timeStep);
+		this.biasTranslation.setZero();
+		if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) {
+			this.biasTranslation = x2.addNew(this.r2World).less(x1).less(this.r1World).multiply(biasFactor);
+		}
+		
+		// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
+		// contraints (3x3 matrix)
+		this.inverseMassMatrixRotation = this.i1.addNew(this.i2);
+		if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) {
+			this.inverseMassMatrixRotation = this.inverseMassMatrixRotation.inverseNew();
+		}
+		
+		// Compute the bias "b" for the 3 rotation raints
+		this.biasRotation.setZero();
+		if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) {
+			final Quaternion currentOrientationDifference = orientationBody2.multiplyNew(orientationBody1.inverseNew());
+			currentOrientationDifference.normalize();
+			final Quaternion qError = currentOrientationDifference.multiplyNew(this.initOrientationDifferenceInv);
+			this.biasRotation = qError.getVectorV().multiply(biasFactor * 2.0f);
+		}
+		
+		// If warm-starting is not enabled
+		if (!raintSolverData.isWarmStartingActive) {
+			// Reset the accumulated impulses
+			this.impulseTranslation.setZero();
+			this.impulseRotation.setZero();
+		}
+	}
+	
+	@Override
+	public void solvePositionConstraint(final 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 != JointsPositionCorrectionTechnique.NON_LINEAR_GAUSS_SEIDEL) {
+			return;
+		}
+		
+		// Get the bodies positions and orientations
+		final Vector3f x1 = raintSolverData.positions[this.indexBody1];
+		final Vector3f x2 = raintSolverData.positions[this.indexBody2];
+		final Quaternion q1 = raintSolverData.orientations[this.indexBody1];
+		final Quaternion q2 = raintSolverData.orientations[this.indexBody2];
+		
+		// Get the inverse mass and inverse inertia tensors of the bodies
+		final float inverseMassBody1 = this.body1.massInverse;
+		final float inverseMassBody2 = this.body2.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.multiply(this.localAnchorPointBody1);
+		this.r2World = q2.multiply(this.localAnchorPointBody2);
+		
+		// Compute the corresponding skew-symmetric matrices
+		final Matrix3f skewSymmetricMatrixU1 = Matrix3f.computeSkewSymmetricMatrixForCrossProduct(this.r1World);
+		final Matrix3f skewSymmetricMatrixU2 = Matrix3f.computeSkewSymmetricMatrixForCrossProduct(this.r2World);
+		
+		// --------------- Translation Constraints --------------- //
+		
+		// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints
+		final float inverseMassBodies = this.body1.massInverse + this.body2.massInverse;
+		final Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies);
+		massMatrix.add(skewSymmetricMatrixU1.multiplyNew(this.i1).add(skewSymmetricMatrixU1.transposeNew()));
+		massMatrix.add(skewSymmetricMatrixU2.multiplyNew(this.i2).add(skewSymmetricMatrixU2.transposeNew()));
+		this.inverseMassMatrixTranslation.setZero();
+		if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) {
+			this.inverseMassMatrixTranslation = massMatrix.inverseNew();
+		}
+		
+		// Compute position error for the 3 translation raints
+		final Vector3f errorTranslation = x2.addNew(this.r2World).less(x1).less(this.r1World);
+		
+		// Compute the Lagrange multiplier lambda
+		final Vector3f lambdaTranslation = this.inverseMassMatrixTranslation.multiplyNew(errorTranslation.multiplyNew(-1));
+		
+		// Compute the impulse of body 1
+		final Vector3f linearImpulseBody1 = lambdaTranslation.multiplyNew(-1);
+		Vector3f angularImpulseBody1 = lambdaTranslation.cross(this.r1World);
+		
+		// Compute the pseudo velocity of body 1
+		final Vector3f v1 = linearImpulseBody1.multiplyNew(inverseMassBody1);
+		Vector3f w1 = this.i1.multiplyNew(angularImpulseBody1);
+		
+		// Update the body position/orientation of body 1
+		x1.add(v1);
+		q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
+		q1.normalize();
+		
+		// Compute the impulse of body 2
+		final Vector3f angularImpulseBody2 = lambdaTranslation.cross(this.r2World).multiply(-1);
+		
+		// Compute the pseudo velocity of body 2
+		final Vector3f v2 = lambdaTranslation.multiplyNew(inverseMassBody2);
+		Vector3f w2 = this.i2.multiplyNew(angularImpulseBody2);
+		
+		// Update the body position/orientation of body 2
+		x2.add(v2);
+		q2.add((new Quaternion(0, w2)).multiply(q2).multiply(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.addNew(this.i2);
+		if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) {
+			this.inverseMassMatrixRotation = this.inverseMassMatrixRotation.inverseNew();
+		}
+		
+		// Compute the position error for the 3 rotation raints
+		final Quaternion currentOrientationDifference = q2.multiplyNew(q1.inverseNew());
+		currentOrientationDifference.normalize();
+		final Quaternion qError = currentOrientationDifference.multiplyNew(this.initOrientationDifferenceInv);
+		final Vector3f errorRotation = qError.getVectorV().multiply(2.0f);
+		
+		// Compute the Lagrange multiplier lambda for the 3 rotation raints
+		final Vector3f lambdaRotation = this.inverseMassMatrixRotation.multiplyNew(errorRotation.multiplyNew(-1));
+		
+		// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
+		angularImpulseBody1 = lambdaRotation.multiplyNew(-1);
+		
+		// Compute the pseudo velocity of body 1
+		w1 = this.i1.multiplyNew(angularImpulseBody1);
+		
+		// Update the body position/orientation of body 1
+		q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
+		q1.normalize();
+		
+		// Compute the pseudo velocity of body 2
+		w2 = this.i2.multiplyNew(lambdaRotation);
+		
+		// Update the body position/orientation of body 2
+		q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
+		q2.normalize();
+	}
+	
+	@Override
+	public void solveVelocityConstraint(final ConstraintSolverData raintSolverData) {
+		
+		// Get the velocities
+		final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+		final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+		final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+		final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+		
+		// Get the inverse mass of the bodies
+		final float inverseMassBody1 = this.body1.massInverse;
+		final float inverseMassBody2 = this.body2.massInverse;
+		
+		// --------------- Translation Constraints --------------- //
+		
+		// Compute J*v for the 3 translation raints
+		final Vector3f JvTranslation = v2.addNew(w2.cross(this.r2World)).less(v1).less(w1.cross(this.r1World));
+		
+		// Compute the Lagrange multiplier lambda
+		final Vector3f deltaLambda = this.inverseMassMatrixTranslation.multiplyNew(JvTranslation.multiplyNew(-1).less(this.biasTranslation));
+		this.impulseTranslation.add(deltaLambda);
+		
+		// Compute the impulse P=J^T * lambda for body 1
+		final Vector3f linearImpulseBody1 = deltaLambda.multiplyNew(-1);
+		Vector3f angularImpulseBody1 = deltaLambda.cross(this.r1World);
+		
+		// Apply the impulse to the body 1
+		v1.add(linearImpulseBody1.multiplyNew(inverseMassBody1));
+		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		
+		// Compute the impulse P=J^T * lambda  for body 2
+		final Vector3f angularImpulseBody2 = deltaLambda.cross(this.r2World).multiply(-1);
+		
+		// Apply the impulse to the body 2
+		v2.add(deltaLambda.multiplyNew(inverseMassBody2));
+		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+		
+		// --------------- Rotation Constraints --------------- //
+		
+		// Compute J*v for the 3 rotation raints
+		final Vector3f JvRotation = w2.lessNew(w1);
+		
+		// Compute the Lagrange multiplier lambda for the 3 rotation raints
+		final Vector3f deltaLambda2 = this.inverseMassMatrixRotation.multiplyNew(JvRotation.multiplyNew(-1).less(this.biasRotation));
+		this.impulseRotation.add(deltaLambda2);
+		
+		// Compute the impulse P=J^T * lambda for the 3 rotation raints for body 1
+		angularImpulseBody1 = deltaLambda2.multiplyNew(-1);
+		
+		// Apply the impulse to the body 1
+		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		
+		// Apply the impulse to the body 2
+		w2.add(this.i2.multiplyNew(deltaLambda2));
+	}
+	
+	@Override
+	public void warmstart(final ConstraintSolverData raintSolverData) {
+		
+		// Get the velocities
+		final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+		final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+		final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+		final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+		
+		// Get the inverse mass of the bodies
+		final float inverseMassBody1 = this.body1.massInverse;
+		final float inverseMassBody2 = this.body2.massInverse;
+		
+		// Compute the impulse P=J^T * lambda for the 3 translation raints for body 1
+		final Vector3f linearImpulseBody1 = this.impulseTranslation.multiplyNew(-1);
+		final Vector3f angularImpulseBody1 = this.impulseTranslation.cross(this.r1World);
+		
+		// Compute the impulse P=J^T * lambda for the 3 rotation raints for body 1
+		angularImpulseBody1.add(this.impulseRotation.multiplyNew(-1));
+		
+		// Apply the impulse to the body 1
+		v1.add(linearImpulseBody1.multiplyNew(inverseMassBody1));
+		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		
+		// Compute the impulse P=J^T * lambda for the 3 translation raints for body 2
+		final Vector3f angularImpulseBody2 = this.impulseTranslation.cross(this.r2World).multiplyNew(-1);
+		
+		// Compute the impulse P=J^T * lambda for the 3 rotation raints for body 2
+		angularImpulseBody2.add(this.impulseRotation);
+		
+		// Apply the impulse to the body 2
+		v2.add(this.impulseTranslation.multiplyNew(inverseMassBody2));
+		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+		
+	}
+	
+}

src/org/atriasoft/ephysics/constraint/FixedJointInfo.java

@@ -0,0 +1,25 @@
+package org.atriasoft.ephysics.constraint;
+
+import org.atriasoft.etk.math.Vector3f;
+
+import org.atriasoft.ephysics.body.RigidBody;
+
+/**
+ * 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.
+ */
+public class FixedJointInfo extends JointInfo {
+	
+	Vector3f anchorPointWorldSpace; //!< Anchor point (in world-space coordinates)
+	
+	/**
+	 * Constructor
+	 * @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(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace) {
+		super(_rigidBody1, _rigidBody2, JointType.FIXEDJOINT);
+		this.anchorPointWorldSpace = _initAnchorPointWorldSpace;
+	}
+}

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

@@ -0,0 +1,862 @@
+package org.atriasoft.ephysics.constraint;
+
+import org.atriasoft.etk.math.Constant;
+import org.atriasoft.etk.math.FMath;
+import org.atriasoft.etk.math.Matrix3f;
+import org.atriasoft.etk.math.Quaternion;
+import org.atriasoft.etk.math.Transform3D;
+import org.atriasoft.etk.math.Vector2f;
+import org.atriasoft.etk.math.Vector3f;
+
+import org.atriasoft.ephysics.body.BodyType;
+import org.atriasoft.ephysics.engine.ConstraintSolverData;
+import org.atriasoft.ephysics.mathematics.Matrix2f;
+
+/**
+ *  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.
+ */
+public class HingeJoint extends Joint {
+	
+	private static float BETA; //!< Beta value for the bias factor of position correction
+	private final Vector3f localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1)
+	private final Vector3f localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2)
+	private final Vector3f hingeLocalAxisBody1; //!< Hinge rotation axis (in local-space coordinates of body 1)
+	private final Vector3f hingeLocalAxisBody2; //!< Hinge rotation axis (in local-space coordiantes of body 2)
+	private Matrix3f i1; //!< Inertia tensor of body 1 (in world-space coordinates)
+	private Matrix3f i2; //!< Inertia tensor of body 2 (in world-space coordinates)
+	private Vector3f mA1; //!< Hinge rotation axis (in world-space coordinates) computed from body 1
+	private Vector3f r1World; //!< Vector from center of body 2 to anchor point in world-space
+	private Vector3f r2World; //!< Vector from center of body 2 to anchor point in world-space
+	private Vector3f b2CrossA1; //!< Cross product of vector b2 and a1
+	private Vector3f c2CrossA1; //!< Cross product of vector c2 and a1;
+	private final Vector3f impulseTranslation; //!< Impulse for the 3 translation raints
+	private final Vector2f impulseRotation; //!< Impulse for the 2 rotation raints
+	private float impulseLowerLimit; //!< Accumulated impulse for the lower limit raint
+	private float impulseUpperLimit; //!< Accumulated impulse for the upper limit raint
+	private float impulseMotor; //!< Accumulated impulse for the motor raint;
+	private Matrix3f inverseMassMatrixTranslation; //!< Inverse mass matrix K=JM^-1J^t for the 3 translation raints
+	private Matrix2f inverseMassMatrixRotation; //!< Inverse mass matrix K=JM^-1J^t for the 2 rotation raints
+	private float inverseMassMatrixLimitMotor; //!< Inverse of mass matrix K=JM^-1J^t for the limits and motor raints (1x1 matrix)
+	private float inverseMassMatrixMotor; //!< Inverse of mass matrix K=JM^-1J^t for the motor
+	private Vector3f bTranslation; //!< Bias vector for the error correction for the translation raints
+	private Vector2f bRotation; //!< Bias vector for the error correction for the rotation raints
+	private float bLowerLimit; //!< Bias of the lower limit raint
+	private float bUpperLimit; //!< Bias of the upper limit raint
+	private final Quaternion initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the bodies
+	private boolean isLimitEnabled; //!< True if the joint limits are enabled
+	private boolean isMotorEnabled; //!< True if the motor of the joint in enabled
+	private float lowerLimit; //!< Lower limit (minimum allowed rotation angle in radian)
+	private float upperLimit; //!< Upper limit (maximum translation distance)
+	private boolean isLowerLimitViolated; //!< True if the lower limit is violated
+	private boolean isUpperLimitViolated; //!< True if the upper limit is violated
+	private float motorSpeed; //!< Motor speed (in rad/s)
+	private float maxMotorTorque; //!< Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed
+	/// Reset the limits
+	
+	/// Constructor
+	public HingeJoint(final HingeJointInfo jointInfo) {
+		super(jointInfo);
+		this.impulseTranslation = new Vector3f(0, 0, 0);
+		this.impulseRotation = new Vector2f(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 * Constant.PI);
+		assert (this.upperLimit >= 0 && this.upperLimit <= 2.0 * Constant.PI);
+		
+		// Compute the local-space anchor point for each body
+		final Transform3D transform1 = this.body1.getTransform();
+		final Transform3D transform2 = this.body2.getTransform();
+		this.localAnchorPointBody1 = transform1.inverseNew().multiply(jointInfo.anchorPointWorldSpace);
+		this.localAnchorPointBody2 = transform2.inverseNew().multiply(jointInfo.anchorPointWorldSpace);
+		
+		// Compute the local-space hinge axis
+		this.hingeLocalAxisBody1 = transform1.getOrientation().inverseNew().multiply(jointInfo.rotationAxisWorld);
+		this.hingeLocalAxisBody2 = transform2.getOrientation().inverseNew().multiply(jointInfo.rotationAxisWorld);
+		this.hingeLocalAxisBody1.normalize();
+		this.hingeLocalAxisBody2.normalize();
+		
+		// Compute the inverse of the initial orientation difference between the two bodies
+		this.initOrientationDifferenceInv = transform2.getOrientation().multiplyNew(transform1.getOrientation().inverseNew());
+		this.initOrientationDifferenceInv.normalize();
+		this.initOrientationDifferenceInv.inverse();
+	}
+	
+	/// 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.
+	private float computeCorrespondingAngleNearLimits(final float inputAngle, final float lowerLimitAngle, final float upperLimitAngle) {
+		if (upperLimitAngle <= lowerLimitAngle) {
+			return inputAngle;
+		} else if (inputAngle > upperLimitAngle) {
+			final float diffToUpperLimit = FMath.abs(computeNormalizedAngle(inputAngle - upperLimitAngle));
+			final float diffToLowerLimit = FMath.abs(computeNormalizedAngle(inputAngle - lowerLimitAngle));
+			return (diffToUpperLimit > diffToLowerLimit) ? (inputAngle - Constant.PI_2) : inputAngle;
+		} else if (inputAngle < lowerLimitAngle) {
+			final float diffToUpperLimit = FMath.abs(computeNormalizedAngle(upperLimitAngle - inputAngle));
+			final float diffToLowerLimit = FMath.abs(computeNormalizedAngle(lowerLimitAngle - inputAngle));
+			return (diffToUpperLimit > diffToLowerLimit) ? inputAngle : (inputAngle + Constant.PI_2);
+		} else {
+			return inputAngle;
+		}
+	}
+	
+	/// Compute the current angle around the hinge axis
+	private float computeCurrentHingeAngle(final Quaternion orientationBody1, final Quaternion orientationBody2) {
+		
+		float hingeAngle;
+		
+		// Compute the current orientation difference between the two bodies
+		final Quaternion currentOrientationDiff = orientationBody2.multiplyNew(orientationBody1.inverseNew());
+		currentOrientationDiff.normalize();
+		
+		// Compute the relative rotation idering the initial orientation difference
+		final Quaternion relativeRotation = currentOrientationDiff.multiplyNew(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).
+		final float cosHalfAngle = relativeRotation.getW();
+		final float sinHalfAngleAbs = relativeRotation.getVectorV().length();
+		
+		// Compute the dot product of the relative rotation axis and the hinge axis
+		final float dotProduct = relativeRotation.getVectorV().dot(this.mA1);
+		
+		// If the relative rotation axis and the hinge axis are pointing the same direction
+		if (dotProduct >= 0.0f) {
+			hingeAngle = 2.0f * FMath.atan2(sinHalfAngleAbs, cosHalfAngle);
+		} else {
+			hingeAngle = 2.0f * FMath.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);
+	}
+	
+	/// Given an angle in radian, this method returns the corresponding
+	/// angle in the range [-pi; pi]
+	private float computeNormalizedAngle(float angle) {
+		
+		// Convert it into the range [-2*pi; 2*pi]
+		angle = FMath.mod(angle, Constant.PI_2);
+		
+		// Convert it into the range [-pi; pi]
+		if (angle < -Constant.PI) {
+			return angle + Constant.PI_2;
+		} else if (angle > Constant.PI) {
+			return angle - Constant.PI_2;
+		} else {
+			return angle;
+		}
+	}
+	
+	/**Enable/Disable the limits of the joint
+	 * @param isLimitEnabled True if you want to enable the limits of the joint and
+	 *					   false otherwise
+	 */
+	public void enableLimit(final boolean isLimitEnabled) {
+		
+		if (this.isLimitEnabled != 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
+	 */
+	public void enableMotor(final boolean isMotorEnabled) {
+		
+		this.isMotorEnabled = isMotorEnabled;
+		this.impulseMotor = 0.0f;
+		
+		// Wake up the two bodies of the joint
+		this.body1.setIsSleeping(false);
+		this.body2.setIsSleeping(false);
+	}
+	
+	/// Return the maximum angle limit
+	/**
+	 * @return The maximum limit angle of the joint (in radian)
+	 */
+	public float getMaxAngleLimit() {
+		return this.upperLimit;
+	}
+	
+	/// Return the maximum motor torque
+	/**
+	 * @return The maximum torque of the joint motor (in Newtons)
+	 */
+	public float getMaxMotorTorque() {
+		return this.maxMotorTorque;
+	}
+	
+	/// Return the minimum angle limit
+	/**
+	 * @return The minimum limit angle of the joint (in radian)
+	 */
+	public float getMinAngleLimit() {
+		return this.lowerLimit;
+	}
+	
+	/// Return the motor speed
+	/**
+	 * @return The current speed of the joint motor (in radian per second)
+	 */
+	public float getMotorSpeed() {
+		return this.motorSpeed;
+	}
+	
+	/// 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
+	 */
+	public float getMotorTorque(final float timeStep) {
+		return this.impulseMotor / timeStep;
+	}
+	
+	@Override
+	public void initBeforeSolve(final ConstraintSolverData raintSolverData) {
+		
+		// Initialize the bodies index in the velocity array
+		this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.get(this.body1);
+		this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.get(this.body2);
+		
+		// Get the bodies positions and orientations
+		final Vector3f x1 = this.body1.centerOfMassWorld;
+		final Vector3f x2 = this.body2.centerOfMassWorld;
+		final Quaternion orientationBody1 = this.body1.getTransform().getOrientation();
+		final 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.multiply(this.localAnchorPointBody1);
+		this.r2World = orientationBody2.multiply(this.localAnchorPointBody2);
+		
+		// Compute the current angle around the hinge axis
+		final float hingeAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2);
+		
+		// Check if the limit raints are violated or not
+		final float lowerLimitError = hingeAngle - this.lowerLimit;
+		final float upperLimitError = this.upperLimit - hingeAngle;
+		final boolean oldIsLowerLimitViolated = this.isLowerLimitViolated;
+		this.isLowerLimitViolated = lowerLimitError <= 0;
+		if (this.isLowerLimitViolated != oldIsLowerLimitViolated) {
+			this.impulseLowerLimit = 0.0f;
+		}
+		final boolean oldIsUpperLimitViolated = this.isUpperLimitViolated;
+		this.isUpperLimitViolated = upperLimitError <= 0;
+		if (this.isUpperLimitViolated != oldIsUpperLimitViolated) {
+			this.impulseUpperLimit = 0.0f;
+		}
+		
+		// Compute vectors needed in the Jacobian
+		this.mA1 = orientationBody1.multiply(this.hingeLocalAxisBody1);
+		final Vector3f a2 = orientationBody2.multiply(this.hingeLocalAxisBody2);
+		this.mA1.normalize();
+		a2.normalize();
+		final Vector3f b2 = a2.getOrthoVector();
+		final Vector3f c2 = a2.cross(b2);
+		this.b2CrossA1 = b2.cross(this.mA1);
+		this.c2CrossA1 = c2.cross(this.mA1);
+		
+		// Compute the corresponding skew-symmetric matrices
+		final Matrix3f skewSymmetricMatrixU1 = Matrix3f.computeSkewSymmetricMatrixForCrossProduct(this.r1World);
+		final Matrix3f skewSymmetricMatrixU2 = Matrix3f.computeSkewSymmetricMatrixForCrossProduct(this.r2World);
+		
+		// Compute the inverse mass matrix K=JM^-1J^t for the 3 translation raints (3x3 matrix)
+		final float inverseMassBodies = this.body1.massInverse + this.body2.massInverse;
+		final Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies);
+		massMatrix.add(skewSymmetricMatrixU1.multiplyNew(this.i1).multiply(skewSymmetricMatrixU1.transposeNew()));
+		massMatrix.add(skewSymmetricMatrixU2.multiplyNew(this.i2).multiply(skewSymmetricMatrixU2.transposeNew()));
+		this.inverseMassMatrixTranslation.setZero();
+		if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) {
+			this.inverseMassMatrixTranslation = massMatrix.inverseNew();
+		}
+		
+		// Compute the bias "b" of the translation raints
+		this.bTranslation.setZero();
+		final float biasFactor = (BETA / raintSolverData.timeStep);
+		if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) {
+			this.bTranslation = x2.addNew(this.r2World).less(x1).less(this.r1World).multiply(biasFactor);
+		}
+		
+		// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation raints (2x2 matrix)
+		final Vector3f I1B2CrossA1 = this.i1.multiplyNew(this.b2CrossA1);
+		final Vector3f I1C2CrossA1 = this.i1.multiplyNew(this.c2CrossA1);
+		final Vector3f I2B2CrossA1 = this.i2.multiplyNew(this.b2CrossA1);
+		final Vector3f I2C2CrossA1 = this.i2.multiplyNew(this.c2CrossA1);
+		final float el11 = this.b2CrossA1.dot(I1B2CrossA1) + this.b2CrossA1.dot(I2B2CrossA1);
+		final float el12 = this.b2CrossA1.dot(I1C2CrossA1) + this.b2CrossA1.dot(I2C2CrossA1);
+		final float el21 = this.c2CrossA1.dot(I1B2CrossA1) + this.c2CrossA1.dot(I2B2CrossA1);
+		final float el22 = this.c2CrossA1.dot(I1C2CrossA1) + this.c2CrossA1.dot(I2C2CrossA1);
+		
+		final Matrix2f matrixKRotation = new Matrix2f(el11, el12, el21, el22);
+		this.inverseMassMatrixRotation.setZero();
+		if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) {
+			this.inverseMassMatrixRotation = matrixKRotation.inverseNew();
+		}
+		
+		// Compute the bias "b" of the rotation raints
+		this.bRotation.setZero();
+		if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) {
+			this.bRotation = new Vector2f(this.mA1.dot(b2) * biasFactor, this.mA1.dot(c2) * biasFactor);
+		}
+		
+		// If warm-starting is not enabled
+		if (!raintSolverData.isWarmStartingActive) {
+			
+			// Reset all the accumulated impulses
+			this.impulseTranslation.setZero();
+			this.impulseRotation.setZero();
+			this.impulseLowerLimit = 0.0f;
+			this.impulseUpperLimit = 0.0f;
+			this.impulseMotor = 0.0f;
+		}
+		
+		// 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 = this.mA1.dot(this.i1.multiplyNew(this.mA1)) + this.mA1.dot(this.i2.multiplyNew(this.mA1));
+			this.inverseMassMatrixLimitMotor = (this.inverseMassMatrixLimitMotor > 0.0f) ? 1.0f / this.inverseMassMatrixLimitMotor : 0.0f;
+			
+			if (this.isLimitEnabled) {
+				
+				// Compute the bias "b" of the lower limit raint
+				this.bLowerLimit = 0.0f;
+				if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) {
+					this.bLowerLimit = biasFactor * lowerLimitError;
+				}
+				
+				// Compute the bias "b" of the upper limit raint
+				this.bUpperLimit = 0.0f;
+				if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) {
+					this.bUpperLimit = biasFactor * upperLimitError;
+				}
+			}
+		}
+		
+	}
+	
+	/// Return true if the limits or the joint are enabled
+	/**
+	 * @return True if the limits of the joint are enabled and false otherwise
+	 */
+	public boolean 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
+	 */
+	public boolean isMotorEnabled() {
+		return this.isMotorEnabled;
+	}
+	
+	private void resetLimits() {
+		
+		// Reset the accumulated impulses for the limits
+		this.impulseLowerLimit = 0.0f;
+		this.impulseUpperLimit = 0.0f;
+		
+		// Wake up the two bodies of the joint
+		this.body1.setIsSleeping(false);
+		this.body2.setIsSleeping(false);
+	}
+	
+	/// Set the maximum angle limit
+	/**
+	 * @param upperLimit The maximum limit angle of the joint (in radian)
+	 */
+	public void setMaxAngleLimit(final float upperLimit) {
+		
+		assert (upperLimit >= 0 && upperLimit <= 2.0 * Constant.PI);
+		
+		if (upperLimit != this.upperLimit) {
+			
+			this.upperLimit = upperLimit;
+			
+			// Reset the limits
+			resetLimits();
+		}
+	}
+	
+	/// Set the maximum motor torque
+	/**
+	 * @param maxMotorTorque The maximum torque (in Newtons) of the joint motor
+	 */
+	public void setMaxMotorTorque(final float maxMotorTorque) {
+		
+		if (maxMotorTorque != this.maxMotorTorque) {
+			
+			assert (this.maxMotorTorque >= 0.0f);
+			this.maxMotorTorque = maxMotorTorque;
+			
+			// 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)
+	 */
+	public void setMinAngleLimit(final float lowerLimit) {
+		
+		assert (this.lowerLimit <= 0 && this.lowerLimit >= -2.0 * Constant.PI);
+		
+		if (lowerLimit != this.lowerLimit) {
+			
+			this.lowerLimit = lowerLimit;
+			
+			// Reset the limits
+			resetLimits();
+		}
+	}
+	
+	/// Set the motor speed
+	public void setMotorSpeed(final 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);
+		}
+	}
+	
+	@Override
+	public void solvePositionConstraint(final 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 != JointsPositionCorrectionTechnique.NON_LINEAR_GAUSS_SEIDEL) {
+			return;
+		}
+		
+		// Get the bodies positions and orientations
+		final Vector3f x1 = raintSolverData.positions[this.indexBody1];
+		final Vector3f x2 = raintSolverData.positions[this.indexBody2];
+		final Quaternion q1 = raintSolverData.orientations[this.indexBody1];
+		final Quaternion q2 = raintSolverData.orientations[this.indexBody2];
+		
+		// Get the inverse mass and inverse inertia tensors of the bodies
+		final float inverseMassBody1 = this.body1.massInverse;
+		final float inverseMassBody2 = this.body2.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.multiply(this.localAnchorPointBody1);
+		this.r2World = q2.multiply(this.localAnchorPointBody2);
+		
+		// Compute the current angle around the hinge axis
+		final float hingeAngle = computeCurrentHingeAngle(q1, q2);
+		
+		// Check if the limit raints are violated or not
+		final float lowerLimitError = hingeAngle - this.lowerLimit;
+		final float upperLimitError = this.upperLimit - hingeAngle;
+		this.isLowerLimitViolated = lowerLimitError <= 0;
+		this.isUpperLimitViolated = upperLimitError <= 0;
+		
+		// Compute vectors needed in the Jacobian
+		this.mA1 = q1.multiply(this.hingeLocalAxisBody1);
+		final Vector3f a2 = q2.multiply(this.hingeLocalAxisBody2);
+		this.mA1.normalize();
+		a2.normalize();
+		final Vector3f b2 = a2.getOrthoVector();
+		final Vector3f c2 = a2.cross(b2);
+		this.b2CrossA1 = b2.cross(this.mA1);
+		this.c2CrossA1 = c2.cross(this.mA1);
+		
+		// Compute the corresponding skew-symmetric matrices
+		final Matrix3f skewSymmetricMatrixU1 = Matrix3f.computeSkewSymmetricMatrixForCrossProduct(this.r1World);
+		final Matrix3f skewSymmetricMatrixU2 = Matrix3f.computeSkewSymmetricMatrixForCrossProduct(this.r2World);
+		
+		// --------------- Translation Constraints --------------- //
+		
+		// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints
+		final float inverseMassBodies = this.body1.massInverse + this.body2.massInverse;
+		final Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies);
+		massMatrix.add(skewSymmetricMatrixU1.multiplyNew(this.i1).multiply(skewSymmetricMatrixU1.transposeNew()));
+		massMatrix.add(skewSymmetricMatrixU2.multiplyNew(this.i2).multiply(skewSymmetricMatrixU2.transposeNew()));
+		this.inverseMassMatrixTranslation.setZero();
+		if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) {
+			this.inverseMassMatrixTranslation = massMatrix.inverseNew();
+		}
+		
+		// Compute position error for the 3 translation raints
+		final Vector3f errorTranslation = x2.addNew(this.r2World).less(x1).less(this.r1World);
+		
+		// Compute the Lagrange multiplier lambda
+		final Vector3f lambdaTranslation = this.inverseMassMatrixTranslation.multiplyNew(errorTranslation.multiplyNew(-1));
+		
+		// Compute the impulse of body 1
+		final Vector3f linearImpulseBody1 = lambdaTranslation.multiplyNew(-1);
+		Vector3f angularImpulseBody1 = lambdaTranslation.cross(this.r1World);
+		
+		// Compute the pseudo velocity of body 1
+		final Vector3f v1 = linearImpulseBody1.multiplyNew(inverseMassBody1);
+		Vector3f w1 = this.i1.multiplyNew(angularImpulseBody1);
+		
+		// Update the body position/orientation of body 1
+		x1.add(v1);
+		q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
+		q1.normalize();
+		
+		// Compute the impulse of body 2
+		Vector3f angularImpulseBody2 = lambdaTranslation.cross(this.r2World).multiplyNew(-1);
+		
+		// Compute the pseudo velocity of body 2
+		final Vector3f v2 = lambdaTranslation.multiplyNew(inverseMassBody2);
+		Vector3f w2 = this.i2.multiplyNew(angularImpulseBody2);
+		
+		// Update the body position/orientation of body 2
+		x2.add(v2);
+		q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
+		q2.normalize();
+		
+		// --------------- Rotation Constraints --------------- //
+		
+		// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation raints (2x2 matrix)
+		final Vector3f I1B2CrossA1 = this.i1.multiplyNew(this.b2CrossA1);
+		final Vector3f I1C2CrossA1 = this.i1.multiplyNew(this.c2CrossA1);
+		final Vector3f I2B2CrossA1 = this.i2.multiplyNew(this.b2CrossA1);
+		final Vector3f I2C2CrossA1 = this.i2.multiplyNew(this.c2CrossA1);
+		final float el11 = this.b2CrossA1.dot(I1B2CrossA1) + this.b2CrossA1.dot(I2B2CrossA1);
+		final float el12 = this.b2CrossA1.dot(I1C2CrossA1) + this.b2CrossA1.dot(I2C2CrossA1);
+		final float el21 = this.c2CrossA1.dot(I1B2CrossA1) + this.c2CrossA1.dot(I2B2CrossA1);
+		final float el22 = this.c2CrossA1.dot(I1C2CrossA1) + this.c2CrossA1.dot(I2C2CrossA1);
+		
+		final Matrix2f matrixKRotation = new Matrix2f(el11, el12, el21, el22);
+		this.inverseMassMatrixRotation.setZero();
+		if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) {
+			this.inverseMassMatrixRotation = matrixKRotation.inverseNew();
+		}
+		
+		// Compute the position error for the 3 rotation raints
+		final Vector2f errorRotation = new Vector2f(this.mA1.dot(b2), this.mA1.dot(c2));
+		
+		// Compute the Lagrange multiplier lambda for the 3 rotation raints
+		final Vector2f lambdaRotation = this.inverseMassMatrixRotation.multiplyNew(errorRotation.multiplyNew(-1));
+		
+		// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
+		angularImpulseBody1 = this.b2CrossA1.multiplyNew(-1).multiply(lambdaRotation.x).less(this.c2CrossA1.multiplyNew(lambdaRotation.y));
+		
+		// Compute the pseudo velocity of body 1
+		w1 = this.i1.multiplyNew(angularImpulseBody1);
+		
+		// Update the body position/orientation of body 1
+		q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
+		q1.normalize();
+		
+		// Compute the impulse of body 2
+		angularImpulseBody2 = this.b2CrossA1.multiplyNew(lambdaRotation.x).add(this.c2CrossA1.multiplyNew(lambdaRotation.y));
+		
+		// Compute the pseudo velocity of body 2
+		w2 = this.i2.multiplyNew(angularImpulseBody2);
+		
+		// Update the body position/orientation of body 2
+		q2.add((new Quaternion(0, w2)).multiply(q2).multiply(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 = this.mA1.dot(this.i1.multiplyNew(this.mA1)) + this.mA1.dot(this.i2.multiplyNew(this.mA1));
+				this.inverseMassMatrixLimitMotor = (this.inverseMassMatrixLimitMotor > 0.0f) ? 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
+				final float lambdaLowerLimit = this.inverseMassMatrixLimitMotor * (-lowerLimitError);
+				
+				// Compute the impulse P=J^T * lambda of body 1
+				final Vector3f angularImpulseBody1_tmp = this.mA1.multiplyNew(lambdaLowerLimit);
+				
+				// Compute the pseudo velocity of body 1
+				final Vector3f w1_tmp = this.i1.multiplyNew(angularImpulseBody1_tmp);
+				
+				// Update the body position/orientation of body 1
+				q1.add((new Quaternion(0, w1_tmp)).multiply(q1).multiply(0.5f));
+				q1.normalize();
+				
+				// Compute the impulse P=J^T * lambda of body 2
+				final Vector3f angularImpulseBody2_tmp = this.mA1.multiplyNew(lambdaLowerLimit);
+				
+				// Compute the pseudo velocity of body 2
+				final Vector3f w2_tmp = this.i2.multiplyNew(angularImpulseBody2_tmp);
+				
+				// Update the body position/orientation of body 2
+				q2.add((new Quaternion(0, w2_tmp)).multiply(q2).multiply(0.5f));
+				q2.normalize();
+			}
+			
+			// If the upper limit is violated
+			if (this.isUpperLimitViolated) {
+				
+				// Compute the Lagrange multiplier lambda for the upper limit raint
+				final float lambdaUpperLimit = this.inverseMassMatrixLimitMotor * (-upperLimitError);
+				
+				// Compute the impulse P=J^T * lambda of body 1
+				final Vector3f angularImpulseBody1_tmp = this.mA1.multiplyNew(lambdaUpperLimit);
+				
+				// Compute the pseudo velocity of body 1
+				final Vector3f w1_tmp = this.i1.multiplyNew(angularImpulseBody1_tmp);
+				
+				// Update the body position/orientation of body 1
+				q1.add((new Quaternion(0, w1_tmp)).multiply(q1).multiply(0.5f));
+				q1.normalize();
+				
+				// Compute the impulse P=J^T * lambda of body 2
+				final Vector3f angularImpulseBody2_tmp = this.mA1.multiplyNew(-lambdaUpperLimit);
+				
+				// Compute the pseudo velocity of body 2
+				final Vector3f w2_tmp = this.i2.multiplyNew(angularImpulseBody2_tmp);
+				
+				// Update the body position/orientation of body 2
+				q2.add((new Quaternion(0, w2_tmp)).multiply(q2).multiply(0.5f));
+				q2.normalize();
+			}
+		}
+	}
+	
+	@Override
+	public void solveVelocityConstraint(final ConstraintSolverData raintSolverData) {
+		
+		// Get the velocities
+		final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+		final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+		final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+		final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+		
+		// Get the inverse mass and inverse inertia tensors of the bodies
+		final float inverseMassBody1 = this.body1.massInverse;
+		final float inverseMassBody2 = this.body2.massInverse;
+		
+		// --------------- Translation Constraints --------------- //
+		
+		// Compute J*v
+		final Vector3f JvTranslation = v2.addNew(w2.cross(this.r2World)).less(v1).less(w1.cross(this.r1World));
+		
+		// Compute the Lagrange multiplier lambda
+		final Vector3f deltaLambdaTranslation = this.inverseMassMatrixTranslation.multiplyNew(JvTranslation.multiplyNew(-1).less(this.bTranslation));
+		this.impulseTranslation.add(deltaLambdaTranslation);
+		
+		// Compute the impulse P=J^T * lambda of body 1
+		final Vector3f linearImpulseBody1 = deltaLambdaTranslation.multiplyNew(-1);
+		Vector3f angularImpulseBody1 = deltaLambdaTranslation.cross(this.r1World);
+		
+		// Apply the impulse to the body 1
+		v1.add(linearImpulseBody1.multiplyNew(inverseMassBody1));
+		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		
+		// Compute the impulse P=J^T * lambda of body 2
+		Vector3f angularImpulseBody2 = deltaLambdaTranslation.cross(this.r2World).multiplyNew(-1);
+		
+		// Apply the impulse to the body 2
+		v2.add(deltaLambdaTranslation.multiplyNew(inverseMassBody2));
+		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+		
+		// --------------- Rotation Constraints --------------- //
+		
+		// Compute J*v for the 2 rotation raints
+		final Vector2f JvRotation = new Vector2f(-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
+		final Vector2f deltaLambdaRotation = this.inverseMassMatrixRotation.multiplyNew(JvRotation.multiplyNew(-1).less(this.bRotation));
+		this.impulseRotation.add(deltaLambdaRotation);
+		
+		// Compute the impulse P=J^T * lambda for the 2 rotation raints of body 1
+		angularImpulseBody1 = this.b2CrossA1.multiplyNew(-1).multiply(deltaLambdaRotation.x).less(this.c2CrossA1.multiplyNew(deltaLambdaRotation.y));
+		
+		// Apply the impulse to the body 1
+		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		
+		// Compute the impulse P=J^T * lambda for the 2 rotation raints of body 2
+		angularImpulseBody2 = this.b2CrossA1.multiplyNew(deltaLambdaRotation.x).add(this.c2CrossA1.multiplyNew(deltaLambdaRotation.y));
+		
+		// Apply the impulse to the body 2
+		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+		
+		// --------------- Limits Constraints --------------- //
+		
+		if (this.isLimitEnabled) {
+			
+			// If the lower limit is violated
+			if (this.isLowerLimitViolated) {
+				
+				// Compute J*v for the lower limit raint
+				final float JvLowerLimit = w2.lessNew(w1).dot(this.mA1);
+				
+				// Compute the Lagrange multiplier lambda for the lower limit raint
+				float deltaLambdaLower = this.inverseMassMatrixLimitMotor * (-JvLowerLimit - this.bLowerLimit);
+				final float lambdaTemp = this.impulseLowerLimit;
+				this.impulseLowerLimit = FMath.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
+				final Vector3f angularImpulseBody1_tmp = this.mA1.multiplyNew(-deltaLambdaLower);
+				
+				// Apply the impulse to the body 1
+				w1.add(this.i1.multiplyNew(angularImpulseBody1_tmp));
+				
+				// Compute the impulse P=J^T * lambda for the lower limit raint of body 2
+				final Vector3f angularImpulseBody2_tmp = this.mA1.multiplyNew(deltaLambdaLower);
+				
+				// Apply the impulse to the body 2
+				w2.add(this.i2.multiplyNew(angularImpulseBody2_tmp));
+			}
+			
+			// If the upper limit is violated
+			if (this.isUpperLimitViolated) {
+				
+				// Compute J*v for the upper limit raint
+				final float JvUpperLimit = w2.lessNew(w1).multiplyNew(-1).dot(this.mA1);
+				
+				// Compute the Lagrange multiplier lambda for the upper limit raint
+				float deltaLambdaUpper = this.inverseMassMatrixLimitMotor * (-JvUpperLimit - this.bUpperLimit);
+				final float lambdaTemp = this.impulseUpperLimit;
+				this.impulseUpperLimit = FMath.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
+				final Vector3f angularImpulseBody1_tmp = this.mA1.multiplyNew(deltaLambdaUpper);
+				
+				// Apply the impulse to the body 1
+				w1.add(this.i1.multiplyNew(angularImpulseBody1_tmp));
+				
+				// Compute the impulse P=J^T * lambda for the upper limit raint of body 2
+				final Vector3f angularImpulseBody2_tmp = this.mA1.multiplyNew(-deltaLambdaUpper);
+				
+				// Apply the impulse to the body 2
+				w2.add(this.i2.multiplyNew(angularImpulseBody2_tmp));
+			}
+		}
+		
+		// --------------- Motor --------------- //
+		
+		// If the motor is enabled
+		if (this.isMotorEnabled) {
+			
+			// Compute J*v for the motor
+			final float JvMotor = this.mA1.dot(w1.lessNew(w2));
+			
+			// Compute the Lagrange multiplier lambda for the motor
+			final float maxMotorImpulse = this.maxMotorTorque * raintSolverData.timeStep;
+			float deltaLambdaMotor = this.inverseMassMatrixLimitMotor * (-JvMotor - this.motorSpeed);
+			final float lambdaTemp = this.impulseMotor;
+			this.impulseMotor = FMath.clamp(this.impulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
+			deltaLambdaMotor = this.impulseMotor - lambdaTemp;
+			
+			// Compute the impulse P=J^T * lambda for the motor of body 1
+			final Vector3f angularImpulseBody1_tmp = this.mA1.multiplyNew(-deltaLambdaMotor);
+			
+			// Apply the impulse to the body 1
+			w1.add(this.i1.multiplyNew(angularImpulseBody1_tmp));
+			
+			// Compute the impulse P=J^T * lambda for the motor of body 2
+			final Vector3f angularImpulseBody2_tmp = this.mA1.multiplyNew(deltaLambdaMotor);
+			
+			// Apply the impulse to the body 2
+			w2.add(this.i2.multiplyNew(angularImpulseBody2_tmp));
+		}
+		
+	}
+	
+	@Override
+	public void warmstart(final ConstraintSolverData raintSolverData) {
+		
+		// Get the velocities
+		final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+		final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+		final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+		final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+		
+		// Get the inverse mass and inverse inertia tensors of the bodies
+		final float inverseMassBody1 = this.body1.massInverse;
+		final float inverseMassBody2 = this.body2.massInverse;
+		
+		// Compute the impulse P=J^T * lambda for the 2 rotation raints
+		final Vector3f rotationImpulse = this.b2CrossA1.multiplyNew(-1).multiply(this.impulseRotation.x).less(this.c2CrossA1.multiplyNew(this.impulseRotation.y));
+		
+		// Compute the impulse P=J^T * lambda for the lower and upper limits raints
+		final Vector3f limitsImpulse = this.mA1.multiplyNew(this.impulseUpperLimit - this.impulseLowerLimit);
+		
+		// Compute the impulse P=J^T * lambda for the motor raint
+		final Vector3f motorImpulse = this.mA1.multiplyNew(-this.impulseMotor);
+		
+		// Compute the impulse P=J^T * lambda for the 3 translation raints of body 1
+		final Vector3f linearImpulseBody1 = this.impulseTranslation.multiplyNew(-1);
+		final Vector3f angularImpulseBody1 = this.impulseTranslation.cross(this.r1World);
+		
+		// Compute the impulse P=J^T * lambda for the 2 rotation raints of body 1
+		angularImpulseBody1.add(rotationImpulse);
+		
+		// Compute the impulse P=J^T * lambda for the lower and upper limits raints of body 1
+		angularImpulseBody1.add(limitsImpulse);
+		
+		// Compute the impulse P=J^T * lambda for the motor raint of body 1
+		angularImpulseBody1.add(motorImpulse);
+		
+		// Apply the impulse to the body 1
+		v1.add(linearImpulseBody1.multiplyNew(inverseMassBody1));
+		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		
+		// Compute the impulse P=J^T * lambda for the 3 translation raints of body 2
+		final Vector3f angularImpulseBody2 = this.impulseTranslation.cross(this.r2World).multiplyNew(-1);
+		
+		// Compute the impulse P=J^T * lambda for the 2 rotation raints of body 2
+		angularImpulseBody2.add(rotationImpulse.multiplyNew(-1));
+		
+		// Compute the impulse P=J^T * lambda for the lower and upper limits raints of body 2
+		angularImpulseBody2.add(limitsImpulse.multiplyNew(-1));
+		
+		// Compute the impulse P=J^T * lambda for the motor raint of body 2
+		angularImpulseBody2.add(motorImpulse.multiplyNew(-1));
+		
+		// Apply the impulse to the body 2
+		v2.add(this.impulseTranslation.multiplyNew(inverseMassBody2));
+		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+	}
+	
+}

src/org/atriasoft/ephysics/constraint/HingeJointInfo.java

@@ -0,0 +1,89 @@
+package org.atriasoft.ephysics.constraint;
+
+import org.atriasoft.etk.math.Vector3f;
+
+import org.atriasoft.ephysics.body.RigidBody;
+
+/**
+ *  It is used to gather the information needed to create a hinge joint.
+ * This structure will be used to create the actual hinge joint.
+ */
+public class HingeJointInfo extends JointInfo {
+	
+	public Vector3f anchorPointWorldSpace; //!< Anchor point (in world-space coordinates)
+	public Vector3f rotationAxisWorld; //!< Hinge rotation axis (in world-space coordinates)
+	public boolean isLimitEnabled; //!< True if the hinge joint limits are enabled
+	public boolean isMotorEnabled; //!< True if the hinge joint motor is enabled
+	public float minAngleLimit; //!< Minimum allowed rotation angle (in radian) if limits are enabled. The angle must be in the range [-2*pi, 0]
+	public float maxAngleLimit; //!< Maximum allowed rotation angle (in radian) if limits are enabled. The angle must be in the range [0, 2*pi]
+	public float motorSpeed; //!< Motor speed (in radian/second)
+	public float maxMotorTorque; //!< Maximum motor torque (in Newtons * meters) that can be applied to reach to desired motor speed
+	
+	/**
+	 *  Constructor without limits and without motor
+	 * @param _rigidBody1 The first body of the joint
+	 * @param _rigidBody2 The second body of the joint
+	 * @param _initAnchorPointWorldSpace The initial anchor point in world-space coordinates
+	 * @param _initRotationAxisWorld The initial rotation axis in world-space coordinates
+	 */
+	public HingeJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace, final Vector3f _initRotationAxisWorld) {
+		super(_rigidBody1, _rigidBody2, JointType.HINGEJOINT);
+		this.anchorPointWorldSpace = _initAnchorPointWorldSpace;
+		this.rotationAxisWorld = _initRotationAxisWorld;
+		this.isLimitEnabled = false;
+		this.isMotorEnabled = false;
+		this.minAngleLimit = -1;
+		this.maxAngleLimit = 1;
+		this.motorSpeed = 0;
+		this.maxMotorTorque = 0;
+		
+	}
+	
+	/**
+	 *  Constructor with limits but without motor
+	 * @param _rigidBody1 The first body of the joint
+	 * @param _rigidBody2 The second body of the joint
+	 * @param _initAnchorPointWorldSpace The initial anchor point in world-space coordinates
+	 * @param _initRotationAxisWorld The intial rotation axis in world-space coordinates
+	 * @param _initMinAngleLimit The initial minimum limit angle (in radian)
+	 * @param _initMaxAngleLimit The initial maximum limit angle (in radian)
+	 */
+	public HingeJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace, final Vector3f _initRotationAxisWorld, final float _initMinAngleLimit,
+			final float _initMaxAngleLimit) {
+		super(_rigidBody1, _rigidBody2, JointType.HINGEJOINT);
+		this.anchorPointWorldSpace = _initAnchorPointWorldSpace;
+		this.rotationAxisWorld = _initRotationAxisWorld;
+		this.isLimitEnabled = true;
+		this.isMotorEnabled = false;
+		this.minAngleLimit = _initMinAngleLimit;
+		this.maxAngleLimit = _initMaxAngleLimit;
+		this.motorSpeed = 0;
+		this.maxMotorTorque = 0;
+		
+	}
+	
+	/**
+	 *  Constructor with limits and motor
+	 * @param _rigidBody1 The first body of the joint
+	 * @param _rigidBody2 The second body of the joint
+	 * @param _initAnchorPointWorldSpace The initial anchor point in world-space
+	 * @param _initRotationAxisWorld The initial rotation axis in world-space
+	 * @param _initMinAngleLimit The initial minimum limit angle (in radian)
+	 * @param _initMaxAngleLimit The initial maximum limit angle (in radian)
+	 * @param _initMotorSpeed The initial motor speed of the joint (in radian per second)
+	 * @param _initMaxMotorTorque The initial maximum motor torque (in Newtons)
+	 */
+	public HingeJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace, final Vector3f _initRotationAxisWorld, final float _initMinAngleLimit,
+			final float _initMaxAngleLimit, final float _initMotorSpeed, final float _initMaxMotorTorque) {
+		super(_rigidBody1, _rigidBody2, JointType.HINGEJOINT);
+		this.anchorPointWorldSpace = _initAnchorPointWorldSpace;
+		this.rotationAxisWorld = _initRotationAxisWorld;
+		this.isLimitEnabled = true;
+		this.isMotorEnabled = false;
+		this.minAngleLimit = _initMinAngleLimit;
+		this.maxAngleLimit = _initMaxAngleLimit;
+		this.motorSpeed = _initMotorSpeed;
+		this.maxMotorTorque = _initMaxMotorTorque;
+		
+	}
+};

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

@@ -0,0 +1,80 @@
+package org.atriasoft.ephysics.constraint;
+
+import org.atriasoft.ephysics.body.RigidBody;
+import org.atriasoft.ephysics.engine.ConstraintSolverData;
+
+/**
+ *  It represents a joint between two bodies.
+ */
+public abstract class Joint {
+	
+	protected RigidBody body1; //!< Pointer to the first body of the joint
+	protected RigidBody body2; //!< Pointer to the second body of the joint
+	protected JointType type; //!< Type of the joint
+	protected int indexBody1; //!< Body 1 index in the velocity array to solve the raint
+	protected int indexBody2; //!< Body 2 index in the velocity array to solve the raint
+	protected JointsPositionCorrectionTechnique positionCorrectionTechnique; //!< Position correction technique used for the raint (used for joints)
+	protected boolean isCollisionEnabled; //!< True if the two bodies of the raint are allowed to collide with each other
+	public boolean isAlreadyInIsland; //!< True if the joint has already been added into an island
+	
+	/// Constructor
+	public Joint(final 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);
+	}
+	
+	/// Return the reference to the body 1
+	public RigidBody getBody1() {
+		return this.body1;
+	}
+	
+	/// Return the reference to the body 2
+	public RigidBody getBody2() {
+		return this.body2;
+	}
+	
+	/** Return the type of the raint
+	* @return The type of the joint
+	*/
+	public JointType getType() {
+		return this.type;
+	}
+	
+	/// Initialize before solving the joint
+	public abstract void initBeforeSolve(final ConstraintSolverData raintSolverData);
+	
+	/** Return true if the joint is active
+	* @return True if the joint is active
+	*/
+	public boolean isActive() {
+		return (this.body1.isActive() && this.body2.isActive());
+	}
+	
+	/// Return true if the joint has already been added into an island
+	public boolean isAlreadyInIsland() {
+		return this.isAlreadyInIsland;
+	}
+	
+	/**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
+	 */
+	public boolean isCollisionEnabled() {
+		return this.isCollisionEnabled;
+	}
+	
+	/// Solve the position raint
+	public abstract void solvePositionConstraint(final ConstraintSolverData raintSolverData);
+	
+	/// Solve the velocity raint
+	public abstract void solveVelocityConstraint(final ConstraintSolverData raintSolverData);
+	
+	/// Warm start the joint (apply the previous impulse at the beginning of the step)
+	public abstract void warmstart(final ConstraintSolverData raintSolverData);
+}

src/org/atriasoft/ephysics/constraint/JointInfo.java

@@ -0,0 +1,37 @@
+package org.atriasoft.ephysics.constraint;
+
+import org.atriasoft.ephysics.body.RigidBody;
+
+/**
+ *  It is used to gather the information needed to create a joint.
+ */
+public class JointInfo {
+	//!< First rigid body of the joint
+	public final RigidBody body1;
+	//!< Second rigid body of the joint
+	public final RigidBody body2;
+	//!< Type of the joint
+	public final JointType type;
+	//!< Position correction technique used for the raint (used for joints). By default, the BAUMGARTE technique is used
+	public final JointsPositionCorrectionTechnique positionCorrectionTechnique;
+	//!< True if the two bodies of the joint are allowed to collide with each other
+	public final boolean isCollisionEnabled;
+	
+	/// Constructor
+	JointInfo(final JointType _raintType) {
+		this.body1 = null;
+		this.body2 = null;
+		this.type = _raintType;
+		this.positionCorrectionTechnique = JointsPositionCorrectionTechnique.NON_LINEAR_GAUSS_SEIDEL;
+		this.isCollisionEnabled = true;
+	}
+	
+	/// Constructor
+	JointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final JointType _raintType) {
+		this.body1 = _rigidBody1;
+		this.body2 = _rigidBody2;
+		this.type = _raintType;
+		this.positionCorrectionTechnique = JointsPositionCorrectionTechnique.NON_LINEAR_GAUSS_SEIDEL;
+		this.isCollisionEnabled = true;
+	}
+}

src/org/atriasoft/ephysics/constraint/JointListElement.java

@@ -0,0 +1,16 @@
+package org.atriasoft.ephysics.constraint;
+
+public class JointListElement {
+	
+	public Joint joint; //!< Pointer to the actual joint
+	public JointListElement next; //!< Next element of the list
+	
+	/**
+	 * Constructor
+	 */
+	public JointListElement(final Joint _initJoint, final JointListElement _initNext) {
+		this.joint = _initJoint;
+		this.next = _initNext;
+		
+	}
+}

src/org/atriasoft/ephysics/constraint/JointType.java

@@ -0,0 +1,9 @@
+package org.atriasoft.ephysics.constraint;
+
+/// Enumeration for the type of a raint
+public enum JointType {
+	BALLSOCKETJOINT,
+	SLIDERJOINT,
+	HINGEJOINT,
+	FIXEDJOINT
+}

src/org/atriasoft/ephysics/constraint/JointsPositionCorrectionTechnique.java

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

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

@@ -0,0 +1,853 @@
+package org.atriasoft.ephysics.constraint;
+
+import org.atriasoft.etk.math.FMath;
+import org.atriasoft.etk.math.Matrix3f;
+import org.atriasoft.etk.math.Quaternion;
+import org.atriasoft.etk.math.Transform3D;
+import org.atriasoft.etk.math.Vector2f;
+import org.atriasoft.etk.math.Vector3f;
+
+import org.atriasoft.ephysics.body.BodyType;
+import org.atriasoft.ephysics.engine.ConstraintSolverData;
+import org.atriasoft.ephysics.mathematics.Matrix2f;
+
+public class SliderJoint extends Joint {
+	private static final float BETA = 0.2f; //!< Beta value for the position correction bias factor
+	private final Vector3f localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1)
+	private final Vector3f localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2)
+	private final Vector3f sliderAxisBody1; //!< Slider axis (in local-space coordinates of body 1)
+	private Matrix3f i1; //!< Inertia tensor of body 1 (in world-space coordinates)
+	private Matrix3f i2; //!< Inertia tensor of body 2 (in world-space coordinates)
+	private final Quaternion initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the two bodies
+	private Vector3f N1; //!< First vector orthogonal to the slider axis local-space of body 1
+	private Vector3f N2; //!< Second vector orthogonal to the slider axis and N1 in local-space of body 1
+	private Vector3f R1; //!< Vector r1 in world-space coordinates
+	private Vector3f R2; //!< Vector r2 in world-space coordinates
+	private Vector3f R2CrossN1; //!< Cross product of r2 and n1
+	private Vector3f R2CrossN2; //!< Cross product of r2 and n2
+	private Vector3f R2CrossSliderAxis; //!< Cross product of r2 and the slider axis
+	private Vector3f R1PlusUCrossN1; //!< Cross product of vector (r1 + u) and n1
+	private Vector3f R1PlusUCrossN2; //!< Cross product of vector (r1 + u) and n2
+	private Vector3f R1PlusUCrossSliderAxis; //!< Cross product of vector (r1 + u) and the slider axis
+	private Vector2f bTranslation; //!< Bias of the 2 translation raints
+	private Vector3f bRotation; //!< Bias of the 3 rotation raints
+	private float bLowerLimit; //!< Bias of the lower limit raint
+	private float bUpperLimit; //!< Bias of the upper limit raint
+	private Matrix2f inverseMassMatrixTranslationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the translation raint (2x2 matrix)
+	private Matrix3f inverseMassMatrixRotationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the rotation raint (3x3 matrix)
+	private float inverseMassMatrixLimit; //!< Inverse of mass matrix K=JM^-1J^t for the upper and lower limit raints (1x1 matrix)
+	private float inverseMassMatrixMotor; //!< Inverse of mass matrix K=JM^-1J^t for the motor
+	private final Vector2f impulseTranslation; //!< Accumulated impulse for the 2 translation raints
+	private final Vector3f impulseRotation; //!< Accumulated impulse for the 3 rotation raints
+	private float impulseLowerLimit; //!< Accumulated impulse for the lower limit raint
+	private float impulseUpperLimit; //!< Accumulated impulse for the upper limit raint
+	private float impulseMotor; //!< Accumulated impulse for the motor
+	private boolean isLimitEnabled; //!< True if the slider limits are enabled
+	private boolean isMotorEnabled; //!< True if the motor of the joint in enabled
+	private Vector3f sliderAxisWorld; //!< Slider axis in world-space coordinates
+	private float lowerLimit; //!< Lower limit (minimum translation distance)
+	private float upperLimit; //!< Upper limit (maximum translation distance)
+	private boolean isLowerLimitViolated; //!< True if the lower limit is violated
+	private boolean isUpperLimitViolated; //!< True if the upper limit is violated
+	private float motorSpeed; //!< Motor speed (in m/s)
+	private float maxMotorForce; //!< Maximum motor force (in Newtons) that can be applied to reach to desired motor speed
+	/// Constructor
+	
+	public SliderJoint(final SliderJointInfo jointInfo) {
+		super(jointInfo);
+		this.impulseTranslation = new Vector2f(0, 0);
+		this.impulseRotation = new Vector3f(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.0f);
+		assert (this.lowerLimit <= 0.0f);
+		assert (this.maxMotorForce >= 0.0f);
+		
+		// Compute the local-space anchor point for each body
+		final Transform3D transform1 = this.body1.getTransform();
+		final Transform3D transform2 = this.body2.getTransform();
+		this.localAnchorPointBody1 = transform1.inverseNew().multiply(jointInfo.anchorPointWorldSpace);
+		this.localAnchorPointBody2 = transform2.inverseNew().multiply(jointInfo.anchorPointWorldSpace);
+		
+		// Compute the inverse of the initial orientation difference between the two bodies
+		this.initOrientationDifferenceInv = transform2.getOrientation().multiplyNew(transform1.getOrientation().inverseNew());
+		this.initOrientationDifferenceInv.normalize();
+		this.initOrientationDifferenceInv.inverse();
+		
+		// Compute the slider axis in local-space of body 1
+		this.sliderAxisBody1 = this.body1.getTransform().getOrientation().inverseNew().multiply(jointInfo.sliderAxisWorldSpace);
+		this.sliderAxisBody1.normalize();
+	}
+	
+	/// Enable/Disable the limits of the joint
+	/**
+	 * @param isLimitEnabled True if you want to enable the joint limits and false
+	 *					   otherwise
+	 */
+	public void enableLimit(final 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
+	 */
+	public void enableMotor(final boolean isMotorEnabled) {
+		
+		this.isMotorEnabled = isMotorEnabled;
+		this.impulseMotor = 0.0f;
+		
+		// Wake up the two bodies of the joint
+		this.body1.setIsSleeping(false);
+		this.body2.setIsSleeping(false);
+	}
+	
+	/// Return the maximum motor force
+	/**
+	 * @return The maximum force of the joint motor (in Newton x meters)
+	 */
+	public float getMaxMotorForce() {
+		return this.maxMotorForce;
+	}
+	
+	/// Return the maximum translation limit
+	/**
+	 * @return The maximum translation limit of the joint (in meters)
+	 */
+	public float getMaxTranslationLimit() {
+		return this.upperLimit;
+	}
+	
+	/// Return the minimum translation limit
+	/**
+	 * @return The minimum translation limit of the joint (in meters)
+	 */
+	public float getMinTranslationLimit() {
+		return this.lowerLimit;
+	}
+	
+	/// 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)
+	 */
+	public float getMotorForce(final float timeStep) {
+		return this.impulseMotor / timeStep;
+	}
+	
+	/// Return the motor speed
+	/**
+	 * @return The current motor speed of the joint (in meters per second)
+	 */
+	public float getMotorSpeed() {
+		return this.motorSpeed;
+	}
+	
+	/// Return the current translation value of the joint
+	/**
+	 * @return The current translation distance of the joint (in meters)
+	 */
+	public float getTranslation() {
+		
+		// TODO : Check if we need to compare rigid body position or center of mass here
+		
+		// Get the bodies positions and orientations
+		final Vector3f x1 = this.body1.getTransform().getPosition();
+		final Vector3f x2 = this.body2.getTransform().getPosition();
+		final Quaternion q1 = this.body1.getTransform().getOrientation();
+		final Quaternion q2 = this.body2.getTransform().getOrientation();
+		
+		// Compute the two anchor points in world-space coordinates
+		final Vector3f anchorBody1 = x1.addNew(q1.multiply(this.localAnchorPointBody1));
+		final Vector3f anchorBody2 = x2.addNew(q2.multiply(this.localAnchorPointBody2));
+		
+		// Compute the vector u (difference between anchor points)
+		final Vector3f u = anchorBody2.lessNew(anchorBody1);
+		
+		// Compute the slider axis in world-space
+		final Vector3f sliderAxisWorld = q1.multiply(this.sliderAxisBody1);
+		sliderAxisWorld.normalize();
+		
+		// Compute and return the translation value
+		return u.dot(sliderAxisWorld);
+	}
+	
+	@Override
+	public void initBeforeSolve(final ConstraintSolverData raintSolverData) {
+		{
+			
+			// Initialize the bodies index in the veloc ity array
+			this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.get(this.body1);
+			this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.get(this.body2);
+			
+			// Get the bodies positions and orientations
+			final Vector3f x1 = this.body1.centerOfMassWorld;
+			final Vector3f x2 = this.body2.centerOfMassWorld;
+			final Quaternion orientationBody1 = this.body1.getTransform().getOrientation();
+			final 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.multiply(this.localAnchorPointBody1);
+			this.R2 = orientationBody2.multiply(this.localAnchorPointBody2);
+			
+			// Compute the vector u (difference between anchor points)
+			final Vector3f u = x2.addNew(this.R2).less(x1).less(this.R1);
+			
+			// Compute the two orthogonal vectors to the slider axis in world-space
+			this.sliderAxisWorld = orientationBody1.multiply(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
+			final float uDotSliderAxis = u.dot(this.sliderAxisWorld);
+			final float lowerLimitError = uDotSliderAxis - this.lowerLimit;
+			final float upperLimitError = this.upperLimit - uDotSliderAxis;
+			final boolean oldIsLowerLimitViolated = this.isLowerLimitViolated;
+			this.isLowerLimitViolated = lowerLimitError <= 0;
+			if (this.isLowerLimitViolated != oldIsLowerLimitViolated) {
+				this.impulseLowerLimit = 0.0f;
+			}
+			final boolean oldIsUpperLimitViolated = this.isUpperLimitViolated;
+			this.isUpperLimitViolated = upperLimitError <= 0;
+			if (this.isUpperLimitViolated != oldIsUpperLimitViolated) {
+				this.impulseUpperLimit = 0.0f;
+			}
+			
+			// 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);
+			final Vector3f r1PlusU = this.R1.addNew(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)
+			final float sumInverseMass = this.body1.massInverse + this.body2.massInverse;
+			final Vector3f I1R1PlusUCrossN1 = this.i1.multiplyNew(this.R1PlusUCrossN1);
+			final Vector3f I1R1PlusUCrossN2 = this.i1.multiplyNew(this.R1PlusUCrossN2);
+			final Vector3f I2R2CrossN1 = this.i2.multiplyNew(this.R2CrossN1);
+			final Vector3f I2R2CrossN2 = this.i2.multiplyNew(this.R2CrossN2);
+			final float el11 = sumInverseMass + this.R1PlusUCrossN1.dot(I1R1PlusUCrossN1) + this.R2CrossN1.dot(I2R2CrossN1);
+			final float el12 = this.R1PlusUCrossN1.dot(I1R1PlusUCrossN2) + this.R2CrossN1.dot(I2R2CrossN2);
+			final float el21 = this.R1PlusUCrossN2.dot(I1R1PlusUCrossN1) + this.R2CrossN2.dot(I2R2CrossN1);
+			final float el22 = sumInverseMass + this.R1PlusUCrossN2.dot(I1R1PlusUCrossN2) + this.R2CrossN2.dot(I2R2CrossN2);
+			
+			final Matrix2f matrixKTranslation = new Matrix2f(el11, el12, el21, el22);
+			this.inverseMassMatrixTranslationConstraint.setZero();
+			if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) {
+				this.inverseMassMatrixTranslationConstraint = matrixKTranslation.inverseNew();
+			}
+			
+			// Compute the bias "b" of the translation raint
+			this.bTranslation.setZero();
+			final float biasFactor = (BETA / raintSolverData.timeStep);
+			if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) {
+				this.bTranslation.setX(u.dot(this.N1));
+				this.bTranslation.setY(u.dot(this.N2));
+				this.bTranslation.multiply(biasFactor);
+			}
+			
+			// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
+			// contraints (3x3 matrix)
+			this.inverseMassMatrixRotationConstraint = this.i1.addNew(this.i2);
+			if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) {
+				this.inverseMassMatrixRotationConstraint = this.inverseMassMatrixRotationConstraint.inverseNew();
+			}
+			
+			// Compute the bias "b" of the rotation raint
+			this.bRotation.setZero();
+			if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) {
+				final Quaternion currentOrientationDifference = orientationBody2.multiplyNew(orientationBody1.inverseNew());
+				currentOrientationDifference.normalize();
+				final Quaternion qError = currentOrientationDifference.multiplyNew(this.initOrientationDifferenceInv);
+				this.bRotation = qError.getVectorV().multiplyNew(biasFactor * 2.0f);
+			}
+			// 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.massInverse + this.body2.massInverse + this.R1PlusUCrossSliderAxis.dot(this.i1.multiplyNew(this.R1PlusUCrossSliderAxis))
+						+ this.R2CrossSliderAxis.dot(this.i2.multiplyNew(this.R2CrossSliderAxis));
+				this.inverseMassMatrixLimit = (this.inverseMassMatrixLimit > 0.0f) ? 1.0f / this.inverseMassMatrixLimit : 0.0f;
+				// Compute the bias "b" of the lower limit raint
+				this.bLowerLimit = 0.0f;
+				if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) {
+					this.bLowerLimit = biasFactor * lowerLimitError;
+				}
+				// Compute the bias "b" of the upper limit raint
+				this.bUpperLimit = 0.0f;
+				if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) {
+					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.massInverse + this.body2.massInverse;
+				this.inverseMassMatrixMotor = (this.inverseMassMatrixMotor > 0.0f) ? 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.0f;
+				this.impulseUpperLimit = 0.0f;
+				this.impulseMotor = 0.0f;
+			}
+		}
+	}
+	
+	/// Return true if the limits or the joint are enabled
+	/**
+	 * @return True if the joint limits are enabled
+	 */
+	public boolean isLimitEnabled() {
+		return this.isLimitEnabled;
+	}
+	
+	/// Return true if the motor of the joint is enabled
+	/**
+	 * @return True if the joint motor is enabled
+	 */
+	public boolean isMotorEnabled() {
+		return this.isMotorEnabled;
+	}
+	
+	/// Reset the limits
+	private void resetLimits() {
+		
+		// Reset the accumulated impulses for the limits
+		this.impulseLowerLimit = 0.0f;
+		this.impulseUpperLimit = 0.0f;
+		
+		// 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)
+	 */
+	public void setMaxMotorForce(final float maxMotorForce) {
+		
+		if (maxMotorForce != this.maxMotorForce) {
+			
+			assert (this.maxMotorForce >= 0.0f);
+			this.maxMotorForce = maxMotorForce;
+			
+			// Wake up the two bodies of the joint
+			this.body1.setIsSleeping(false);
+			this.body2.setIsSleeping(false);
+		}
+	}
+	
+	/// Set the maximum translation limit
+	/**
+	 * @param lowerLimit The maximum translation limit of the joint (in meters)
+	 */
+	public void setMaxTranslationLimit(final float upperLimit) {
+		
+		assert (this.lowerLimit <= upperLimit);
+		
+		if (upperLimit != this.upperLimit) {
+			
+			this.upperLimit = upperLimit;
+			
+			// Reset the limits
+			resetLimits();
+		}
+	}
+	
+	/// Set the minimum translation limit
+	/**
+	 * @param lowerLimit The minimum translation limit of the joint (in meters)
+	 */
+	public void setMinTranslationLimit(final float lowerLimit) {
+		
+		assert (lowerLimit <= this.upperLimit);
+		
+		if (lowerLimit != this.lowerLimit) {
+			
+			this.lowerLimit = lowerLimit;
+			
+			// Reset the limits
+			resetLimits();
+		}
+	}
+	
+	/// Set the motor speed
+	/**
+	 * @param motorSpeed The speed of the joint motor (in meters per second)
+	 */
+	public void setMotorSpeed(final 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);
+		}
+	}
+	
+	@Override
+	public void solvePositionConstraint(final 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 != JointsPositionCorrectionTechnique.NON_LINEAR_GAUSS_SEIDEL) {
+			return;
+		}
+		
+		// Get the bodies positions and orientations
+		final Vector3f x1 = raintSolverData.positions[this.indexBody1];
+		final Vector3f x2 = raintSolverData.positions[this.indexBody2];
+		final Quaternion q1 = raintSolverData.orientations[this.indexBody1];
+		final Quaternion q2 = raintSolverData.orientations[this.indexBody2];
+		
+		// Get the inverse mass and inverse inertia tensors of the bodies
+		final float inverseMassBody1 = this.body1.massInverse;
+		final float inverseMassBody2 = this.body2.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.multiply(this.localAnchorPointBody1);
+		this.R2 = q2.multiply(this.localAnchorPointBody2);
+		
+		// Compute the vector u (difference between anchor points)
+		final Vector3f u = x2.addNew(this.R2).less(x1).less(this.R1);
+		
+		// Compute the two orthogonal vectors to the slider axis in world-space
+		this.sliderAxisWorld = q1.multiply(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
+		final float uDotSliderAxis = u.dot(this.sliderAxisWorld);
+		final float lowerLimitError = uDotSliderAxis - this.lowerLimit;
+		final 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);
+		final Vector3f r1PlusU = this.R1.addNew(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)
+		final float sumInverseMass = this.body1.massInverse + this.body2.massInverse;
+		final Vector3f I1R1PlusUCrossN1 = this.i1.multiplyNew(this.R1PlusUCrossN1);
+		final Vector3f I1R1PlusUCrossN2 = this.i1.multiplyNew(this.R1PlusUCrossN2);
+		final Vector3f I2R2CrossN1 = this.i2.multiplyNew(this.R2CrossN1);
+		final Vector3f I2R2CrossN2 = this.i2.multiplyNew(this.R2CrossN2);
+		final float el11 = sumInverseMass + this.R1PlusUCrossN1.dot(I1R1PlusUCrossN1) + this.R2CrossN1.dot(I2R2CrossN1);
+		final float el12 = this.R1PlusUCrossN1.dot(I1R1PlusUCrossN2) + this.R2CrossN1.dot(I2R2CrossN2);
+		final float el21 = this.R1PlusUCrossN2.dot(I1R1PlusUCrossN1) + this.R2CrossN2.dot(I2R2CrossN1);
+		final float el22 = sumInverseMass + this.R1PlusUCrossN2.dot(I1R1PlusUCrossN2) + this.R2CrossN2.dot(I2R2CrossN2);
+		
+		final Matrix2f matrixKTranslation = new Matrix2f(el11, el12, el21, el22);
+		this.inverseMassMatrixTranslationConstraint.setZero();
+		if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) {
+			this.inverseMassMatrixTranslationConstraint = matrixKTranslation.inverseNew();
+		}
+		
+		// Compute the position error for the 2 translation raints
+		final Vector2f translationError = new Vector2f(u.dot(this.N1), u.dot(this.N2));
+		
+		// Compute the Lagrange multiplier lambda for the 2 translation raints
+		final Vector2f lambdaTranslation = this.inverseMassMatrixTranslationConstraint.multiplyNew(translationError.multiplyNew(-1));
+		
+		// Compute the impulse P=J^T * lambda for the 2 translation raints of body 1
+		final Vector3f linearImpulseBody1 = this.N1.multiplyNew(-1).multiplyNew(lambdaTranslation.x).lessNew(this.N2.multiplyNew(lambdaTranslation.y));
+		Vector3f angularImpulseBody1 = this.R1PlusUCrossN1.multiplyNew(-1).multiplyNew(lambdaTranslation.x).lessNew(this.R1PlusUCrossN2.multiplyNew(lambdaTranslation.y));
+		
+		// Apply the impulse to the body 1
+		final Vector3f v1 = linearImpulseBody1.multiplyNew(inverseMassBody1);
+		Vector3f w1 = this.i1.multiplyNew(angularImpulseBody1);
+		
+		// Update the body position/orientation of body 1
+		x1.add(v1);
+		q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
+		q1.normalize();
+		
+		// Compute the impulse P=J^T * lambda for the 2 translation raints of body 2
+		final Vector3f linearImpulseBody2 = this.N1.multiplyNew(lambdaTranslation.x).addNew(this.N2.multiplyNew(lambdaTranslation.y));
+		Vector3f angularImpulseBody2 = this.R2CrossN1.multiplyNew(lambdaTranslation.x).add(this.R2CrossN2.multiplyNew(lambdaTranslation.y));
+		
+		// Apply the impulse to the body 2
+		final Vector3f v2 = linearImpulseBody2.multiplyNew(inverseMassBody2);
+		Vector3f w2 = this.i2.multiplyNew(angularImpulseBody2);
+		
+		// Update the body position/orientation of body 2
+		x2.add(v2);
+		q2.add((new Quaternion(0, w2)).multiply(q2).multiply(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.addNew(this.i2);
+		if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) {
+			this.inverseMassMatrixRotationConstraint = this.inverseMassMatrixRotationConstraint.inverseNew();
+		}
+		
+		// Compute the position error for the 3 rotation raints
+		final Quaternion currentOrientationDifference = q2.multiplyNew(q1.inverseNew());
+		currentOrientationDifference.normalize();
+		final Quaternion qError = currentOrientationDifference.multiplyNew(this.initOrientationDifferenceInv);
+		final Vector3f errorRotation = qError.getVectorV().multiply(2.0f);
+		
+		// Compute the Lagrange multiplier lambda for the 3 rotation raints
+		final Vector3f lambdaRotation = this.inverseMassMatrixRotationConstraint.multiplyNew(errorRotation.multiplyNew(-1));
+		
+		// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
+		angularImpulseBody1 = lambdaRotation.multiplyNew(-1);
+		
+		// Apply the impulse to the body 1
+		w1 = this.i1.multiplyNew(angularImpulseBody1);
+		
+		// Update the body position/orientation of body 1
+		q1.add((new Quaternion(0, w1)).multiply(q1).multiply(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.multiplyNew(angularImpulseBody2);
+		
+		// Update the body position/orientation of body 2
+		q2.add((new Quaternion(0, w2)).multiply(q2).multiply(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.massInverse + this.body2.massInverse + this.R1PlusUCrossSliderAxis.dot(this.i1.multiplyNew(this.R1PlusUCrossSliderAxis))
+						+ this.R2CrossSliderAxis.dot(this.i2.multiplyNew(this.R2CrossSliderAxis));
+				this.inverseMassMatrixLimit = (this.inverseMassMatrixLimit > 0.0f) ? 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
+				final float lambdaLowerLimit = this.inverseMassMatrixLimit * (-lowerLimitError);
+				
+				// Compute the impulse P=J^T * lambda for the lower limit raint of body 1
+				final Vector3f linearImpulseBody1_tmp = this.sliderAxisWorld.multiplyNew(-lambdaLowerLimit);
+				final Vector3f angularImpulseBody1_tmp = this.R1PlusUCrossSliderAxis.multiplyNew(-lambdaLowerLimit);
+				
+				// Apply the impulse to the body 1
+				final Vector3f v1_tmp = linearImpulseBody1_tmp.multiplyNew(inverseMassBody1);
+				final Vector3f w1_tmp = this.i1.multiplyNew(angularImpulseBody1_tmp);
+				
+				// Update the body position/orientation of body 1
+				x1.add(v1_tmp);
+				q1.add((new Quaternion(0, w1_tmp)).multiply(q1).multiply(0.5f));
+				q1.normalize();
+				
+				// Compute the impulse P=J^T * lambda for the lower limit raint of body 2
+				final Vector3f linearImpulseBody2_tmp = this.sliderAxisWorld.multiplyNew(lambdaLowerLimit);
+				final Vector3f angularImpulseBody2_tmp = this.R2CrossSliderAxis.multiplyNew(lambdaLowerLimit);
+				
+				// Apply the impulse to the body 2
+				final Vector3f v2_tmp = linearImpulseBody2_tmp.multiplyNew(inverseMassBody2);
+				final Vector3f w2_tmp = this.i2.multiplyNew(angularImpulseBody2_tmp);
+				
+				// Update the body position/orientation of body 2
+				x2.add(v2_tmp);
+				q2.add((new Quaternion(0, w2_tmp)).multiply(q2).multiply(0.5f));
+				q2.normalize();
+			}
+			
+			// If the upper limit is violated
+			if (this.isUpperLimitViolated) {
+				
+				// Compute the Lagrange multiplier lambda for the upper limit raint
+				final float lambdaUpperLimit = this.inverseMassMatrixLimit * (-upperLimitError);
+				
+				// Compute the impulse P=J^T * lambda for the upper limit raint of body 1
+				final Vector3f linearImpulseBody1_tmp = this.sliderAxisWorld.multiplyNew(lambdaUpperLimit);
+				final Vector3f angularImpulseBody1_tmp = this.R1PlusUCrossSliderAxis.multiplyNew(lambdaUpperLimit);
+				
+				// Apply the impulse to the body 1
+				final Vector3f v1_tmp = linearImpulseBody1_tmp.multiplyNew(inverseMassBody1);
+				final Vector3f w1_tmp = this.i1.multiplyNew(angularImpulseBody1_tmp);
+				
+				// Update the body position/orientation of body 1
+				x1.add(v1_tmp);
+				q1.add((new Quaternion(0, w1_tmp)).multiply(q1).multiply(0.5f));
+				q1.normalize();
+				
+				// Compute the impulse P=J^T * lambda for the upper limit raint of body 2
+				final Vector3f linearImpulseBody2_tmp = this.sliderAxisWorld.multiplyNew(-lambdaUpperLimit);
+				final Vector3f angularImpulseBody2_tmp = this.R2CrossSliderAxis.multiplyNew(-lambdaUpperLimit);
+				
+				// Apply the impulse to the body 2
+				final Vector3f v2_tmp = linearImpulseBody2_tmp.multiplyNew(inverseMassBody2);
+				final Vector3f w2_tmp = this.i2.multiplyNew(angularImpulseBody2_tmp);
+				
+				// Update the body position/orientation of body 2
+				x2.add(v2_tmp);
+				q2.add((new Quaternion(0, w2_tmp)).multiply(q2).multiply(0.5f));
+				q2.normalize();
+			}
+		}
+	}
+	
+	@Override
+	public void solveVelocityConstraint(final ConstraintSolverData raintSolverData) {
+		
+		// Get the velocities
+		final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+		final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+		final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+		final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+		
+		// Get the inverse mass and inverse inertia tensors of the bodies
+		final float inverseMassBody1 = this.body1.massInverse;
+		final float inverseMassBody2 = this.body2.massInverse;
+		
+		// --------------- Translation Constraints --------------- //
+		
+		// Compute J*v for the 2 translation raints
+		final float el1 = -this.N1.dot(v1) - w1.dot(this.R1PlusUCrossN1) + this.N1.dot(v2) + w2.dot(this.R2CrossN1);
+		final float el2 = -this.N2.dot(v1) - w1.dot(this.R1PlusUCrossN2) + this.N2.dot(v2) + w2.dot(this.R2CrossN2);
+		
+		final Vector2f JvTranslation = new Vector2f(el1, el2);
+		
+		// Compute the Lagrange multiplier lambda for the 2 translation raints
+		final Vector2f deltaLambda = this.inverseMassMatrixTranslationConstraint.multiplyNew(JvTranslation.multiplyNew(-1).less(this.bTranslation));
+		this.impulseTranslation.add(deltaLambda);
+		
+		// Compute the impulse P=J^T * lambda for the 2 translation raints of body 1
+		final Vector3f linearImpulseBody1 = this.N1.multiplyNew(-1).multiply(deltaLambda.x).less(this.N2.multiplyNew(deltaLambda.y));
+		Vector3f angularImpulseBody1 = this.R1PlusUCrossN1.multiplyNew(-1).multiply(deltaLambda.x).less(this.R1PlusUCrossN2.multiplyNew(deltaLambda.y));
+		
+		// Apply the impulse to the body 1
+		v1.add(linearImpulseBody1.multiplyNew(inverseMassBody1));
+		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		
+		// Compute the impulse P=J^T * lambda for the 2 translation raints of body 2
+		final Vector3f linearImpulseBody2 = this.N1.multiplyNew(deltaLambda.x).add(this.N2.multiplyNew(deltaLambda.y));
+		Vector3f angularImpulseBody2 = this.R2CrossN1.multiplyNew(deltaLambda.x).add(this.R2CrossN2.multiplyNew(deltaLambda.y));
+		
+		// Apply the impulse to the body 2
+		v2.add(linearImpulseBody2.multiplyNew(inverseMassBody2));
+		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+		
+		// --------------- Rotation Constraints --------------- //
+		
+		// Compute J*v for the 3 rotation raints
+		final Vector3f JvRotation = w2.lessNew(w1);
+		
+		// Compute the Lagrange multiplier lambda for the 3 rotation raints
+		final Vector3f deltaLambda2 = this.inverseMassMatrixRotationConstraint.multiplyNew(JvRotation.multiplyNew(-1).less(this.bRotation));
+		this.impulseRotation.add(deltaLambda2);
+		
+		// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
+		angularImpulseBody1 = deltaLambda2.multiplyNew(-1);
+		
+		// Apply the impulse to the body to body 1
+		w1.add(this.i1.multiplyNew(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.add(this.i2.multiplyNew(angularImpulseBody2));
+		
+		// --------------- Limits Constraints --------------- //
+		
+		if (this.isLimitEnabled) {
+			
+			// If the lower limit is violated
+			if (this.isLowerLimitViolated) {
+				
+				// Compute J*v for the lower limit raint
+				final 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);
+				final float lambdaTemp = this.impulseLowerLimit;
+				this.impulseLowerLimit = FMath.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
+				final Vector3f linearImpulseBody1_tmp = this.sliderAxisWorld.multiplyNew(-deltaLambdaLower);
+				final Vector3f angularImpulseBody1_tmp = this.R1PlusUCrossSliderAxis.multiplyNew(-deltaLambdaLower);
+				
+				// Apply the impulse to the body 1
+				v1.add(linearImpulseBody1_tmp.multiplyNew(inverseMassBody1));
+				w1.add(this.i1.multiplyNew(angularImpulseBody1_tmp));
+				
+				// Compute the impulse P=J^T * lambda for the lower limit raint of body 2
+				final Vector3f linearImpulseBody2_tmp = this.sliderAxisWorld.multiplyNew(deltaLambdaLower);
+				final Vector3f angularImpulseBody2_tmp = this.R2CrossSliderAxis.multiplyNew(deltaLambdaLower);
+				
+				// Apply the impulse to the body 2
+				v2.add(linearImpulseBody2_tmp.multiplyNew(inverseMassBody2));
+				w2.add(this.i2.multiplyNew(angularImpulseBody2_tmp));
+			}
+			
+			// If the upper limit is violated
+			if (this.isUpperLimitViolated) {
+				
+				// Compute J*v for the upper limit raint
+				final 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);
+				final float lambdaTemp = this.impulseUpperLimit;
+				this.impulseUpperLimit = FMath.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
+				final Vector3f linearImpulseBody1_tmp = this.sliderAxisWorld.multiplyNew(deltaLambdaUpper);
+				final Vector3f angularImpulseBody1_tmp = this.R1PlusUCrossSliderAxis.multiplyNew(deltaLambdaUpper);
+				
+				// Apply the impulse to the body 1
+				v1.add(linearImpulseBody1_tmp.multiplyNew(inverseMassBody1));
+				w1.add(this.i1.multiplyNew(angularImpulseBody1_tmp));
+				
+				// Compute the impulse P=J^T * lambda for the upper limit raint of body 2
+				final Vector3f linearImpulseBody2_tmp = this.sliderAxisWorld.multiplyNew(-deltaLambdaUpper);
+				final Vector3f angularImpulseBody2_tmp = this.R2CrossSliderAxis.multiplyNew(-deltaLambdaUpper);
+				
+				// Apply the impulse to the body 2
+				v2.add(linearImpulseBody2_tmp.multiplyNew(inverseMassBody2));
+				w2.add(this.i2.multiplyNew(angularImpulseBody2_tmp));
+			}
+		}
+		
+		// --------------- Motor --------------- //
+		
+		if (this.isMotorEnabled) {
+			
+			// Compute J*v for the motor
+			final float JvMotor = this.sliderAxisWorld.dot(v1) - this.sliderAxisWorld.dot(v2);
+			
+			// Compute the Lagrange multiplier lambda for the motor
+			final float maxMotorImpulse = this.maxMotorForce * raintSolverData.timeStep;
+			float deltaLambdaMotor = this.inverseMassMatrixMotor * (-JvMotor - this.motorSpeed);
+			final float lambdaTemp = this.impulseMotor;
+			this.impulseMotor = FMath.clamp(this.impulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
+			deltaLambdaMotor = this.impulseMotor - lambdaTemp;
+			
+			// Compute the impulse P=J^T * lambda for the motor of body 1
+			final Vector3f linearImpulseBody1_tmp = this.sliderAxisWorld.multiplyNew(deltaLambdaMotor);
+			
+			// Apply the impulse to the body 1
+			v1.add(linearImpulseBody1_tmp.multiplyNew(inverseMassBody1));
+			
+			// Compute the impulse P=J^T * lambda for the motor of body 2
+			final Vector3f linearImpulseBody2_tmp = this.sliderAxisWorld.multiplyNew(-deltaLambdaMotor);
+			
+			// Apply the impulse to the body 2
+			v2.add(linearImpulseBody2_tmp.multiplyNew(inverseMassBody2));
+		}
+	}
+	
+	@Override
+	public void warmstart(final ConstraintSolverData raintSolverData) {
+		// Get the velocities
+		final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+		final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+		final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+		final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+		
+		// Get the inverse mass and inverse inertia tensors of the bodies
+		final float inverseMassBody1 = this.body1.massInverse;
+		final float inverseMassBody2 = this.body2.massInverse;
+		
+		// Compute the impulse P=J^T * lambda for the lower and upper limits raints of body 1
+		final float impulseLimits = this.impulseUpperLimit - this.impulseLowerLimit;
+		final Vector3f linearImpulseLimits = this.sliderAxisWorld.multiplyNew(impulseLimits);
+		
+		// Compute the impulse P=J^T * lambda for the motor raint of body 1
+		final Vector3f impulseMotor = this.sliderAxisWorld.multiplyNew(this.impulseMotor);
+		
+		// Compute the impulse P=J^T * lambda for the 2 translation raints of body 1
+		final Vector3f linearImpulseBody1 = this.N1.multiplyNew(-1).multiply(this.impulseTranslation.x).less(this.N2.multiplyNew(this.impulseTranslation.y));
+		final Vector3f angularImpulseBody1 = this.R1PlusUCrossN1.multiplyNew(-1).multiply(this.impulseTranslation.x).less(this.R1PlusUCrossN2.multiplyNew(this.impulseTranslation.y));
+		
+		// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
+		angularImpulseBody1.add(this.impulseRotation.multiplyNew(-1));
+		
+		// Compute the impulse P=J^T * lambda for the lower and upper limits raints of body 1
+		linearImpulseBody1.add(linearImpulseLimits);
+		angularImpulseBody1.add(this.R1PlusUCrossSliderAxis.multiplyNew(impulseLimits));
+		
+		// Compute the impulse P=J^T * lambda for the motor raint of body 1
+		linearImpulseBody1.add(impulseMotor);
+		
+		// Apply the impulse to the body 1
+		v1.add(linearImpulseBody1.multiplyNew(inverseMassBody1));
+		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		
+		// Compute the impulse P=J^T * lambda for the 2 translation raints of body 2
+		final Vector3f linearImpulseBody2 = this.N1.multiplyNew(this.impulseTranslation.x).add(this.N2.multiplyNew(this.impulseTranslation.y));
+		final Vector3f angularImpulseBody2 = this.R2CrossN1.multiplyNew(this.impulseTranslation.x).add(this.R2CrossN2.multiplyNew(this.impulseTranslation.y));
+		
+		// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 2
+		angularImpulseBody2.add(this.impulseRotation);
+		
+		// Compute the impulse P=J^T * lambda for the lower and upper limits raints of body 2
+		linearImpulseBody2.less(linearImpulseLimits);
+		angularImpulseBody2.add(this.R2CrossSliderAxis.multiplyNew(-impulseLimits));
+		
+		// Compute the impulse P=J^T * lambda for the motor raint of body 2
+		linearImpulseBody2.add(impulseMotor.multiplyNew(-1));
+		
+		// Apply the impulse to the body 2
+		v2.add(linearImpulseBody2.multiplyNew(inverseMassBody2));
+		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+	}
+	
+}

src/org/atriasoft/ephysics/constraint/SliderJointInfo.java

@@ -0,0 +1,89 @@
+package org.atriasoft.ephysics.constraint;
+
+import org.atriasoft.etk.math.Vector3f;
+
+import org.atriasoft.ephysics.body.RigidBody;
+
+/**
+ * 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.
+ */
+public class SliderJointInfo extends JointInfo {
+	
+	public Vector3f anchorPointWorldSpace; //!< Anchor point (in world-space coordinates)
+	public Vector3f sliderAxisWorldSpace; //!< Slider axis (in world-space coordinates)
+	public boolean isLimitEnabled; //!< True if the slider limits are enabled
+	public boolean isMotorEnabled; //!< True if the slider motor is enabled
+	public float minTranslationLimit; //!< Mininum allowed translation if limits are enabled
+	public float maxTranslationLimit; //!< Maximum allowed translation if limits are enabled
+	public float motorSpeed; //!< Motor speed
+	public float maxMotorForce; //!< Maximum motor force (in Newtons) that can be applied to reach to desired motor speed
+	
+	/** 
+		 *  Constructor without limits and without motor
+		 * @param _rigidBody1 The first body of the joint
+		 * @param _rigidBody2 The second body of the joint
+		 * @param _initAnchorPointWorldSpace The initial anchor point in world-space
+		 * @param _initSliderAxisWorldSpace The initial slider axis in world-space
+		 */
+	public SliderJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace, final Vector3f _initSliderAxisWorldSpace) {
+		super(_rigidBody1, _rigidBody2, JointType.SLIDERJOINT);
+		this.anchorPointWorldSpace = _initAnchorPointWorldSpace;
+		this.sliderAxisWorldSpace = _initSliderAxisWorldSpace;
+		this.isLimitEnabled = false;
+		this.isMotorEnabled = false;
+		this.minTranslationLimit = -1.0f;
+		this.maxTranslationLimit = 1.0f;
+		this.motorSpeed = 0;
+		this.maxMotorForce = 0;
+		
+	}
+	
+	/**
+		 *  Constructor with limits and no motor
+		 * @param _rigidBody1 The first body of the joint
+		 * @param _rigidBody2 The second body of the joint
+		 * @param _initAnchorPointWorldSpace The initial anchor point in world-space
+		 * @param _initSliderAxisWorldSpace The initial slider axis in world-space
+		 * @param _initMinTranslationLimit The initial minimum translation limit (in meters)
+		 * @param _initMaxTranslationLimit The initial maximum translation limit (in meters)
+		 */
+	public SliderJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace, final Vector3f _initSliderAxisWorldSpace,
+			final float _initMinTranslationLimit, final float _initMaxTranslationLimit) {
+		super(_rigidBody1, _rigidBody2, JointType.SLIDERJOINT);
+		this.anchorPointWorldSpace = _initAnchorPointWorldSpace;
+		this.sliderAxisWorldSpace = _initSliderAxisWorldSpace;
+		this.isLimitEnabled = true;
+		this.isMotorEnabled = false;
+		this.minTranslationLimit = _initMinTranslationLimit;
+		this.maxTranslationLimit = _initMaxTranslationLimit;
+		this.motorSpeed = 0;
+		this.maxMotorForce = 0;
+		
+	}
+	
+	/**
+		 *  Constructor with limits and motor
+		 * @param _rigidBody1 The first body of the joint
+		 * @param _rigidBody2 The second body of the joint
+		 * @param _initAnchorPointWorldSpace The initial anchor point in world-space
+		 * @param _initSliderAxisWorldSpace The initial slider axis in world-space
+		 * @param _initMinTranslationLimit The initial minimum translation limit (in meters)
+		 * @param _initMaxTranslationLimit The initial maximum translation limit (in meters)
+		 * @param _initMotorSpeed The initial speed of the joint motor (in meters per second)
+		 * @param _initMaxMotorForce The initial maximum motor force of the joint (in Newtons x meters)
+		 */
+	public SliderJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace, final Vector3f _initSliderAxisWorldSpace,
+			final float _initMinTranslationLimit, final float _initMaxTranslationLimit, final float _initMotorSpeed, final float _initMaxMotorForce) {
+		super(_rigidBody1, _rigidBody2, JointType.SLIDERJOINT);
+		this.anchorPointWorldSpace = _initAnchorPointWorldSpace;
+		this.sliderAxisWorldSpace = _initSliderAxisWorldSpace;
+		this.isLimitEnabled = true;
+		this.isMotorEnabled = true;
+		this.minTranslationLimit = _initMinTranslationLimit;
+		this.maxTranslationLimit = _initMaxTranslationLimit;
+		this.motorSpeed = _initMotorSpeed;
+		this.maxMotorForce = _initMaxMotorForce;
+		
+	}
+}

src/org/atriasoft/ephysics/engine/CollisionCallback.java

@@ -0,0 +1,17 @@
+package org.atriasoft.ephysics.engine;
+
+import org.atriasoft.ephysics.constraint.ContactPointInfo;
+
+/**
+ *  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.
+ */
+public interface CollisionCallback {
+	/**
+	 *  This method will be called for contact.
+	 * @param _contactPointInfo Contact information property.
+	 */
+	void notifyContact(ContactPointInfo _contactPointInfo);
+}

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

@@ -0,0 +1,254 @@
+package org.atriasoft.ephysics.engine;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import org.atriasoft.ephysics.RaycastCallback;
+import org.atriasoft.ephysics.body.CollisionBody;
+import org.atriasoft.ephysics.collision.CollisionDetection;
+import org.atriasoft.ephysics.collision.ProxyShape;
+import org.atriasoft.ephysics.collision.broadphase.DTree;
+import org.atriasoft.ephysics.collision.narrowphase.CollisionDispatch;
+import org.atriasoft.ephysics.collision.shapes.AABB;
+import org.atriasoft.ephysics.mathematics.Ray;
+import org.atriasoft.ephysics.mathematics.Set;
+import org.atriasoft.etk.math.Transform3D;
+
+/**
+ *  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.
+ */
+public class CollisionWorld {
+	
+	public CollisionDetection collisionDetection; //!< Reference to the collision detection
+	protected Set<CollisionBody> bodies = new Set<CollisionBody>(Set.getCollisionBodyCoparator()); //!< All the bodies (rigid and soft) of the world
+	protected int currentBodyID; //!< Current body ID
+	protected List<Integer> freeBodiesIDs = new ArrayList<>(); //!< List of free ID for rigid bodies
+	public EventListener eventListener; //!< Pointer to an event listener object
+	/// Return the next available body ID
+	
+	/// Constructor
+	public CollisionWorld() {
+		this.collisionDetection = new CollisionDetection(this);
+		this.currentBodyID = 0;
+		this.eventListener = null;
+	}
+	
+	int computeNextAvailableBodyID() {
+		// Compute the body ID
+		int bodyID;
+		if (!this.freeBodiesIDs.isEmpty()) {
+			bodyID = this.freeBodiesIDs.get(0);
+			this.freeBodiesIDs.remove(bodyID);
+		} else {
+			bodyID = this.currentBodyID;
+			this.currentBodyID++;
+		}
+		return bodyID;
+	}
+	
+	/*
+	public Set<CollisionBody>::Iterator getBodiesEndIterator() {
+			return this.bodies.end();
+		}
+	*/
+	/**
+	*  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
+	*/
+	public CollisionBody createCollisionBody(final Transform3D transform) {
+		// Get the next available body ID
+		final int bodyID = computeNextAvailableBodyID();
+		// Largest index cannot be used (it is used for invalid index)
+		assert (bodyID < Integer.MAX_VALUE);
+		// Create the collision body
+		final CollisionBody collisionBody = new CollisionBody(transform, this, bodyID);
+		assert (collisionBody != null);
+		// Add the collision body to the world
+		this.bodies.add(collisionBody);
+		// Return the pointer to the rigid body
+		return collisionBody;
+	}
+	
+	/**
+	 *  Destroy a collision body
+	 * @param collisionBody Pointer to the body to destroy
+	 */
+	public void 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.add(collisionBody.getID());
+		// Remove the collision body from the list of bodies
+		this.bodies.erase(collisionBody);
+		collisionBody = null;
+	}
+	
+	/**
+	*  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
+	*/
+	/*
+	public Set<CollisionBody>::Iterator getBodiesBeginIterator() {
+			return this.bodies.begin();
+		}
+	*/
+	/**
+	*  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
+	*/
+	
+	public CollisionDetection getCollisionDetection() {
+		return this.collisionDetection;
+	}
+	
+	/**
+	 *  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
+	 */
+	public void raycast(final Ray _ray, final RaycastCallback _raycastCallback) {
+		raycast(_ray, _raycastCallback, 0xFFFF);
+	}
+	
+	public void raycast(final Ray _ray, final RaycastCallback _raycastCallback, final int _raycastWithCategoryMaskBits) {
+		this.collisionDetection.raycast(_raycastCallback, _ray, _raycastWithCategoryMaskBits);
+	}
+	
+	/// Reset all the contact manifolds linked list of each body
+	void resetContactManifoldListsOfBodies() {
+		// For each rigid body of the world
+		for (final CollisionBody it : this.bodies) {
+			// Reset the contact manifold list of the body
+			it.resetContactManifoldsList();
+		}
+	}
+	
+	/**
+	 *  Set the collision dispatch configuration
+	 * This can be used to replace default collision detection algorithms by your
+	 * custom algorithm for instance.
+	 * @param _CollisionDispatch Pointer to a collision dispatch object describing
+	 * which collision detection algorithm to use for two given collision shapes
+	 */
+	public void setCollisionDispatch(final CollisionDispatch _collisionDispatch) {
+		this.collisionDetection.setCollisionDispatch(_collisionDispatch);
+	}
+	
+	/**
+	 *  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
+	 */
+	public boolean testAABBOverlap(final CollisionBody _body1, final 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
+		final AABB body1AABB = _body1.getAABB();
+		final AABB body2AABB = _body2.getAABB();
+		// Return true if the two AABBs overlap
+		return body1AABB.testCollision(body2AABB);
+	}
+	
+	/**
+	 *  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
+	 */
+	public boolean testAABBOverlap(final ProxyShape _shape1, final ProxyShape _shape2) {
+		return this.collisionDetection.testAABBOverlap(_shape1, _shape2);
+	}
+	
+	/**
+	 *  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
+	 */
+	public void testCollision(final CollisionBody _body1, final CollisionBody _body2, final CollisionCallback _callback) {
+		// Reset all the contact manifolds lists of each body
+		resetContactManifoldListsOfBodies();
+		// Create the sets of shapes
+		final Set<DTree> shapes1 = new Set<DTree>(Set.getDTreeCoparator());
+		for (ProxyShape shape = _body1.getProxyShapesList(); shape != null; shape = shape.getNext()) {
+			shapes1.add(shape.broadPhaseID);
+		}
+		final Set<DTree> shapes2 = new Set<DTree>(Set.getDTreeCoparator());
+		for (ProxyShape shape = _body2.getProxyShapesList(); shape != null; shape = shape.getNext()) {
+			shapes2.add(shape.broadPhaseID);
+		}
+		// Perform the collision detection and report contacts
+		this.collisionDetection.testCollisionBetweenShapes(_callback, shapes1, shapes2);
+	}
+	
+	/**
+	 *  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
+	 */
+	public void testCollision(final CollisionBody _body, final CollisionCallback _callback) {
+		// Reset all the contact manifolds lists of each body
+		resetContactManifoldListsOfBodies();
+		// Create the sets of shapes
+		final Set<DTree> shapes1 = new Set<DTree>(Set.getDTreeCoparator());
+		// For each shape of the body
+		for (ProxyShape shape = _body.getProxyShapesList(); shape != null; shape = shape.getNext()) {
+			shapes1.add(shape.broadPhaseID);
+		}
+		final Set<DTree> emptySet = new Set<DTree>(Set.getDTreeCoparator());
+		// Perform the collision detection and report contacts
+		this.collisionDetection.testCollisionBetweenShapes(_callback, shapes1, emptySet);
+	}
+	
+	/**
+	 *  Test and report collisions between all shapes of the world
+	 * @param _callback Pointer to the object with the callback method
+	 */
+	public void testCollision(final CollisionCallback _callback) {
+		// Reset all the contact manifolds lists of each body
+		resetContactManifoldListsOfBodies();
+		final Set<DTree> emptySet = new Set<DTree>(Set.getDTreeCoparator());
+		// Perform the collision detection and report contacts
+		this.collisionDetection.testCollisionBetweenShapes(_callback, emptySet, emptySet);
+	}
+	
+	/**
+	 *  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
+	 */
+	public void testCollision(final ProxyShape _shape, final CollisionCallback _callback) {
+		// Reset all the contact manifolds lists of each body
+		resetContactManifoldListsOfBodies();
+		// Create the sets of shapes
+		final Set<DTree> shapes = new Set<DTree>(Set.getDTreeCoparator());
+		shapes.add(_shape.broadPhaseID);
+		final Set<DTree> emptySet = new Set<DTree>(Set.getDTreeCoparator());
+		// Perform the collision detection and report contacts
+		this.collisionDetection.testCollisionBetweenShapes(_callback, shapes, emptySet);
+	}
+	
+	/**
+	 * Test 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
+	 */
+	public void testCollision(final ProxyShape _shape1, final ProxyShape _shape2, final CollisionCallback _callback) {
+		// Reset all the contact manifolds lists of each body
+		resetContactManifoldListsOfBodies();
+		// Create the sets of shapes
+		final Set<DTree> shapes1 = new Set<DTree>(Set.getDTreeCoparator());
+		shapes1.add(_shape1.broadPhaseID);
+		final Set<DTree> shapes2 = new Set<DTree>(Set.getDTreeCoparator());
+		shapes2.add(_shape2.broadPhaseID);
+		// Perform the collision detection and report contacts
+		this.collisionDetection.testCollisionBetweenShapes(_callback, shapes1, shapes2);
+	}
+}

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

@@ -0,0 +1,163 @@
+package org.atriasoft.ephysics.engine;
+
+import java.util.List;
+import java.util.Map;
+
+import org.atriasoft.etk.math.Quaternion;
+import org.atriasoft.etk.math.Vector3f;
+
+import org.atriasoft.ephysics.body.RigidBody;
+import org.atriasoft.ephysics.constraint.Joint;
+
+/**
+ *  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.
+ */
+public class ConstraintSolver {
+	
+	private final Map<RigidBody, Integer> mapBodyToConstrainedVelocityIndex; //!< Reference to the map that associates rigid body to their index in the rained velocities array
+	private float timeStep; //!< Current time step
+	private boolean isWarmStartingActive; //!< True if the warm starting of the solver is active
+	private final ConstraintSolverData raintSolverData; //!< Constraint solver data used to initialize and solve the raints
+	/// Constructor
+	
+	public ConstraintSolver(final Map<RigidBody, Integer> mapBodyToVelocityIndex) {
+		this.mapBodyToConstrainedVelocityIndex = mapBodyToVelocityIndex;
+		this.isWarmStartingActive = true;
+		this.raintSolverData = new ConstraintSolverData(mapBodyToVelocityIndex);
+		
+	}
+	
+	/// Return true if the Non-Linear-Gauss-Seidel position correction technique is active
+	public boolean getIsNonLinearGaussSeidelPositionCorrectionActive() {
+		return this.isWarmStartingActive;
+	}
+	
+	/// Initialize the raint solver for a given island
+	public void initializeForIsland(final float dt, final Island island) {
+		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
+		final List<Joint> joints = island.getJoints();
+		for (int iii = 0; iii < island.getNbJoints(); ++iii) {
+			// Initialize the raint before solving it
+			joints.get(iii).initBeforeSolve(this.raintSolverData);
+			// Warm-start the raint if warm-starting is enabled
+			if (this.isWarmStartingActive) {
+				joints.get(iii).warmstart(this.raintSolverData);
+			}
+		}
+	}
+	
+	/// Set the rained positions/orientations arrays
+	public void setConstrainedPositionsArrays(final Vector3f[] rainedPositions, final Quaternion[] rainedOrientations) {
+		assert (rainedPositions != null);
+		assert (rainedOrientations != null);
+		this.raintSolverData.positions = rainedPositions;
+		this.raintSolverData.orientations = rainedOrientations;
+	}
+	
+	/// Set the rained velocities arrays
+	public void setConstrainedVelocitiesArrays(final Vector3f[] rainedLinearVelocities, final Vector3f[] rainedAngularVelocities) {
+		assert (rainedLinearVelocities != null);
+		assert (rainedAngularVelocities != null);
+		this.raintSolverData.linearVelocities = rainedLinearVelocities;
+		this.raintSolverData.angularVelocities = rainedAngularVelocities;
+	}
+	
+	/// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique.
+	public void setIsNonLinearGaussSeidelPositionCorrectionActive(final boolean isActive) {
+		this.isWarmStartingActive = isActive;
+	}
+	
+	/// Solve the position raints
+	public void solvePositionConstraints(final Island island) {
+		assert (island != null);
+		assert (island.getNbJoints() > 0);
+		final List<Joint> joints = island.getJoints();
+		for (int iii = 0; iii < joints.size(); ++iii) {
+			joints.get(iii).solvePositionConstraint(this.raintSolverData);
+		}
+	}
+	
+	/// Solve the raints
+	public void solveVelocityConstraints(final Island island) {
+		assert (island != null);
+		assert (island.getNbJoints() > 0);
+		// For each joint of the island
+		final List<Joint> joints = island.getJoints();
+		for (int iii = 0; iii < island.getNbJoints(); ++iii) {
+			joints.get(iii).solveVelocityConstraint(this.raintSolverData);
+		}
+	}
+}

src/org/atriasoft/ephysics/engine/ConstraintSolverData.java

@@ -0,0 +1,28 @@
+package org.atriasoft.ephysics.engine;
+
+import java.util.Map;
+
+import org.atriasoft.etk.math.Quaternion;
+import org.atriasoft.etk.math.Vector3f;
+
+import org.atriasoft.ephysics.body.RigidBody;
+
+/**
+ * This structure contains data from the raint solver that are used to solve
+ * each joint raint.
+ */
+public class ConstraintSolverData {
+	
+	public float timeStep; //!< Current time step of the simulation
+	public Vector3f[] linearVelocities = null; //!< Array with the bodies linear velocities
+	public Vector3f[] angularVelocities = null; //!< Array with the bodies angular velocities
+	public Vector3f[] positions = null; //!< Reference to the bodies positions
+	public Quaternion[] orientations = null; //!< Reference to the bodies orientations
+	public Map<RigidBody, Integer> mapBodyToConstrainedVelocityIndex; //!< Reference to the map that associates rigid body to their index in the rained velocities array
+	public boolean isWarmStartingActive; //!< True if warm starting of the solver is active
+	
+	public ConstraintSolverData(final Map<RigidBody, Integer> refMapBodyToConstrainedVelocityIndex) {
+		this.mapBodyToConstrainedVelocityIndex = refMapBodyToConstrainedVelocityIndex;
+		
+	}
+}

src/org/atriasoft/ephysics/engine/ContactManifoldSolver.java

@@ -0,0 +1,53 @@
+package org.atriasoft.ephysics.engine;
+
+import org.atriasoft.ephysics.collision.ContactManifold;
+import org.atriasoft.etk.math.Matrix3f;
+import org.atriasoft.etk.math.Vector3f;
+
+/**
+ *  Contact solver internal data structure to store all the information relative to a contact manifold.
+ */
+public class 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 = new Matrix3f(); //!< Inverse inertia tensor of body 1
+	Matrix3f inverseInertiaTensorBody2 = new Matrix3f(); //!< Inverse inertia tensor of body 2
+	ContactPointSolver[] contacts = new ContactPointSolver[ContactManifold.MAX_CONTACT_POINTS_IN_MANIFOLD]; //!< 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 = new Vector3f(0, 0, 0); //!< Average normal vector of the contact manifold
+	Vector3f frictionPointBody1 = new Vector3f(0, 0, 0); //!< Point on body 1 where to apply the friction raints
+	Vector3f frictionPointBody2 = new Vector3f(0, 0, 0); //!< Point on body 2 where to apply the friction raints
+	Vector3f r1Friction = new Vector3f(0, 0, 0); //!< R1 vector for the friction raints
+	Vector3f r2Friction = new Vector3f(0, 0, 0); //!< R2 vector for the friction raints
+	Vector3f r1CrossT1 = new Vector3f(0, 0, 0); //!< Cross product of r1 with 1st friction vector
+	Vector3f r1CrossT2 = new Vector3f(0, 0, 0); //!< Cross product of r1 with 2nd friction vector
+	Vector3f r2CrossT1 = new Vector3f(0, 0, 0); //!< Cross product of r2 with 1st friction vector
+	Vector3f r2CrossT2 = new Vector3f(0, 0, 0); //!< 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 = new Matrix3f(); //!< Matrix K for the rolling resistance raint
+	Vector3f frictionVector1 = new Vector3f(0, 0, 0); //!< First friction direction at contact manifold center
+	Vector3f frictionvec2 = new Vector3f(0, 0, 0); //!< Second friction direction at contact manifold center
+	Vector3f oldFrictionVector1 = new Vector3f(0, 0, 0); //!< Old 1st friction direction at contact manifold center
+	Vector3f oldFrictionvec2 = new Vector3f(0, 0, 0); //!< 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 = new Vector3f(0, 0, 0); //!< Rolling resistance impulse
+	
+	public ContactManifoldSolver() {
+		for (int iii = 0; iii < ContactManifold.MAX_CONTACT_POINTS_IN_MANIFOLD; iii++) {
+			this.contacts[iii] = new ContactPointSolver();
+		}
+	}
+}

src/org/atriasoft/ephysics/engine/ContactPointSolver.java

@@ -0,0 +1,37 @@
+package org.atriasoft.ephysics.engine;
+
+import org.atriasoft.etk.math.Vector3f;
+
+import org.atriasoft.ephysics.constraint.ContactPoint;
+
+/**
+ * Contact solver internal data structure that to store all the
+ * information relative to a contact point
+ */
+public class 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 = new Vector3f(0, 0, 0); //!< Accumulated rolling resistance impulse
+	Vector3f normal = new Vector3f(0, 0, 0); //!< Normal vector of the contact
+	Vector3f frictionVector1 = new Vector3f(0, 0, 0); //!< First friction vector in the tangent plane
+	Vector3f frictionvec2 = new Vector3f(0, 0, 0); //!< Second friction vector in the tangent plane
+	Vector3f oldFrictionVector1 = new Vector3f(0, 0, 0); //!< Old first friction vector in the tangent plane
+	Vector3f oldFrictionvec2 = new Vector3f(0, 0, 0); //!< Old second friction vector in the tangent plane
+	Vector3f r1 = new Vector3f(0, 0, 0); //!< Vector from the body 1 center to the contact point
+	Vector3f r2 = new Vector3f(0, 0, 0); //!< Vector from the body 2 center to the contact point
+	Vector3f r1CrossT1 = new Vector3f(0, 0, 0); //!< Cross product of r1 with 1st friction vector
+	Vector3f r1CrossT2 = new Vector3f(0, 0, 0); //!< Cross product of r1 with 2nd friction vector
+	Vector3f r2CrossT1 = new Vector3f(0, 0, 0); //!< Cross product of r2 with 1st friction vector
+	Vector3f r2CrossT2 = new Vector3f(0, 0, 0); //!< Cross product of r2 with 2nd friction vector
+	Vector3f r1CrossN = new Vector3f(0, 0, 0); //!< Cross product of r1 with the contact normal
+	Vector3f r2CrossN = new Vector3f(0, 0, 0); //!< 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
+}

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

@@ -0,0 +1,40 @@
+package org.atriasoft.ephysics.engine;
+
+import org.atriasoft.ephysics.constraint.ContactPointInfo;
+
+/**
+ *  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.
+ */
+public interface EventListener {
+	/**
+	 *  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);
+	
+	/**
+	 *  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();
+	
+	/**
+	 *  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();
+	
+	/**
+	 *  Called when a new contact point is found between two bodies
+	 * @param contact Information about the contact
+	 */
+	void newContact(ContactPointInfo contact);
+}

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

@@ -0,0 +1,28 @@
+package org.atriasoft.ephysics.engine;
+
+import org.atriasoft.etk.math.Vector3f;
+
+/**
+ *  Represents an impulse that we can apply to bodies in the contact or raint solver.
+ */
+public class Impulse {
+	public final Vector3f linearImpulseBody1; //!< Linear impulse applied to the first body
+	public final Vector3f angularImpulseBody1; //!< Angular impulse applied to the first body
+	public final Vector3f linearImpulseBody2; //!< Linear impulse applied to the second body
+	public final Vector3f angularImpulseBody2; //!< Angular impulse applied to the second body
+	
+	public Impulse(final Impulse impulse) {
+		this.linearImpulseBody1 = impulse.linearImpulseBody1.clone();
+		this.angularImpulseBody1 = impulse.angularImpulseBody1.clone();
+		this.linearImpulseBody2 = impulse.linearImpulseBody2.clone();
+		this.angularImpulseBody2 = impulse.angularImpulseBody2.clone();
+		
+	}
+	
+	public Impulse(final Vector3f _initLinearImpulseBody1, final Vector3f _initAngularImpulseBody1, final Vector3f _initLinearImpulseBody2, final Vector3f _initAngularImpulseBody2) {
+		this.linearImpulseBody1 = _initLinearImpulseBody1.clone();
+		this.angularImpulseBody1 = _initAngularImpulseBody1.clone();
+		this.linearImpulseBody2 = _initLinearImpulseBody2.clone();
+		this.angularImpulseBody2 = _initAngularImpulseBody2.clone();
+	}
+}

src/org/atriasoft/ephysics/engine/Island.java

@@ -0,0 +1,111 @@
+package org.atriasoft.ephysics.engine;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import org.atriasoft.ephysics.body.BodyType;
+import org.atriasoft.ephysics.body.RigidBody;
+import org.atriasoft.ephysics.collision.ContactManifold;
+import org.atriasoft.ephysics.constraint.Joint;
+import org.atriasoft.ephysics.internal.Log;
+
+/**
+ *  An island represent an isolated group of awake bodies that are connected with each other by
+ * some contraints (contacts or joints).
+ */
+public class Island {
+	private final List<RigidBody> bodies = new ArrayList<>(); //!< Array with all the bodies of the island
+	private final List<ContactManifold> contactManifolds = new ArrayList<>(); //!< Array with all the contact manifolds between bodies of the island
+	private final List<Joint> joints = new ArrayList<>(); //!< Array with all the joints between bodies of the island
+	
+	/**
+	 *  Constructor
+	 */
+	public Island(final int nbMaxBodies, final int nbMaxContactManifolds, final int nbMaxJoints) {
+		// Allocate memory for the arrays
+		//this.bodies.reserve(_nbMaxBodies);
+		//this.contactManifolds.reserve(_nbMaxContactManifolds);
+		//this.joints.reserve(_nbMaxJoints);
+	}
+	
+	/** 
+		 * Add a body.
+		 */
+	public void addBody(final RigidBody _body) {
+		if (_body.isSleeping() == true) {
+			Log.error("Try to add a body that is sleeping ...");
+			return;
+		}
+		this.bodies.add(_body);
+	}
+	
+	/** 
+		 * Add a contact manifold.
+		 */
+	public void addContactManifold(final ContactManifold _contactManifold) {
+		this.contactManifolds.add(_contactManifold);
+	}
+	
+	/** 
+		 * Add a joint.
+		 */
+	public void addJoint(final Joint _joint) {
+		this.joints.add(_joint);
+	}
+	
+	/** 
+	 * Return a pointer to the array of bodies
+	 */
+	public List<RigidBody> getBodies() {
+		return this.bodies;
+	}
+	
+	/** 
+	 * Return a pointer to the array of contact manifolds
+	 */
+	public List<ContactManifold> getContactManifold() {
+		return this.contactManifolds;
+	}
+	
+	/** 
+	 * Return a pointer to the array of joints
+	 */
+	public List<Joint> getJoints() {
+		return this.joints;
+	}
+	
+	/** 
+	 *  Get the number of body
+	 * @return Number of bodies.
+	 */
+	public int getNbBodies() {
+		return this.bodies.size();
+	}
+	
+	/** 
+	 * @ Get the number of contact manifolds
+	 * Return the number of contact manifolds in the island
+	 */
+	public int getNbContactManifolds() {
+		return this.contactManifolds.size();
+	}
+	
+	/** 
+	 * Return the number of joints in the island
+	 */
+	public int getNbJoints() {
+		return this.joints.size();
+	}
+	
+	/**
+	 *  Reset the isAlreadyIsland variable of the static bodies so that they can also be included in the other islands
+	 */
+	public void resetStaticBobyNotInIsland() {
+		for (final RigidBody it : this.bodies) {
+			if (it.getType() == BodyType.STATIC) {
+				it.isAlreadyInIsland = false;
+			}
+		}
+	}
+	
+}

src/org/atriasoft/ephysics/engine/Material.java

@@ -0,0 +1,91 @@
+package org.atriasoft.ephysics.engine;
+
+import org.atriasoft.ephysics.Configuration;
+
+public class Material {
+	
+	private float frictionCoefficient; //!< Friction coefficient (positive value)
+	private float rollingResistance; //!< Rolling resistance factor (positive value)
+	private float bounciness; //!< Bounciness during collisions (between 0 and 1) where 1 is for a very bouncy body
+	
+	/// Constructor
+	public Material() {
+		this.frictionCoefficient = Configuration.DEFAULT_FRICTION_COEFFICIENT;
+		this.rollingResistance = Configuration.DEFAULT_ROLLING_RESISTANCE;
+		this.bounciness = Configuration.DEFAULT_BOUNCINESS;
+	}
+	
+	/// Copy-ructor
+	public Material(final Material material) {
+		this.frictionCoefficient = material.frictionCoefficient;
+		this.rollingResistance = material.rollingResistance;
+		this.bounciness = material.bounciness;
+	}
+	
+	/// Return the bounciness
+	/// @return Bounciness factor (between 0 and 1) where 1 is very bouncy
+	public float getBounciness() {
+		return this.bounciness;
+	}
+	
+	/**Return the friction coefficient
+	 * @return Friction coefficient (positive value)
+	 */
+	public float getFrictionCoefficient() {
+		return this.frictionCoefficient;
+	}
+	
+	/** Return the rolling resistance factor. If this value is larger than zero,
+	 * it will be used to slow down the body when it is rolling
+	 * against another body.
+	 * @return The rolling resistance factor (positive value)
+	 */
+	public float getRollingResistance() {
+		return this.rollingResistance;
+	}
+	
+	/// Overloaded assignment operator
+	public Material set(final Material material) {
+		
+		// Check for self-assignment
+		if (this != material) {
+			this.frictionCoefficient = material.frictionCoefficient;
+			this.bounciness = material.bounciness;
+			this.rollingResistance = material.rollingResistance;
+		}
+		
+		// Return this material
+		return this;
+	}
+	
+	/** Set the bounciness.
+	 * The bounciness should be a value between 0 and 1. The value 1 is used for a
+	 * very bouncy body and zero is used for a body that is not bouncy at all.
+	 * @param bounciness Bounciness factor (between 0 and 1) where 1 is very bouncy
+	 */
+	public void setBounciness(final float bounciness) {
+		assert (bounciness >= 0.0f && bounciness <= 1.0f);
+		this.bounciness = bounciness;
+	}
+	
+	/** Set the friction coefficient.
+	 * The friction coefficient has to be a positive value. The value zero is used for no
+	 * friction at all.
+	 * @param frictionCoefficient Friction coefficient (positive value)
+	 */
+	public void setFrictionCoefficient(final float frictionCoefficient) {
+		assert (frictionCoefficient >= 0.0f);
+		this.frictionCoefficient = frictionCoefficient;
+	}
+	
+	/** Set the rolling resistance factor. If this value is larger than zero,
+	 * it will be used to slow down the body when it is rolling
+	 * against another body.
+	 * @param rollingResistance The rolling resistance factor
+	 */
+	public void setRollingResistance(final float rollingResistance) {
+		assert (rollingResistance >= 0);
+		this.rollingResistance = rollingResistance;
+	}
+	
+}

src/org/atriasoft/ephysics/engine/OverlappingPair.java

@@ -0,0 +1,88 @@
+package org.atriasoft.ephysics.engine;
+
+import org.atriasoft.ephysics.body.CollisionBody;
+import org.atriasoft.ephysics.collision.ContactManifoldSet;
+import org.atriasoft.ephysics.collision.ProxyShape;
+import org.atriasoft.ephysics.collision.broadphase.PairDTree;
+import org.atriasoft.ephysics.constraint.ContactPoint;
+import org.atriasoft.ephysics.mathematics.PairInt;
+import org.atriasoft.etk.math.Vector3f;
+
+/**
+ *  This class represents a pair of two proxy collision shapes that are overlapping
+ * during the broad-phase collision detection. It is created when
+ * the two proxy collision shapes start to overlap and is destroyed when they do not
+ * overlap anymore. This class contains a contact manifold that
+ * store all the contact points between the two bodies.
+ */
+public class OverlappingPair {
+	/// Return the pair of bodies index of the pair
+	public static PairInt computeBodiesIndexPair(final CollisionBody body1, final CollisionBody body2) {
+		// Construct the pair of body index
+		final PairInt indexPair = body1.getID() < body2.getID() ? new PairInt(body1.getID(), body2.getID()) : new PairInt(body2.getID(), body1.getID());
+		assert (indexPair.first != indexPair.second);
+		return indexPair;
+	}
+	
+	/// Return the pair of bodies index
+	public static PairDTree computeID(final ProxyShape shape1, final ProxyShape shape2) {
+		assert (shape1.getBroadPhaseID() != null && shape2.getBroadPhaseID() != null);
+		// Construct the pair of body index
+		return new PairDTree(shape1.getBroadPhaseID(), shape2.getBroadPhaseID());
+	}
+	
+	private final ContactManifoldSet contactManifoldSet; //!< Set of persistent contact manifolds
+	
+	private Vector3f cachedSeparatingAxis; //!< Cached previous separating axis
+	
+	/// Constructor
+	public OverlappingPair(final ProxyShape shape1, final ProxyShape shape2, final int nbMaxContactManifolds) {
+		this.contactManifoldSet = new ContactManifoldSet(shape1, shape2, nbMaxContactManifolds);
+		this.cachedSeparatingAxis = new Vector3f(1.0f, 1.0f, 1.0f);
+	}
+	
+	/// Add a contact to the contact cache
+	public void addContact(final ContactPoint contact) {
+		this.contactManifoldSet.addContactPoint(contact);
+	}
+	
+	/// Clear the contact points of the contact manifold
+	public void clearContactPoints() {
+		this.contactManifoldSet.clear();
+	}
+	
+	/// Return the cached separating axis
+	public Vector3f getCachedSeparatingAxis() {
+		return this.cachedSeparatingAxis;
+	}
+	
+	/// Return the a reference to the contact manifold set
+	public ContactManifoldSet getContactManifoldSet() {
+		return this.contactManifoldSet;
+	}
+	
+	/// Return the number of contacts in the cache
+	public int getNbContactPoints() {
+		return this.contactManifoldSet.getTotalNbContactPoints();
+	}
+	
+	/// Return the pointer to first proxy collision shape
+	public ProxyShape getShape1() {
+		return this.contactManifoldSet.getShape1();
+	}
+	
+	/// Return the pointer to second body
+	public ProxyShape getShape2() {
+		return this.contactManifoldSet.getShape2();
+	}
+	
+	/// Set the cached separating axis
+	public void setCachedSeparatingAxis(final Vector3f axis) {
+		this.cachedSeparatingAxis = axis;
+	}
+	
+	/// Update the contact cache
+	public void update() {
+		this.contactManifoldSet.update();
+	}
+}

src/org/atriasoft/ephysics/internal/Log.java

@@ -0,0 +1,68 @@
+package org.atriasoft.ephysics.internal;
+
+import io.scenarium.logger.LogLevel;
+import io.scenarium.logger.Logger;
+
+public class Log {
+	private static final String LIB_NAME = "ephysics";
+	private static final String LIB_NAME_DRAW = Logger.getDrawableName(LIB_NAME);
+	private static final boolean PRINT_CRITICAL = Logger.getNeedPrint(LIB_NAME, LogLevel.CRITICAL);
+	private static final boolean PRINT_ERROR = Logger.getNeedPrint(LIB_NAME, LogLevel.ERROR);
+	private static final boolean PRINT_WARNING = Logger.getNeedPrint(LIB_NAME, LogLevel.WARNING);
+	private static final boolean PRINT_INFO = Logger.getNeedPrint(LIB_NAME, LogLevel.INFO);
+	private static final boolean PRINT_DEBUG = Logger.getNeedPrint(LIB_NAME, LogLevel.DEBUG);
+	private static final boolean PRINT_VERBOSE = Logger.getNeedPrint(LIB_NAME, LogLevel.VERBOSE);
+	private static final boolean PRINT_TODO = Logger.getNeedPrint(LIB_NAME, LogLevel.TODO);
+	private static final boolean PRINT_PRINT = Logger.getNeedPrint(LIB_NAME, LogLevel.PRINT);
+	
+	public static void critical(final String data) {
+		if (PRINT_CRITICAL) {
+			Logger.critical(LIB_NAME_DRAW, data);
+		}
+	}
+	
+	public static void debug(final String data) {
+		if (PRINT_DEBUG) {
+			Logger.debug(LIB_NAME_DRAW, data);
+		}
+	}
+	
+	public static void error(final String data) {
+		if (PRINT_ERROR) {
+			Logger.error(LIB_NAME_DRAW, data);
+		}
+	}
+	
+	public static void info(final String data) {
+		if (PRINT_INFO) {
+			Logger.info(LIB_NAME_DRAW, data);
+		}
+	}
+	
+	public static void print(final String data) {
+		if (PRINT_PRINT) {
+			Logger.print(LIB_NAME_DRAW, data);
+		}
+	}
+	
+	public static void todo(final String data) {
+		if (PRINT_TODO) {
+			Logger.todo(LIB_NAME_DRAW, data);
+		}
+	}
+	
+	public static void verbose(final String data) {
+		if (PRINT_VERBOSE) {
+			Logger.verbose(LIB_NAME_DRAW, data);
+		}
+	}
+	
+	public static void warning(final String data) {
+		if (PRINT_WARNING) {
+			Logger.warning(LIB_NAME_DRAW, data);
+		}
+	}
+	
+	private Log() {}
+	
+}

src/org/atriasoft/ephysics/mathematics/Mathematics.java

@@ -0,0 +1,96 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.mathematics;
+
+import org.atriasoft.etk.math.Vector3f;
+
+/**
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class Mathematics {
+	
+	// Function to test if two real numbers are (almost) equal
+	// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON]
+	public static boolean ApproxEqual(final float a, final float b, final float epsilon) {
+		final float difference = a - b;
+		return (difference < epsilon && difference > -epsilon);
+	}
+	
+	public static float ArcCos(final float radians) {
+		return (float) StrictMath.acos(radians);
+	}
+	
+	public static float ArcSin(final float radians) {
+		return (float) StrictMath.asin(radians);
+	}
+	
+	public static float ArcTan2(final float a, final float b) {
+		return (float) StrictMath.atan2(a, b);
+	}
+	
+	// Function that returns the result of the "value" clamped by
+	// two others values "lowerLimit" and "upperLimit"
+	public static float Clamp(final float value, final float lowerLimit, final float upperLimit) {
+		assert (lowerLimit <= upperLimit);
+		return Math.min(Math.max(value, lowerLimit), upperLimit);
+	}
+	
+	/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c)
+	/// This method uses the technique described in the book Real-Time collision detection by
+	/// Christer Ericson.
+	public static void computeBarycentricCoordinatesInTriangle(final Vector3f a, final Vector3f b, final Vector3f c, final Vector3f p, Float u, Float v, Float w) {
+		final Vector3f v0 = b.lessNew(a);
+		final Vector3f v1 = c.lessNew(a);
+		final Vector3f v2 = p.lessNew(a);
+		
+		final float d00 = v0.dot(v0);
+		final float d01 = v0.dot(v1);
+		final float d11 = v1.dot(v1);
+		final float d20 = v2.dot(v0);
+		final float d21 = v2.dot(v1);
+		
+		final float denom = d00 * d11 - d01 * d01;
+		v = (d11 * d20 - d01 * d21) / denom;
+		w = (d00 * d21 - d01 * d20) / denom;
+		u = 1.0f - v - w;
+	}
+	
+	public static float Cos(final float radians) {
+		return (float) StrictMath.cos(radians);
+	}
+	
+	public static float Sin(final float radians) {
+		return (float) StrictMath.sin(radians);
+	}
+	
+	public static float Sqrt(final float a) {
+		return (float) StrictMath.sqrt(a);
+	}
+	
+	public static float Tan(final float radians) {
+		return (float) StrictMath.tan(radians);
+	}
+}

src/org/atriasoft/ephysics/mathematics/Matrix2f.java

@@ -0,0 +1,249 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.mathematics;
+
+import org.atriasoft.etk.math.Constant;
+import org.atriasoft.etk.math.Vector2f;
+
+/**
+ * This class represents a 2x2 matrix.
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class Matrix2f {
+	
+	// Rows of the matrix (m[row][column])
+	float m00;
+	float m01;
+	float m10;
+	float m11;
+	
+	// Constructor
+	public Matrix2f() {
+		setZero();
+	}
+	
+	// Constructor
+	public Matrix2f(final float value) {
+		set(value, value, value, value);
+	}
+	
+	// Constructor with arguments
+	public Matrix2f(final float a1, final float a2, final float b1, final float b2) {
+		set(a1, a2, b1, b2);
+	}
+	
+	// Copy-constructor
+	public Matrix2f(final Matrix2f matrix) {
+		set(matrix);
+	}
+	
+	// Return the matrix with absolute values
+	public Matrix2f abs() {
+		this.m00 = Math.abs(this.m00);
+		this.m01 = Math.abs(this.m01);
+		this.m10 = Math.abs(this.m10);
+		this.m11 = Math.abs(this.m11);
+		return this;
+	}
+	
+	// Overloaded operator for addition with assignment
+	public Matrix2f add(final Matrix2f matrix) {
+		this.m00 += matrix.m00;
+		this.m01 += matrix.m01;
+		this.m10 += matrix.m10;
+		this.m11 += matrix.m11;
+		return this;
+	}
+	
+	@Override
+	public boolean equals(final Object obj) {
+		
+		if (obj == null) {
+			return false;
+		}
+		if (getClass() != obj.getClass()) {
+			return false;
+		}
+		final Matrix2f other = (Matrix2f) obj;
+		if (Float.floatToIntBits(this.m00) != Float.floatToIntBits(other.m00)) {
+			return false;
+		}
+		if (Float.floatToIntBits(this.m10) != Float.floatToIntBits(other.m10)) {
+			return false;
+		}
+		if (Float.floatToIntBits(this.m01) != Float.floatToIntBits(other.m01)) {
+			return false;
+		}
+		return Float.floatToIntBits(this.m11) == Float.floatToIntBits(other.m11);
+	}
+	
+	// Return a column
+	public Vector2f getColumn(final int index) {
+		if (index == 0) {
+			return new Vector2f(this.m00, this.m10);
+		} else if (index == 1) {
+			return new Vector2f(this.m01, this.m11);
+		}
+		throw new IllegalArgumentException("Unknown column index: " + index);
+	}
+	
+	// Return the determinant of the matrix
+	public float getDeterminant() {
+		return this.m00 * this.m11 - this.m10 * this.m01;
+	}
+	
+	// Return a row
+	public Vector2f getRow(final int index) {
+		if (index == 0) {
+			return new Vector2f(this.m00, this.m01);
+		} else if (index == 1) {
+			return new Vector2f(this.m10, this.m11);
+		}
+		throw new IllegalArgumentException("Unknown row index: " + index);
+	}
+	
+	// Return the trace of the matrix
+	public float getTrace() {
+		return this.m00 + this.m11;
+	}
+	
+	@Override
+	public int hashCode() {
+		int hash = 5;
+		hash = 23 * hash + Float.floatToIntBits(this.m00);
+		hash = 23 * hash + Float.floatToIntBits(this.m10);
+		hash = 23 * hash + Float.floatToIntBits(this.m01);
+		hash = 23 * hash + Float.floatToIntBits(this.m11);
+		return hash;
+	}
+	
+	// Set the matrix to the identity matrix
+	public Matrix2f identity() {
+		this.m00 = 1.0f;
+		this.m01 = 0.0f;
+		this.m10 = 0.0f;
+		this.m11 = 1.0f;
+		return this;
+	}
+	
+	// Return the inverse matrix
+	public Matrix2f inverse() {
+		
+		// Compute the determinant of the matrix
+		final float determinant = getDeterminant();
+		
+		// Check if the determinant is equal to zero
+		assert (Math.abs(determinant) > Constant.FLOAT_EPSILON);
+		final float invDeterminant = 1.0f / determinant;
+		
+		set(this.m11, -this.m01, -this.m10, this.m00);
+		
+		// Return the inverse matrix
+		return multiply(invDeterminant);
+	}
+	
+	public Matrix2f inverseNew() {
+		final Matrix2f tmp = new Matrix2f(this);
+		tmp.inverse();
+		return tmp;
+	}
+	
+	// Overloaded operator for the negative of the matrix
+	public Matrix2f invert() {
+		this.m00 = -this.m00;
+		this.m01 = -this.m01;
+		this.m10 = -this.m10;
+		this.m11 = -this.m11;
+		return this;
+	}
+	
+	// Overloaded operator for substraction with assignment
+	public Matrix2f less(final Matrix2f matrix) {
+		this.m00 -= matrix.m00;
+		this.m01 -= matrix.m01;
+		this.m10 -= matrix.m10;
+		this.m11 -= matrix.m11;
+		return this;
+	}
+	
+	// Overloaded operator for multiplication with a number with assignment
+	public Matrix2f multiply(final float number) {
+		this.m00 *= number;
+		this.m01 *= number;
+		this.m10 *= number;
+		this.m11 *= number;
+		return this;
+	}
+	
+	// Overloaded operator for matrix multiplication
+	public Matrix2f multiply(final Matrix2f matrix) {
+		set(this.m00 * matrix.m00 + this.m01 * matrix.m10, this.m00 * matrix.m01 + this.m01 * matrix.m11, this.m10 * matrix.m00 + this.m11 * matrix.m10, this.m10 * matrix.m01 + this.m11 * matrix.m11);
+		return this;
+	}
+	
+	// Overloaded operator for multiplication with a vector
+	public Vector2f multiplyNew(final Vector2f vector) {
+		return new Vector2f(this.m00 * vector.x + this.m01 * vector.y, this.m10 * vector.x + this.m11 * vector.y);
+	}
+	
+	// Method to set all the values in the matrix
+	public final Matrix2f set(final float a1, final float a2, final float b1, final float b2) {
+		this.m00 = a1;
+		this.m01 = a2;
+		this.m10 = b1;
+		this.m11 = b2;
+		return this;
+	}
+	
+	public final Matrix2f set(final Matrix2f matrix) {
+		this.m00 = matrix.m00;
+		this.m01 = matrix.m01;
+		this.m10 = matrix.m10;
+		this.m11 = matrix.m11;
+		return this;
+	}
+	
+	// Set the matrix to zero
+	public final Matrix2f setZero() {
+		this.m00 = 0.0f;
+		this.m01 = 0.0f;
+		this.m10 = 0.0f;
+		this.m11 = 0.0f;
+		return this;
+	}
+	
+	@Override
+	public String toString() {
+		return "(00= " + this.m00 + ", 01= " + this.m01 + ", 10= " + this.m10 + ", 11= " + this.m11 + ")";
+	}
+	
+	// Return the transpose matrix
+	public Matrix2f transpose() {
+		set(this.m00, this.m10, this.m01, this.m11);
+		return this;
+	}
+	
+}

src/org/atriasoft/ephysics/mathematics/PairInt.java

@@ -0,0 +1,67 @@
+package org.atriasoft.ephysics.mathematics;
+
+import java.util.Set;
+
+import javafx.collections.transformation.SortedList;
+
+public class PairInt {
+	public static int countInSet(final Set<PairInt> values, final PairInt sample) {
+		int count = 0;
+		for (final PairInt elem : values) {
+			if (elem.first != sample.first) {
+				continue;
+			}
+			if (elem.second != sample.second) {
+				continue;
+			}
+			count++;
+		}
+		return count;
+	}
+	
+	public static int countInSet(final SortedList<PairInt> values, final PairInt sample) {
+		int count = 0;
+		for (final PairInt elem : values) {
+			if (elem.first != sample.first) {
+				continue;
+			}
+			if (elem.second != sample.second) {
+				continue;
+			}
+			count++;
+		}
+		return count;
+	}
+	
+	public final int first;
+	
+	public final int second;
+	
+	public PairInt(final int first, final int second) {
+		this.first = first;
+		this.second = second;
+	}
+	
+	@Override
+	public boolean equals(final Object obj) {
+		if (this == obj) {
+			return true;
+		}
+		if (obj == null || getClass() != obj.getClass()) {
+			return false;
+		}
+		final PairInt tmp = (PairInt) obj;
+		if (this.first != tmp.first) {
+			return false;
+		}
+		if (this.second != tmp.second) {
+			return false;
+		}
+		return true;
+	}
+	
+	@Override
+	public String toString() {
+		return "PairInt [first=" + this.first + ", second=" + this.second + "]";
+	}
+}

src/org/atriasoft/ephysics/mathematics/PairIntVector3f.java

@@ -0,0 +1,14 @@
+package org.atriasoft.ephysics.mathematics;
+
+import org.atriasoft.etk.math.Vector3f;
+
+public class PairIntVector3f {
+	public final int first;
+	public final Vector3f second;
+	
+	public PairIntVector3f(final int first, final Vector3f second) {
+		this.first = first;
+		this.second = second;
+	}
+	
+}

src/org/atriasoft/ephysics/mathematics/Ray.java

@@ -0,0 +1,52 @@
+package org.atriasoft.ephysics.mathematics;
+
+import org.atriasoft.etk.math.Vector3f;
+
+public class Ray {
+	public Vector3f point1; //!<First point of the ray (origin)
+	public Vector3f point2; //!< Second point of the ray
+	public float maxFraction; //!< Maximum fraction value
+	
+	public Ray(final Ray obj) {
+		this.point1 = new Vector3f(obj.point1);
+		this.point2 = new Vector3f(obj.point2);
+		this.maxFraction = obj.maxFraction;
+	}
+	
+	/// Constructor with arguments
+	public Ray(final Vector3f _p1, final Vector3f _p2) {
+		this.point1 = _p1;
+		this.point2 = _p2;
+		this.maxFraction = 1.0f;
+	}
+	
+	public Ray(final Vector3f _p1, final Vector3f _p2, final float _maxFrac) {
+		this.point1 = _p1;
+		this.point2 = _p2;
+		this.maxFraction = _maxFrac;
+	}
+	
+	@Override
+	protected Object clone() throws CloneNotSupportedException {
+		return new Ray(this);
+	}
+	
+	@Override
+	public boolean equals(final Object obj) {
+		if (obj == null) {
+			return false;
+		}
+		if (getClass() != obj.getClass()) {
+			return false;
+		}
+		final Ray other = (Ray) obj;
+		
+		return this.point1.equals(other.point1) && this.point2.equals(other.point2) && this.maxFraction == other.maxFraction;
+	}
+	
+	@Override
+	public String toString() {
+		return super.toString();
+	}
+	
+}

src/org/atriasoft/ephysics/mathematics/Set.java

@@ -0,0 +1,325 @@
+package org.atriasoft.ephysics.mathematics;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.Comparator;
+import java.util.Iterator;
+import java.util.List;
+
+import org.atriasoft.ephysics.body.CollisionBody;
+import org.atriasoft.ephysics.collision.broadphase.DTree;
+import org.atriasoft.ephysics.collision.broadphase.PairDTree;
+
+public class Set<TYPE> implements Iterable<TYPE> {
+	public static Comparator<CollisionBody> getCollisionBodyCoparator() {
+		return new Comparator<CollisionBody>() {
+			@Override
+			public int compare(final CollisionBody a, final CollisionBody b) {
+				if (a.getID() < b.getID()) {
+					return -1;
+				} else if (a.getID() == b.getID()) {
+					return 0;
+				}
+				return -1;
+			}
+		};
+	}
+	
+	public static Comparator<DTree> getDTreeCoparator() {
+		return new Comparator<DTree>() {
+			@Override
+			public int compare(final DTree a, final DTree b) {
+				if (a.uid < b.uid) {
+					return -1;
+				} else if (a.uid == b.uid) {
+					return 0;
+				}
+				return -1;
+			}
+		};
+	}
+	
+	public static Comparator<Integer> getIntegerComparator() {
+		return new Comparator<Integer>() {
+			@Override
+			public int compare(final Integer a, final Integer b) {
+				if (a < b) {
+					return -1;
+				} else if (a == b) {
+					return 0;
+				}
+				return -1;
+			}
+		};
+	}
+	
+	public static Comparator<PairDTree> getPairDTreeCoparator() {
+		return new Comparator<PairDTree>() {
+			@Override
+			public int compare(final PairDTree _pair1, final PairDTree _pair2) {
+				if (_pair1.first.uid < _pair2.first.uid) {
+					return -1;
+				}
+				if (_pair1.first.uid == _pair2.first.uid) {
+					if (_pair1.second.uid < _pair2.second.uid) {
+						return -1;
+					}
+					if (_pair1.second.uid == _pair2.second.uid) {
+						return 0;
+					}
+					return +1;
+				}
+				return +1;
+			}
+		};
+	}
+	
+	public static Comparator<PairInt> getPairIntCoparator() {
+		return new Comparator<PairInt>() {
+			@Override
+			public int compare(final PairInt _pair1, final PairInt _pair2) {
+				if (_pair1.first < _pair2.first) {
+					return -1;
+				}
+				if (_pair1.first == _pair2.first) {
+					if (_pair1.second < _pair2.second) {
+						return -1;
+					}
+					if (_pair1.second == _pair2.second) {
+						return 0;
+					}
+					return +1;
+				}
+				return +1;
+			}
+		};
+	}
+	
+	protected final List<TYPE> data = new ArrayList<>(); //!< Data of the Set ==> the Set table is composed of pointer, this permit to have high speed when resize the vector ...
+	
+	protected Comparator<TYPE> comparator = null;
+	
+	/*
+		new Comparator<TYPE>() {
+		@Override
+		public int compare(final TYPE _pair1, final TYPE _pair2) {
+			if (_pair1.first < _pair2.first) {
+				return -1;
+			}
+			if (_pair1.first == _pair2.first) {
+				if (_pair1.second < _pair2.second) {
+					return -1;
+				}
+				if (_pair1.second == _pair2.second) {
+					return 0;
+				}
+				return +1;
+			}
+			return +1;
+		}
+	};
+	*/
+	/**
+	 * @brief Constructor of the Set table.
+	 */
+	public Set(final Comparator<TYPE> comparator) {
+		this.comparator = comparator;
+	}
+	
+	/**
+	 * @brief Add an element OR set an element value
+	 * @note add and set is the same function.
+	 * @param[in] _key Name of the value to set in the Set table.
+	 * @param[in] _value Value to set in the Set table.
+	 */
+	public void add(final TYPE _key) {
+		for (int iii = 0; iii < this.data.size(); ++iii) {
+			if (_key == this.data.get(iii)) {
+				return;
+			}
+			// For multiple element:
+			if (this.comparator.compare(_key, this.data.get(iii)) == 0) {
+				// Find a position
+				this.data.add(iii, _key);
+				return;
+			}
+		}
+		this.data.add(_key);
+	}
+	
+	/**
+	 * @brief Remove all entry in the Set table.
+	 * @note It does not delete pointer if your value is a pointer type...
+	 */
+	public void clear() {
+		this.data.clear();
+	}
+	
+	/**
+	 * @brief Count the number of occurence of a specific element.
+	 * @param[in] _key Name of the element to count iterence
+	 * @return 0 No element was found
+	 * @return 1 One element was found
+	 */
+	public int count(final TYPE _key) {
+		// TODO: search in a dichotomic way.
+		for (int iii = 0; iii < this.data.size(); iii++) {
+			if (this.comparator.compare(this.data.get(iii), _key) == 0) {
+				return 1;
+			}
+		}
+		return 0;
+	}
+	
+	public void erase(final int id) {
+		this.data.remove(id);
+	}
+	
+	public void erase(final TYPE id) {
+		this.data.remove(id);
+	}
+	
+	/**
+	 * @brief Check if an element exist or not
+	 * @param[in] _key Name of the Set requested
+	 * @return true if the element exist
+	 */
+	public boolean exist(final TYPE _key) {
+		final int it = find(_key);
+		if (it == -1) {
+			return false;
+		}
+		return true;
+	}
+	
+	/**
+	 * @brief Find an element position the the Set table
+	 * @param[in] _key Name of the element to find
+	 * @return Iterator on the element find or end()
+	 */
+	public int find(final TYPE _key) {
+		// TODO: search in a dichotomic way.
+		for (int iii = 0; iii < this.data.size(); iii++) {
+			if (this.comparator.compare(this.data.get(iii), _key) == 0) {
+				return iii;
+			}
+		}
+		return -1;
+	}
+	
+	/**
+	 * @brief Get Element an a special position
+	 * @param[in] _position Position in the Set
+	 * @return An reference on the selected element
+	 */
+	public TYPE get(final int _position) {
+		return this.data.get(_position);
+	}
+	
+	/**
+	 * @brief Check if the container have some element
+	 * @return true The container is empty
+	 * @return false The container have some element
+	 */
+	public boolean isEmpty() {
+		return this.data.size() == 0;
+	}
+	
+	@Override
+	public Iterator iterator() {
+		return this.data.iterator();
+	}
+	
+	/**
+	 * @brief Remove the last element of the vector
+	 */
+	public void popBack() {
+		this.data.remove(this.data.size() - 1);
+	}
+	
+	/**
+	 * @brief Remove the first element of the vector
+	 */
+	public void popFront() {
+		this.data.remove(0);
+	}
+	
+	/**
+	 * @brief: QuickSort implementation of sorting vector (all elements.
+	 * @param[in,out] _data Vector to sort.
+	 * @param[in] _high Comparator function of this element.
+	 */
+	void quickSort(final List<TYPE> _data, final Comparator<TYPE> _comparator) {
+		quickSort2(_data, 0, _data.size() - 1, _comparator);
+	}
+	
+	/**
+	 * @brief: QuickSort implementation of sorting vector.
+	 * @param[in,out] _data Vector to sort.
+	 * @param[in] _low Lowest element to sort.
+	 * @param[in] _high Highest element to sort.
+	 * @param[in] _high Comparator function of this element.
+	 */
+	void quickSort(final List<TYPE> _data, final int _low, int _high, final Comparator<TYPE> _comparator) {
+		if (_high >= _data.size()) {
+			_high = _data.size() - 1;
+		}
+		/*if (_low >= _data.size()) {
+			_low = _data.size()-1;
+		}*/
+		quickSort2(_data, _low, _high, _comparator);
+	}
+	
+	private void quickSort2(final List<TYPE> _data, final int _low, final int _high, final Comparator<TYPE> _comparator) {
+		if (_low >= _high) {
+			return;
+		}
+		// pi is partitioning index, arr[p] is now at right place
+		final int pi = quickSortPartition(_data, _low, _high, _comparator);
+		// Separately sort elements before partition and after partition
+		//if (pi != 0) {
+		quickSort2(_data, _low, pi - 1, _comparator);
+		//}
+		quickSort2(_data, pi + 1, _high, _comparator);
+	}
+	
+	private int quickSortPartition(final List<TYPE> _data, final int _low, final int _high, final Comparator<TYPE> _comparator) {
+		int iii = (_low - 1);
+		for (int jjj = _low; jjj < _high; ++jjj) {
+			if (_comparator.compare(_data.get(jjj), _data.get(_high)) < 0) {
+				iii++;
+				Collections.swap(_data, iii, jjj);
+			}
+		}
+		Collections.swap(_data, iii + 1, _high);
+		return (iii + 1);
+	}
+	
+	public void remove(final TYPE obj) {
+		this.data.remove(obj);
+	}
+	
+	/**
+	 * @brief Set the comparator of the set.
+	 * @param[in] _comparator comparing function.
+	 */
+	public void setComparator(final Comparator<TYPE> _comparator) {
+		this.comparator = _comparator;
+		sort();
+	}
+	
+	/**
+		 * @brief Get the number of element in the Set table
+		 * @return number of elements
+		 */
+	public int size() {
+		return this.data.size();
+	}
+	
+	/**
+	 * @brief Order the Set with the corect functor
+	 */
+	private void sort() {
+		quickSort(this.data, this.comparator);
+	}
+}

src/org/atriasoft/ephysics/mathematics/SetInteger.java

@@ -0,0 +1,212 @@
+package org.atriasoft.ephysics.mathematics;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.Comparator;
+import java.util.List;
+
+public class SetInteger {
+	private static void quickSort2(final List<Integer> _data, final int _low, final int _high, final Comparator<Integer> _comparator) {
+		if (_low >= _high) {
+			return;
+		}
+		// pi is partitioning index, arr[p] is now at right place
+		final int pi = quickSortPartition(_data, _low, _high, _comparator);
+		// Separately sort elements before partition and after partition
+		//if (pi != 0) {
+		quickSort2(_data, _low, pi - 1, _comparator);
+		//}
+		quickSort2(_data, pi + 1, _high, _comparator);
+	}
+	
+	private static int quickSortPartition(final List<Integer> _data, final int _low, final int _high, final Comparator<Integer> _comparator) {
+		int iii = (_low - 1);
+		for (int jjj = _low; jjj < _high; ++jjj) {
+			if (_comparator.compare(_data.get(jjj), _data.get(_high)) < 0) {
+				iii++;
+				Collections.swap(_data, iii, jjj);
+			}
+		}
+		Collections.swap(_data, iii + 1, _high);
+		return (iii + 1);
+	}
+	
+	private final List<Integer> data = new ArrayList<>(); //!< Data of the Set ==> the Set table is composed of pointer, this permit to have high speed when resize the vector ...
+	
+	private Comparator<Integer> comparator = new Comparator<Integer>() {
+		@Override
+		public int compare(final Integer a, final Integer b) {
+			if (a < b) {
+				return -1;
+			} else if (a == b) {
+				return 0;
+			}
+			return -1;
+		}
+	};
+	
+	/**
+	 * @brief Constructor of the Set table.
+	 */
+	public SetInteger() {
+		
+	}
+	
+	/**
+	 * @brief Add an element OR set an element value
+	 * @note add and set is the same function.
+	 * @param[in] _key Name of the value to set in the Set table.
+	 * @param[in] _value Value to set in the Set table.
+	 */
+	public void add(final Integer _key) {
+		for (int iii = 0; iii < this.data.size(); ++iii) {
+			if (_key == this.data.get(iii)) {
+				return;
+			}
+			if (this.comparator.compare(_key, this.data.get(iii)) == 0) {
+				// Find a position
+				this.data.add(iii, _key);
+				return;
+			}
+		}
+		this.data.add(_key);
+	}
+	
+	/**
+	 * @brief Remove all entry in the Set table.
+	 * @note It does not delete pointer if your value is a pointer type...
+	 */
+	public void clear() {
+		this.data.clear();
+	}
+	
+	/**
+	 * @brief Count the number of occurence of a specific element.
+	 * @param[in] _key Name of the element to count iterence
+	 * @return 0 No element was found
+	 * @return 1 One element was found
+	 */
+	public int count(final Integer _key) {
+		// TODO: search in a dichotomic way.
+		for (int iii = 0; iii < this.data.size(); iii++) {
+			if (this.comparator.compare(this.data.get(iii), _key) == 0) {
+				return 1;
+			}
+		}
+		return 0;
+	}
+	
+	/**
+	 * @brief Check if an element exist or not
+	 * @param[in] _key Name of the Set requested
+	 * @return true if the element exist
+	 */
+	public boolean exist(final Integer _key) {
+		final int it = find(_key);
+		if (it == -1) {
+			return false;
+		}
+		return true;
+	}
+	
+	/**
+	 * @brief Find an element position the the Set table
+	 * @param[in] _key Name of the element to find
+	 * @return Iterator on the element find or end()
+	 */
+	public int find(final Integer _key) {
+		// TODO: search in a dichotomic way.
+		for (int iii = 0; iii < this.data.size(); iii++) {
+			if (this.comparator.compare(this.data.get(iii), _key) == 0) {
+				return iii;
+			}
+		}
+		return -1;
+	}
+	
+	/**
+	 * @brief Get Element an a special position
+	 * @param[in] _position Position in the Set
+	 * @return An reference on the selected element
+	 */
+	public Integer get(final int _position) {
+		return this.data.get(_position);
+	}
+	
+	public List<Integer> getRaw() {
+		return this.data;
+	}
+	
+	/**
+	 * @brief Check if the container have some element
+	 * @return true The container is empty
+	 * @return false The container have some element
+	 */
+	public boolean isEmpty() {
+		return this.data.size() == 0;
+	}
+	
+	/**
+	 * @brief Remove the last element of the vector
+	 */
+	public void popBack() {
+		this.data.remove(this.data.size() - 1);
+	}
+	
+	/**
+	 * @brief Remove the first element of the vector
+	 */
+	public void popFront() {
+		this.data.remove(0);
+	}
+	
+	/**
+	 * @brief: QuickSort implementation of sorting vector (all elements.
+	 * @param[in,out] _data Vector to sort.
+	 * @param[in] _high Comparator function of this element.
+	 */
+	void quickSort(final List<Integer> _data, final Comparator<Integer> _comparator) {
+		quickSort2(_data, 0, _data.size() - 1, _comparator);
+	}
+	
+	/**
+	 * @brief: QuickSort implementation of sorting vector.
+	 * @param[in,out] _data Vector to sort.
+	 * @param[in] _low Lowest element to sort.
+	 * @param[in] _high Highest element to sort.
+	 * @param[in] _high Comparator function of this element.
+	 */
+	void quickSort(final List<Integer> _data, final int _low, int _high, final Comparator<Integer> _comparator) {
+		if (_high >= _data.size()) {
+			_high = _data.size() - 1;
+		}
+		/*if (_low >= _data.size()) {
+			_low = _data.size()-1;
+		}*/
+		quickSort2(_data, _low, _high, _comparator);
+	}
+	
+	/**
+	 * @brief Set the comparator of the set.
+	 * @param[in] _comparator comparing function.
+	 */
+	public void setComparator(final Comparator<Integer> _comparator) {
+		this.comparator = _comparator;
+		sort();
+	}
+	
+	/**
+		 * @brief Get the number of element in the Set table
+		 * @return number of elements
+		 */
+	public int size() {
+		return this.data.size();
+	}
+	
+	/**
+	 * @brief Order the Set with the corect functor
+	 */
+	private void sort() {
+		quickSort(this.data, this.comparator);
+	}
+}

src/org/atriasoft/ephysics/mathematics/SetMultiple.java

@@ -0,0 +1,41 @@
+package org.atriasoft.ephysics.mathematics;
+
+import java.util.Comparator;
+
+public class SetMultiple<TYPE> extends Set<TYPE> {
+	/**
+	 * @brief Constructor of the Set table.
+	 */
+	public SetMultiple(final Comparator<TYPE> comparator) {
+		super(comparator);
+	}
+	
+	/**
+	 * @brief Add an element OR set an element value
+	 * @note add and set is the same function.
+	 * @param[in] _key Name of the value to set in the Set table.
+	 * @param[in] _value Value to set in the Set table.
+	 */
+	@Override
+	public void add(final TYPE _key) {
+		for (int iii = 0; iii < this.data.size(); ++iii) {
+			if (_key == this.data.get(iii)) {
+				return;
+			}
+			final int compareValue = this.comparator.compare(_key, this.data.get(iii));
+			if (compareValue == 0) {
+				// Find a position
+				this.data.set(iii, _key);
+				return;
+			}
+			// for single Element
+			if (compareValue == 1) {
+				// Find a position
+				this.data.add(iii, _key);
+				return;
+			}
+		}
+		this.data.add(_key);
+	}
+	
+}