[DEV] firc collision that is really good ==> bad implement but this is the first steop ?....

.classpath

@@ -12,16 +12,6 @@
 			<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" exported="true" kind="src" path="/atriasoft-ephysics">
 		<attributes>
 			<attribute name="module" value="true"/>
@@ -37,5 +27,11 @@
 			<attribute name="module" value="true"/>
 		</attributes>
 	</classpathentry>
+	<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER"/>
+	<classpathentry kind="con" path="org.eclipse.jdt.junit.JUNIT_CONTAINER/5">
+		<attributes>
+			<attribute name="test" value="true"/>
+		</attributes>
+	</classpathentry>
 	<classpathentry kind="output" path="out/eclipse/classes"/>
 </classpath>

.project

@@ -19,6 +19,5 @@
 	</buildSpec>
 	<natures>
 		<nature>org.eclipse.jdt.core.javanature</nature>
-		<nature>org.python.pydev.pythonNature</nature>
 	</natures>
 </projectDescription>

samples/src/sample/atriasoft/ege/collisiontest/CollisionTestApplication.java

@@ -27,7 +27,6 @@ import org.atriasoft.ege.components.PhysicBodyType;
 import org.atriasoft.ege.engines.EngineLight;
 import org.atriasoft.ege.engines.EnginePhysics;
 import org.atriasoft.ege.map.MapVoxel;
-import org.atriasoft.ege.physics.shape.Box;
 import org.atriasoft.ege.tools.MeshGenerator;
 import org.atriasoft.etk.Color;
 import org.atriasoft.etk.Uri;
@@ -46,6 +45,7 @@ import org.atriasoft.gale.key.KeySpecial;
 import org.atriasoft.gale.key.KeyStatus;
 import org.atriasoft.gale.key.KeyType;
 import org.atriasoft.gale.resource.ResourceColored3DObject;
+import org.atriasoft.phyligram.shape.Box;
 
 public class CollisionTestApplication extends GaleApplication {
 	public static Vector3f box1HalfSize;

samples/src/sample/atriasoft/ege/loxelEngine/LoxelApplication.java

@@ -28,7 +28,6 @@ import org.atriasoft.ege.components.PhysicBodyType;
 import org.atriasoft.ege.engines.EngineLight;
 import org.atriasoft.ege.engines.EnginePhysics;
 import org.atriasoft.ege.map.MapVoxel;
-import org.atriasoft.ege.physics.shape.Box;
 import org.atriasoft.ege.tools.MeshGenerator;
 import org.atriasoft.etk.Color;
 import org.atriasoft.etk.Uri;
@@ -47,6 +46,7 @@ import org.atriasoft.gale.key.KeySpecial;
 import org.atriasoft.gale.key.KeyStatus;
 import org.atriasoft.gale.key.KeyType;
 import org.atriasoft.gale.resource.ResourceColored3DObject;
+import org.atriasoft.phyligram.shape.Box;
 
 public class LoxelApplication extends GaleApplication {
 	public static Vector3f box1HalfSize;

samples/src/sample/atriasoft/ege/loxelEnginePerso/LoxelApplicationPerso.java

@@ -4,7 +4,6 @@ import java.util.ArrayList;
 import java.util.List;
 
 import org.atriasoft.ege.ControlCameraPlayer;
-import org.atriasoft.ege.Engine;
 import org.atriasoft.ege.Entity;
 import org.atriasoft.ege.Environement;
 import org.atriasoft.ege.GameStatus;
@@ -26,9 +25,7 @@ import org.atriasoft.ege.components.ComponentStaticMesh;
 import org.atriasoft.ege.components.ComponentTexture;
 import org.atriasoft.ege.components.PhysicBodyType;
 import org.atriasoft.ege.engines.EngineLight;
-import org.atriasoft.ege.engines.EnginePhysics;
 import org.atriasoft.ege.map.MapVoxel;
-import org.atriasoft.phyligram.PhysicBox;
 import org.atriasoft.ege.tools.MeshGenerator;
 import org.atriasoft.etk.Color;
 import org.atriasoft.etk.Uri;
@@ -47,6 +44,9 @@ import org.atriasoft.gale.key.KeySpecial;
 import org.atriasoft.gale.key.KeyStatus;
 import org.atriasoft.gale.key.KeyType;
 import org.atriasoft.gale.resource.ResourceColored3DObject;
+import org.atriasoft.phyligram.PhysicBox;
+import org.atriasoft.phyligram.PhysicSphere;
+import org.atriasoft.phyligram.PhysicTriangle;
 
 public class LoxelApplicationPerso extends GaleApplication {
 	public static Vector3f box1HalfSize;
@@ -58,6 +58,7 @@ public class LoxelApplicationPerso extends GaleApplication {
 	public static List<Boolean> testPointsCollide = new ArrayList<>();
 	public static Quaternion testQTransfert;
 	public static Vector3f testRpos;
+	public boolean disable = false;
 	private float angleLight = 0;
 	private Quaternion basicRotation = Quaternion.IDENTITY;
 	private Quaternion basicRotation2 = Quaternion.IDENTITY;
@@ -107,7 +108,7 @@ public class LoxelApplicationPerso extends GaleApplication {
 		//				new Uri("DATA", "basic.vert"),
 		//				new Uri("DATA", "basic.frag")));
 		//		env.addEntity(localLight);
-		{
+		if (this.disable) {
 			// add a cube to test collision ...
 			final Entity localBox = new Entity(this.env);
 			localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj")));
@@ -117,40 +118,40 @@ public class LoxelApplicationPerso extends GaleApplication {
 			localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(0, 0, 5))));
 			final ComponentPhysicsPerso physics2 = new ComponentPhysicsPerso(this.env);
 			final PhysicBox box2 = new PhysicBox();
-			box2.setSize(new Vector3f(0.5f, 0.5f, 0.5f));
+			box2.setSize(new Vector3f(1.001f, 1.001f, 1.001f));
 			box2.setOrigin(new Vector3f(0, 0, 0));
 			box2.setMass(1);
 			physics2.addShape(box2);
 			localBox.addComponent(physics2);
 			this.env.addEntity(localBox);
 		}
-		{
+		if (this.disable) {
 			// add a cube to test collision ...
 			final Entity localBox = new Entity(this.env);
 			localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj")));
 			localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png", "loxelEngine")));
-			localBox.addComponent(new ComponentLight(new Light(new Color(0.0f,1.0f, 0.0f), new Vector3f(0, 0, 0), new Vector3f(0.8f, 0.03f, 0.002f))));
+			localBox.addComponent(new ComponentLight(new Light(new Color(0.0f, 1.0f, 0.0f), new Vector3f(0, 0, 0), new Vector3f(0.8f, 0.03f, 0.002f))));
 			localBox.addComponent(new ComponentRenderTexturedStaticMesh(new Uri("DATA", "basic.vert", "loxelEngine"), new Uri("DATA", "basic.frag", "loxelEngine")));
-			localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(0, 4, 12.5f))));
+			localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(0, 4, 2.5f))));
 			final ComponentPhysicsPerso physics2 = new ComponentPhysicsPerso(this.env);
 			final PhysicBox box2 = new PhysicBox();
-			box2.setSize(new Vector3f(0.5f, 0.5f, 0.5f));
+			box2.setSize(new Vector3f(2.0f, 2.0f, 2.0f));
 			box2.setOrigin(new Vector3f(0, 0, 0));
 			box2.setMass(1);
 			physics2.addShape(box2);
 			localBox.addComponent(physics2);
 			this.env.addEntity(localBox);
 		}
-		{
+		if (this.disable) {
 			// add a cube to test collision ...
 			final Entity localBox = new Entity(this.env);
 			localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj")));
 			localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png", "loxelEngine")));
 			localBox.addComponent(new ComponentRenderTexturedStaticMesh(new Uri("DATA", "basic.vert", "loxelEngine"), new Uri("DATA", "basic.frag", "loxelEngine")));
-			localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(-2, 2, 14.5f))));
+			localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(-2, 2, 1.5f))));
 			final ComponentPhysicsPerso physics2 = new ComponentPhysicsPerso(this.env);
 			final PhysicBox box2 = new PhysicBox();
-			box2.setSize(new Vector3f(0.5f, 0.5f, 0.5f));
+			box2.setSize(new Vector3f(3.0f, 3.0f, 3.0f));
 			box2.setOrigin(new Vector3f(0, 0, 0));
 			box2.setMass(1);
 			physics2.addShape(box2);
@@ -158,13 +159,13 @@ public class LoxelApplicationPerso extends GaleApplication {
 			this.env.addEntity(localBox);
 		}
 		
-		{
+		if (this.disable) {
 			// add a cube to test collision ...
 			final Entity localBox = new Entity(this.env);
 			localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj")));
 			localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png", "loxelEngine")));
 			localBox.addComponent(new ComponentRenderTexturedStaticMesh(new Uri("DATA", "basic.vert", "loxelEngine"), new Uri("DATA", "basic.frag", "loxelEngine")));
-			localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(-5, -5, 14))));
+			localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(-5, -5, 0))));
 			final ComponentPhysicsPerso physics2 = new ComponentPhysicsPerso(this.env);
 			final PhysicBox box2 = new PhysicBox();
 			box2.setSize(new Vector3f(2, 2, 2));
@@ -174,7 +175,7 @@ public class LoxelApplicationPerso extends GaleApplication {
 			localBox.addComponent(physics2);
 			this.env.addEntity(localBox);
 		}
-		{
+		if (this.disable) {
 			// add a cube to test collision ...
 			final Entity localBox = new Entity(this.env);
 			Quaternion orientation = new Quaternion(0.5f, 0.2f, 0.4f, 1);
@@ -182,7 +183,7 @@ public class LoxelApplicationPerso extends GaleApplication {
 			localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj")));
 			localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png", "loxelEngine")));
 			localBox.addComponent(new ComponentRenderTexturedStaticMesh(new Uri("DATA", "basic.vert", "loxelEngine"), new Uri("DATA", "basic.frag", "loxelEngine")));
-			localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(15, 15, 14), orientation)));
+			localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(15, 15, 0), orientation)));
 			final ComponentPhysicsPerso physics2 = new ComponentPhysicsPerso(this.env);
 			final PhysicBox box2 = new PhysicBox();
 			box2.setSize(new Vector3f(4, 4, 4));
@@ -192,7 +193,7 @@ public class LoxelApplicationPerso extends GaleApplication {
 			localBox.addComponent(physics2);
 			this.env.addEntity(localBox);
 		}
-		{
+		if (this.disable) {
 			// add a cube to test collision ...
 			final Entity localBox = new Entity(this.env);
 			final Quaternion orientation = new Quaternion(0.3f, 1.3f, 0.4f, 1);
@@ -200,7 +201,7 @@ public class LoxelApplicationPerso extends GaleApplication {
 			localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj")));
 			localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png", "loxelEngine")));
 			localBox.addComponent(new ComponentRenderTexturedStaticMesh(new Uri("DATA", "basic.vert", "loxelEngine"), new Uri("DATA", "basic.frag", "loxelEngine")));
-			localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(2, -2, 14.2f), orientation)));
+			localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(2, -2, 0.2f), orientation)));
 			final ComponentPhysicsPerso physics2 = new ComponentPhysicsPerso(this.env);
 			// TODO: physics2.setAngularReactionEnable(false);
 			final PhysicBox box2 = new PhysicBox();
@@ -211,7 +212,7 @@ public class LoxelApplicationPerso extends GaleApplication {
 			localBox.addComponent(physics2);
 			this.env.addEntity(localBox);
 		}
-		{
+		if (this.disable) {
 			// this is the floor
 			final Entity localBox = new Entity(this.env);
 			Quaternion orientation = new Quaternion(0, 0, 0, 1);
@@ -261,6 +262,41 @@ public class LoxelApplicationPerso extends GaleApplication {
 		//			env.addEntity(localBox);
 		//		}
 		
+		if (true) {
+			// add a cube to test collision ...
+			final Entity localBox = new Entity(this.env);
+			localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj")));
+			localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png", "loxelEngine")));
+			localBox.addComponent(new ComponentLight(new Light(new Color(0.0f, 1.0f, 0.0f), new Vector3f(0, 0, 0), new Vector3f(0.8f, 0.03f, 0.002f))));
+			localBox.addComponent(new ComponentRenderTexturedStaticMesh(new Uri("DATA", "basic.vert", "loxelEngine"), new Uri("DATA", "basic.frag", "loxelEngine")));
+			localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(0, 4, 0))));
+			final ComponentPhysicsPerso physics2 = new ComponentPhysicsPerso(this.env);
+			final PhysicTriangle box2 = new PhysicTriangle();
+			box2.setPoints(new Vector3f(2, 0, 0), new Vector3f(0, 2, 0), new Vector3f(3, 3, 2));
+			box2.setOrigin(new Vector3f(0, 0, 0));
+			box2.setMass(0);
+			physics2.addShape(box2);
+			localBox.addComponent(physics2);
+			this.env.addEntity(localBox);
+		}
+		if (true) {
+			// add a cube to test collision ...
+			final Entity localBox = new Entity(this.env);
+			localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj")));
+			localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png", "loxelEngine")));
+			localBox.addComponent(new ComponentLight(new Light(new Color(0.0f, 1.0f, 0.0f), new Vector3f(0, 0, 0), new Vector3f(0.8f, 0.03f, 0.002f))));
+			localBox.addComponent(new ComponentRenderTexturedStaticMesh(new Uri("DATA", "basic.vert", "loxelEngine"), new Uri("DATA", "basic.frag", "loxelEngine")));
+			localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(1, 5, 4))));
+			final ComponentPhysicsPerso physics2 = new ComponentPhysicsPerso(this.env);
+			final PhysicSphere box2 = new PhysicSphere();
+			box2.setSize(1.0f);
+			box2.setOrigin(new Vector3f(0, 0, 0));
+			box2.setMass(1);
+			physics2.addShape(box2);
+			localBox.addComponent(physics2);
+			this.env.addEntity(localBox);
+		}
+		
 		final Entity gird = new Entity(this.env);
 		gird.addComponent(new ComponentPosition(new Transform3D(new Vector3f(0, 0, 0))));
 		gird.addComponent(new ComponentStaticMesh(MeshGenerator.createGrid(5)));
@@ -304,6 +340,14 @@ public class LoxelApplicationPerso extends GaleApplication {
 			player.addComponent(new ComponentTexture(new Uri("RES", "playerTexture.png")));
 			player.addComponent(new ComponentRenderTexturedMaterialsStaticMesh(new Uri("DATA", "basicMaterial.vert", "loxelEngine"), new Uri("DATA", "basicMaterial.frag", "loxelEngine"),
 					(EngineLight) this.env.getEngine(EngineLight.ENGINE_NAME)));
+			final ComponentPhysicsPerso physics = new ComponentPhysicsPerso(this.env);
+			physics.setBodyType(PhysicBodyType.BODY_DYNAMIC);
+			final PhysicBox box = new PhysicBox();
+			box.setSize(new Vector3f(0.6f, 0.6f, 1.8f));
+			box.setOrigin(new Vector3f(0, 0, 0.9f));
+			box.setMass(0);
+			physics.addShape(box);
+			player.addComponent(physics);
 			this.env.addEntity(player);
 		}
 		final Camera mainView = new Camera();
@@ -320,13 +364,6 @@ public class LoxelApplicationPerso extends GaleApplication {
 		this.basicRotation = Quaternion.fromEulerAngles(new Vector3f(0.005f, 0.005f, 0.01f));
 		this.basicRotation2 = Quaternion.fromEulerAngles(new Vector3f(0.003f, 0.01f, 0.001f));
 		
-		final Engine tmpEngine = this.env.getEngine("physics");
-		if (tmpEngine != null) {
-			final EnginePhysics physicsEngine = (EnginePhysics) tmpEngine;
-			//Disable gravity for test ... 
-			physicsEngine.setGravity(new Vector3f(0.0f, 0.0f, -9.0f));
-		}
-		
 		// ready to let Gale & Ege manage the display
 		Log.info("==> Init APPL (END)");
 	}

src/module-info.java

@@ -9,9 +9,11 @@ open module org.atriasoft.ege {
 	exports org.atriasoft.ege.engines;
 	exports org.atriasoft.ege.geometry;
 	exports org.atriasoft.ege.map;
-	exports org.atriasoft.ege.physics.shape;
 	exports org.atriasoft.ege.tools;
 	exports org.atriasoft.phyligram;
+	exports org.atriasoft.phyligram.shape;
+	exports org.atriasoft.phyligram.math;
+	exports org.atriasoft.phyligram.tree;
 	exports entities;
 	exports guis;
 	exports models;

src/org/atriasoft/ege/Environement.java

@@ -8,13 +8,11 @@ import java.util.List;
 import java.util.Map;
 
 import org.atriasoft.ege.camera.Camera;
-import org.atriasoft.ege.components.ComponentPhysicsPerso;
 import org.atriasoft.ege.engines.EngineAI;
 import org.atriasoft.ege.engines.EngineDynamicMeshs;
 import org.atriasoft.ege.engines.EngineGravity;
 import org.atriasoft.ege.engines.EngineLight;
 import org.atriasoft.ege.engines.EngineParticle;
-import org.atriasoft.ege.engines.EnginePhysics;
 import org.atriasoft.ege.engines.EnginePhysicsPerso;
 import org.atriasoft.ege.engines.EnginePlayer;
 import org.atriasoft.ege.engines.EngineRender;
@@ -73,11 +71,11 @@ public class Environement {
 		addEngine(new EngineAI(this));
 		addEngine(new EngineDynamicMeshs(this));
 		addEngine(new EngineRender(this));
-		addEngine(new EnginePhysics(this));
+		//addEngine(new EnginePhysics(this));
 		addEngine(new EnginePhysicsPerso(this));
 		addEngine(new EngineParticle(this));
 		addEngine(new EngineLight(this));
-		startClock = Clock.systemUTC();
+		this.startClock = Clock.systemUTC();
 	}
 	
 	/**
@@ -338,14 +336,15 @@ public class Environement {
 		final long lastUpdate = this.lastCallTime;
 		this.lastCallTime = System.nanoTime();
 		Clock currentClock = Clock.systemUTC();
-		final EventTime event = new EventTime(currentClock, this.startClock, this.lastCallTime , this.startTime, Duration.ofNanos(this.lastCallTime - lastUpdate), Duration.ofNanos(this.lastCallTime - lastUpdate));
+		final EventTime event = new EventTime(currentClock, this.startClock, this.lastCallTime, this.startTime, Duration.ofNanos(this.lastCallTime - lastUpdate),
+				Duration.ofNanos(this.lastCallTime - lastUpdate));
 		for (final ControlInterface elem : this.controls) {
 			elem.periodicCall(event);
 		}
 		for (final Engine engine : this.engines) {
-			engine.update((long)((this.lastCallTime - lastUpdate)/1000000));
+			engine.update((this.lastCallTime - lastUpdate) / 1000000);
 		}
-	};
+	}
 	
 	public void removeControlInterface(final ControlInterface ref) {
 		this.controls.remove(ref);

src/org/atriasoft/ege/components/ComponentPhysics.java

@@ -10,20 +10,11 @@ import org.atriasoft.ege.camera.Camera;
 import org.atriasoft.ege.components.part.PositionningInterface;
 import org.atriasoft.ege.engines.EnginePhysics;
 import org.atriasoft.ege.internal.Log;
-import org.atriasoft.ege.physics.shape.Box;
-import org.atriasoft.ege.physics.shape.Capsule;
-import org.atriasoft.ege.physics.shape.Concave;
-import org.atriasoft.ege.physics.shape.Cone;
-import org.atriasoft.ege.physics.shape.ConvexHull;
-import org.atriasoft.ege.physics.shape.Cylinder;
-import org.atriasoft.ege.physics.shape.Shape;
-import org.atriasoft.ege.physics.shape.Sphere;
 import org.atriasoft.ephysics.body.BodyType;
 import org.atriasoft.ephysics.body.RigidBody;
 import org.atriasoft.ephysics.collision.ProxyShape;
 import org.atriasoft.ephysics.collision.TriangleMesh;
 import org.atriasoft.ephysics.collision.TriangleVertexArray;
-import org.atriasoft.ephysics.collision.shapes.AABB;
 import org.atriasoft.ephysics.collision.shapes.BoxShape;
 import org.atriasoft.ephysics.collision.shapes.CapsuleShape;
 import org.atriasoft.ephysics.collision.shapes.CollisionShape;
@@ -38,6 +29,14 @@ import org.atriasoft.etk.math.Quaternion;
 import org.atriasoft.etk.math.Transform3D;
 import org.atriasoft.etk.math.Vector3f;
 import org.atriasoft.gale.resource.ResourceColored3DObject;
+import org.atriasoft.phyligram.shape.Box;
+import org.atriasoft.phyligram.shape.Capsule;
+import org.atriasoft.phyligram.shape.Concave;
+import org.atriasoft.phyligram.shape.Cone;
+import org.atriasoft.phyligram.shape.ConvexHull;
+import org.atriasoft.phyligram.shape.Cylinder;
+import org.atriasoft.phyligram.shape.Shape;
+import org.atriasoft.phyligram.shape.Sphere;
 
 public class ComponentPhysics extends Component implements PositionningInterface {
 	public Signal<Transform3D> signalPosition = new Signal<>();
@@ -226,56 +225,50 @@ public class ComponentPhysics extends Component implements PositionningInterface
 		final Matrix4f transformationMatrix = mmm.transpose();
 		final Color tmpColor = new Color(1.0f, 0.0f, 0.0f, 0.3f);
 		for (final Shape it : this.shape) {
-			if (it.isBox()) {
+			if (it instanceof Box tmpElement) {
 				Log.debug("    Box");
-				final Box tmpElement = (Box) it;
 				final Transform3D transformLocal = new Transform3D(it.getOrigin(), it.getOrientation());
 				
 				Matrix4f transformationMatrixLocal = transformLocal.getOpenGLMatrix();
 				transformationMatrixLocal = transformationMatrixLocal.transpose();
 				transformationMatrixLocal = transformationMatrix.multiply(transformationMatrixLocal);
 				_draw.drawSquare(tmpElement.getSize(), transformationMatrixLocal, tmpColor);
-			} else if (it.isCylinder()) {
+			} else if (it instanceof Cylinder tmpElement) {
 				Log.debug("    Cylinder");
-				final Cylinder tmpElement = (Cylinder) it;
 				final Transform3D transformLocal = new Transform3D(it.getOrigin(), it.getOrientation());
 				
 				Matrix4f transformationMatrixLocal = transformLocal.getOpenGLMatrix();
 				transformationMatrixLocal = transformationMatrixLocal.transpose();
 				transformationMatrixLocal = transformationMatrix.multiply(transformationMatrixLocal);
 				_draw.drawCylinder(tmpElement.getRadius(), tmpElement.getSize(), 10, 10, transformationMatrixLocal, tmpColor);
-			} else if (it.isCapsule()) {
+			} else if (it instanceof Capsule tmpElement) {
 				Log.debug("    Capsule");
-				final Capsule tmpElement = (Capsule) it;
 				final Transform3D transformLocal = new Transform3D(it.getOrigin(), it.getOrientation());
 				
 				Matrix4f transformationMatrixLocal = transformLocal.getOpenGLMatrix();
 				transformationMatrixLocal = transformationMatrixLocal.transpose();
 				transformationMatrixLocal = transformationMatrix.multiply(transformationMatrixLocal);
 				_draw.drawCapsule(tmpElement.getRadius(), tmpElement.getSize(), 10, 10, transformationMatrixLocal, tmpColor);
-			} else if (it.isCone()) {
+			} else if (it instanceof Cone tmpElement) {
 				Log.debug("    Cone");
-				final Cone tmpElement = (Cone) it;
 				final Transform3D transformLocal = new Transform3D(it.getOrigin(), it.getOrientation());
 				
 				Matrix4f transformationMatrixLocal = transformLocal.getOpenGLMatrix();
 				transformationMatrixLocal = transformationMatrixLocal.transpose();
 				transformationMatrixLocal = transformationMatrix.multiply(transformationMatrixLocal);
 				_draw.drawCone(tmpElement.getRadius(), tmpElement.getSize(), 10, 10, transformationMatrixLocal, tmpColor);
-			} else if (it.isSphere()) {
+			} else if (it instanceof Sphere tmpElement) {
 				
 				Log.debug("    Sphere");
-				final Sphere tmpElement = (Sphere) it;
 				final Transform3D transformLocal = new Transform3D(it.getOrigin(), it.getOrientation());
 				
 				Matrix4f transformationMatrixLocal = transformLocal.getOpenGLMatrix();
 				transformationMatrixLocal = transformationMatrixLocal.transpose();
 				transformationMatrixLocal = transformationMatrix.multiply(transformationMatrixLocal);
 				_draw.drawSphere(tmpElement.getRadius(), 10, 10, transformationMatrixLocal, tmpColor);
-			} else if (it.isConcave()) {
+			} else if (it instanceof Concave tmpElement) {
 				
 				Log.debug("    concave");
-				final Concave tmpElement = (Concave) it;
 				final Transform3D transformLocal = new Transform3D(it.getOrigin(), it.getOrientation());
 				
 				Matrix4f transformationMatrixLocal = transformLocal.getOpenGLMatrix();
@@ -283,9 +276,8 @@ public class ComponentPhysics extends Component implements PositionningInterface
 				transformationMatrixLocal = transformationMatrixLocal.multiply(transformationMatrixLocal);
 				
 				_draw.drawTriangles(tmpElement.getVertex(), tmpElement.getIndices(), transformationMatrixLocal, tmpColor);
-			} else if (it.isConvexHull()) {
+			} else if (it instanceof ConvexHull tmpElement) {
 				Log.debug("    convexHull");
-				final ConvexHull tmpElement = (ConvexHull) it;
 				break;
 			}
 		}
@@ -312,9 +304,8 @@ public class ComponentPhysics extends Component implements PositionningInterface
 			if (it == null) {
 				continue;
 			}
-			if (it.isBox()) {
+			if (it instanceof Box tmpElement) {
 				Log.debug("    Box");
-				final Box tmpElement = (Box) it;
 				// Half extents of the box in the x, y and z directions
 				final Vector3f halfExtents = new Vector3f(tmpElement.getSize().x(), tmpElement.getSize().y(), tmpElement.getSize().z());
 				// Create the box shape
@@ -326,9 +317,8 @@ public class ComponentPhysics extends Component implements PositionningInterface
 				final ProxyShape proxyShape = this.rigidBody.addCollisionShape(shape, transform, it.getMass());
 				proxyShape.setUserData(this);
 				this.listProxyShape.add(proxyShape);
-			} else if (it.isCylinder()) {
+			} else if (it instanceof Cylinder tmpElement) {
 				Log.debug("    Cylinder");
-				final Cylinder tmpElement = (Cylinder) it;
 				// Create the Cylinder shape
 				// Create the Cylinder shape
 				final CylinderShape shape = new CylinderShape(tmpElement.getRadius(), tmpElement.getSize());
@@ -338,9 +328,8 @@ public class ComponentPhysics extends Component implements PositionningInterface
 				final ProxyShape proxyShape = this.rigidBody.addCollisionShape(shape, transform, it.getMass());
 				proxyShape.setUserData(this);
 				this.listProxyShape.add(proxyShape);
-			} else if (it.isCapsule()) {
+			} else if (it instanceof Capsule tmpElement) {
 				Log.debug("    Capsule");
-				final Capsule tmpElement = (Capsule) it;
 				// Create the Capsule shape
 				final CapsuleShape shape = new CapsuleShape(tmpElement.getRadius(), tmpElement.getSize());
 				// The ephysic use Y as UP ==> ege use Z as UP
@@ -349,9 +338,8 @@ public class ComponentPhysics extends Component implements PositionningInterface
 				final ProxyShape proxyShape = this.rigidBody.addCollisionShape(shape, transform, it.getMass());
 				proxyShape.setUserData(this);
 				this.listProxyShape.add(proxyShape);
-			} else if (it.isCone()) {
+			} else if (it instanceof Cone tmpElement) {
 				Log.debug("    Cone");
-				final Cone tmpElement = (Cone) it;
 				// Create the Cone shape
 				final ConeShape shape = new ConeShape(tmpElement.getRadius(), tmpElement.getSize());
 				// The ephysic use Y as UP ==> ege use Z as UP
@@ -360,9 +348,8 @@ public class ComponentPhysics extends Component implements PositionningInterface
 				final ProxyShape proxyShape = this.rigidBody.addCollisionShape(shape, transform, it.getMass());
 				proxyShape.setUserData(this);
 				this.listProxyShape.add(proxyShape);
-			} else if (it.isSphere()) {
+			} else if (it instanceof Sphere tmpElement) {
 				Log.debug("    Sphere");
-				final Sphere tmpElement = (Sphere) it;
 				// Create the box shape
 				final SphereShape shape = new SphereShape(tmpElement.getRadius());
 				// The ephysic use Y as UP ==> ege use Z as UP
@@ -371,9 +358,8 @@ public class ComponentPhysics extends Component implements PositionningInterface
 				final ProxyShape proxyShape = this.rigidBody.addCollisionShape(shape, transform, it.getMass());
 				proxyShape.setUserData(this);
 				this.listProxyShape.add(proxyShape);
-			} else if (it.isConcave()) {
+			} else if (it instanceof Concave tmpElement) {
 				Log.debug("    Concave");
-				final Concave tmpElement = (Concave) it;
 				//static  etk::Vector<Vector3f> vertices = {Vector3f(-100.0f,-100.0f,-50.0f),Vector3f(100.0f,-100.0f,-50.0f),Vector3f(100.0f,100.0f,-50.0f)};
 				//static  etk::Vector<uint32_t> indices = {0,1,2};
 				
@@ -480,14 +466,13 @@ public class ComponentPhysics extends Component implements PositionningInterface
 	}
 	
 	public void renderDebug(final ResourceColored3DObject _draw, final Camera _camera) {
-		if (this.rigidBody == null) {
+		if (this.rigidBody == null) {}
-			return;
+		/*
-		}
 		final Matrix4f transformationMatrix = Matrix4f.IDENTITY;
 		final Color tmpColor = new Color(0.0f, 1.0f, 0.0f, 0.8f);
 		final AABB value = this.rigidBody.getAABB();
 		_draw.drawCubeLine(value.getMin(), value.getMax(), tmpColor, transformationMatrix, true, true);
-		
+		*/
 	}
 	
 	public void setAngularReactionEnable(final boolean value) {

src/org/atriasoft/ege/components/ComponentPhysicsPerso.java

@@ -3,51 +3,50 @@ package org.atriasoft.ege.components;
 import java.util.ArrayList;
 import java.util.List;
 
+import org.atriasoft.ege.Component;
+import org.atriasoft.ege.Environement;
+import org.atriasoft.ege.engines.EngineGravity;
+import org.atriasoft.ege.engines.EnginePhysicsPerso;
+import org.atriasoft.ege.internal.Log;
 import org.atriasoft.etk.Color;
 import org.atriasoft.etk.math.Matrix4f;
 import org.atriasoft.etk.math.Vector3f;
 import org.atriasoft.gale.resource.ResourceColored3DObject;
-import org.atriasoft.ege.internal.Log;
+import org.atriasoft.phyligram.ColisionPoint;
-import org.atriasoft.ege.Component;
-import org.atriasoft.ege.Environement;
-import org.atriasoft.ege.engines.EngineGravity;
-import org.atriasoft.ege.engines.EnginePhysics;
-import org.atriasoft.ege.engines.EnginePhysicsPerso;
 import org.atriasoft.phyligram.PhysicBox;
-import org.atriasoft.phyligram.PhysicCollisionAABB;
+import org.atriasoft.phyligram.PhysicHeightMapChunk;
 import org.atriasoft.phyligram.PhysicMapVoxel;
 import org.atriasoft.phyligram.PhysicShape;
 import org.atriasoft.phyligram.PhysicSphere;
+import org.atriasoft.phyligram.PhysicTriangle;
 import org.atriasoft.phyligram.ToolCollisionOBBWithOBB;
-
+import org.atriasoft.phyligram.ToolCollisionSphereWithHeightMapChunk;
+import org.atriasoft.phyligram.ToolCollisionSphereWithSphere;
+import org.atriasoft.phyligram.ToolCollisionSphereWithTriangle;
+import org.atriasoft.phyligram.shape.AABB;
 
 public class ComponentPhysicsPerso extends Component {
-	private PhysicCollisionAABB aabb;
+	public static float globalMaxSpeed = Float.MAX_VALUE;
-	private List<ComponentPhysicsPerso> aabbIntersection = new ArrayList<ComponentPhysicsPerso>();
+	private AABB aabb;
-	private List<ComponentPhysicsPerso> narrowIntersection = new ArrayList<ComponentPhysicsPerso>();
+	private List<ComponentPhysicsPerso> aabbIntersection = new ArrayList<>();
-	private List<PhysicShape> shapes = new ArrayList<PhysicShape>();
+	private List<ComponentPhysicsPerso> narrowIntersection = new ArrayList<>();
+	List<ColisionPoint> collisionPoints = new ArrayList<>();
+	private List<PhysicShape> shapes = new ArrayList<>();
 	private ComponentPosition position;
 	private boolean staticObject = false;
-	private boolean manageGravity = false;
+	private boolean manageGravity = true;
-	public static float globalMaxSpeed = Float.MAX_VALUE;
 	private float maxSpeed = globalMaxSpeed;
 	// current speed of the object
-	private Vector3f speed = new Vector3f(0,0,0);
+	private Vector3f speed = new Vector3f(0, 0, 0);
 	// current acceleration of the object
-	private Vector3f acceleration = new Vector3f(0,0,0);
+	private Vector3f acceleration = new Vector3f(0, 0, 0);
 	// Applied static force on it
-	private Vector3f staticForce = new Vector3f(0,0,0);
+	private Vector3f staticForce = new Vector3f(0, 0, 0);
 	// Apply dynamic force on it
-	private Vector3f dynamicForce = new Vector3f(0,0,0);
+	private Vector3f dynamicForce = new Vector3f(0, 0, 0);
 	private EnginePhysicsPerso engine;
 	private PhysicBodyType bodyType;
 	
-	
-	@Override
-	public String getType() {
-		return EnginePhysicsPerso.ENGINE_NAME;
-	}
-	
 	public ComponentPhysicsPerso(final Environement _env) {
 		this.engine = (EnginePhysicsPerso) _env.getEngine(getType());
 	}
@@ -56,123 +55,114 @@ public class ComponentPhysicsPerso extends Component {
 	public void addFriendComponent(Component component) {
 		if (component.getType().contentEquals("position")) {
 			if (component instanceof ComponentPosition tmp) {
-				position = tmp;
+				this.position = tmp;
 			} else {
 				Log.error("Not manage position model...");
 			}
 		}
 	}
-	@Override
+	
-	public void removeFriendComponent(Component component) {
+	public void addIntersection(ComponentPhysicsPerso component) {
-		// nothing to do.
+		// do not add multiple times
+		if (this.aabbIntersection.contains(component)) {
+			return;
+		}
+		this.aabbIntersection.add(component);
 	}
 	
-	public void updateAABB() {
+	public void addShape(PhysicShape shape) {
-		if (position == null) {
+		this.shapes.add(shape);
-			Log.info("No position in Entity ");
-			return;
-		}
-		// TODO Add a flag to check if it is needed to update the AABB...
-		PhysicCollisionAABB aabbNew = PhysicCollisionAABB.beforeCalculated();
-		for (PhysicShape shape : shapes) {
-			shape.updateAABB(position.getTransform(), aabbNew);
-		}
-		aabb = aabbNew;
 	}
 	
-	public PhysicCollisionAABB getAABB() {
+	public void applyColisionForce() {
-		return aabb;
+		for (ColisionPoint impact : this.collisionPoints) {
+			this.position.applyForce(impact.force);
+		}
+		
 	}
 	
-	public void updateForNarrowCollision() {
+	public void applyForces(float timeStep, EngineGravity gravity) {
-		narrowIntersection.clear();
+		// get the gravity at the specific position...
-		if (aabbIntersection.size() == 0) {
+		Vector3f gravityForce;
-			return;
+		if (this.manageGravity) {
+			gravityForce = gravity.getGravityAtPosition(this.position.getTransform().getPosition()).multiply(timeStep);
+		} else {
+			gravityForce = new Vector3f(0, 0, 0);
 		}
-		if (position == null) {
+		// apply this force on the Object
-			Log.info("No position in Entity ");
+		Log.info("apply gravity: " + gravityForce);
-			return;
+		// relative to the object
-		}
+		Vector3f staticForce = this.staticForce.multiply(timeStep);
-		for (PhysicShape shape : shapes) {
+		float globalMass = 0;
-			shape.updateForNarrowCollision(position.getTransform());
+		for (PhysicShape shape : this.shapes) {
-		}
+			globalMass += shape.getMass();
-	}
-	public boolean isNarrowCollide() {
-		if (narrowIntersection.size() == 0) {
-			return false;
-		}
-		return true;
-	}
-	public boolean checkNarrowCollision() {
-		if (this.staticObject == true) {
-			return false;
-		}
-		for (ComponentPhysicsPerso elem : aabbIntersection) {
-			boolean collide = false;
-			for (PhysicShape shapeCurrent : shapes) {
-				if (elem.checkCollide(shapeCurrent) == true) {
-					collide = true;
-					break;
-				}
-			}
-			if (collide == true) {
-				narrowIntersection.add(elem);
-				elem.narrowIntersection.add(this);
-			}
-		}
-		return isNarrowCollide();
-	}
-	public void narrowCollisionCreateContactAndForce() {
-		if (narrowIntersection.size() == 0) {
-			return;
-		}
-		for (ComponentPhysicsPerso elem : narrowIntersection) {
-			for (PhysicShape shapeCurrent : this.shapes) {
-				//TODO Do a better method we do this many times ...
-				if (elem.checkCollide(shapeCurrent) == false) {
-					continue;
-				}
-				elem.getCollidePoints(shapeCurrent, this.staticObject);
-			}
 		}
+		// note the acceleration is not real, it depend on the current delta time...
+		this.acceleration = gravityForce.add(this.position.getTransform().getOrientation().multiply(staticForce)).add(this.position.getTransform().getOrientation().multiply(this.dynamicForce))
+				.multiply(globalMass);
+		this.dynamicForce = new Vector3f(0, 0, 0);
+		this.speed = this.speed.add(this.acceleration);
+		limitWithMaxSpeed();
+		Log.info("apply acceleration: " + this.acceleration);
+		Log.info("apply speed: " + this.speed);
+		this.position.setTransform(this.position.getTransform().withPosition(this.position.getTransform().getPosition().add(this.speed.multiply(timeStep))));
 	}
 	
 	private boolean checkCollide(PhysicShape shapeCurrent) {
-		if (shapeCurrent instanceof PhysicBox) {
+		if (shapeCurrent instanceof PhysicBox shape111) {
-			PhysicBox shape111 = (PhysicBox)shapeCurrent;
+			for (PhysicShape shape : this.shapes) {
-			for (PhysicShape shape : shapes) {
+				if (shape instanceof PhysicHeightMapChunk shape222) {
-				if (shape instanceof PhysicBox) {
+					// detect collision from cube on height-map !!!
-					PhysicBox shape222 = (PhysicBox)shape;
+					
-					if (ToolCollisionOBBWithOBB.testCollide(shape111, shape222) == true) {
+				} else if (shape instanceof PhysicBox shape222) {
+					// detect collision between 2 cubes
+					if (ToolCollisionOBBWithOBB.testCollide(shape111, shape222)) {
 						return true;
 					}
-				} else if (shape instanceof PhysicSphere) {
+				} else if (shape instanceof PhysicSphere shape222) {
 					
-				} else if (shape instanceof PhysicMapVoxel) {
+				} else if (shape instanceof PhysicMapVoxel shape222) {
 					
 				} else {
 					Log.error("Not manage collision model... " + shape);
 				}
 			}
-		} else if (shapeCurrent instanceof PhysicSphere) {
+		} else if (shapeCurrent instanceof PhysicSphere shape111) {
-			for (PhysicShape shape : shapes) {
+			for (PhysicShape shape : this.shapes) {
-				if (shape instanceof PhysicBox) {
+				if (shape instanceof PhysicHeightMapChunk shape222) {
+					// detect collision from sphere on height-map !!!
+					if (ToolCollisionSphereWithHeightMapChunk.testCollide(shape111, shape222)) {
+						return true;
+					}
 					
-				} else if (shape instanceof PhysicSphere) {
+				} else if (shape instanceof PhysicTriangle shape222) {
+					// detect collision from sphere on height-map !!!
+					if (ToolCollisionSphereWithTriangle.testCollide(shape111, shape222)) {
+						return true;
+					}
 					
-				} else if (shape instanceof PhysicMapVoxel) {
+				} else if (shape instanceof PhysicBox shape222) {
+					// detect collision from sphere on cube !!!
+					
+				} else if (shape instanceof PhysicSphere shape222) {
+					// detect collision from sphere on sphere !!!
+					if (ToolCollisionSphereWithSphere.testCollide(shape111, shape222)) {
+						return true;
+					}
+					
+				} else if (shape instanceof PhysicMapVoxel shape222) {
 					
 				} else {
 					Log.error("Not manage collision model... " + shape);
 				}
 			}
-		} else if (shapeCurrent instanceof PhysicMapVoxel) {
+		} else if (shapeCurrent instanceof PhysicMapVoxel shape111) {
-			for (PhysicShape shape : shapes) {
+			for (PhysicShape shape : this.shapes) {
-				if (shape instanceof PhysicBox) {
+				if (shape instanceof PhysicBox shape222) {
 					
-				} else if (shape instanceof PhysicSphere) {
+				} else if (shape instanceof PhysicSphere shape222) {
 					
-				} else if (shape instanceof PhysicMapVoxel) {
+				} else if (shape instanceof PhysicMapVoxel shape222) {
 					
 				} else {
 					Log.error("Not manage collision model... " + shape);
@@ -183,152 +173,204 @@ public class ComponentPhysicsPerso extends Component {
 		}
 		return false;
 	}
-	private void getCollidePoints(PhysicShape shapeCurrent, boolean isStatic) {
-		if (shapeCurrent instanceof PhysicBox) {
-			PhysicBox shape111 = (PhysicBox)shapeCurrent;
-			for (PhysicShape shape : this.shapes) {
-				if (shape instanceof PhysicBox) {
-					PhysicBox shape222 = (PhysicBox)shape;
-					ToolCollisionOBBWithOBB.getCollidePoints(shape111, isStatic, shape222, this.staticObject);
-				} else if (shape instanceof PhysicSphere) {
 	
-				} else if (shape instanceof PhysicMapVoxel) {
+	public boolean checkNarrowCollision() {
-					
+		if (this.staticObject) {
-				} else {
+			return false;
-					Log.error("Not manage collision model... " + shape);
-				}
-			}
-		} else if (shapeCurrent instanceof PhysicSphere) {
-			for (PhysicShape shape : this.shapes) {
-				if (shape instanceof PhysicBox) {
-					
-				} else if (shape instanceof PhysicSphere) {
-					
-				} else if (shape instanceof PhysicMapVoxel) {
-					
-				} else {
-					Log.error("Not manage collision model... " + shape);
-				}
-			}
-		} else if (shapeCurrent instanceof PhysicMapVoxel) {
-			for (PhysicShape shape : this.shapes) {
-				if (shape instanceof PhysicBox) {
-					
-				} else if (shape instanceof PhysicSphere) {
-					
-				} else if (shape instanceof PhysicMapVoxel) {
-					
-				} else {
-					Log.error("Not manage collision model... " + shape);
-				}
-			}
-		} else {
-			Log.error("Not manage collision model... " + shapeCurrent);
 		}
-		return;
+		for (ComponentPhysicsPerso elem : this.aabbIntersection) {
+			boolean collide = false;
+			for (PhysicShape shapeCurrent : this.shapes) {
+				if (elem.checkCollide(shapeCurrent)) {
+					collide = true;
+					break;
+				}
+			}
+			if (collide) {
+				this.narrowIntersection.add(elem);
+				elem.narrowIntersection.add(this);
+			}
+		}
+		return isNarrowCollide();
 	}
 	
-	public void applyForces(float timeStep, EngineGravity gravity) {
+	public void clearAABBIntersection() {
-		// get the gravity at the specific position...
+		this.aabbIntersection.clear();
-		Vector3f gravityForce;
+	}
-		if (manageGravity == true) {
+	
-			gravityForce = gravity.getGravityAtPosition(position.getTransform().getPosition()).multiply(timeStep);
+	public void clearPreviousCollision() {
+		this.collisionPoints.clear();
+		this.aabbIntersection.clear();
+		this.narrowIntersection.clear();
+	}
+	
+	public void clearShape() {
+		this.shapes.clear();
+	}
+	
+	public AABB getAABB() {
+		return this.aabb;
+	}
+	
+	public List<ComponentPhysicsPerso> getAabbIntersection() {
+		return this.aabbIntersection;
+	}
+	
+	public PhysicBodyType getBodyType() {
+		return this.bodyType;
+	}
+	
+	private List<ColisionPoint> getCollidePoints(PhysicShape shapeRemote) {
+		List<ColisionPoint> out = new ArrayList<>();
+		if (shapeRemote instanceof PhysicSphere remoteShere) {
+			for (PhysicShape shape : this.shapes) {
+				if (shape instanceof PhysicSphere localShape) {
+					ColisionPoint point = ToolCollisionSphereWithSphere.getCollisionPoint(remoteShere, localShape);
+					if (point != null) {
+						out.add(point);
+					}
+				} else if (shape instanceof PhysicTriangle localShape) {
+					ColisionPoint point = ToolCollisionSphereWithTriangle.getCollisionPoint(remoteShere, localShape);
+					if (point != null) {
+						out.add(point);
+					}
+					
+				}
+			}
+		} else if (shapeRemote instanceof PhysicTriangle) {
+			// nothing can happens ...
 		} else {
-			gravityForce = new Vector3f(0,0,0);
+			Log.error("Not manage collision model... " + shapeRemote);
 		}
-		// apply this force on the Object
+		return out;
-		Log.info("apply gravity: " + gravityForce);
+	}
-		// relative to the object
+	
-		Vector3f staticForce = this.staticForce.multiply(timeStep);
+	public float getMaxSpeed() {
-		float globalMass = 0;
+		return this.maxSpeed;
-		for (PhysicShape shape : shapes) {
+	}
-			globalMass += shape.getMass();
+	
+	@Override
+	public String getType() {
+		return EnginePhysicsPerso.ENGINE_NAME;
+	}
+	
+	public boolean isManageGravity() {
+		return this.manageGravity;
+	}
+	
+	public boolean isNarrowCollide() {
+		if (this.narrowIntersection.size() == 0) {
+			return false;
 		}
-		// note the acceleration is not real, it depend on the current delta time...
+		return true;
-		this.acceleration = gravityForce.add(this.position.getTransform().getOrientation().multiply(staticForce)).add(this.position.getTransform().getOrientation().multiply(dynamicForce)).multiply(globalMass);
+	}
-		this.dynamicForce = new Vector3f(0,0,0);
+	
-		this.speed.add(this.acceleration);
+	public boolean isStaticObject() {
-		limitWithMaxSpeed();
+		return this.staticObject;
-		Log.info("apply acceleration: " + this.acceleration);
+	}
-		Log.info("apply speed: " + this.speed);
+	
-		this.position.getTransform().getPosition().add(this.speed.multiply(timeStep));
+	private void limitWithMaxSpeed() {
+		if (this.speed.length2() > this.maxSpeed * this.maxSpeed) {
+			this.speed = this.speed.safeNormalize().multiply(this.maxSpeed);
+		}
+	}
+	
+	public void narrowCollisionCreateContactAndForce() {
+		
+		if (this.staticObject) {
+			return;
+		}
+		if (this.narrowIntersection.size() == 0) {
+			return;
+		}
+		for (ComponentPhysicsPerso elem : this.narrowIntersection) {
+			for (PhysicShape shapeCurrent : this.shapes) {
+				//TODO Do a better method we do this many times ...
+				/*
+				if (!elem.checkCollide(shapeCurrent)) {
+					continue;
+				}
+				*/
+				List<ColisionPoint> points = elem.getCollidePoints(shapeCurrent);
+				this.collisionPoints.addAll(points);
+			}
+		}
+	}
+	
+	@Override
+	public void removeFriendComponent(Component component) {
+		// nothing to do.
 	}
 	
 	public void renderDebug(ResourceColored3DObject debugDrawProperty) {
 		Color displayColor;
+		displayColor = new Color(1.0f, 0.0f, 0.0f, 1.0f);
+		for (ColisionPoint impact : this.collisionPoints) {
+			debugDrawProperty.drawSquare(new Vector3f(0.02f, 0.02f, 0.02f), Matrix4f.createMatrixTranslate(impact.position), displayColor);
+		}
 		if (this.aabbIntersection.size() == 0) {
-			displayColor = new Color(1,1,1,1);
+			displayColor = new Color(1.0f, 1.0f, 1.0f, 1.0f);
 		} else {
 			if (this.narrowIntersection.size() == 0) {
-				displayColor = new Color(1,1,0,1);
+				displayColor = new Color(0.0f, 1.0f, 0.0f, 1.0f);
 			} else {
-				displayColor = new Color(1,0,0,1);
+				displayColor = new Color(1.0f, 0.0f, 0.0f, 1.0f);
 			}
 		}
-		if (aabb != null) {
+		if (this.aabb != null) {
-			debugDrawProperty.drawCubeLine(aabb.getMin(), aabb.getMax(), displayColor, Matrix4f.IDENTITY, true, true);
+			debugDrawProperty.drawCubeLine(this.aabb.getMin(), this.aabb.getMax(), displayColor, Matrix4f.IDENTITY, true, true);
 			//debugDrawProperty.drawCubeLine(new Vector3f(0,0,0), new Vector3f(32,32,32), new Color(1,0,1,1), Matrix4f.identity(), true, true);
 		} else {
 			Log.error("no AABB");
 		}
-		for (PhysicShape shape : shapes) {
+		
-			shape.renderDebug(position.getTransform(), debugDrawProperty);
+		for (PhysicShape shape : this.shapes) {
+			shape.renderDebug(this.position.getTransform(), debugDrawProperty);
 		}
+		
 	}
-	public void addShape(PhysicShape shape) {
+	
-		shapes.add(shape);
+	public void setBodyType(PhysicBodyType bodyType) {
-	}
+		this.bodyType = bodyType;
-	public void clearShape() {
-		shapes.clear();
-	}
-	public boolean isManageGravity() {
-		return manageGravity;
 	}
+	
 	public void setManageGravity(boolean manageGravity) {
 		this.manageGravity = manageGravity;
 	}
-	private void limitWithMaxSpeed() {
-		if (this.speed.length2() > this.maxSpeed*this.maxSpeed) {
-			this.speed.safeNormalize().multiply(this.maxSpeed);
-		}
-	}
-	public float getMaxSpeed() {
-		return maxSpeed;
-	}
 	
 	public void setMaxSpeed(float maxSpeed) {
 		this.maxSpeed = maxSpeed;
 	}
 	
-	public void clearAABBIntersection() {
-		this.aabbIntersection.clear();
-	}
-	public void addIntersection(ComponentPhysicsPerso component) {
-		// do not add multiple times
-		for (ComponentPhysicsPerso elem : this.aabbIntersection) {
-			if (elem == component) {
-				return;
-			}
-		}
-		this.aabbIntersection.add(component);
-	}
-	public List<ComponentPhysicsPerso> getAabbIntersection() {
-		return aabbIntersection;
-	}
-
-	public boolean isStaticObject() {
-		return staticObject;
-	}
-
 	public void setStaticObject(boolean staticObject) {
 		this.staticObject = staticObject;
 	}
 	
-	public PhysicBodyType getBodyType() {
+	public void updateAABB() {
-		return bodyType;
+		
+		if (this.position == null) {
+			Log.info("No position in Entity ");
+			return;
+		}
+		// TODO Add a flag to check if it is needed to update the AABB...
+		AABB aabbNew = AABB.createInvertedEmpty();
+		for (PhysicShape shape : this.shapes) {
+			shape.updateAABB(this.position.getTransform(), aabbNew);
+		}
+		this.aabb = aabbNew;
 	}
 	
-	public void setBodyType(PhysicBodyType bodyType) {
+	public void updateForNarrowCollision() {
-		this.bodyType = bodyType;
+		this.narrowIntersection.clear();
+		if (this.aabbIntersection.size() == 0) {
+			return;
+		}
+		if (this.position == null) {
+			Log.info("No position in Entity ");
+			return;
+		}
+		for (PhysicShape shape : this.shapes) {
+			shape.updateForNarrowCollision(this.position.getTransform());
+		}
 	}
 }

src/org/atriasoft/ege/components/ComponentPosition.java

@@ -5,9 +5,10 @@ import org.atriasoft.ege.Signal;
 import org.atriasoft.ege.components.part.PositionningInterface;
 import org.atriasoft.ege.internal.Log;
 import org.atriasoft.etk.math.Transform3D;
+import org.atriasoft.etk.math.Vector3f;
 
 public class ComponentPosition extends Component implements PositionningInterface {
-	public final Signal<Transform3D> signalPosition = new Signal<Transform3D>();
+	public final Signal<Transform3D> signalPosition = new Signal<>();
 	protected Transform3D transform;
 	
 	/**
@@ -32,6 +33,10 @@ public class ComponentPosition extends Component implements PositionningInterfac
 		}
 	}
 	
+	public void applyForce(Vector3f force) {
+		this.transform = this.transform.withPosition(this.transform.getPosition().add(force));
+	}
+	
 	/**
 	 * set a new transformation
 	 * @return Transformation of the position

src/org/atriasoft/ege/engines/EnginePhysics.java

@@ -87,10 +87,9 @@ public class EnginePhysics extends Engine implements EventListener {
 	
 	@Override
 	public void componentAdd(final Component ref) {
-		if (!(ref instanceof ComponentPhysics)) {
+		if (!(ref instanceof ComponentPhysics elem)) {
 			return;
 		}
-		final ComponentPhysics elem = (ComponentPhysics) ref;
 		this.components.add(elem);
 		elem.generate();
 	}
@@ -180,10 +179,8 @@ public class EnginePhysics extends Engine implements EventListener {
 		for (final ContactManifold it : listContact) {
 			for (int iii = 0; iii < it.getNbContactPoints(); iii++) {
 				final ContactPoint contact = it.getContactPoint(iii);
-				this.debugDrawProperty.drawSquare(new Vector3f(0.05f, 0.05f, 0.05f), Matrix4f.IDENTITY.multiply(Matrix4f.createMatrixTranslate(contact.getWorldPointOnBody1())),
+				this.debugDrawProperty.drawSquare(new Vector3f(0.05f, 0.05f, 0.05f), Matrix4f.IDENTITY.multiply(Matrix4f.createMatrixTranslate(contact.getWorldPointOnBody1())), new Color(0, 1, 0, 1));
-						new Color(0, 1, 0, 1));
+				this.debugDrawProperty.drawSquare(new Vector3f(0.05f, 0.05f, 0.05f), Matrix4f.IDENTITY.multiply(Matrix4f.createMatrixTranslate(contact.getWorldPointOnBody2())), new Color(0, 1, 0, 1));
-				this.debugDrawProperty.drawSquare(new Vector3f(0.05f, 0.05f, 0.05f), Matrix4f.IDENTITY.multiply(Matrix4f.createMatrixTranslate(contact.getWorldPointOnBody2())),
-						new Color(0, 1, 0, 1));
 			}
 		}
 		

src/org/atriasoft/ege/engines/EnginePhysicsPerso.java

@@ -2,35 +2,54 @@ package org.atriasoft.ege.engines;
 
 import java.util.Vector;
 
-import org.atriasoft.gale.resource.ResourceColored3DObject;
-import org.atriasoft.ege.internal.Log;
 import org.atriasoft.ege.Component;
 import org.atriasoft.ege.Engine;
 import org.atriasoft.ege.Environement;
 import org.atriasoft.ege.camera.Camera;
 import org.atriasoft.ege.components.ComponentPhysicsPerso;
-import org.atriasoft.phyligram.PhysicCollisionAABB;
+import org.atriasoft.ege.internal.Log;
+import org.atriasoft.gale.resource.ResourceColored3DObject;
+import org.atriasoft.phyligram.DebugDisplay;
+import org.atriasoft.phyligram.shape.AABB;
 
 public class EnginePhysicsPerso extends Engine {
 	public static final String ENGINE_NAME = "physicsPerso";
-	private float accumulator = 0;
 	private static final float TIME_STEP = 0.005f;
+	private float accumulator = 0;
 	private EngineGravity gravity;
 	protected EnginePhysicsPerso engine;
-	private Vector<ComponentPhysicsPerso> components = new Vector<ComponentPhysicsPerso>();
+	private Vector<ComponentPhysicsPerso> components = new Vector<>();
+	private Vector<ComponentPhysicsPerso> componentsWithCollision = new Vector<>();
 	private ResourceColored3DObject debugDrawProperty = ResourceColored3DObject.create();
 	
 	public EnginePhysicsPerso(Environement env) {
 		super(env);
-		this.gravity = (EngineGravity)env.getEngine("gravity");
+		this.gravity = (EngineGravity) env.getEngine("gravity");
 		if (this.gravity == null) {
 			Log.critical("Must initialyse Gravity before physics...");
 		}
 	}
 	
-	@Override
+	private void addIncomponentWithCollision(ComponentPhysicsPerso elem) {
-	public void componentRemove(Component ref) {
+		if (this.componentsWithCollision.contains(elem)) {
-		components.remove(ref);
+			return;
+		}
+		this.componentsWithCollision.add(elem);
+	}
+	
+	private void applyForces(float timeStep) {
+		for (ComponentPhysicsPerso it : this.components) {
+			it.applyForces(TIME_STEP, this.gravity);
+		}
+	}
+	
+	/**
+	 *  Clear the previous data of collision.
+	 */
+	private void clearPreviousCycle() {
+		for (ComponentPhysicsPerso it : this.components) {
+			it.clearPreviousCollision();
+		}
 	}
 	
 	@Override
@@ -38,98 +57,36 @@ public class EnginePhysicsPerso extends Engine {
 		if (ref instanceof ComponentPhysicsPerso == false) {
 			return;
 		}
-		components.add((ComponentPhysicsPerso)ref);
+		this.components.add((ComponentPhysicsPerso) ref);
 	}
 	
 	@Override
-	public void update(long deltaMili) {
+	public void componentRemove(Component ref) {
-		// Add the time difference in the accumulator
+		this.components.remove(ref);
-		accumulator += (float)deltaMili*0.0001f;
-		// While there is enough accumulated time to take one or several physics steps
-		while (accumulator >= TIME_STEP) {
-			Log.info("update physic ... " + accumulator);
-			//applyForces(TIME_STEP);
-			updateAABB(TIME_STEP);
-			updateCollisionsAABB(TIME_STEP);
-			updateCollisionsNarrowPhase(TIME_STEP);
-			generateResultCollisionsForces(TIME_STEP);
-			// Decrease the accumulated time
-			accumulator -= TIME_STEP;
-		}
-		
 	}
 	
-	private void applyForces(float timeStep) {
-		for (ComponentPhysicsPerso it: components) {
-			it.applyForces(TIME_STEP, gravity);
-		}
-	}
-	/**
-	 * Collision detection STEP 1: Upadte the AABB positioning of each elements
-	 * @param timeStep Delta time since the last check
-	 */
-	private void updateAABB(float timeStep) {
-		for (ComponentPhysicsPerso it: components) {
-			it.updateAABB();
-		}
-	}
-	/**
-	 * Collision Detection STEP 2: update the list of each element that collide together in the AABB Boxs (update is done between each boxes)
-	 * @param timeStep Delta time since the last check
-	 */
-	private void updateCollisionsAABB(float timeStep) {
-		// clear all object intersection
-		for (ComponentPhysicsPerso it: components) {
-			it.clearAABBIntersection();
-		}
-		// update the current object intersection...
-		for (int iii=0; iii< components.size(); iii++) {
-			ComponentPhysicsPerso current = components.get(iii);
-			PhysicCollisionAABB currentAABB = current.getAABB();
-			for (int jjj=iii+1; jjj< components.size(); jjj++) {
-				ComponentPhysicsPerso remote = components.get(jjj);
-				if (currentAABB.intersect(components.get(jjj).getAABB()) == true) {
-					current.addIntersection(remote); 
-					remote.addIntersection(current);
-				}
-			}
-		}
-	}
-	/**
-	 * Collision Detection STEP 3: Narrow phase: process the collision between every OBB boxes (or other..)  
-	 * @param timeStep Delta time since the last check
-	 */
-	private void updateCollisionsNarrowPhase(float timeStep) {
-		// clear all object intersection
-		for (ComponentPhysicsPerso it: components) {
-			it.updateForNarrowCollision();
-		}
-		// check for every component if the narrow collision is available.
-		for (int iii=0; iii< components.size(); iii++) {
-			ComponentPhysicsPerso current = components.get(iii);
-			boolean collide = current.checkNarrowCollision();
-			
-		}
-		// update the force of collision available.
-		for (int iii=0; iii< components.size(); iii++) {
-			ComponentPhysicsPerso current = components.get(iii);
-			current.narrowCollisionCreateContactAndForce();
-		}
-	}
 	/**
 	 * Collision Detection STEP 4: apply all calculated forces (with containts)
 	 * @param timeStep
 	 */
 	private void generateResultCollisionsForces(float timeStep) {
+		for (ComponentPhysicsPerso it : this.componentsWithCollision) {
+			it.applyColisionForce();
+		}
+	}
 	
+	@Override
+	public String getType() {
+		// TODO Auto-generated method stub
+		return ENGINE_NAME;
 	}
 	
 	@Override
 	public void render(long deltaMili, Camera camera) {
 		// TODO Auto-generated method stub
-		for (ComponentPhysicsPerso it: this.components) {
+		for (ComponentPhysicsPerso it : this.components) {
 			//Log.info("Render " + it);
-			it.renderDebug(debugDrawProperty);
+			it.renderDebug(this.debugDrawProperty);
 		}
 		//debugDrawProperty.drawCone(2, 5, 9, 12, Matrix4f.identity(), new Color(1,1,0,1));
 		//debugDrawProperty.drawSquare(new Vector3f(1,1,1), Matrix4f.identity(), new Color(1,1,0,1));
@@ -140,13 +97,89 @@ public class EnginePhysicsPerso extends Engine {
 	@Override
 	public void renderDebug(long deltaMili, Camera camera) {
 		// TODO Auto-generated method stub
-		
+		DebugDisplay.onDraw();
+		DebugDisplay.clear();
 	}
 	
 	@Override
-	public String getType() {
+	public void update(long deltaMili) {
-		// TODO Auto-generated method stub
+		// Add the time difference in the accumulator
-		return ENGINE_NAME;
+		this.accumulator += deltaMili * 0.0001f;
+		// While there is enough accumulated time to take one or several physics steps
+		while (this.accumulator >= TIME_STEP) {
+			Log.info("update physic ... " + this.accumulator);
+			clearPreviousCycle();
+			applyForces(TIME_STEP);
+			// update AABB after because in rotation force, the Bounding box change...
+			updateAABB(TIME_STEP);
+			// update the colision tree between each object in the room
+			updateCollisionsAABB(TIME_STEP);
+			updateCollisionsNarrowPhase(TIME_STEP);
+			generateResultCollisionsForces(TIME_STEP);
+			// Decrease the accumulated time
+			this.accumulator -= TIME_STEP;
+		}
+		
+	}
+	
+	/**
+	 * Collision detection STEP 1: Upadte the AABB positioning of each elements
+	 * @param timeStep Delta time since the last check
+	 */
+	private void updateAABB(float timeStep) {
+		for (ComponentPhysicsPerso it : this.components) {
+			it.updateAABB();
+		}
+	}
+	
+	/**
+	 * Collision Detection STEP 2: update the list of each element that collide together in the AABB Boxs (update is done between each boxes)
+	 * @param timeStep Delta time since the last check
+	 */
+	// TODO : generate a B-TREE to manage collision, it is faster, but now, this is not the purpose ...
+	private void updateCollisionsAABB(float timeStep) {
+		this.componentsWithCollision.clear();
+		// clear all object intersection
+		for (ComponentPhysicsPerso it : this.components) {
+			it.clearAABBIntersection();
+		}
+		// update the current object intersection...
+		for (int iii = 0; iii < this.components.size(); iii++) {
+			ComponentPhysicsPerso current = this.components.get(iii);
+			AABB currentAABB = current.getAABB();
+			for (int jjj = iii + 1; jjj < this.components.size(); jjj++) {
+				ComponentPhysicsPerso remote = this.components.get(jjj);
+				// prefer checking the collision, this a time-constant operation, check if collision already exist is a unpredictable time.
+				if (currentAABB.intersect(this.components.get(jjj).getAABB()) == true) {
+					current.addIntersection(remote);
+					remote.addIntersection(current);
+					addIncomponentWithCollision(remote);
+					addIncomponentWithCollision(current);
+				}
+			}
+		}
+	}
+	
+	/**
+	 * Collision Detection STEP 3: Narrow phase: process the collision between every OBB boxes (or other..)
+	 * @param timeStep Delta time since the last check
+	 */
+	private void updateCollisionsNarrowPhase(float timeStep) {
+		// clear all object intersection
+		for (ComponentPhysicsPerso it : this.componentsWithCollision) {
+			it.updateForNarrowCollision();
+		}
+		// check for every component if the narrow collision is available.
+		for (int iii = 0; iii < this.componentsWithCollision.size(); iii++) {
+			ComponentPhysicsPerso current = this.componentsWithCollision.get(iii);
+			boolean collide = current.checkNarrowCollision();
+			
+		}
+		// update the force of collision available.
+		for (int iii = 0; iii < this.components.size(); iii++) {
+			ComponentPhysicsPerso current = this.components.get(iii);
+			current.narrowCollisionCreateContactAndForce();
+		}
 	}
 	
 }

src/org/atriasoft/ege/geometry/Geometry3D.java

@@ -52,7 +52,7 @@ public class Geometry3D {
 		return true;
 	}
 	
-	public static boolean pointInPlane(final Vector3f point, final Plane plane) {
+	public static boolean pointInPlane(final Vector3f point, final Plane____ plane) {
 		// This should probably use an epsilon!
 		//return Dot(point, plane.normal) - plane.distance == 0.0f;
 		return CMP(point.dot(plane.normal) - plane.distance, 0.0f);

src/org/atriasoft/ege/geometry/Plane____.java

@@ -2,15 +2,15 @@ package org.atriasoft.ege.geometry;
 
 import org.atriasoft.etk.math.Vector3f;
 
-public class Plane {
+public class Plane____ {
 	public Vector3f normal;
 	public float distance;
 
-	public Plane(Vector3f normal, float distance) {
+	public Plane____(Vector3f normal, float distance) {
 		this.normal = normal;
 		this.distance = distance;
 	}
-	public Plane() {
+	public Plane____() {
 		this.normal = new Vector3f(1.0f, 0.0f, 0.0f);
 		this.distance = 0;
 	}

src/org/atriasoft/ege/geometry/Triangle.java

@@ -3,8 +3,19 @@ package org.atriasoft.ege.geometry;
 import org.atriasoft.etk.math.Vector3f;
 
 public class Triangle {
+	public static Vector3f getCenter(Vector3f p1, Vector3f p2, Vector3f p3) {
+		return p1.add(p2).add(p3).multiply(0.33333333333f);
+	}
+	
+	public static Vector3f getNormal(Vector3f p1, Vector3f p2, Vector3f p3) {
+		Vector3f dir = p2.less(p1).cross(p3.less(p1));
+		return dir.normalize();
+	}
+	
 	public Vector3f p1;
+	
 	public Vector3f p2;
+	
 	public Vector3f p3;
 	
 	public Triangle() {
@@ -19,9 +30,26 @@ public class Triangle {
 		this.p3 = p3;
 	}
 	
+	@Override
+	public Triangle clone() {
+		return new Triangle(this.p1, this.p2, this.p3);
+	}
+	
+	public Vector3f getCenter() {
+		return this.p1.add(this.p2).add(this.p3).multiply(0.33333333333f);
+	}
+	
+	public Vector3f getNormal() {
+		Vector3f dir = this.p2.less(this.p1).cross(this.p3.less(this.p1));
+		return dir.normalize();
+	}
+	
+	public Plane getPlane() {
+		return new Plane(getNormal(), getCenter());
+	}
+	
 	@Override
 	public String toString() {
 		return "Triangle [p1=" + this.p1 + ", p2=" + this.p2 + ", p3=" + this.p3 + "]";
 	}
-	
 }

src/org/atriasoft/phyligram/ColisionPoint.java

@@ -2,11 +2,11 @@ package org.atriasoft.phyligram;
 
 import org.atriasoft.etk.math.Vector3f;
 
-public class ColisionPoints {
+public class ColisionPoint {
 	public Vector3f position;
 	public Vector3f force;
 
-	public ColisionPoints(Vector3f position, Vector3f force) {
+	public ColisionPoint(Vector3f position, Vector3f force) {
 		this.position = position;
 		this.force = force;
 	}

src/org/atriasoft/phyligram/Collision.java

@@ -1,12 +1,12 @@
 package org.atriasoft.phyligram;
 
 public class Collision {
-	public final ColisionPoints[] colisionPointLocal;
+	public final ColisionPoint[] colisionPointLocal;
 	public final PhysicShape shapeRemote;
-	public final ColisionPoints[] colisionPointRemote;
+	public final ColisionPoint[] colisionPointRemote;
 	public final boolean staticRemote;
-	public Collision(ColisionPoints[] colisionPointLocal, PhysicShape shapeRemote,
+	public Collision(ColisionPoint[] colisionPointLocal, PhysicShape shapeRemote,
-			ColisionPoints[] colisionPointRemote, boolean staticRemote) {
+			ColisionPoint[] colisionPointRemote, boolean staticRemote) {
 		super();
 		this.colisionPointLocal = colisionPointLocal;
 		this.shapeRemote = shapeRemote;

src/org/atriasoft/phyligram/DebugDisplay.java

@@ -8,303 +8,62 @@ import org.atriasoft.etk.math.Matrix4f;
 import org.atriasoft.etk.math.Quaternion;
 import org.atriasoft.etk.math.Vector3f;
 import org.atriasoft.gale.backend3d.OpenGL;
-import org.atriasoft.gale.context.GaleContext;
 import org.atriasoft.gale.resource.ResourceColored3DObject;
 
 public class DebugDisplay {
-//	public static ComponentPosition relativeTestPos;
+	//	public static ComponentPosition relativeTestPos;
-//	public static PhysicBox boxTest;
+	//	public static PhysicBox boxTest;
-	public static List<Vector3f> testPoints = new ArrayList<Vector3f>();
+	public static List<Vector3f> testPoints = new ArrayList<>();
-	public static List<Vector3f> testPointsBox = new ArrayList<Vector3f>();
+	public static List<Vector3f> testPointsBox = new ArrayList<>();
-	public static List<Boolean> testPointsCollide = new ArrayList<Boolean>();
+	public static List<Boolean> testPointsCollide = new ArrayList<>();
 	public static Vector3f testRpos;
 	public static Quaternion testQTransfert;
 	public static Vector3f box1HalfSize;
 	public static Vector3f box2HalfSize;
-	private ResourceColored3DObject debugDrawProperty;
+	private static ResourceColored3DObject debugDrawProperty;
-	public DebugDisplay(){
+	
+	static {
+		if (debugDrawProperty == null) {
+			debugDrawProperty = ResourceColored3DObject.create();
+		}
 		
 	}
-	/*
-	@Override
-	public void onCreate(Context context) {
-		// set the system global max speed
-		ComponentPhysics.globalMaxSpeed = 3;
-		Gale.getContext().grabPointerEvents(true, new Vector2f(0,0));
-		env = new Environement();
-		this.canDraw = true;
-		setSize(new Vector2f(1500, 1500));
-		setTitle("Loxel sample");
-		map = new MapVoxel(this.env);
-//		this.env.addEngine(map);
-//		map.init();
 	
-		// simple sun to have a global light ...
+	public static void clear() {
-		Entity globalGravity = new Entity(this.env);
+		testPoints.clear();
-		globalGravity.addComponent(new ComponentGravityStatic(new Vector3f(0,0,-1)));
+		testPointsBox.clear();
-		env.addEntity(globalGravity);
+		testPointsCollide.clear();
 		
-		// simple sun to have a global light ...
-		Entity sun = new Entity(this.env);
-		sun.addComponent(new ComponentPosition(new Transform3D(new Vector3f(1000,1000,1000))));
-		sun.addComponent(new ComponentLightSun(new Light(new Vector3f(0.4f,0.4f,0.4f), new Vector3f(0,0,0), new Vector3f(0.8f,0,0))));
-		env.addEntity(sun);
-
-		// add a cube to show where in the light ...
-		Entity localLight = new Entity(this.env);
-		lightPosition = new ComponentPosition(new Transform3D(new Vector3f(-10,-10,17)));
-//		localLight.addComponent(lightPosition);
-//		localLight.addComponent(new ComponentStaticMesh(new Uri("RES", "cube.obj")));
-//		localLight.addComponent(new ComponentTexture(new Uri("RES", "grass.png")));
-//		localLight.addComponent(new ComponentLight(new Light(new Vector3f(0,1,0), new Vector3f(0,0,0), new Vector3f(0.8f,0.03f,0.002f))));
-//		localLight.addComponent(new ComponentRenderTexturedStaticMesh(
-//				new Uri("DATA", "basic.vert"),
-//				new Uri("DATA", "basic.frag")));
-//		env.addEntity(localLight);
-		{
-			// add a cube to test collision ...
-			Entity localBox = new Entity(this.env);
-			localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(-2,-2,14))));
-			localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj")));
-			localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png")));
-			localBox.addComponent(new ComponentLight(new Light(new Vector3f(0,1,0), new Vector3f(0,0,0), new Vector3f(0.8f,0.03f,0.002f))));
-			localBox.addComponent(new ComponentRenderTexturedStaticMesh(
-					new Uri("DATA", "basic.vert"),
-					new Uri("DATA", "basic.frag")));
-			ComponentPhysics physics2 = new ComponentPhysics(true);
-			PhysicBox box2 = new PhysicBox();
-			box2.setSize(new Vector3f(1,1,1));
-			box2.setOrigin(new Vector3f(0,0,0));
-			box2.setMass(1);
-			physics2.addShape(box2);
-			localBox.addComponent(physics2);
-			env.addEntity(localBox);
-		}
-		{
-			// add a cube to test collision ...
-			Entity localBox = new Entity(this.env);
-			localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(0,4,12.5f))));
-			localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj")));
-			localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png")));
-			localBox.addComponent(new ComponentLight(new Light(new Vector3f(0,1,0), new Vector3f(0,0,0), new Vector3f(0.8f,0.03f,0.002f))));
-			localBox.addComponent(new ComponentRenderTexturedStaticMesh(
-					new Uri("DATA", "basic.vert"),
-					new Uri("DATA", "basic.frag")));
-			ComponentPhysics physics2 = new ComponentPhysics(true);
-			PhysicBox box2 = new PhysicBox();
-			box2.setSize(new Vector3f(1,1,1));
-			box2.setOrigin(new Vector3f(0,0,0));
-			box2.setMass(1);
-			physics2.addShape(box2);
-			localBox.addComponent(physics2);
-			env.addEntity(localBox);
-		}
-		{
-			// add a cube to test collision ...
-			Entity localBox = new Entity(this.env);
-			localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(-2,2,14.5f))));
-			localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj")));
-			localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png")));
-			localBox.addComponent(new ComponentRenderTexturedStaticMesh(
-					new Uri("DATA", "basic.vert"),
-					new Uri("DATA", "basic.frag")));
-			ComponentPhysics physics2 = new ComponentPhysics(true);
-			PhysicBox box2 = new PhysicBox();
-			box2.setSize(new Vector3f(1,1,1));
-			box2.setOrigin(new Vector3f(0,0,0));
-			box2.setMass(1);
-			physics2.addShape(box2);
-			localBox.addComponent(physics2);
-			env.addEntity(localBox);
-		}
-
-		{
-			// add a cube to test collision ...
-			Entity localBox = new Entity(this.env);
-			localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(-5,-5,14))));
-			localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj")));
-			localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png")));
-			localBox.addComponent(new ComponentRenderTexturedStaticMesh(
-					new Uri("DATA", "basic.vert"),
-					new Uri("DATA", "basic.frag")));
-			ComponentPhysics physics2 = new ComponentPhysics(true);
-			PhysicBox box2 = new PhysicBox();
-			box2.setSize(new Vector3f(4,4,4));
-			box2.setOrigin(new Vector3f(0,0,0));
-			box2.setMass(1);
-			physics2.addShape(box2);
-			localBox.addComponent(physics2);
-			env.addEntity(localBox);
-		}
-		{
-			// add a cube to test collision ...
-			Entity localBox = new Entity(this.env);
-			Quaternion transform = new Quaternion(0.5f,0.2f,0.4f,1);
-			transform.normalize();
-			localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(15,15,14), transform)));
-			localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj")));
-			localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png")));
-			localBox.addComponent(new ComponentRenderTexturedStaticMesh(
-					new Uri("DATA", "basic.vert"),
-					new Uri("DATA", "basic.frag")));
-			ComponentPhysics physics2 = new ComponentPhysics(true);
-			PhysicBox box2 = new PhysicBox();
-			box2.setSize(new Vector3f(8,8,8));
-			box2.setOrigin(new Vector3f(0,0,0));
-			box2.setMass(1);
-			physics2.addShape(box2);
-			localBox.addComponent(physics2);
-			env.addEntity(localBox);
-		}
-		{
-			// add a cube to test collision ...
-			Entity localBox = new Entity(this.env);
-			Quaternion transform = new Quaternion(0.3f,0.3f,0.4f,1);
-			transform.normalize();
-			localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(2,-2,14.2f),transform)));
-			localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj")));
-			localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png")));
-			localBox.addComponent(new ComponentRenderTexturedStaticMesh(
-					new Uri("DATA", "basic.vert"),
-					new Uri("DATA", "basic.frag")));
-			ComponentPhysics physics2 = new ComponentPhysics(true);
-			PhysicBox box2 = new PhysicBox();
-			box2.setSize(new Vector3f(1,1,1));
-			box2.setOrigin(new Vector3f(0,0,0));
-			box2.setMass(1);
-			physics2.addShape(box2);
-			localBox.addComponent(physics2);
-			env.addEntity(localBox);
-		}
-		{
-			// add a cube to test collision ...
-			Entity localBox = new Entity(this.env);
-			Quaternion transform = new Quaternion(0,0,1,1);
-			transform.normalize();
-			localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(2,2,14.2f),transform)));
-			localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj")));
-			localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png")));
-			localBox.addComponent(new ComponentRenderTexturedStaticMesh(
-					new Uri("DATA", "basic.vert"),
-					new Uri("DATA", "basic.frag")));
-			ComponentPhysics physics2 = new ComponentPhysics(true);
-			PhysicBox box2 = new PhysicBox();
-			box2.setSize(new Vector3f(1,1,1));
-			box2.setOrigin(new Vector3f(0,0,0));
-			box2.setMass(1);
-			physics2.addShape(box2);
-			localBox.addComponent(physics2);
-			env.addEntity(localBox);
-		}
-//		{
-//			// add a cube to test collision ...
-//			Entity localBox = new Entity(this.env);
-//			relativeTestPos = new ComponentPosition(new Transform3D(new Vector3f(0,0,14),new Quaternion(0.5f,0.2f,0.4f,1)));
-//			localBox.addComponent(relativeTestPos);
-////			localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj")));
-////			localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png")));
-////			localBox.addComponent(new ComponentLight(new Light(new Vector3f(0,1,0), new Vector3f(0,0,0), new Vector3f(0.8f,0.03f,0.002f))));
-////			localBox.addComponent(new ComponentRenderTexturedStaticMesh(
-////					new Uri("DATA", "basic.vert"),
-////					new Uri("DATA", "basic.frag")));
-//			ComponentPhysics physics2 = new ComponentPhysics(true);
-//			boxTest = new PhysicBox();
-//			boxTest.setSize(new Vector3f(1,1,1));
-//			boxTest.setOrigin(new Vector3f(0,0,0));
-//			boxTest.setMass(1);
-//			physics2.addShape(boxTest);
-//			localBox.addComponent(physics2);
-//			env.addEntity(localBox);
-//		}
-//		{
-//			Entity localBox = new Entity(this.env);
-//			localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(0,0,14))));
-//			localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj")));
-//			localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png")));
-//			localBox.addComponent(new ComponentRenderTexturedStaticMesh(
-//					new Uri("DATA", "basic.vert"),
-//					new Uri("DATA", "basic.frag")));
-//			env.addEntity(localBox);
-//		}
-		
-		Entity gird = new Entity(this.env);
-		gird.addComponent(new ComponentPosition(new Transform3D(new Vector3f(0,0,13))));
-		gird.addComponent(new ComponentStaticMesh(MeshGenerator.createGrid(5)));
-		gird.addComponent(new ComponentRenderColoredStaticMesh(
-				new Uri("DATA_EGE", "wireColor.vert"),
-				new Uri("DATA_EGE", "wireColor.frag")));
-		env.addEntity(gird);
-
-		Entity player = new Entity(this.env);
-		objectPosition = new ComponentPositionPlayer(new Transform3D(new Vector3f(5,5,13)));
-		player.addComponent(objectPosition);
-		objectPlayer = new ComponentPlayer();
-		player.addComponent(objectPlayer);
-		player.addComponent(new ComponentMaterial(new Material()));
-		//player.addComponent(new ComponentStaticMesh(new Uri("RES", "person.obj")));
-		player.addComponent(new ComponentStaticMesh(new Uri("RES", "person_-yfw_zup.obj")));
-		player.addComponent(new ComponentTexture(new Uri("RES", "playerTexture.png")));
-		player.addComponent(new ComponentRenderTexturedMaterialsStaticMesh(
-				new Uri("DATA", "basicMaterial.vert"),
-				new Uri("DATA", "basicMaterial.frag"),
-				(EngineLight)env.getEngine(EngineLight.ENGINE_NAME)));
-		ComponentPhysics physics = new ComponentPhysics(true);
-		PhysicBox box = new PhysicBox();
-		box.setSize(new Vector3f(0.6f,0.6f,1.8f));
-		box.setOrigin(new Vector3f(0,0,0.9f));
-		box.setMass(1);
-		physics.addShape(box);
-		player.addComponent(physics);
-		env.addEntity(player);
-		
-		
-		Camera mainView = new Camera();
-		env.addCamera("default", mainView);
-		mainView.setPitch((float)Math.PI*-0.25f);
-		mainView.setPosition(new Vector3f(0,-5,5));
-		
-		this.simpleControl = new ControlCameraPlayer(mainView, player);
-		env.addControlInterface(simpleControl);
-		
-		// start the engine.
-		env.setPropertyStatus(GameStatus.gameStart);
-		
-		basicRotation.setEulerAngles(new Vector3f(0.005f,0.005f,0.01f));
-		basicRotation2.setEulerAngles(new Vector3f(0.003f,0.01f,0.001f));
-		// ready to let Gale & Ege manage the display
-		Log.info("==> Init APPL (END)");
-		creationDone = true;
 	}
-	*/
 	
-	public void onDraw(GaleContext context) {
+	public static void onDraw() {
-		if (this.debugDrawProperty == null) {
+		if (debugDrawProperty == null) {
 			debugDrawProperty = ResourceColored3DObject.create();
 		}
 		// now render the point test collision ...
-		for (int iii=0; iii<DebugDisplay.testPoints.size(); iii++) {
+		for (int iii = 0; iii < DebugDisplay.testPoints.size(); iii++) {
 			Vector3f elem = DebugDisplay.testPoints.get(iii);
 			boolean collide = DebugDisplay.testPointsCollide.get(iii);
 			if (collide) {
-				debugDrawProperty.drawSquare(new Vector3f(0.1f,0.1f,0.1f), Matrix4f.createMatrixTranslate(new Vector3f(elem.x(),elem.y(),elem.z()+14)), new Color(1,0,0,1));
+				debugDrawProperty.drawSquare(new Vector3f(0.1f, 0.1f, 0.1f), Matrix4f.createMatrixTranslate(new Vector3f(elem.x(), elem.y(), elem.z() + 14)), new Color(1, 0, 0, 1));
 			} else {
-				if (iii==0) {
+				if (iii == 0) {
-					debugDrawProperty.drawSquare(new Vector3f(0.05f,0.05f,0.05f), Matrix4f.createMatrixTranslate(new Vector3f(elem.x(),elem.y(),elem.z()+14)), new Color(0,1,0,1));
+					debugDrawProperty.drawSquare(new Vector3f(0.05f, 0.05f, 0.05f), Matrix4f.createMatrixTranslate(new Vector3f(elem.x(), elem.y(), elem.z() + 14)), new Color(0, 1, 0, 1));
-				} else if (iii==7) {
+				} else if (iii == 7) {
-					debugDrawProperty.drawSquare(new Vector3f(0.05f,0.05f,0.05f), Matrix4f.createMatrixTranslate(new Vector3f(elem.x(),elem.y(),elem.z()+14)), new Color(1,1,0,1));
+					debugDrawProperty.drawSquare(new Vector3f(0.05f, 0.05f, 0.05f), Matrix4f.createMatrixTranslate(new Vector3f(elem.x(), elem.y(), elem.z() + 14)), new Color(1, 1, 0, 1));
 				} else {
-					debugDrawProperty.drawSquare(new Vector3f(0.1f,0.1f,0.1f), Matrix4f.createMatrixTranslate(new Vector3f(elem.x(),elem.y(),elem.z()+14)), new Color(1,1,1,1));
+					debugDrawProperty.drawSquare(new Vector3f(0.1f, 0.1f, 0.1f), Matrix4f.createMatrixTranslate(new Vector3f(elem.x(), elem.y(), elem.z() + 14)), new Color(1, 1, 1, 1));
 				}
 			}
 		}
-		for (int iii=0; iii<DebugDisplay.testPointsBox.size(); iii++) {
+		for (int iii = 0; iii < DebugDisplay.testPointsBox.size(); iii++) {
 			Vector3f elem = DebugDisplay.testPointsBox.get(iii);
-			if (iii==0) {
+			if (iii == 0) {
-				debugDrawProperty.drawSquare(new Vector3f(0.05f,0.05f,0.05f), Matrix4f.createMatrixTranslate(new Vector3f(elem.x(),elem.y(),elem.z()+14)), new Color(0,1,0,1));
+				debugDrawProperty.drawSquare(new Vector3f(0.05f, 0.05f, 0.05f), Matrix4f.createMatrixTranslate(new Vector3f(elem.x(), elem.y(), elem.z() + 14)), new Color(0, 1, 0, 1));
-			} else if (iii==7) {
+			} else if (iii == 7) {
-				debugDrawProperty.drawSquare(new Vector3f(0.05f,0.05f,0.05f), Matrix4f.createMatrixTranslate(new Vector3f(elem.x(),elem.y(),elem.z()+14)), new Color(1,1,0,1));
+				debugDrawProperty.drawSquare(new Vector3f(0.05f, 0.05f, 0.05f), Matrix4f.createMatrixTranslate(new Vector3f(elem.x(), elem.y(), elem.z() + 14)), new Color(1, 1, 0, 1));
 			} else {
-				debugDrawProperty.drawSquare(new Vector3f(0.1f,0.1f,0.1f), Matrix4f.createMatrixTranslate(new Vector3f(elem.x(),elem.y(),elem.z()+14)), new Color(0,0,1,1));
+				debugDrawProperty.drawSquare(new Vector3f(0.1f, 0.1f, 0.1f), Matrix4f.createMatrixTranslate(new Vector3f(elem.x(), elem.y(), elem.z() + 14)), new Color(0, 0, 1, 1));
 			}
 		}
 		
@@ -313,12 +72,13 @@ public class DebugDisplay {
 			//Matrix4f transformation = Matrix4f.createMatrixTranslate(new Vector3f(testRpos.x,testRpos.y,testRpos.z)).multiply(testQTransfert.getMatrix4()).multiply(Matrix4f.createMatrixTranslate(new Vector3f(0,0,14)));
 			//Matrix4f transformation = testQTransfert.getMatrix4().multiply(Matrix4f.createMatrixTranslate(new Vector3f(testRpos.x,testRpos.y,testRpos.z))).multiply(Matrix4f.createMatrixTranslate(new Vector3f(0,0,14)));
 			//Matrix4f transformation = testQTransfert.getMatrix4().multiply(Matrix4f.createMatrixTranslate(new Vector3f(testRpos.x,testRpos.y,testRpos.z))).multiply(Matrix4f.createMatrixTranslate(new Vector3f(0,0,14)));
-			Matrix4f trensformation = Matrix4f.createMatrixTranslate(new Vector3f(testRpos.x(), testRpos.y(),testRpos.z())).multiply(Matrix4f.createMatrixTranslate(new Vector3f(0,0,14))).multiply(testQTransfert.getMatrix4());
+			Matrix4f trensformation = Matrix4f.createMatrixTranslate(new Vector3f(testRpos.x(), testRpos.y(), testRpos.z())).multiply(Matrix4f.createMatrixTranslate(new Vector3f(0, 0, 14)))
+					.multiply(testQTransfert.getMatrix4());
 			// OK sans la box1 orientation ...
 			//Matrix4f transformation = Matrix4f.createMatrixTranslate(new Vector3f(testRpos.x,testRpos.y,testRpos.z)).multiply(testQTransfert.getMatrix4()).multiply(Matrix4f.createMatrixTranslate(new Vector3f(0,0,14)));
 			//Matrix4f transformation = Matrix4f.createMatrixTranslate(new Vector3f(testRpos.x,testRpos.y,testRpos.z)).multiply(testQTransfert.getMatrix4()).multiply(Matrix4f.createMatrixTranslate(new Vector3f(0,0,14)));
-			debugDrawProperty.drawSquare(box2HalfSize, trensformation, new Color(0,1,0,0.5f));
+			debugDrawProperty.drawSquare(box2HalfSize, trensformation, new Color(0, 1, 0, 0.5f));
-			debugDrawProperty.drawSquare(box1HalfSize, Matrix4f.createMatrixTranslate(new Vector3f(0,0,14)), new Color(0,0,1,0.5f));
+			debugDrawProperty.drawSquare(box1HalfSize, Matrix4f.createMatrixTranslate(new Vector3f(0, 0, 14)), new Color(0, 0, 1, 0.5f));
 		}
 		
 		// Restore context of matrix

src/org/atriasoft/phyligram/PhysicBox.java

@@ -9,53 +9,52 @@ import org.atriasoft.etk.math.Transform3D;
 import org.atriasoft.etk.math.Vector3f;
 import org.atriasoft.gale.resource.ResourceColored3DObject;
 import org.atriasoft.phyligram.internal.Log;
+import org.atriasoft.phyligram.shape.AABB;
 
 public class PhysicBox extends PhysicShape {
 	// Box size property in X, Y and Z
 	private Vector3f size = new Vector3f(1, 1, 1);
 	
-	public PhysicBox() {
-		super(PhysicShapeType.BOX);
-	}
-
-	public Vector3f getSize() {
-		return size;
-	}
-
-	public void setSize(Vector3f size) {
-		this.size = size;
-	}
-
-	@Override
-	public void updateAABB(Transform3D transformGlobal, PhysicCollisionAABB aabb) {
-		// store it, many time usefull...
-		this.transformGlobal = transformGlobal;
-		this.colisionPoints.clear();
-		// TODO Auto-generated method stub
-		aabb.update(transformGlobal.multiply(this.transform.multiply(new Vector3f(this.size.x() * 0.5f, this.size.y() * 0.5f, this.size.z() * 0.5f))));
-		aabb.update(transformGlobal.multiply(this.transform.multiply(new Vector3f(-this.size.x() * 0.5f, this.size.y() * 0.5f, this.size.z() * 0.5f))));
-		aabb.update(transformGlobal.multiply(this.transform.multiply(new Vector3f(-this.size.x() * 0.5f, -this.size.y() * 0.5f, this.size.z() * 0.5f))));
-		aabb.update(transformGlobal.multiply(this.transform.multiply(new Vector3f(this.size.x() * 0.5f, -this.size.y() * 0.5f, this.size.z() * 0.5f))));
-		aabb.update(transformGlobal.multiply(this.transform.multiply(new Vector3f(this.size.x() * 0.5f, this.size.y() * 0.5f, -this.size.z() * 0.5f))));
-		aabb.update(transformGlobal.multiply(this.transform.multiply(new Vector3f(-this.size.x() * 0.5f, this.size.y() * 0.5f, -this.size.z() * 0.5f))));
-		aabb.update(transformGlobal.multiply(this.transform.multiply(new Vector3f(-this.size.x() * 0.5f, -this.size.y() * 0.5f, -this.size.z() * 0.5f))));
-		aabb.update(transformGlobal.multiply(this.transform.multiply(new Vector3f(this.size.x() * 0.5f, -this.size.y() * 0.5f, -this.size.z() * 0.5f))));
-	}
-
 	// only needed for the narrow phase calculation ...
 	public Vector3f narrowPhaseGlobalPos;
+	
 	public Vector3f narrowPhaseAxisX = new Vector3f(1, 0, 0);
+	
 	public Vector3f narrowPhaseAxisY = new Vector3f(1, 0, 0);
+	
 	public Vector3f narrowPhaseAxisZ = new Vector3f(1, 0, 0);
+	
 	public Vector3f narrowPhaseHalfSize;
 	
+	public PhysicBox() {}
+	
+	public Vector3f getSize() {
+		return this.size;
+	}
+	
 	@Override
-	public void updateForNarrowCollision(Transform3D transformGlobal) {
+	public void renderDebug(Transform3D transformGlobal, ResourceColored3DObject debugDrawProperty) {
-		this.narrowPhaseGlobalPos = transformGlobal.multiply(this.transform.multiply(new Vector3f(0, 0, 0)));
+		debugDrawProperty.drawSquare(this.size.multiply(0.5f), this.transform.getOpenGLMatrix().multiply(transformGlobal.getOpenGLMatrix()), new Color(0, 1, 0, 0.25f));
-		this.narrowPhaseAxisX = transformGlobal.multiply(this.transform.multiply(new Vector3f(1, 0, 0))).less(this.narrowPhaseGlobalPos);
+		Vector3f dimention = this.size.multiply(0.5f);
-		this.narrowPhaseAxisY = transformGlobal.multiply(this.transform.multiply(new Vector3f(0, 1, 0))).less(this.narrowPhaseGlobalPos);
+		renderPoint2(new Vector3f(+dimention.x(), +dimention.y(), +dimention.z()), transformGlobal, debugDrawProperty);
-		this.narrowPhaseAxisZ = transformGlobal.multiply(this.transform.multiply(new Vector3f(0, 0, 1))).less(this.narrowPhaseGlobalPos);
+		renderPoint(new Vector3f(-dimention.x(), +dimention.y(), +dimention.z()), transformGlobal, debugDrawProperty);
-		this.narrowPhaseHalfSize = this.size.multiply(0.5f);
+		renderPoint(new Vector3f(+dimention.x(), -dimention.y(), +dimention.z()), transformGlobal, debugDrawProperty);
+		renderPoint(new Vector3f(-dimention.x(), -dimention.y(), +dimention.z()), transformGlobal, debugDrawProperty);
+		renderPoint(new Vector3f(+dimention.x(), +dimention.y(), -dimention.z()), transformGlobal, debugDrawProperty);
+		renderPoint(new Vector3f(-dimention.x(), +dimention.y(), -dimention.z()), transformGlobal, debugDrawProperty);
+		renderPoint(new Vector3f(+dimention.x(), -dimention.y(), -dimention.z()), transformGlobal, debugDrawProperty);
+		renderPoint3(new Vector3f(-dimention.x(), -dimention.y(), -dimention.z()), transformGlobal, debugDrawProperty);
+		for (Collision elem : this.colisionPoints) {
+			if (elem != null) {
+				if (elem.colisionPointLocal == null) {
+					Log.error("colision point must be set !!!");
+					continue;
+				}
+				for (int iii = 0; iii < elem.colisionPointLocal.length; iii++) {
+					renderPoint4(elem.colisionPointLocal[iii].position, elem.colisionPointLocal[iii].force, debugDrawProperty);
+				}
+			}
+		}
 	}
 	
 	private void renderPoint(Vector3f subPosition, Transform3D transformGlobal, ResourceColored3DObject debugDrawProperty) {
@@ -79,34 +78,38 @@ public class PhysicBox extends PhysicShape {
 	private void renderPoint4(Vector3f subPosition, Vector3f force, ResourceColored3DObject debugDrawProperty) {
 		debugDrawProperty.drawSquare(new Vector3f(0.1f, 0.1f, 0.1f), Matrix4f.createMatrixTranslate(subPosition), new Color(1, 0, 0, 1));
 		List<Vector3f> tmp = new ArrayList<>();
-		tmp.add(new Vector3f(0,0,0));
+		tmp.add(new Vector3f(0, 0, 0));
 		tmp.add(force);
 		debugDrawProperty.drawLine(tmp, new Color(1, 0, 0, 1), Matrix4f.createMatrixTranslate(subPosition), true, false);
 	}
 	
+	public void setSize(Vector3f size) {
+		this.size = size;
+	}
+	
 	@Override
-	public void renderDebug(Transform3D transformGlobal, ResourceColored3DObject debugDrawProperty) {
+	public void updateAABB(Transform3D transformGlobal, AABB aabb) {
-		debugDrawProperty.drawSquare(this.size.multiply(0.5f), this.transform.getOpenGLMatrix().multiply(transformGlobal.getOpenGLMatrix()), new Color(0, 1, 0, 0.25f));
+		// store it, many time usefull...
-		Vector3f dimention = this.size.multiply(0.5f);
+		this.transformGlobal = transformGlobal;
-		renderPoint2(new Vector3f(+dimention.x(), +dimention.y(), +dimention.z()), transformGlobal, debugDrawProperty);
+		this.colisionPoints.clear();
-		renderPoint(new Vector3f(-dimention.x(), +dimention.y(), +dimention.z()), transformGlobal, debugDrawProperty);
+		// TODO Auto-generated method stub
-		renderPoint(new Vector3f(+dimention.x(), -dimention.y(), +dimention.z()), transformGlobal, debugDrawProperty);
+		aabb.update(transformGlobal.multiply(this.transform.multiply(new Vector3f(this.size.x() * 0.5f, this.size.y() * 0.5f, this.size.z() * 0.5f))));
-		renderPoint(new Vector3f(-dimention.x(), -dimention.y(), +dimention.z()), transformGlobal, debugDrawProperty);
+		aabb.update(transformGlobal.multiply(this.transform.multiply(new Vector3f(-this.size.x() * 0.5f, this.size.y() * 0.5f, this.size.z() * 0.5f))));
-		renderPoint(new Vector3f(+dimention.x(), +dimention.y(), -dimention.z()), transformGlobal, debugDrawProperty);
+		aabb.update(transformGlobal.multiply(this.transform.multiply(new Vector3f(-this.size.x() * 0.5f, -this.size.y() * 0.5f, this.size.z() * 0.5f))));
-		renderPoint(new Vector3f(-dimention.x(), +dimention.y(), -dimention.z()), transformGlobal, debugDrawProperty);
+		aabb.update(transformGlobal.multiply(this.transform.multiply(new Vector3f(this.size.x() * 0.5f, -this.size.y() * 0.5f, this.size.z() * 0.5f))));
-		renderPoint(new Vector3f(+dimention.x(), -dimention.y(), -dimention.z()), transformGlobal, debugDrawProperty);
+		aabb.update(transformGlobal.multiply(this.transform.multiply(new Vector3f(this.size.x() * 0.5f, this.size.y() * 0.5f, -this.size.z() * 0.5f))));
-		renderPoint3(new Vector3f(-dimention.x(), -dimention.y(), -dimention.z()), transformGlobal, debugDrawProperty);
+		aabb.update(transformGlobal.multiply(this.transform.multiply(new Vector3f(-this.size.x() * 0.5f, this.size.y() * 0.5f, -this.size.z() * 0.5f))));
-		for (Collision elem: this.colisionPoints) {
+		aabb.update(transformGlobal.multiply(this.transform.multiply(new Vector3f(-this.size.x() * 0.5f, -this.size.y() * 0.5f, -this.size.z() * 0.5f))));
-			if (elem != null) {
+		aabb.update(transformGlobal.multiply(this.transform.multiply(new Vector3f(this.size.x() * 0.5f, -this.size.y() * 0.5f, -this.size.z() * 0.5f))));
-				if (elem.colisionPointLocal == null) {
+	}
-					Log.error("colision point must be set !!!");
+	
-					continue;
+	@Override
-				}
+	public void updateForNarrowCollision(Transform3D transformGlobal) {
-				for (int iii = 0; iii < elem.colisionPointLocal.length; iii++) {
+		this.narrowPhaseGlobalPos = transformGlobal.multiply(this.transform.multiply(new Vector3f(0, 0, 0)));
-					renderPoint4(elem.colisionPointLocal[iii].position, elem.colisionPointLocal[iii].force, debugDrawProperty);
+		this.narrowPhaseAxisX = transformGlobal.multiply(this.transform.multiply(new Vector3f(1, 0, 0))).less(this.narrowPhaseGlobalPos);
-				}
+		this.narrowPhaseAxisY = transformGlobal.multiply(this.transform.multiply(new Vector3f(0, 1, 0))).less(this.narrowPhaseGlobalPos);
-			}
+		this.narrowPhaseAxisZ = transformGlobal.multiply(this.transform.multiply(new Vector3f(0, 0, 1))).less(this.narrowPhaseGlobalPos);
-		}
+		this.narrowPhaseHalfSize = this.size.multiply(0.5f);
 	}
 	
 }

src/org/atriasoft/phyligram/PhysicCollisionAABB.java

@@ -1,77 +0,0 @@
-package org.atriasoft.phyligram;
-
-import org.atriasoft.etk.math.Vector3f;
-
-public class PhysicCollisionAABB {
-	public float minX;
-	public float minY;
-	public float minZ;
-	public float maxX;
-	public float maxY;
-	public float maxZ;
-	public PhysicCollisionAABB(float minX, float minY, float minZ, float maxX, float maxY, float maxZ) {
-		super();
-		this.minX = minX;
-		this.minY = minY;
-		this.minZ = minZ;
-		this.maxX = maxX;
-		this.maxY = maxY;
-		this.maxZ = maxZ;
-	}
-	public boolean intersect(PhysicCollisionAABB other) {
-		if (this == other) {
-			return false;
-		}
-		if (minX > other.maxX) {
-			return false;
-		}
-		if (maxX < other.minX) {
-			return false;
-		}
-		if (minY > other.maxY) {
-			return false;
-		}
-		if (maxY < other.minY) {
-			return false;
-		}
-		if (minZ > other.maxZ) {
-			return false;
-		}
-		if (maxZ < other.minZ) {
-			return false;
-		}
-		return true;
-	}
-	public void update(Vector3f point) {
-		if (minX > point.x()) {
-			minX = point.x();
-		}
-		if (maxX < point.x()) {
-			maxX = point.x();
-		}
-		if (minY > point.y()) {
-			minY = point.y();
-		}
-		if (maxY < point.y()) {
-			maxY = point.y();
-		}
-		if (minZ > point.z()) {
-			minZ = point.z();
-		}
-		if (maxZ < point.z()) {
-			maxZ = point.z();
-		}
-	}
-	public Vector3f getMin() {
-		return new Vector3f(minX, minY, minZ);
-	}
-	public Vector3f getMax() {
-		return new Vector3f(maxX, maxY, maxZ);
-	}
-	public static PhysicCollisionAABB beforeCalculated() {
-		// TODO Auto-generated method stub
-		return new PhysicCollisionAABB(999999,999999,999999,-999999,-999999,-999999);
-	}
-}
-
-

src/org/atriasoft/phyligram/PhysicHeightMapChunk.java

@@ -0,0 +1,63 @@
+package org.atriasoft.phyligram;
+
+import org.atriasoft.ege.geometry.Triangle;
+import org.atriasoft.etk.math.Transform3D;
+import org.atriasoft.etk.math.Vector3f;
+import org.atriasoft.gale.resource.ResourceColored3DObject;
+import org.atriasoft.phyligram.shape.AABB;
+
+public class PhysicHeightMapChunk extends PhysicShape {
+	// Box size property in X, Y and Z
+	private Triangle[] triangles = null;
+	public Triangle[] trianglesGlobalPos = null;
+	
+	public PhysicHeightMapChunk() {
+		
+	}
+	
+	public PhysicHeightMapChunk(Triangle[] triangles) {
+		this.triangles = triangles;
+	}
+	
+	public Triangle[] getTriangles() {
+		return this.triangles;
+	}
+	
+	@Override
+	public void renderDebug(Transform3D transformGlobal, ResourceColored3DObject debugDrawProperty) {
+		
+	}
+	
+	public void setTriangles(Triangle[] triangles) {
+		this.triangles = triangles;
+		this.trianglesGlobalPos = new Triangle[triangles.length];
+		for (int iii = 0; iii < this.triangles.length; iii++) {
+			Vector3f data1 = this.transformGlobal.multiply(this.transform.multiply(this.triangles[iii].p1));
+			Vector3f data2 = this.transformGlobal.multiply(this.transform.multiply(this.triangles[iii].p2));
+			Vector3f data3 = this.transformGlobal.multiply(this.transform.multiply(this.triangles[iii].p3));
+			this.trianglesGlobalPos[iii] = new Triangle(data1, data2, data3);
+		}
+	}
+	
+	@Override
+	public void updateAABB(Transform3D transformGlobal, AABB aabb) {
+		// store it, many time usefull...
+		this.transformGlobal = transformGlobal;
+		this.colisionPoints.clear();
+		for (int iii = 0; iii < this.triangles.length; iii++) {
+			aabb.update(transformGlobal.multiply(this.transform.multiply(this.triangles[iii].p1)));
+			aabb.update(transformGlobal.multiply(this.transform.multiply(this.triangles[iii].p2)));
+			aabb.update(transformGlobal.multiply(this.transform.multiply(this.triangles[iii].p3)));
+		}
+	}
+	
+	@Override
+	public void updateForNarrowCollision(Transform3D transformGlobal) {
+		for (int iii = 0; iii < this.triangles.length; iii++) {
+			this.trianglesGlobalPos[iii].p1 = this.transformGlobal.multiply(this.transform.multiply(this.triangles[iii].p1));
+			this.trianglesGlobalPos[iii].p2 = this.transformGlobal.multiply(this.transform.multiply(this.triangles[iii].p2));
+			this.trianglesGlobalPos[iii].p3 = this.transformGlobal.multiply(this.transform.multiply(this.triangles[iii].p3));
+		}
+	}
+	
+}

src/org/atriasoft/phyligram/PhysicMapVoxel.java

@@ -1,42 +1,43 @@
 package org.atriasoft.phyligram;
 
+import org.atriasoft.ege.map.VoxelChunk;
 import org.atriasoft.etk.math.Transform3D;
 import org.atriasoft.etk.math.Vector3f;
 import org.atriasoft.gale.resource.ResourceColored3DObject;
-import org.atriasoft.ege.map.VoxelChunk;
+import org.atriasoft.phyligram.shape.AABB;
 
 public class PhysicMapVoxel extends PhysicShape {
 	// Box size property in X, Y and Z
 	private VoxelChunk chunk;
+	// only needed for the narrow phase calculation ...
+	private Vector3f narrowPhaseGlobalPos;
+	private Vector3f narrowPhaseAxisX = new Vector3f(1, 0, 0);
+	private Vector3f narrowPhaseAxisY = new Vector3f(1, 0, 0);
+	private Vector3f narrowPhaseAxisZ = new Vector3f(1, 0, 0);
+	private Vector3f narrowPhaseHalfSize = new Vector3f(0.5f, 0.5f, 0.5f);
+	
 	public PhysicMapVoxel(VoxelChunk chunk) {
-		super(PhysicShapeType.MAP_VOXEL);
 		this.chunk = chunk;
 	}
+	
 	@Override
-	public void updateAABB(Transform3D transform, PhysicCollisionAABB aabb) {
+	public void renderDebug(Transform3D transform, ResourceColored3DObject debugDrawProperty) {
+		
+	}
+	
+	@Override
+	public void updateAABB(Transform3D transform, AABB aabb) {
 		if (this.chunk == null) {
 			return;
 		}
 		this.colisionPoints.clear();
-		aabb.update(new Vector3f(this.chunk.getPosition().x(),this.chunk.getPosition().y(),this.chunk.getPosition().z()));
+		aabb.update(new Vector3f(this.chunk.getPosition().x(), this.chunk.getPosition().y(), this.chunk.getPosition().z()));
-		aabb.update(new Vector3f(
+		aabb.update(new Vector3f(this.chunk.getPosition().x() + VoxelChunk.VOXEL_CHUNK_SIZE, this.chunk.getPosition().y() + VoxelChunk.VOXEL_CHUNK_SIZE,
-				this.chunk.getPosition().x() + VoxelChunk.VOXEL_CHUNK_SIZE,
-				this.chunk.getPosition().y() + VoxelChunk.VOXEL_CHUNK_SIZE,
 				this.chunk.getPosition().z() + VoxelChunk.VOXEL_CHUNK_SIZE));
 	}
-	// only needed for the narrow phase calculation ...
+	
-	private Vector3f narrowPhaseGlobalPos;
-	private Vector3f narrowPhaseAxisX = new Vector3f(1,0,0);
-	private Vector3f narrowPhaseAxisY = new Vector3f(1,0,0);
-	private Vector3f narrowPhaseAxisZ = new Vector3f(1,0,0);
-	private Vector3f narrowPhaseHalfSize = new Vector3f(0.5f,0.5f,0.5f);
 	@Override
 	public void updateForNarrowCollision(Transform3D transform) {
-		this.narrowPhaseGlobalPos = transform.multiply(this.transform.multiply(new Vector3f(0,0,0)));
+		this.narrowPhaseGlobalPos = transform.multiply(this.transform.multiply(new Vector3f(0, 0, 0)));
-	}
-	@Override
-	public void renderDebug(Transform3D transform, ResourceColored3DObject debugDrawProperty) {
-		
-		
 	}
 }

src/org/atriasoft/phyligram/PhysicShape.java

@@ -7,95 +7,83 @@ import org.atriasoft.etk.math.Quaternion;
 import org.atriasoft.etk.math.Transform3D;
 import org.atriasoft.etk.math.Vector3f;
 import org.atriasoft.gale.resource.ResourceColored3DObject;
-
+import org.atriasoft.phyligram.shape.AABB;
-
-
 
 public abstract class PhysicShape {
 	
-	
-	
 	protected List<Collision> colisionPoints = new ArrayList<>();
 	// protected Quaternion quaternion;
 	// protected Vector3f origin;
 	protected Transform3D transform;
 	protected Transform3D transformGlobal;
 	protected float mass = 0;
-	protected final PhysicShapeType type;
 	
-	public PhysicShape(PhysicShapeType type) {
+	public PhysicShape() {
-		this.type = type;
 		this.transform = Transform3D.IDENTITY;
 		// this.quaternion = Quaternion.identity();
 		// this.origin = Vector3f.zero();
 		this.mass = 0;
 	}
 	
-	public PhysicShape(PhysicShapeType type, Quaternion quaternion, Vector3f origin, float mass) {
+	public PhysicShape(Quaternion quaternion, Vector3f origin, float mass) {
-		this.type = type;
 		this.transform = new Transform3D(origin, quaternion);
 		// this.quaternion = quaternion;
 		// this.origin = origin;
 		this.mass = mass;
 	}
 	
-	public Quaternion getQuaternionFull() {
+	public void addColision(Collision colision) {
-		return transformGlobal.getOrientation().multiply(transform.getOrientation());
+		this.colisionPoints.add(colision);
 	}
 	
-	public Quaternion getQuaternion() {
+	public float getMass() {
-		return transform.getOrientation();
+		return this.mass;
-	}
-
-	public void setQuaternion(Quaternion quaternion) {
-		this.transform = this.transform.withOrientation(quaternion);
 	}
 	
 	public Vector3f getOrigin() {
 		return this.transform.getPosition();
 	}
 	
+	public Quaternion getQuaternion() {
+		return this.transform.getOrientation();
+	}
+	
+	public Quaternion getQuaternionFull() {
+		return this.transformGlobal.getOrientation().multiply(this.transform.getOrientation());
+	}
+	
+	public Transform3D getTransform() {
+		return this.transform;
+	}
+	
+	public Transform3D getTransformGlobal() {
+		return this.transformGlobal;
+	}
+	
+	public abstract void renderDebug(Transform3D transform, ResourceColored3DObject debugDrawProperty);
+	
+	public void setMass(float mass) {
+		this.mass = mass;
+	}
+	
 	public void setOrigin(Vector3f origin) {
 		this.transform = this.transform.withPosition(origin);
 	}
 	
-	public Transform3D getTransform() {
+	public void setQuaternion(Quaternion quaternion) {
-		return transform;
+		this.transform = this.transform.withOrientation(quaternion);
 	}
 	
 	public void setTransform(Transform3D transform) {
 		this.transform = transform;
 	}
 	
-	public Transform3D getTransformGlobal() {
-		return transformGlobal;
-	}
-
 	public void setTransformGlobal(Transform3D transform) {
 		this.transformGlobal = transform;
 	}
 	
-	public float getMass() {
+	public abstract void updateAABB(Transform3D transform, AABB aabb);
-		return mass;
-	}
-
-	public void setMass(float mass) {
-		this.mass = mass;
-	}
-
-	public PhysicShapeType getType() {
-		return type;
-	}
-
-	public void addColision(Collision colision) {
-		colisionPoints.add(colision);
-	}
-	
-	
-	public abstract void updateAABB(Transform3D transform, PhysicCollisionAABB aabb);
 	
 	public abstract void updateForNarrowCollision(Transform3D transform);
 	
-	public abstract void renderDebug(Transform3D transform, ResourceColored3DObject debugDrawProperty);
-
 }

src/org/atriasoft/phyligram/PhysicShapeType.java

@@ -1,13 +0,0 @@
-package org.atriasoft.phyligram;
-
-public enum PhysicShapeType {
-	UNKNOWN,
-	BOX,
-	CAPSULE,
-	CONE,
-	CONVEXHULL,
-	CYLINDER,
-	SPHERE,
-	CONCAVE,
-	MAP_VOXEL
-}

src/org/atriasoft/phyligram/PhysicSphere.java

@@ -4,35 +4,46 @@ import org.atriasoft.etk.Color;
 import org.atriasoft.etk.math.Transform3D;
 import org.atriasoft.etk.math.Vector3f;
 import org.atriasoft.gale.resource.ResourceColored3DObject;
+import org.atriasoft.phyligram.shape.AABB;
 
 public class PhysicSphere extends PhysicShape {
 	// Box size property in X, Y and Z
 	private float size;
-	public PhysicSphere() {
+	
-		super(PhysicShapeType.SPHERE);
+	// only needed for the narrow phase calculation ...
-	}
+	public Vector3f narrowPhaseGlobalPos;
+	
+	public PhysicSphere() {}
+	
 	public float getSize() {
-		return size;
+		return this.size;
 	}
+	
+	@Override
+	public void renderDebug(Transform3D transform, ResourceColored3DObject debugDrawProperty) {
+		debugDrawProperty.drawSphere(this.size, 9, 9, this.transform.getOpenGLMatrix().multiply(transform.getOpenGLMatrix()), new Color(0, 1, 0, 1));
+		
+	}
+	
 	public void setSize(float size) {
 		this.size = size;
 	}
+	
 	@Override
-	public void updateAABB(Transform3D transform, PhysicCollisionAABB aabb) {
+	public void updateAABB(Transform3D transformGlobal, AABB aabb) {
-		aabb.update(transform.multiply(this.transform.getPosition()).add(this.size,0,0));
+		// store it, many time usefull...
-		aabb.update(transform.multiply(this.transform.getPosition()).add(-this.size,0,0));
+		this.transformGlobal = transformGlobal;
-		aabb.update(transform.multiply(this.transform.getPosition()).add(0,this.size,0));
+		Vector3f basePosition = transformGlobal.multiply(this.transform.getPosition());
-		aabb.update(transform.multiply(this.transform.getPosition()).add(0,-this.size,0));
+		aabb.update(basePosition.add(this.size, 0, 0));
-		aabb.update(transform.multiply(this.transform.getPosition()).add(0,0,this.size));
+		aabb.update(basePosition.add(-this.size, 0, 0));
-		aabb.update(transform.multiply(this.transform.getPosition()).add(0,0,-this.size));
+		aabb.update(basePosition.add(0, this.size, 0));
+		aabb.update(basePosition.add(0, -this.size, 0));
+		aabb.update(basePosition.add(0, 0, this.size));
+		aabb.update(basePosition.add(0, 0, -this.size));
 	}
+	
 	@Override
 	public void updateForNarrowCollision(Transform3D transform) {
-		
+		this.narrowPhaseGlobalPos = this.transformGlobal.multiply(this.transform.multiply(new Vector3f(0, 0, 0)));
-	}
-	@Override
-	public void renderDebug(Transform3D transform, ResourceColored3DObject debugDrawProperty) {
-		debugDrawProperty.drawSphere(this.size, 9, 9, this.transform.getOpenGLMatrix().multiply(transform.getOpenGLMatrix()), new Color(0,1,0,1));
-		
 	}
 }

src/org/atriasoft/phyligram/PhysicTriangle.java

@@ -0,0 +1,61 @@
+package org.atriasoft.phyligram;
+
+import org.atriasoft.ege.geometry.Triangle;
+import org.atriasoft.etk.Color;
+import org.atriasoft.etk.math.Transform3D;
+import org.atriasoft.etk.math.Vector3f;
+import org.atriasoft.gale.resource.ResourceColored3DObject;
+import org.atriasoft.phyligram.shape.AABB;
+
+public class PhysicTriangle extends PhysicShape {
+	// Box size property in X, Y and Z
+	private Triangle triangle;
+	
+	// only needed for the narrow phase calculation ...
+	public Triangle narrowPhaseGlobalTriangle;
+	
+	public PhysicTriangle() {}
+	
+	public Triangle getTriangle() {
+		return this.triangle;
+	}
+	
+	public Triangle getTriangleGlobalPos() {
+		return this.narrowPhaseGlobalTriangle;
+	}
+	
+	@Override
+	public void renderDebug(Transform3D transform, ResourceColored3DObject debugDrawProperty) {
+		debugDrawProperty.drawTriangle(this.triangle.p1, this.triangle.p2, this.triangle.p3, this.transform.getOpenGLMatrix().multiply(transform.getOpenGLMatrix()), new Color(0, 1, 0, 1));
+		
+	}
+	
+	public void setPoints(Vector3f p1, Vector3f p2, Vector3f p3) {
+		this.triangle = new Triangle(p1, p2, p3);
+	}
+	
+	public void setTriangle(Triangle data) {
+		this.triangle = data.clone();
+	}
+	
+	@Override
+	public void updateAABB(Transform3D transformGlobal, AABB aabb) {
+		// store it, many time usefull...
+		this.transformGlobal = transformGlobal;
+		Vector3f basePositionP1 = transformGlobal.multiply(this.transform.getPosition().add(this.triangle.p1));
+		Vector3f basePositionP2 = transformGlobal.multiply(this.transform.getPosition().add(this.triangle.p2));
+		Vector3f basePositionP3 = transformGlobal.multiply(this.transform.getPosition().add(this.triangle.p3));
+		this.narrowPhaseGlobalTriangle = new Triangle(basePositionP1, basePositionP2, basePositionP3);
+		aabb.update(basePositionP1);
+		aabb.update(basePositionP2);
+		aabb.update(basePositionP3);
+	}
+	
+	@Override
+	public void updateForNarrowCollision(Transform3D transform) {
+		Vector3f basePositionP1 = this.transformGlobal.multiply(this.transform.multiply(this.triangle.p1));
+		Vector3f basePositionP2 = this.transformGlobal.multiply(this.transform.multiply(this.triangle.p2));
+		Vector3f basePositionP3 = this.transformGlobal.multiply(this.transform.multiply(this.triangle.p3));
+		this.narrowPhaseGlobalTriangle = new Triangle(basePositionP1, basePositionP2, basePositionP3);
+	}
+}

src/org/atriasoft/phyligram/ToolCollisionOBBWithOBB.java

@@ -141,7 +141,7 @@ public class ToolCollisionOBBWithOBB {
 		// getCollidePointsAABBCenteredWithOBB(box2.narrowPhaseHalfSize, box1.narrowPhaseHalfSize, quatTransfer2, rPos1);
 		// fonctionne quand le block est trourner de 90% petit pb de positionnement en hauteur....
 		// getCollidePointsAABBCenteredWithOBB(box2.narrowPhaseHalfSize, box1.narrowPhaseHalfSize, quatTransfer2, quat2.multiply(rPos2));
-		ColisionPoints[] collide1 = getCollidePointsAABBCenteredWithOBB(box2.narrowPhaseHalfSize, box1.narrowPhaseHalfSize, quatTransfer2, quat2.inverse().getMatrix4().multiply(rPos1));
+		ColisionPoint[] collide1 = getCollidePointsAABBCenteredWithOBB(box2.narrowPhaseHalfSize, box1.narrowPhaseHalfSize, quatTransfer2, quat2.inverse().getMatrix4().multiply(rPos1));
 		// transfer detection point collision in global environement:
 		if (collide1 != null) {
 			for (int iii = 0; iii < collide1.length; iii++) {
@@ -155,7 +155,7 @@ public class ToolCollisionOBBWithOBB {
 		DebugDisplay.testQTransfert = quatTransfer1;
 		DebugDisplay.box1HalfSize = box1.narrowPhaseHalfSize;
 		DebugDisplay.box2HalfSize = box2.narrowPhaseHalfSize;
-		ColisionPoints[] collide2 = getCollidePointsAABBCenteredWithOBB(box1.narrowPhaseHalfSize, box2.narrowPhaseHalfSize, quatTransfer1, quat1.inverse().getMatrix4().multiply(rPos2));
+		ColisionPoint[] collide2 = getCollidePointsAABBCenteredWithOBB(box1.narrowPhaseHalfSize, box2.narrowPhaseHalfSize, quatTransfer1, quat1.inverse().getMatrix4().multiply(rPos2));
 		if (collide2 != null) {
 			for (int iii = 0; iii < collide2.length; iii++) {
 				collide2[iii].position = quat2.multiply(collide2[iii].position).add(box2.narrowPhaseGlobalPos);
@@ -174,7 +174,7 @@ public class ToolCollisionOBBWithOBB {
 
 	}
 
-	public static ColisionPoints[] getCollidePointsAABBCenteredWithOBB(Vector3f box1HalfSize, Vector3f box2HalfSize, Quaternion box2Orientation, Vector3f box2Position) {
+	public static ColisionPoint[] getCollidePointsAABBCenteredWithOBB(Vector3f box1HalfSize, Vector3f box2HalfSize, Quaternion box2Orientation, Vector3f box2Position) {
 
 		// point in AABB
 		Vector3f topBackRight = box2Orientation.multiply(new Vector3f(+box2HalfSize.x(), +box2HalfSize.y(), +box2HalfSize.z())).add(box2Position);
@@ -245,38 +245,38 @@ public class ToolCollisionOBBWithOBB {
 		if (insideBottomFrontLeft != null) {
 			count++;
 		}
-		ColisionPoints[] out = new ColisionPoints[count];
+		ColisionPoint[] out = new ColisionPoint[count];
 		count = 0;
 		if (insideTopBackRight != null) {
-			out[count] = new ColisionPoints(new Vector3f(+box2HalfSize.x(), +box2HalfSize.y(), +box2HalfSize.z()), insideTopBackRight);
+			out[count] = new ColisionPoint(new Vector3f(+box2HalfSize.x(), +box2HalfSize.y(), +box2HalfSize.z()), insideTopBackRight);
 			count++;
 		}
 		if (insideTopBackLeft != null) {
-			out[count] = new ColisionPoints(new Vector3f(-box2HalfSize.x(), +box2HalfSize.y(), +box2HalfSize.z()), insideTopBackLeft);
+			out[count] = new ColisionPoint(new Vector3f(-box2HalfSize.x(), +box2HalfSize.y(), +box2HalfSize.z()), insideTopBackLeft);
 			count++;
 		}
 		if (insideTopFrontRight != null) {
-			out[count] = new ColisionPoints(new Vector3f(+box2HalfSize.x(), -box2HalfSize.y(), +box2HalfSize.z()), insideTopFrontRight);
+			out[count] = new ColisionPoint(new Vector3f(+box2HalfSize.x(), -box2HalfSize.y(), +box2HalfSize.z()), insideTopFrontRight);
 			count++;
 		}
 		if (insideTopFrontLeft != null) {
-			out[count] = new ColisionPoints(new Vector3f(-box2HalfSize.x(), -box2HalfSize.y(), +box2HalfSize.z()), insideTopFrontLeft);
+			out[count] = new ColisionPoint(new Vector3f(-box2HalfSize.x(), -box2HalfSize.y(), +box2HalfSize.z()), insideTopFrontLeft);
 			count++;
 		}
 		if (insideBottomBackRight != null) {
-			out[count] = new ColisionPoints(new Vector3f(+box2HalfSize.x(), +box2HalfSize.y(), -box2HalfSize.z()), insideBottomBackRight);
+			out[count] = new ColisionPoint(new Vector3f(+box2HalfSize.x(), +box2HalfSize.y(), -box2HalfSize.z()), insideBottomBackRight);
 			count++;
 		}
 		if (insideBottomBackLeft != null) {
-			out[count] = new ColisionPoints(new Vector3f(-box2HalfSize.x(), +box2HalfSize.y(), -box2HalfSize.z()), insideBottomBackLeft);
+			out[count] = new ColisionPoint(new Vector3f(-box2HalfSize.x(), +box2HalfSize.y(), -box2HalfSize.z()), insideBottomBackLeft);
 			count++;
 		}
 		if (insideBottomFrontRight != null) {
-			out[count] = new ColisionPoints(new Vector3f(+box2HalfSize.x(), -box2HalfSize.y(), -box2HalfSize.z()), insideBottomFrontRight);
+			out[count] = new ColisionPoint(new Vector3f(+box2HalfSize.x(), -box2HalfSize.y(), -box2HalfSize.z()), insideBottomFrontRight);
 			count++;
 		}
 		if (insideBottomFrontLeft != null) {
-			out[count] = new ColisionPoints(new Vector3f(-box2HalfSize.x(), -box2HalfSize.y(), -box2HalfSize.z()), insideBottomFrontLeft);
+			out[count] = new ColisionPoint(new Vector3f(-box2HalfSize.x(), -box2HalfSize.y(), -box2HalfSize.z()), insideBottomFrontLeft);
 			count++;
 		}
 		if (count != 0) {

src/org/atriasoft/phyligram/ToolCollisionSphereWithHeightMapChunk.java

@@ -0,0 +1,63 @@
+package org.atriasoft.phyligram;
+
+import org.atriasoft.ege.geometry.Triangle;
+import org.atriasoft.etk.math.Vector3f;
+
+public class ToolCollisionSphereWithHeightMapChunk {
+	//intersection entre 2 droite (en 2d avec estimation en 3D
+	public static Vector3f lineIntersect(Vector3f p1, Vector3f p2, Vector3f p3, Vector3f p4) {
+		float denom = (p4.y() - p3.y()) * (p2.x() - p1.x()) - (p4.x() - p3.x()) * (p2.y() - p1.y());
+		if (denom == 0.0f) { // Lines are parallel.
+			return null;
+		}
+		float ua = ((p4.x() - p3.x()) * (p1.y() - p3.y()) - (p4.y() - p3.y()) * (p1.x() - p3.x())) / denom;
+		float ub = ((p2.x() - p1.x()) * (p1.y() - p3.y()) - (p2.y() - p1.y()) * (p1.x() - p3.x())) / denom;
+		if (ua >= 0.0f && ua <= 1.0f && ub >= 0.0f && ub <= 1.0f) {
+			// Get the intersection point.
+			return new Vector3f(p1.x() + ua * (p2.x() - p1.x()), p1.y() + ua * (p2.y() - p1.y()), p1.z() + ua * (p2.z() - p1.z()));
+		}
+		return null;
+	}
+	
+	public static Vector3f middlePoint(Vector3f p1, Vector3f p2, Vector3f p3) {
+		float distance1 = p1.distance(p2);
+		float distance2 = p1.distance(p3);
+		float ratio = distance1 / distance2;
+		return p3.add(p3.less(p1).multiply(ratio));
+	}
+	
+	private static boolean pointInTriangle(Vector3f pt, Triangle triangle) {
+		float d1, d2, d3;
+		boolean has_neg, has_pos;
+		
+		d1 = sign(pt, triangle.p1, triangle.p2);
+		d2 = sign(pt, triangle.p2, triangle.p3);
+		d3 = sign(pt, triangle.p3, triangle.p1);
+		
+		has_neg = (d1 < 0) || (d2 < 0) || (d3 < 0);
+		has_pos = (d1 > 0) || (d2 > 0) || (d3 > 0);
+		
+		return !(has_neg && has_pos);
+	}
+	
+	private static float sign(Vector3f p1, Vector3f p2, Vector3f p3) {
+		return (p1.x() - p3.x()) * (p2.y() - p3.y()) - (p2.x() - p3.x()) * (p1.y() - p3.y());
+	}
+	
+	public static boolean testCollide(PhysicSphere sphere1, PhysicHeightMapChunk map) {
+		for (Triangle elem : map.trianglesGlobalPos) {
+			if (pointInTriangle(sphere1.narrowPhaseGlobalPos, elem)) {
+				// calculate linear extrapolation of the z height at this position:
+				Vector3f position = lineIntersect(elem.p1, sphere1.narrowPhaseGlobalPos, elem.p2, elem.p3);
+				Vector3f middlePoint = middlePoint(elem.p1, sphere1.narrowPhaseGlobalPos, position);
+				float distanceSquare = middlePoint.distance2(sphere1.narrowPhaseGlobalPos);
+				if (sphere1.getSize() * sphere1.getSize() < distanceSquare) {
+					return true;
+				}
+			}
+		}
+		return false;
+	}
+	
+	private ToolCollisionSphereWithHeightMapChunk() {}
+}

src/org/atriasoft/phyligram/ToolCollisionSphereWithSphere.java

@@ -0,0 +1,30 @@
+package org.atriasoft.phyligram;
+
+import org.atriasoft.etk.math.Vector3f;
+import org.atriasoft.phyligram.internal.Log;
+
+public class ToolCollisionSphereWithSphere {
+	// Note sphere 2 is the reference ...
+	public static ColisionPoint getCollisionPoint(PhysicSphere sphere1, PhysicSphere shapeReference) {
+		if (sphere1.getSize() > shapeReference.getSize()) {
+			Log.todo("implement then reference is smaller than moving");
+		}
+		Vector3f force = sphere1.narrowPhaseGlobalPos.less(shapeReference.narrowPhaseGlobalPos);
+		float distance = shapeReference.getSize() + sphere1.getSize() - force.length();
+		force = force.safeNormalize();
+		Vector3f impact = force.multiply(shapeReference.getSize());
+		force = force.multiply(distance);
+		force = force.multiply(sphere1.getSize() + distance);
+		return new ColisionPoint(impact, force);
+	}
+	
+	// Note sphere 2 is the reference ...
+	public static boolean testCollide(PhysicSphere sphere1, PhysicSphere shapeReference) {
+		float distance1 = sphere1.narrowPhaseGlobalPos.distance2(shapeReference.narrowPhaseGlobalPos);
+		float distance2 = sphere1.getSize() * shapeReference.getSize();
+		distance2 = distance2 * distance2;
+		return distance1 <= distance2;
+	}
+	
+	private ToolCollisionSphereWithSphere() {}
+}

src/org/atriasoft/phyligram/ToolCollisionSphereWithTriangle.java

@@ -0,0 +1,155 @@
+package org.atriasoft.phyligram;
+
+import org.atriasoft.ege.geometry.Plane;
+import org.atriasoft.etk.math.FMath;
+import org.atriasoft.etk.math.Vector3f;
+
+// https://realtimecollisiondetection.net/blog/?p=103
+public class ToolCollisionSphereWithTriangle {
+	public static float clamp(float val, float min, float max) {
+		return Math.max(min, Math.min(max, val));
+	}
+	
+	public static Vector3f getClosestPointOnFiniteLine(Vector3f point, Vector3f lineStart, Vector3f lineEnd) {
+		Vector3f lineDirection = lineEnd.less(lineStart);
+		float lineLength = lineDirection.length();
+		lineDirection = lineDirection.normalize();
+		float position = point.less(lineStart).dot(lineDirection);
+		float ProjectionLength = clamp(position, 0, lineLength);
+		return lineStart.add(lineDirection.multiply(ProjectionLength));
+	}
+	
+	public static ColisionPoint getCollisionPoint(PhysicSphere sphere1, PhysicTriangle shapeReference) {
+		Plane plane = new Plane(shapeReference.getTriangleGlobalPos());
+		float distance = plane.distance(sphere1.narrowPhaseGlobalPos);
+		float distance1 = distance * distance;
+		float distance2 = sphere1.getSize() * sphere1.getSize();
+		if (distance1 > distance2) {
+			return null;
+		}
+		/*
+		if (distance < 0.0f) {
+			System.out.println("Distance : (BOTTOM) " + distance);
+		} else {
+			System.out.println("Distance :   (TOP)  " + distance);
+		}
+		*/
+		boolean inCenter = false;
+		// check the position with the perpendicular plan of each side of the triangle
+		Plane plane1 = new Plane(shapeReference.getTriangleGlobalPos().p1, shapeReference.getTriangleGlobalPos().p2, shapeReference.getTriangleGlobalPos().p1.add(plane.normal()));
+		float distanceBorder = plane1.distance(sphere1.narrowPhaseGlobalPos);
+		inCenter = distanceBorder < 0.0f;
+		if (inCenter == true) {
+			Plane plane2 = new Plane(shapeReference.getTriangleGlobalPos().p2, shapeReference.getTriangleGlobalPos().p3, shapeReference.getTriangleGlobalPos().p2.add(plane.normal()));
+			distanceBorder = plane2.distance(sphere1.narrowPhaseGlobalPos);
+			inCenter = distanceBorder < 0.0f;
+		}
+		if (inCenter == true) {
+			Plane plane3 = new Plane(shapeReference.getTriangleGlobalPos().p3, shapeReference.getTriangleGlobalPos().p1, shapeReference.getTriangleGlobalPos().p3.add(plane.normal()));
+			distanceBorder = plane3.distance(sphere1.narrowPhaseGlobalPos);
+			inCenter = distanceBorder < 0.0f;
+		}
+		if (inCenter == true) {
+			Vector3f impact = plane.minimalIntersectionPoint(sphere1.narrowPhaseGlobalPos);
+			Vector3f force = plane.normal();
+			if (distance < 0) {
+				force = force.multiply(-(sphere1.getSize() + distance));
+			} else {
+				force = force.multiply(-(sphere1.getSize() + distance));
+			}
+			return new ColisionPoint(impact, force);
+		}
+		
+		System.out.println("Not in center");
+		
+		// now we need to check if we have a collision with the border.
+		Vector3f nearestPointP1 = getClosestPointOnFiniteLine(sphere1.narrowPhaseGlobalPos, shapeReference.getTriangleGlobalPos().p1, shapeReference.getTriangleGlobalPos().p2);
+		float distanceP1Square = Vector3f.length2(nearestPointP1, sphere1.narrowPhaseGlobalPos);
+		System.out.println("distanceP1Square=" + distanceP1Square);
+		Vector3f nearestPointP2 = getClosestPointOnFiniteLine(sphere1.narrowPhaseGlobalPos, shapeReference.getTriangleGlobalPos().p2, shapeReference.getTriangleGlobalPos().p3);
+		float distanceP2Square = Vector3f.length2(nearestPointP2, sphere1.narrowPhaseGlobalPos);
+		System.out.println("distanceP2Square=" + distanceP2Square);
+		Vector3f nearestPointP3 = getClosestPointOnFiniteLine(sphere1.narrowPhaseGlobalPos, shapeReference.getTriangleGlobalPos().p3, shapeReference.getTriangleGlobalPos().p1);
+		float distanceP3Square = Vector3f.length2(nearestPointP3, sphere1.narrowPhaseGlobalPos);
+		System.out.println("distanceP3Square=" + distanceP3Square);
+		float distanceFinal;
+		Vector3f impact;
+		if (distanceP1Square < distanceP2Square && distanceP1Square < distanceP3Square) {
+			// P1 is the smallest
+			distanceFinal = FMath.sqrt(distanceP1Square);
+			impact = nearestPointP1;
+		} else if (distanceP2Square < distanceP3Square) {
+			// P2 is the smallest
+			distanceFinal = FMath.sqrt(distanceP2Square);
+			impact = nearestPointP2;
+		} else {
+			// P3 is the smallest
+			distanceFinal = FMath.sqrt(distanceP3Square);
+			impact = nearestPointP3;
+		}
+		
+		Vector3f force = sphere1.narrowPhaseGlobalPos.less(impact).safeNormalize();
+		force = force.multiply(sphere1.getSize() - distanceFinal);
+		return new ColisionPoint(impact, force);
+	}
+	
+	public static boolean testCollide(PhysicSphere sphere1, PhysicTriangle shapeReference) {
+		Plane plane = new Plane(shapeReference.getTriangleGlobalPos());
+		float distance = plane.distance(sphere1.narrowPhaseGlobalPos);
+		float distance1 = distance * distance;
+		float distance2 = sphere1.getSize() * sphere1.getSize();
+		if (distance1 > distance2) {
+			return false;
+		}
+		/*
+		if (distance < 0.0f) {
+			System.out.println("Distance : (BOTTOM) " + distance);
+		} else {
+			System.out.println("Distance :   (TOP)  " + distance);
+		}
+		*/
+		boolean inCenter = false;
+		// check the position with the perpendicular plan of each side of the triangle
+		Plane plane1 = new Plane(shapeReference.getTriangleGlobalPos().p1, shapeReference.getTriangleGlobalPos().p2, shapeReference.getTriangleGlobalPos().p1.add(plane.normal()));
+		float distanceBorder = plane1.distance(sphere1.narrowPhaseGlobalPos);
+		inCenter = distanceBorder < 0.0f;
+		if (inCenter == true) {
+			Plane plane2 = new Plane(shapeReference.getTriangleGlobalPos().p2, shapeReference.getTriangleGlobalPos().p3, shapeReference.getTriangleGlobalPos().p2.add(plane.normal()));
+			distanceBorder = plane2.distance(sphere1.narrowPhaseGlobalPos);
+			inCenter = distanceBorder < 0.0f;
+		}
+		if (inCenter == true) {
+			Plane plane3 = new Plane(shapeReference.getTriangleGlobalPos().p3, shapeReference.getTriangleGlobalPos().p1, shapeReference.getTriangleGlobalPos().p3.add(plane.normal()));
+			distanceBorder = plane3.distance(sphere1.narrowPhaseGlobalPos);
+			inCenter = distanceBorder < 0.0f;
+		}
+		if (inCenter == true) {
+			return true;
+		}
+		
+		System.out.println("Not in center");
+		
+		// now we need to check if we have a collision with the border.
+		Vector3f nearestPointP1 = getClosestPointOnFiniteLine(sphere1.narrowPhaseGlobalPos, shapeReference.getTriangleGlobalPos().p1, shapeReference.getTriangleGlobalPos().p2);
+		float distanceP1Square = Vector3f.length2(nearestPointP1, sphere1.narrowPhaseGlobalPos);
+		System.out.println("distanceP1Square=" + distanceP1Square);
+		if (distanceP1Square < distance2) {
+			return true;
+		}
+		Vector3f nearestPointP2 = getClosestPointOnFiniteLine(sphere1.narrowPhaseGlobalPos, shapeReference.getTriangleGlobalPos().p2, shapeReference.getTriangleGlobalPos().p3);
+		float distanceP2Square = Vector3f.length2(nearestPointP2, sphere1.narrowPhaseGlobalPos);
+		System.out.println("distanceP2Square=" + distanceP2Square);
+		if (distanceP2Square < distance2) {
+			return true;
+		}
+		Vector3f nearestPointP3 = getClosestPointOnFiniteLine(sphere1.narrowPhaseGlobalPos, shapeReference.getTriangleGlobalPos().p3, shapeReference.getTriangleGlobalPos().p1);
+		float distanceP3Square = Vector3f.length2(nearestPointP3, sphere1.narrowPhaseGlobalPos);
+		System.out.println("distanceP3Square=" + distanceP3Square);
+		if (distanceP3Square < distance2) {
+			return true;
+		}
+		return false;
+	}
+	
+	private ToolCollisionSphereWithTriangle() {}
+}

src/org/atriasoft/phyligram/math/Ray.java

@@ -0,0 +1,53 @@
+package org.atriasoft.phyligram.math;
+
+import org.atriasoft.etk.math.Vector3f;
+
+public class Ray {
+	public float maxFraction; //!< Maximum fraction value
+	public Vector3f point1; //!<First point of the ray (origin)
+	public Vector3f point2; //!< Second point of the ray
+	
+	public Ray(final Ray obj) {
+		this.point1 = obj.point1;
+		this.point2 = 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 int hashCode() {
+		// TODO Auto-generated method stub
+		return super.hashCode();
+	}
+	
+}

src/org/atriasoft/phyligram/shape/AABB.java

@@ -0,0 +1,228 @@
+package org.atriasoft.phyligram.shape;
+
+import org.atriasoft.etk.math.FMath;
+import org.atriasoft.etk.math.Vector3f;
+import org.atriasoft.phyligram.math.Ray;
+
+public class AABB {
+	/**
+	 * Create an inverted AABB cube (min* has maximum and max* has minimum)
+	 * @return A new AABB(max-float, min-float)
+	 */
+	public static AABB createInvertedEmpty() {
+		// TODO Auto-generated method stub
+		return new AABB(Float.MAX_VALUE, Float.MAX_VALUE, Float.MAX_VALUE, -Float.MAX_VALUE, -Float.MAX_VALUE, -Float.MAX_VALUE);
+	}
+	
+	public static AABB mergeAABB(AABB aaa, AABB bbb) {
+		return new AABB(Math.min(aaa.minX, bbb.minX), Math.min(aaa.minY, bbb.minY), Math.min(aaa.minZ, bbb.minZ), Math.max(aaa.maxX, bbb.maxX), Math.max(aaa.maxY, bbb.maxY),
+				Math.max(aaa.maxZ, bbb.maxZ));
+	}
+	
+	public float minX;
+	public float minY;
+	public float minZ;
+	public float maxX;
+	public float maxY;
+	public float maxZ;
+	
+	public AABB() {}
+	
+	public AABB(float minX, float minY, float minZ, float maxX, float maxY, float maxZ) {
+		this.minX = minX;
+		this.minY = minY;
+		this.minZ = minZ;
+		this.maxX = maxX;
+		this.maxY = maxY;
+		this.maxZ = maxZ;
+	}
+	
+	public void addMax(float deltaX, float deltaY, float deltaZ) {
+		this.maxX += deltaX;
+		this.maxY += deltaY;
+		this.maxZ += deltaZ;
+	}
+	
+	public void addMin(float deltaX, float deltaY, float deltaZ) {
+		this.minX += deltaX;
+		this.minY += deltaY;
+		this.minZ += deltaZ;
+	}
+	
+	@Override
+	public AABB clone() {
+		return new AABB(this.minX, this.minY, this.minZ, this.maxX, this.maxY, this.maxZ);
+	}
+	
+	/**
+	 * Return true if the current AABB contains the AABB given in parameter
+	 * @param aabb AABB box that is contains in the current.
+	 * @return true The parameter in contained inside
+	 */
+	public boolean contains(final AABB aabb) {
+		if (this.minX > aabb.minX) {
+			return false;
+		}
+		if (this.minY > aabb.minY) {
+			return false;
+		}
+		if (this.minZ > aabb.minZ) {
+			return false;
+		}
+		if (this.maxX < aabb.maxX) {
+			return false;
+		}
+		if (this.maxY < aabb.maxY) {
+			return false;
+		}
+		if (this.maxZ < aabb.maxZ) {
+			return false;
+		}
+		return true;
+	}
+	
+	public Vector3f getMax() {
+		return new Vector3f(this.maxX, this.maxY, this.maxZ);
+	}
+	
+	public Vector3f getMin() {
+		return new Vector3f(this.minX, this.minY, this.minZ);
+	}
+	
+	/**
+	 * Get the cube total volume.
+	 * @return
+	 */
+	public float getVolume() {
+		return ((this.maxX - this.minX) * (this.maxY - this.minY) * (this.maxZ - this.minZ));
+	}
+	
+	public boolean intersect(AABB other) {
+		if (this == other) {
+			return false;
+		}
+		if (null == other) {
+			return false;
+		}
+		if (this.minX > other.maxX) {
+			return false;
+		}
+		if (this.maxX < other.minX) {
+			return false;
+		}
+		if (this.minY > other.maxY) {
+			return false;
+		}
+		if (this.maxY < other.minY) {
+			return false;
+		}
+		if (this.minZ > other.maxZ) {
+			return false;
+		}
+		if (this.maxZ < other.minZ) {
+			return false;
+		}
+		return true;
+	}
+	
+	public void lessMax(float deltaX, float deltaY, float deltaZ) {
+		this.maxX -= deltaX;
+		this.maxY -= deltaY;
+		this.maxZ -= deltaZ;
+	}
+	
+	public void lessMin(float deltaX, float deltaY, float deltaZ) {
+		this.minX -= deltaX;
+		this.minY -= deltaY;
+		this.minZ -= deltaZ;
+	}
+	
+	public void mergeTwoAABBs(AABB aaa, AABB bbb) {
+		this.minX = Math.min(aaa.minX, bbb.minX);
+		this.minY = Math.min(aaa.minY, bbb.minY);
+		this.minZ = Math.min(aaa.minZ, bbb.minZ);
+		this.minX = Math.max(aaa.maxX, bbb.maxX);
+		this.minY = Math.max(aaa.maxY, bbb.maxY);
+		this.minZ = Math.max(aaa.maxZ, bbb.maxZ);
+	}
+	
+	public void set(float minX, float minY, float minZ, float maxX, float maxY, float maxZ) {
+		this.minX = minX;
+		this.minY = minY;
+		this.minZ = minZ;
+		this.maxX = maxX;
+		this.maxY = maxY;
+		this.maxZ = maxZ;
+	}
+	
+	/*
+	 * 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 ray Ray to test
+	 * @return true The raytest intersect the AABB box
+	 */
+	public boolean testRayIntersect(final Ray ray) {
+		final Vector3f point2 = ray.point2.less(ray.point1).multiply(ray.maxFraction).add(ray.point1);
+		final float eeeX = this.maxX - this.minX;
+		final float eeeY = this.maxY - this.minY;
+		final float eeeZ = this.maxZ - this.minZ;
+		final Vector3f d = point2.less(ray.point1);
+		final float mmmX = ray.point1.x() - ray.point2.x() - this.minX - this.maxX;
+		final float mmmY = ray.point1.y() - ray.point2.y() - this.minY - this.maxY;
+		final float mmmZ = ray.point1.z() - ray.point2.z() - this.minZ - this.maxZ;
+		// Test if the AABB face normals are separating axis
+		float adx = FMath.abs(d.x());
+		if (FMath.abs(mmmX) > eeeX + adx) {
+			return false;
+		}
+		float ady = FMath.abs(d.y());
+		if (FMath.abs(mmmY) > eeeY + ady) {
+			return false;
+		}
+		float adz = FMath.abs(d.z());
+		if (FMath.abs(mmmZ) > eeeZ + 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(mmmY * d.z() - mmmZ * d.y()) > eeeY * adz + eeeZ * ady) {
+			return false;
+		}
+		if (FMath.abs(mmmZ * d.x() - mmmX * d.z()) > eeeX * adz + eeeZ * adx) {
+			return false;
+		}
+		if (FMath.abs(mmmX * d.y() - mmmY * d.x()) > eeeX * ady + eeeY * adx) {
+			return false;
+		}
+		// No separating axis has been found
+		return true;
+	}
+	
+	public void update(Vector3f point) {
+		if (this.minX > point.x()) {
+			this.minX = point.x();
+		}
+		if (this.maxX < point.x()) {
+			this.maxX = point.x();
+		}
+		if (this.minY > point.y()) {
+			this.minY = point.y();
+		}
+		if (this.maxY < point.y()) {
+			this.maxY = point.y();
+		}
+		if (this.minZ > point.z()) {
+			this.minZ = point.z();
+		}
+		if (this.maxZ < point.z()) {
+			this.maxZ = point.z();
+		}
+	}
+}

src/org/atriasoft/phyligram/shape/Box.java

@@ -3,7 +3,7 @@
  * @copyright 2011, Edouard DUPIN, all right reserved
  * @license MPL v2.0 (see license file)
  */
-package org.atriasoft.ege.physics.shape;
+package org.atriasoft.phyligram.shape;
 
 import org.atriasoft.etk.math.Vector3f;
 

src/org/atriasoft/phyligram/shape/Capsule.java

@@ -3,7 +3,7 @@
  * @copyright 2011, Edouard DUPIN, all right reserved
  * @license MPL v2.0 (see license file)
  */
-package org.atriasoft.ege.physics.shape;
+package org.atriasoft.phyligram.shape;
 
 import org.atriasoft.etk.math.Vector3f;
 

src/org/atriasoft/phyligram/shape/Concave.java

@@ -3,7 +3,7 @@
  * @copyright 2011, Edouard DUPIN, all right reserved
  * @license MPL v2.0 (see license file)
  */
-package org.atriasoft.ege.physics.shape;
+package org.atriasoft.phyligram.shape;
 
 import java.util.ArrayList;
 import java.util.List;

src/org/atriasoft/phyligram/shape/Cone.java

@@ -3,7 +3,7 @@
  * @copyright 2011, Edouard DUPIN, all right reserved
  * @license MPL v2.0 (see license file)
  */
-package org.atriasoft.ege.physics.shape;
+package org.atriasoft.phyligram.shape;
 
 import org.atriasoft.etk.math.Vector3f;
 

src/org/atriasoft/phyligram/shape/ConvexHull.java

@@ -3,7 +3,7 @@
  * @copyright 2011, Edouard DUPIN, all right reserved
  * @license MPL v2.0 (see license file)
  */
-package org.atriasoft.ege.physics.shape;
+package org.atriasoft.phyligram.shape;
 
 import java.util.ArrayList;
 import java.util.List;

src/org/atriasoft/phyligram/shape/Cylinder.java

@@ -3,7 +3,7 @@
  * @copyright 2011, Edouard DUPIN, all right reserved
  * @license MPL v2.0 (see license file)
  */
-package org.atriasoft.ege.physics.shape;
+package org.atriasoft.phyligram.shape;
 
 import org.atriasoft.etk.math.Vector3f;
 

src/org/atriasoft/phyligram/shape/HeightMapChunk.java

@@ -0,0 +1,16 @@
+package org.atriasoft.phyligram.shape;
+
+import org.atriasoft.ephysics.collision.Triangle;
+
+public class HeightMapChunk extends Shape {
+	private Triangle[] triangles;
+	public HeightMapChunk(Triangle[] triangles) {
+		this.triangles = triangles;
+	}
+	@Override
+	public void display() {
+		// TODO Auto-generated method stub
+		super.display();
+	}
+	
+}

src/org/atriasoft/phyligram/shape/Shape.java

@@ -3,7 +3,7 @@
  * @copyright 2011, Edouard DUPIN, all right reserved
  * @license MPL v2.0 (see license file)
  */
-package org.atriasoft.ege.physics.shape;
+package org.atriasoft.phyligram.shape;
 
 import org.atriasoft.ege.internal.Log;
 import org.atriasoft.etk.math.Quaternion;
@@ -30,34 +30,6 @@ public class Shape {
 		return this.origin;
 	}
 	
-	public boolean isBox() {
-		return this instanceof Box;
-	};
-	
-	public boolean isCapsule() {
-		return this instanceof Capsule;
-	}
-	
-	public boolean isConcave() {
-		return this instanceof Concave;
-	}
-	
-	public boolean isCone() {
-		return this instanceof Cone;
-	}
-	
-	public boolean isConvexHull() {
-		return this instanceof ConvexHull;
-	};
-	
-	public boolean isCylinder() {
-		return this instanceof Cylinder;
-	};
-	
-	public boolean isSphere() {
-		return this instanceof Sphere;
-	};
-	
 	public boolean parse(final String _line) {
 		Log.error("dfgdfg");
 		/*

src/org/atriasoft/phyligram/shape/Sphere.java

@@ -3,7 +3,7 @@
  * @copyright 2011, Edouard DUPIN, all right reserved
  * @license MPL v2.0 (see license file)
  */
-package org.atriasoft.ege.physics.shape;
+package org.atriasoft.phyligram.shape;
 
 import org.atriasoft.etk.math.Vector3f;
 

src/org/atriasoft/phyligram/tree/CallbackOverlapping.java

@@ -0,0 +1,5 @@
+package org.atriasoft.phyligram.tree;
+
+public interface CallbackOverlapping {
+	public void callback(DTree nodeId);
+}

src/org/atriasoft/phyligram/tree/CallbackRaycast.java

@@ -0,0 +1,7 @@
+package org.atriasoft.phyligram.tree;
+
+import org.atriasoft.phyligram.math.Ray;
+
+public interface CallbackRaycast {
+	public float callback(DTree node, Ray ray);
+}

src/org/atriasoft/phyligram/tree/DTree.java

@@ -0,0 +1,28 @@
+package org.atriasoft.phyligram.tree;
+
+import java.lang.ref.WeakReference;
+
+import org.atriasoft.phyligram.shape.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 = AABB.createInvertedEmpty(); //!< Fat axis aligned bounding box (AABB) corresponding to the node
+	
+	public DTree() {
+		this.uid = simpleCounter++;
+	}
+	
+	boolean isLeaf() {
+		return false;
+	}
+	
+}

src/org/atriasoft/phyligram/tree/DTreeLeafData.java

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

src/org/atriasoft/phyligram/tree/DTreeLeafInt.java

@@ -0,0 +1,16 @@
+package org.atriasoft.phyligram.tree;
+
+class DTreeLeafInt extends DTree {
+	int dataInt0 = 0;
+	int dataInt1 = 0;
+	
+	public DTreeLeafInt(final int dataInt0, final int dataInt1) {
+		this.dataInt0 = dataInt0;
+		this.dataInt1 = dataInt1;
+	}
+	
+	@Override
+	boolean isLeaf() {
+		return true;
+	}
+}

src/org/atriasoft/phyligram/tree/DTreeNode.java

@@ -0,0 +1,6 @@
+package org.atriasoft.phyligram.tree;
+
+class DTreeNode extends DTree {
+	DTree childrenleft = null; //!< Left child of the node
+	DTree childrenright = null; //!< Right child of the node
+}

src/org/atriasoft/phyligram/tree/DynamicAABBTree.java

@@ -0,0 +1,560 @@
+package org.atriasoft.phyligram.tree;
+
+import java.lang.ref.WeakReference;
+import java.util.Stack;
+
+import org.atriasoft.phyligram.shape.AABB;
+import org.atriasoft.phyligram.internal.Log;
+import org.atriasoft.phyligram.math.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<INTERNAL_DATA_TYPE> {
+	private DTree rootNode; //!< Pointer to the memory location of the nodes of the tree
+	
+	/// Constructor
+	public DynamicAABBTree() {
+		init();
+	}
+	
+	/// Add an object into the tree (where node data is a pointer)
+	public DTree addObject(final AABB aabb, final INTERNAL_DATA_TYPE data) {
+		final DTreeLeafData<INTERNAL_DATA_TYPE> node = new DTreeLeafData<INTERNAL_DATA_TYPE>(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.clone();
+		// 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.childrenleft;
+		final DTree nodeC = nodeA.childrenright;
+		// 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.childrenleft;
+			final DTree nodeG = nodeCTree.childrenright;
+			nodeCTree.childrenleft = node;
+			nodeCTree.parent = nodeA.parent;
+			nodeA.parent = new WeakReference<>(nodeC);
+			if (nodeC.parent != null) {
+				final DTreeNode nodeCParent = (DTreeNode) nodeC.parent.get();
+				if (nodeCParent.childrenleft == node) {
+					nodeCParent.childrenleft = nodeC;
+				} else {
+					assert (nodeCParent.childrenright == node);
+					nodeCParent.childrenright = 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.childrenright = nodeF;
+				nodeA.childrenright = nodeG;
+				nodeG.parent = new WeakReference<>(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.childrenright = nodeG;
+				nodeA.childrenright = nodeF;
+				nodeF.parent = new WeakReference<>(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.childrenleft;
+			final DTree nodeG = nodeBTree.childrenright;
+			nodeBTree.childrenleft = node;
+			nodeB.parent = nodeA.parent;
+			nodeA.parent = new WeakReference<>(nodeB);
+			if (nodeB.parent != null) {
+				final DTreeNode nodeBParent = (DTreeNode) nodeB.parent.get();
+				if (nodeBParent.childrenleft == node) {
+					nodeBParent.childrenleft = nodeB;
+				} else {
+					assert (nodeBParent.childrenright == node);
+					nodeBParent.childrenright = 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.childrenright = nodeF;
+				nodeA.childrenleft = nodeG;
+				nodeG.parent = new WeakReference<>(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.childrenright = nodeG;
+				nodeA.childrenleft = nodeF;
+				nodeF.parent = new WeakReference<>(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.childrenleft);
+		final int rightHeight = computeHeight(nodeTree.childrenright);
+		// 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 getNodeDataInt0(final DTree node) {
+		assert (node.isLeaf());
+		return ((DTreeLeafInt) node).dataInt0;
+	}
+	
+	public int getNodeDataInt1(final DTree node) {
+		assert (node.isLeaf());
+		return ((DTreeLeafInt) node).dataInt1;
+	}
+	
+	/// 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 inNode) {
+		// If the tree is empty
+		if (this.rootNode == null) {
+			this.rootNode = inNode;
+			return;
+		}
+		// Find the best sibling node for the new node
+		final AABB newNodeAABB = inNode.aabb;
+		DTree currentNode = this.rootNode;
+		while (!currentNode.isLeaf()) {
+			final DTreeNode node = (DTreeNode) currentNode;
+			final DTree leftChild = node.childrenleft;
+			final DTree rightChild = node.childrenright;
+			// Compute the merged AABB
+			final float volumeAABB = currentNode.aabb.getVolume();
+			final AABB mergedAABBs = AABB.mergeAABB(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.childrenleft == siblingNode) {
+				parentNode.childrenleft = newParentNode;
+			} else {
+				parentNode.childrenright = newParentNode;
+			}
+		} else {
+			// If the sibling node was the root node
+			this.rootNode = newParentNode;
+		}
+		// setup the children
+		newParentNode.childrenleft = siblingNode;
+		newParentNode.childrenright = inNode;
+		siblingNode.parent = new WeakReference<>(newParentNode);
+		inNode.parent = new WeakReference<>(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 (inNode.isLeaf());
+			assert (!currentNode.isLeaf());
+			final DTreeNode nodeDouble = (DTreeNode) currentNode;
+			final DTree leftChild = nodeDouble.childrenleft;
+			final DTree rightChild = nodeDouble.childrenright;
+			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 (inNode.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<>();
+		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)) {
+				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.childrenleft);
+				stack.push(tmpNode.childrenright);
+			}
+		}
+		
+	}
+	
+	/// 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.childrenleft == node) {
+			siblingNode = parentNode.childrenright;
+		} else {
+			siblingNode = parentNode.childrenleft;
+		}
+		// 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.childrenleft == parentNode) {
+				grandParentNode.childrenleft = siblingNode;
+			} else {
+				grandParentNode.childrenright = 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.childrenleft;
+				final DTree rightChild = currentTreeNode.childrenright;
+				// 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<>();
+		// 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.intersect(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.childrenleft);
+					stack.push(tmp.childrenright);
+					//Log.error("    add stack: " + tmp.childrenleft);
+					//Log.error("    add stack: " + tmp.childrenright);
+				}
+			}
+		}
+	}
+	
+	/// 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 && 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.clone();
+		// Inflate the fat AABB in direction of the linear motion of the AABB
+		float xmin = node.aabb.getMin().x();
+		float ymin = node.aabb.getMin().y();
+		float zmin = node.aabb.getMin().z();
+		float xmax = node.aabb.getMax().x();
+		float ymax = node.aabb.getMax().y();
+		float zmax = node.aabb.getMax().z();
+		if (displacement.x() < 0.0f) {
+			xmin = node.aabb.getMin().x() + displacement.x();
+		} else {
+			xmax = node.aabb.getMax().x() + displacement.x();
+		}
+		if (displacement.y() < 0.0f) {
+			ymin = node.aabb.getMin().y() + displacement.y();
+		} else {
+			ymax = node.aabb.getMax().y() + displacement.y();
+		}
+		if (displacement.z() < 0.0f) {
+			zmin = node.aabb.getMin().z() + displacement.z();
+		} else {
+			zmax = node.aabb.getMax().z() + displacement.z();
+		}
+		node.aabb.set(xmin, ymin, zmin, xmax, ymax, zmax);
+		//Log.error(" compare : " + node.aabb.getMin() + " " + node.aabb.getMax());
+		//Log.error("         : " + newAABB.getMin() + " " + newAABB.getMax());
+		if (!node.aabb.contains(newAABB)) {
+			//Log.critical("ERROR");
+		}
+		assert (node.aabb.contains(newAABB));
+		// Reinsert the node into the tree
+		insertLeafNode(node);
+		return true;
+	}
+	
+}

src/org/atriasoft/phyligram/tree/PairDTree.java

@@ -0,0 +1,37 @@
+package org.atriasoft.phyligram.tree;
+
+import java.util.Set;
+
+public record PairDTree(
+		DTree first,
+		DTree second) {
+	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 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 String toString() {
+		return "PairDTree [first=" + this.first.uid + ", second=" + this.second.uid + "]";
+	}
+}

test/src/test/atriasoft/ege/TestTransformation3D.java

@@ -3,7 +3,7 @@ package test.atriasoft.ege;
 import org.atriasoft.ege.geometry.AABB;
 import org.atriasoft.ege.geometry.Geometry3D;
 import org.atriasoft.ege.geometry.OBB;
-import org.atriasoft.ege.geometry.Plane;
+import org.atriasoft.ege.geometry.Plane____;
 import org.atriasoft.ege.geometry.Sphere;
 import org.atriasoft.ege.geometry.Triangle;
 import org.atriasoft.etk.math.Matrix3f;
@@ -53,7 +53,7 @@ public class TestTransformation3D {
 	
 	@Test
 	void testPointInPlane() {
-		final Plane shape = new Plane((new Vector3f(4, 4, 4)).normalize(), (float) Math.sqrt(1 * 1 + 1 * 1));
+		final Plane____ shape = new Plane____((new Vector3f(4, 4, 4)).normalize(), (float) Math.sqrt(1 * 1 + 1 * 1));
 		Assert.assertFalse(Geometry3D.pointInPlane(new Vector3f(0, 0, 0), shape));
 		Assert.assertFalse(Geometry3D.pointInPlane(new Vector3f(6, 6, 6), shape));
 		Assert.assertTrue(Geometry3D.pointInPlane(new Vector3f(3, 3, 3), shape));

test/src/test/atriasoft/phyligram/TestCollisionSphereTriangle.java

@@ -0,0 +1,77 @@
+package test.atriasoft.phyligram;
+
+import java.util.stream.Stream;
+
+import org.atriasoft.etk.math.Transform3D;
+import org.atriasoft.etk.math.Vector3f;
+import org.atriasoft.phyligram.PhysicSphere;
+import org.atriasoft.phyligram.PhysicTriangle;
+import org.atriasoft.phyligram.ToolCollisionSphereWithTriangle;
+import org.atriasoft.phyligram.shape.AABB;
+import org.junit.Assert;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+public class TestCollisionSphereTriangle {
+	
+	static Stream<Arguments> generateData() {
+		return Stream.of( // ...
+				Arguments.of(1, new Vector3f(0.0f, 0.0f, 1.001f), false), // test just on top Z
+				Arguments.of(2, new Vector3f(0.0f, 0.0f, -1.001f), false), // test just on bottom Z
+				Arguments.of(2, new Vector3f(2.001f, 0.0f, 0.0f), false), // test on the top X
+				Arguments.of(2, new Vector3f(-2.001f, 0.0f, 0.0f), false), // test on the bottom X
+				Arguments.of(2, new Vector3f(0.0f, 1.7f, 0.0f), false), // test on the top Y
+				Arguments.of(2, new Vector3f(0.0f, -1.7f, 0.0f), false), // test on the bottom Y
+				// test collision
+				Arguments.of(2, new Vector3f(0.0f, 0.0f, 0.9f), true), // in center
+				Arguments.of(1, new Vector3f(0.0f, 0.0f, 0.999f), true), // test just on top Z
+				Arguments.of(2, new Vector3f(0.0f, 0.0f, -0.999f), true), // test just on bottom Z
+				Arguments.of(2, new Vector3f(1.999f, 0.0f, 0.0f), true), // test on the top X
+				Arguments.of(2, new Vector3f(-1.999f, 0.0f, 0.0f), true), // test on the bottom X
+				Arguments.of(2, new Vector3f(0.0f, 1.5f, 0.0f), true), // test on the top Y
+				Arguments.of(2, new Vector3f(0.0f, -1.5f, 0.0f), true), // test on the bottom Y
+				Arguments.of(2, new Vector3f(1.5f, 1.5f, 0.0f), true), // test on corner A
+				Arguments.of(2, new Vector3f(1.5f, -1.5f, 0.0f), true) // test on corner B
+		);
+	}
+	
+	@ParameterizedTest
+	@MethodSource("generateData")
+	void testsphereOut(int testId, Vector3f position, boolean resultTheoricValue) {
+		System.out.println("AAAAA ");
+		float testCoefficient = 1.0f;
+		PhysicSphere sphere = new PhysicSphere();
+		sphere.setSize(testCoefficient);
+		PhysicTriangle triangle = new PhysicTriangle();
+		triangle.setPoints(new Vector3f(testCoefficient, testCoefficient, 0.0f), new Vector3f(testCoefficient, -testCoefficient, 0.0f), new Vector3f(-testCoefficient, 0.0f, 0.0f));
+		Transform3D transformGlobalTriangle = Transform3D.IDENTITY;
+		Transform3D transformGlobalsphere = new Transform3D(position.multiply(testCoefficient));
+		AABB aabb = new AABB();
+		sphere.updateAABB(transformGlobalsphere, aabb);
+		sphere.updateForNarrowCollision(transformGlobalsphere);
+		triangle.updateAABB(transformGlobalTriangle, aabb);
+		triangle.updateForNarrowCollision(transformGlobalTriangle);
+		boolean result = ToolCollisionSphereWithTriangle.testCollide(sphere, triangle);
+		Assert.assertEquals(resultTheoricValue, result);
+	}
+	
+	@Test
+	void testsphereOutTop() {
+		float testCoefficient = 2.0f;
+		PhysicSphere sphere = new PhysicSphere();
+		sphere.setSize(testCoefficient);
+		PhysicTriangle triangle = new PhysicTriangle();
+		triangle.setPoints(new Vector3f(testCoefficient, testCoefficient, 0.0f), new Vector3f(testCoefficient, -testCoefficient, 0.0f), new Vector3f(-testCoefficient, 0.0f, 0.0f));
+		Transform3D transformGlobalTriangle = Transform3D.IDENTITY;
+		Transform3D transformGlobalsphere = new Transform3D(new Vector3f(0.0f, 0.0f, testCoefficient + 0.001f));
+		AABB aabb = new AABB();
+		sphere.updateAABB(transformGlobalsphere, aabb);
+		sphere.updateForNarrowCollision(transformGlobalsphere);
+		triangle.updateAABB(transformGlobalTriangle, aabb);
+		triangle.updateForNarrowCollision(transformGlobalTriangle);
+		boolean result = ToolCollisionSphereWithTriangle.testCollide(sphere, triangle);
+		Assert.assertFalse(result);
+	}
+}