[DEV] continue porting internal physics engine

src/org/atriaSoft/etk/math/Matrix4f.java

@@ -80,6 +80,40 @@ public class Matrix4f {
 			this.mat[iii] = values[iii];
 		}
 	}
+	public Matrix4f(Matrix3f matrix) {
+		this.mat[0] = matrix.mat[0];
+		this.mat[1] = matrix.mat[1];
+		this.mat[2] = matrix.mat[2];
+		this.mat[3] = 0;
+		this.mat[4] = matrix.mat[3];
+		this.mat[5] = matrix.mat[4];
+		this.mat[6] = matrix.mat[5];
+		this.mat[7] = 0;
+		this.mat[8] = matrix.mat[6];
+		this.mat[9] = matrix.mat[7];
+		this.mat[10] = matrix.mat[8];
+		this.mat[11] = 0;
+		this.mat[12] = 0;
+		this.mat[13] = 0;
+		this.mat[14] = 0;
+		this.mat[15] = 1;
+//		this.mat[0] = matrix.mat[0];
+//		this.mat[1] = matrix.mat[3];
+//		this.mat[2] = matrix.mat[6];
+//		this.mat[3] = 0;
+//		this.mat[4] = matrix.mat[1];
+//		this.mat[5] = matrix.mat[4];
+//		this.mat[6] = matrix.mat[7];
+//		this.mat[7] = 0;
+//		this.mat[8] = matrix.mat[2];
+//		this.mat[9] = matrix.mat[5];
+//		this.mat[10] = matrix.mat[8];
+//		this.mat[11] = 0;
+//		this.mat[12] = 0;
+//		this.mat[13] = 0;
+//		this.mat[14] = 0;
+//		this.mat[15] = 1;
+	}
 	/**
 	 * @brief Operator= Asign the current object with an other object
 	 * @param[in] obj Reference on the external object

src/org/atriaSoft/etk/math/Quaternion.java

@@ -2,6 +2,8 @@ package org.atriaSoft.etk.math;
 
 import java.util.Arrays;
 
+import org.atriaSoft.gale.test.sample2.Log;
+
 public class Quaternion {
 	public float x;
 	public float y;
@@ -651,4 +653,11 @@ public class Quaternion {
 	public String toString() {
 		return "Quaternion(" + this.x + "," + this.y + "," + this.z + "," + this.w + ")";
 	}
+	
+	// a * diff = b
+	public static Quaternion diff(Quaternion a, Quaternion b) {
+		//Log.info("diff " + a + "       " + b);
+		Quaternion inv = a.inverse_new();
+		return inv.multiply(b);
+	}
 }

src/org/atriaSoft/gale/context/LWJG_AWT/ContextLWJGL_AWT.java

@@ -2,10 +2,8 @@ package org.atriaSoft.gale.context.LWJG_AWT;
 
 import java.awt.AWTException;
 import java.awt.BorderLayout;
-import java.awt.Color;
 import java.awt.Cursor;
 import java.awt.Dimension;
-import java.awt.Frame;
 import java.awt.Image;
 import java.awt.Point;
 import java.awt.Rectangle;
@@ -17,26 +15,18 @@ import java.awt.event.MouseListener;
 import java.awt.event.MouseMotionListener;
 import java.awt.event.MouseWheelEvent;
 import java.awt.event.MouseWheelListener;
-import java.awt.event.WindowAdapter;
-import java.awt.event.WindowEvent;
 import java.awt.image.MemoryImageSource;
 import java.util.ArrayList;
 import java.util.List;
-import java.util.Timer;
-import java.util.TimerTask;
-
 import java.awt.Robot;
 
 import javax.swing.JFrame;
 import javax.swing.SwingUtilities;
 
-import java.nio.IntBuffer;
-
 import org.atriaSoft.etk.Uri;
 import org.atriaSoft.etk.math.Vector2f;
 import org.atriaSoft.gale.Application;
 import org.atriaSoft.gale.Fps;
-import org.atriaSoft.gale.backend3d.OpenGL;
 import org.atriaSoft.gale.context.Context;
 import org.atriaSoft.gale.key.KeyKeyboard;
 import org.atriaSoft.gale.key.KeySpecial;

src/org/atriaSoft/gameEngine/components/ComponentPhysics.java

@@ -75,6 +75,7 @@ public class ComponentPhysics extends Component {
 	}
 	
 	public void updateForNarrowCollision() {
+		narrowIntersection.clear();
 		if (aabbIntersection.size() == 0) {
 			return;
 		}
@@ -85,7 +86,12 @@ public class ComponentPhysics extends Component {
 		for (PhysicShape shape : shapes) {
 			shape.updateForNarrowCollision(position.getTransform());
 		}
-		narrowIntersection.clear();
+	}
+	public boolean isNarrowCollide() {
+		if (narrowIntersection.size() == 0) {
+			return false;
+		}
+		return true;
 	}
 	public boolean checkNarrowCollision() {
 		if (this.staticObject == true) {
@@ -94,7 +100,7 @@ public class ComponentPhysics extends Component {
 		for (ComponentPhysics elem : aabbIntersection) {
 			boolean collide = false;
 			for (PhysicShape shapeCurrent : shapes) {
-				if (true == elem.checkCollide(shapeCurrent)) {
+				if (elem.checkCollide(shapeCurrent) == true) {
 					collide = true;
 					break;
 				}
@@ -104,7 +110,21 @@ public class ComponentPhysics extends Component {
 				elem.narrowIntersection.add(this);
 			}
 		}
-		return false;
+		return isNarrowCollide();
+	}
+	public void narrowCollisionCreateContactAndForce() {
+		if (narrowIntersection.size() == 0) {
+			return;
+		}
+		for (ComponentPhysics elem : narrowIntersection) {
+			for (PhysicShape shapeCurrent : shapes) {
+				//TODO: Do a better method we do this many times ...
+				if (elem.checkCollide(shapeCurrent) == false) {
+					continue;
+				}
+				elem.getCollidePoints(shapeCurrent);
+			}
+		}
 	}
 
 	private boolean checkCollide(PhysicShape shapeCurrent) {
@@ -153,6 +173,50 @@ public class ComponentPhysics extends Component {
 		}
 		return false;
 	}
+	private void getCollidePoints(PhysicShape shapeCurrent) {
+		if (shapeCurrent instanceof PhysicBox) {
+			PhysicBox shape111 = (PhysicBox)shapeCurrent;
+			for (PhysicShape shape : shapes) {
+				if (shape instanceof PhysicBox) {
+					PhysicBox shape222 = (PhysicBox)shape;
+					ToolCollisionOBBWithOBB.getCollidePoints(shape111, shape222);
+				} else if (shape instanceof PhysicSphere) {
+					
+				} else if (shape instanceof PhysicMapVoxel) {
+					
+				} else {
+					Log.error("Not manage collision model... " + shape);
+				}
+			}
+		} else if (shapeCurrent instanceof PhysicSphere) {
+			for (PhysicShape shape : 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 : 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;
+	}
 
 	public void applyForces(float timeStep, EngineGravity gravity) {
 		// get the gravity at the specific position...

src/org/atriaSoft/gameEngine/engines/EngineLight.java

@@ -71,6 +71,10 @@ public class EngineLight extends Engine {
 		float maxDistance = 50*50;
 		for (ComponentLight elem: componentLights) {
 			Vector3f pos = elem.getPosition();
+			if (count>=8) {
+				Log.error("need to update ligth count");
+				return out;
+			}
 			if (pos.distance2(position) < maxDistance) {
 				out[count] = new Light(elem.getLight().getColor(), pos, elem.getLight().getAttenuation());
 				count++;

src/org/atriaSoft/gameEngine/engines/EnginePhysics.java

@@ -95,6 +95,15 @@ public class EnginePhysics extends Engine {
 		for (int iii=0; iii< components.size(); iii++) {
 			ComponentPhysics current = components.get(iii);
 			boolean collide = current.checkNarrowCollision();
+			
+		}
+		for (int iii=0; iii< components.size(); iii++) {
+			ComponentPhysics current = components.get(iii);
+			if (current.isNarrowCollide() == false) {
+				continue;
+			}
+			current.narrowCollisionCreateContactAndForce();
+			
 		}
 		
 	}

src/org/atriaSoft/gameEngine/physics/PhysicBox.java

@@ -18,16 +18,18 @@ public class PhysicBox extends PhysicShape {
 		this.size = size;
 	}
 	@Override
-	public void updateAABB(Transform3D transform, PhysicCollisionAABB aabb) {
+	public void updateAABB(Transform3D transformGlobal, PhysicCollisionAABB aabb) {
+		// store it, many time usefull...
+		this.transformGlobal = transformGlobal;
 		// TODO Auto-generated method stub
-		aabb.update(transform.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(transform.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(transform.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(transform.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(transform.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(transform.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(transform.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(transform.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;
@@ -36,15 +38,15 @@ public class PhysicBox extends PhysicShape {
 	public Vector3f narrowPhaseAxisZ = new Vector3f(1,0,0);
 	public Vector3f narrowPhaseHalfSize;
 	@Override
-	public void updateForNarrowCollision(Transform3D transform) {
+	public void updateForNarrowCollision(Transform3D transformGlobal) {
-		this.narrowPhaseGlobalPos = transform.multiply(this.transform.multiply(new Vector3f(0,0,0)));
+		this.narrowPhaseGlobalPos = transformGlobal.multiply(this.transform.multiply(new Vector3f(0,0,0)));
-		this.narrowPhaseAxisX = transform.multiply(this.transform.multiply(new Vector3f(1,0,0))).less(this.narrowPhaseGlobalPos);
+		this.narrowPhaseAxisX = transformGlobal.multiply(this.transform.multiply(new Vector3f(1,0,0))).less(this.narrowPhaseGlobalPos);
-		this.narrowPhaseAxisY = transform.multiply(this.transform.multiply(new Vector3f(0,1,0))).less(this.narrowPhaseGlobalPos);
+		this.narrowPhaseAxisY = transformGlobal.multiply(this.transform.multiply(new Vector3f(0,1,0))).less(this.narrowPhaseGlobalPos);
-		this.narrowPhaseAxisZ = transform.multiply(this.transform.multiply(new Vector3f(0,0,1))).less(this.narrowPhaseGlobalPos);
+		this.narrowPhaseAxisZ = transformGlobal.multiply(this.transform.multiply(new Vector3f(0,0,1))).less(this.narrowPhaseGlobalPos);
 		this.narrowPhaseHalfSize = this.size.multiply_new(0.5f);
 	}
 	@Override
-	public void renderDebug(Transform3D transform, ResourceColored3DObject debugDrawProperty) {
+	public void renderDebug(Transform3D transformGlobal, ResourceColored3DObject debugDrawProperty) {
-		debugDrawProperty.drawSquare(this.size.multiply_new(0.5f), this.transform.getOpenGLMatrix().multiply_new(transform.getOpenGLMatrix()), new Color(0,1,0,0.25f));
+		debugDrawProperty.drawSquare(this.size.multiply_new(0.5f), this.transform.getOpenGLMatrix().multiply_new(transformGlobal.getOpenGLMatrix()), new Color(0,1,0,0.25f));
 	}
 }

src/org/atriaSoft/gameEngine/physics/PhysicShape.java

@@ -9,6 +9,7 @@ public abstract class PhysicShape {
 //	protected Quaternion quaternion;
 //	protected Vector3f origin;
 	protected Transform3D transform;
+	protected Transform3D transformGlobal;
 	protected float mass = 0;
 	protected final PhysicShapeType type;
 
@@ -26,6 +27,9 @@ public abstract class PhysicShape {
 //		this.origin = origin;
 		this.mass = mass;
 	}
+	public Quaternion getQuaternionFull() {
+		return transformGlobal.getOrientation().multiply_new(transform.getOrientation());
+	}
 	public Quaternion getQuaternion() {
 		return transform.getOrientation();
 	}
@@ -44,6 +48,12 @@ public abstract class PhysicShape {
 	public void setTransform(Transform3D transform) {
 		this.transform = transform;
 	}
+	public Transform3D getTransformGlobal() {
+		return transformGlobal;
+	}
+	public void setTransformGlobal(Transform3D transform) {
+		this.transformGlobal = transform;
+	}
 	public float getMass() {
 		return mass;
 	}

src/org/atriaSoft/gameEngine/physics/ToolCollisionOBBWithOBB.java

@@ -1,7 +1,9 @@
 package org.atriaSoft.gameEngine.physics;
 
+import org.atriaSoft.etk.math.Quaternion;
 import org.atriaSoft.etk.math.Vector3f;
 import org.atriaSoft.gale.test.sample2.Log;
+import org.atriaSoft.gameEngine.sample.LoxelEngine.LoxelApplication;
 
 // set the relevant elements of our oriented bounding box
 class OBB
@@ -123,5 +125,121 @@ public class ToolCollisionOBBWithOBB {
 				getSeparatingPlane222(RPos, box1.narrowPhaseAxisZ.cross(box2.narrowPhaseAxisZ), box1, box2);
 		return ! ret;
 	}
+	public static void getCollidePoints(PhysicBox box1, PhysicBox box2) {
+		//Log.info("Try to calculare reverse force ........");
+		Vector3f RPos = box2.narrowPhaseGlobalPos.less_new(box1.narrowPhaseGlobalPos);
+		Quaternion quat1 = box1.getQuaternionFull();
+		Quaternion quat2 = box2.getQuaternionFull();
+		// Step 1: set the Box 2 in the repere of the Box 1:
+		//Quaternion quatTransfer = Quaternion.diff(quat2, quat1);
+		Quaternion quatTransfer = Quaternion.diff(quat1, quat2);
+		//quatTransfer.normalize();
 
+		//LoxelApplication.relativeTest = quatTransfer;
+		//Vector3f tmp = RPos.add_new(new Vector3f(0,0,14));
+//		LoxelApplication.relativeTestPos.getTransform().setPosition(tmp);
+//		LoxelApplication.relativeTestPos.getTransform().setOrientation(quatTransfer);
+//		LoxelApplication.boxTest.setSize(box1.getSize());
+		Log.info("" + RPos + quatTransfer);
+//		/*res = */getCollidePointsAABBCenteredWithOBB(box1.narrowPhaseHalfSize, box2.narrowPhaseHalfSize, quatTransfer, RPos);
+		/* res = trenasfert in generic plan the new res ...*/
+		// test origin AABB with OBB collision
+		// Step 2: set the Box 1 in the repere of the Box 2:
+		// test origin AABB with OBB collision
+		RPos = box1.narrowPhaseGlobalPos.less_new(box2.narrowPhaseGlobalPos);
+		//tmp = RPos.add_new(new Vector3f(0,0,14));
+		quatTransfer = Quaternion.diff(quat2, quat1);
+		LoxelApplication.testRpos = RPos;
+		LoxelApplication.testQTransfert = quatTransfer;
+		LoxelApplication.box1HalfSize = box2.narrowPhaseHalfSize;
+		LoxelApplication.box2HalfSize = box1.narrowPhaseHalfSize;
+		
+//		LoxelApplication.relativeTestPos.getTransform().setPosition(tmp);
+//		LoxelApplication.relativeTestPos.getTransform().setOrientation(quatTransfer);
+//		LoxelApplication.boxTest.setSize(box1.getSize());
+		/*res = */getCollidePointsAABBCenteredWithOBB(box2.narrowPhaseHalfSize, box1.narrowPhaseHalfSize, quatTransfer, RPos);
+		/* res = trenasfert in generic plan the new res ...*/
+		
+	}
+
+	public static void getCollidePointsAABBCenteredWithOBB(Vector3f box1HalfSize, Vector3f box2HalfSize, Quaternion box2Orientation, Vector3f box2Position) {
+		// point in AABB
+//		Vector3f topBackRight  = box2Orientation.multiply(new Vector3f(box2Position.x + box2HalfSize.x, box2Position.y + box2HalfSize.y, box2Position.z + box2HalfSize.z));
+//		Vector3f topBackLeft   = box2Orientation.multiply(new Vector3f(box2Position.x - box2HalfSize.x, box2Position.y + box2HalfSize.y, box2Position.z + box2HalfSize.z));
+//		Vector3f topFrontRight = box2Orientation.multiply(new Vector3f(box2Position.x + box2HalfSize.x, box2Position.y - box2HalfSize.y, box2Position.z + box2HalfSize.z));
+//		Vector3f topFrontLeft  = box2Orientation.multiply(new Vector3f(box2Position.x - box2HalfSize.x, box2Position.y - box2HalfSize.y, box2Position.z + box2HalfSize.z));
+//		Vector3f bottomBackRight  = box2Orientation.multiply(new Vector3f(box2Position.x + box2HalfSize.x, box2Position.y + box2HalfSize.y, box2Position.z - box2HalfSize.z));
+//		Vector3f bottomBackLeft   = box2Orientation.multiply(new Vector3f(box2Position.x - box2HalfSize.x, box2Position.y + box2HalfSize.y, box2Position.z - box2HalfSize.z));
+//		Vector3f bottomFrontRight = box2Orientation.multiply(new Vector3f(box2Position.x + box2HalfSize.x, box2Position.y - box2HalfSize.y, box2Position.z - box2HalfSize.z));
+//		Vector3f bottomFrontLeft  = box2Orientation.multiply(new Vector3f(box2Position.x - box2HalfSize.x, box2Position.y - box2HalfSize.y, box2Position.z - box2HalfSize.z));
+		Vector3f topBackRight  = box2Orientation.multiply(new Vector3f(+ box2HalfSize.x, + box2HalfSize.y, + box2HalfSize.z)).add(box2Position);
+		Vector3f topBackLeft   = box2Orientation.multiply(new Vector3f(- box2HalfSize.x, + box2HalfSize.y, + box2HalfSize.z)).add(box2Position);
+		Vector3f topFrontRight = box2Orientation.multiply(new Vector3f(+ box2HalfSize.x, - box2HalfSize.y, + box2HalfSize.z)).add(box2Position);
+		Vector3f topFrontLeft  = box2Orientation.multiply(new Vector3f(- box2HalfSize.x, - box2HalfSize.y, + box2HalfSize.z)).add(box2Position);
+		Vector3f bottomBackRight  = box2Orientation.multiply(new Vector3f(+ box2HalfSize.x, + box2HalfSize.y, - box2HalfSize.z)).add(box2Position);
+		Vector3f bottomBackLeft   = box2Orientation.multiply(new Vector3f(- box2HalfSize.x, + box2HalfSize.y, - box2HalfSize.z)).add(box2Position);
+		Vector3f bottomFrontRight = box2Orientation.multiply(new Vector3f(+ box2HalfSize.x, - box2HalfSize.y, - box2HalfSize.z)).add(box2Position);
+		Vector3f bottomFrontLeft  = box2Orientation.multiply(new Vector3f(- box2HalfSize.x, - box2HalfSize.y, - box2HalfSize.z)).add(box2Position);
+		LoxelApplication.testPoints.clear();
+		LoxelApplication.testPoints.add(topBackRight);
+		LoxelApplication.testPoints.add(topBackLeft);
+		LoxelApplication.testPoints.add(topFrontRight);
+		LoxelApplication.testPoints.add(topFrontLeft);
+		LoxelApplication.testPoints.add(bottomBackRight);
+		LoxelApplication.testPoints.add(bottomBackLeft);
+		LoxelApplication.testPoints.add(bottomFrontRight);
+		LoxelApplication.testPoints.add(bottomFrontLeft);
+		LoxelApplication.testPointsBox.clear();
+		LoxelApplication.testPointsBox.add(new Vector3f(+ box1HalfSize.x, + box1HalfSize.y, + box1HalfSize.z));
+		LoxelApplication.testPointsBox.add(new Vector3f(- box1HalfSize.x, + box1HalfSize.y, + box1HalfSize.z));
+		LoxelApplication.testPointsBox.add(new Vector3f(+ box1HalfSize.x, - box1HalfSize.y, + box1HalfSize.z));
+		LoxelApplication.testPointsBox.add(new Vector3f(- box1HalfSize.x, - box1HalfSize.y, + box1HalfSize.z));
+		LoxelApplication.testPointsBox.add(new Vector3f(+ box1HalfSize.x, + box1HalfSize.y, - box1HalfSize.z));
+		LoxelApplication.testPointsBox.add(new Vector3f(- box1HalfSize.x, + box1HalfSize.y, - box1HalfSize.z));
+		LoxelApplication.testPointsBox.add(new Vector3f(+ box1HalfSize.x, - box1HalfSize.y, - box1HalfSize.z));
+		LoxelApplication.testPointsBox.add(new Vector3f(- box1HalfSize.x, - box1HalfSize.y, - box1HalfSize.z));
+		boolean insideTopBackRight  = pointInAABB(box1HalfSize, topBackRight);
+		boolean insideTopBackLeft   = pointInAABB(box1HalfSize, topBackLeft);
+		boolean insideTopFrontRight = pointInAABB(box1HalfSize, topFrontRight);
+		boolean insideTopFrontLeft  = pointInAABB(box1HalfSize, topFrontLeft);
+		boolean insideBottomBackRight  = pointInAABB(box1HalfSize, bottomBackRight);
+		boolean insideBottomBackLeft   = pointInAABB(box1HalfSize, bottomBackLeft);
+		boolean insideBottomFrontRight = pointInAABB(box1HalfSize, bottomFrontRight);
+		boolean insideBottomFrontLeft  = pointInAABB(box1HalfSize, bottomFrontLeft);
+		LoxelApplication.testPointsCollide.clear();
+		LoxelApplication.testPointsCollide.add(insideTopBackRight);
+		LoxelApplication.testPointsCollide.add(insideTopBackLeft);
+		LoxelApplication.testPointsCollide.add(insideTopFrontRight);
+		LoxelApplication.testPointsCollide.add(insideTopFrontLeft);
+		LoxelApplication.testPointsCollide.add(insideBottomBackRight);
+		LoxelApplication.testPointsCollide.add(insideBottomBackLeft);
+		LoxelApplication.testPointsCollide.add(insideBottomFrontRight);
+		LoxelApplication.testPointsCollide.add(insideBottomFrontLeft);
+		if (insideTopBackRight == true
+				|| insideTopBackLeft == true
+				|| insideTopFrontRight == true
+				|| insideTopFrontLeft == true
+				|| insideBottomBackRight == true
+				|| insideBottomBackLeft == true
+				|| insideBottomFrontRight == true
+				|| insideBottomFrontLeft == true) {
+			// Find a point inside the BOX ...
+			Log.info("Detect point inside ... " + insideTopBackRight    + "  " + insideTopBackLeft    + "  " + insideTopFrontRight    + "  " + insideTopFrontLeft
+					                     + "  " + insideBottomBackRight + "  " + insideBottomBackLeft + "  " + insideBottomFrontRight + "  " + insideBottomFrontLeft);
+		}
+		// line in AABB
+		Log.info("Need to detect line inside ...");
+	}
+	
+	public static boolean pointInAABB(Vector3f halfSize, Vector3f point) {
+		if (point.x > -halfSize.x
+				&& point.x < halfSize.x
+				&& point.y > -halfSize.y
+				&& point.y < halfSize.y
+				&& point.z > -halfSize.z
+				&& point.z < halfSize.z) {
+			return true;
+		}
+		return false;
+	}
 }

src/org/atriaSoft/gameEngine/sample/LoxelEngine/LoxelApplication.java

@@ -1,5 +1,8 @@
 package org.atriaSoft.gameEngine.sample.LoxelEngine;
 
+import java.util.ArrayList;
+import java.util.List;
+
 import org.atriaSoft.etk.Color;
 import org.atriaSoft.etk.Uri;
 import org.atriaSoft.etk.math.Matrix4f;
@@ -41,6 +44,7 @@ import org.atriaSoft.gale.key.KeyKeyboard;
 import org.atriaSoft.gale.key.KeySpecial;
 import org.atriaSoft.gale.key.KeyStatus;
 import org.atriaSoft.gale.key.KeyType;
+import org.atriaSoft.gale.resource.ResourceColored3DObject;
 
 public class LoxelApplication extends Application {
 	private Environement env;
@@ -53,6 +57,16 @@ public class LoxelApplication extends Application {
 	private float angleLight = 0;
 	private MapVoxel map;
 	private ComponentPlayer objectPlayer;
+//	public static ComponentPosition relativeTestPos;
+//	public static PhysicBox boxTest;
+	public static List<Vector3f> testPoints = new ArrayList<Vector3f>();
+	public static List<Vector3f> testPointsBox = new ArrayList<Vector3f>();
+	public static List<Boolean> testPointsCollide = new ArrayList<Boolean>();
+	public static Vector3f testRpos;
+	public static Quaternion testQTransfert;
+	public static Vector3f box1HalfSize;
+	public static Vector3f box2HalfSize;
+	private ResourceColored3DObject debugDrawProperty;
 	public LoxelApplication(){
 		creationDone = false;
 	}
@@ -66,8 +80,8 @@ public class LoxelApplication extends Application {
 		setSize(new Vector2f(800, 600));
 		setTitle("Loxel sample");
 		map = new MapVoxel(this.env);
-		this.env.addEngine(map);
+//		this.env.addEngine(map);
-		map.init();
+//		map.init();
 
 		// simple sun to have a global light ...
 		Entity globalGravity = new Entity(this.env);
@@ -83,32 +97,139 @@ public class LoxelApplication extends Application {
 		// 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(lightPosition);
-		localLight.addComponent(new ComponentStaticMesh(new Uri("RES", "cube.obj")));
+//		localLight.addComponent(new ComponentStaticMesh(new Uri("RES", "cube.obj")));
-		localLight.addComponent(new ComponentTexture(new Uri("RES", "grass.png")));
+//		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 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(
+//		localLight.addComponent(new ComponentRenderTexturedStaticMesh(
-				new Uri("DATA", "basic.vert"),
+//				new Uri("DATA", "basic.vert"),
-				new Uri("DATA", "basic.frag")));
+//				new Uri("DATA", "basic.frag")));
-		env.addEntity(localLight);
+//		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(2,-2,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);
+			// add a cube to test collision ...
-		localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(-2,-2,14))));
+			Entity localBox = new Entity(this.env);
-		localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj")));
+			localBox.addComponent(new ComponentPosition(new Transform3D(new Vector3f(-5,-5,14))));
-		localBox.addComponent(new ComponentTexture(new Uri("DATA", "blocks/clay.png")));
+			localBox.addComponent(new ComponentStaticMesh(new Uri("RES", "cube-one.obj")));
-		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 ComponentTexture(new Uri("DATA", "blocks/clay.png")));
-		localBox.addComponent(new ComponentRenderTexturedStaticMesh(
+			localBox.addComponent(new ComponentRenderTexturedStaticMesh(
-				new Uri("DATA", "basic.vert"),
+					new Uri("DATA", "basic.vert"),
-				new Uri("DATA", "basic.frag")));
+					new Uri("DATA", "basic.frag")));
-		ComponentPhysics physics2 = new ComponentPhysics(true);
+			ComponentPhysics physics2 = new ComponentPhysics(true);
-		PhysicBox box2 = new PhysicBox();
+			PhysicBox box2 = new PhysicBox();
-		box2.setSize(new Vector3f(1,1,1));
+			box2.setSize(new Vector3f(4,4,4));
-		box2.setOrigin(new Vector3f(0,0,0));
+			box2.setOrigin(new Vector3f(0,0,0));
-		box2.setMass(1);
+			box2.setMass(1);
-		physics2.addShape(box2);
+			physics2.addShape(box2);
-		localBox.addComponent(physics2);
+			localBox.addComponent(physics2);
-		env.addEntity(localBox);
+			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(5,-5,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(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))));
@@ -200,6 +321,29 @@ public class LoxelApplication extends Application {
 		OpenGL.enable(Flag.flag_depthTest);
 		
 		env.render(20, "default");
+		if (this.debugDrawProperty == null) {
+			debugDrawProperty = ResourceColored3DObject.create();
+		}
+		// now render the point test collision ...
+		for (int iii=0; iii<LoxelApplication.testPoints.size(); iii++) {
+			Vector3f elem = LoxelApplication.testPoints.get(iii);
+			boolean collide = LoxelApplication.testPointsCollide.get(iii);
+			if (collide == true) {
+				debugDrawProperty.drawSquare(new Vector3f(0.1f,0.1f,0.1f), Matrix4f.identity().multiply_new(Matrix4f.createMatrixTranslate(new Vector3f(elem.x,elem.y,elem.z+14))), new Color(1,0,0,1));
+			} else {
+				debugDrawProperty.drawSquare(new Vector3f(0.1f,0.1f,0.1f), Matrix4f.identity().multiply_new(Matrix4f.createMatrixTranslate(new Vector3f(elem.x,elem.y,elem.z+14))), new Color(1,1,1,1));
+			}
+		}
+		for (Vector3f elem : LoxelApplication.testPointsBox) {
+			debugDrawProperty.drawSquare(new Vector3f(0.1f,0.1f,0.1f), Matrix4f.identity().multiply_new(Matrix4f.createMatrixTranslate(new Vector3f(elem.x,elem.y,elem.z+14))), new Color(0,0,1,1));
+		}
+		
+		if (testRpos != null) {
+			//debugDrawProperty.drawSquare(box2HalfSize, new Matrix4f(testQTransfert.getMatrix()).multiply_new(Matrix4f.createMatrixTranslate(new Vector3f(testRpos.x,testRpos.y,testRpos.z+14))), new Color(0,1,0,0.5f));
+			debugDrawProperty.drawSquare(box2HalfSize, Matrix4f.createMatrixTranslate(new Vector3f(testRpos.x,testRpos.y,testRpos.z)).multiply(new Matrix4f(testQTransfert.getMatrix())).multiply(Matrix4f.createMatrixTranslate(new Vector3f(0,0,14))), new Color(0,1,0,0.5f));
+			debugDrawProperty.drawSquare(box1HalfSize, Matrix4f.identity().multiply_new(Matrix4f.createMatrixTranslate(new Vector3f(0,0,14))), new Color(0,0,1,0.5f));
+		}
+		
 		// Restore context of matrix
 		OpenGL.pop();
 	}