[DEV] better collision

.checkstyle

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<fileset-config file-format-version="1.2.0" simple-config="true" sync-formatter="false">
+  <fileset name="all" enabled="true" check-config-name="Scenarium" local="false">
+    <file-match-pattern match-pattern="." include-pattern="true"/>
+  </fileset>
+</fileset-config>

.project

@@ -10,8 +10,14 @@
 			<arguments>
 			</arguments>
 		</buildCommand>
+		<buildCommand>
+			<name>net.sf.eclipsecs.core.CheckstyleBuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
 	</buildSpec>
 	<natures>
 		<nature>org.eclipse.jdt.core.javanature</nature>
+		<nature>net.sf.eclipsecs.core.CheckstyleNature</nature>
 	</natures>
 </projectDescription>

src/guis/GuiRenderer.java

@@ -19,39 +19,40 @@ public class GuiRenderer {
 	private GuiShader shader;
 
 	public GuiRenderer(Loader loader) {
-		 float[] positions = {-1, 1, -1, -1, 1, 1, 1, -1};
+		float[] positions = { -1, 1, -1, -1, 1, 1, 1, -1 };
-		 quad = loader.loadToVAO(positions, 2);
+		quad = loader.loadToVAO(positions, 2);
-		 shader = new GuiShader();
+		shader = new GuiShader();
 	}
+
 	public void render(List<GuiTexture> guis) {
 		shader.start();
-		//System.out.println("Render Gui " + guis.size());
+		// System.out.println("Render Gui " + guis.size());
 		GL30.glBindVertexArray(quad.getVaoID());
 		GL20.glEnableVertexAttribArray(0);
-		
+
 		OpenGL.enable(OpenGL.Flag.flag_blend);
 		OpenGL.enable(OpenGL.Flag.flag_cullFace);
 		OpenGL.enable(OpenGL.Flag.flag_back);
-		for (GuiTexture gui: guis) {
+		for (GuiTexture gui : guis) {
 			GL13.glActiveTexture(GL13.GL_TEXTURE0);
 			GL11.glBindTexture(GL11.GL_TEXTURE_2D, gui.getTexture());
 			Matrix4f matrix = Maths.createTransformationMatrix(gui.getPosition(), gui.getScale());
 			shader.loadTransformation(matrix);
 			OpenGL.updateAllFlags();
 			GL11.glDrawArrays(GL11.GL_TRIANGLE_STRIP, 0, quad.getVertexCount());
-			
+
 		}
 		OpenGL.disable(OpenGL.Flag.flag_blend);
 		OpenGL.disable(OpenGL.Flag.flag_cullFace);
 		OpenGL.disable(OpenGL.Flag.flag_back);
 
-		GL20.glDisableVertexAttribArray(0);	
+		GL20.glDisableVertexAttribArray(0);
 		GL30.glBindVertexArray(0);
 		shader.stop();
 	}
-	
+
 	public void cleanUp() {
 		shader.cleanUp();
 	}
-	
+
 }

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

@@ -1,77 +1,56 @@
 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;
 	public float z;
 	public float w;
-	/**
+
-	 * @brief No initialization constructor (faster ...)
+	/** @brief No initialization constructor (faster ...) */
-	 */
 	public Quaternion() {
 		this.x = 0.0f;
 		this.y = 0.0f;
 		this.z = 0.0f;
 		this.w = 0.0f;
 	}
-	/*
+
-	void checkValues() {
+	/* void checkValues() { if ( isinf(this.x) == true || isnan(this.x) == true || isinf(this.y) == true || isnan(this.y) == true || isinf(this.z) == true || isnan(this.z) == true || isinf(this.w) ==
-		if (    isinf(this.x) == true
+	 * true || isnan(this.w) == true) { TKCRITICAL("         set transform: (" << this.x << "," << this.y << "," << this.z << "," << this.w << ")"); } } */
-		     || isnan(this.x) == true
+	/** @brief Constructor from scalars.
-		     || isinf(this.y) == true
-		     || isnan(this.y) == true
-		     || isinf(this.z) == true
-		     || isnan(this.z) == true
-		     || isinf(this.w) == true
-		     || isnan(this.w) == true) {
-			TKCRITICAL("         set transform: (" << this.x << "," << this.y << "," << this.z << "," << this.w << ")");
-		}
-	}
-	*/
-	/**
-	 * @brief Constructor from scalars.
 	 * @param xxx X value
 	 * @param yyy Y value
 	 * @param zzz Z value
-	 * @param www W value
+	 * @param www W value */
-	 */
 	public Quaternion(float xxx, float yyy, float zzz, float www) {
 		this.x = xxx;
 		this.y = yyy;
 		this.z = zzz;
 		this.w = www;
 	}
-	/**
+
-	 * @brief Constructor with the component w and a vector 3D.
+	/** @brief Constructor with the component w and a vector 3D.
 	 * @param www W value
-	 * @param vec 3D vector value
+	 * @param vec 3D vector value */
-	 */
 	public Quaternion(float www, Vector3f vec) {
 		this.x = vec.x;
 		this.y = vec.y;
 		this.z = vec.z;
 		this.w = www;
 	}
-	/**
+
-	 * @brief Constructor with Euler angles (in radians) to a quaternion
+	/** @brief Constructor with Euler angles (in radians) to a quaternion
-	 * @param eulerAngles list of all euleu angle
+	 * @param eulerAngles list of all euleu angle */
-	 */
 	public Quaternion(Vector3f eulerAngles) {
 		setEulerAngles(eulerAngles);
 	}
-	/**
+
-	 * @brief Create a unit quaternion from a rotation matrix
+	/** @brief Create a unit quaternion from a rotation matrix
-	 * @param matrix generic matrix
+	 * @param matrix generic matrix */
-	 */
 	public Quaternion(Matrix3f matrix) {
 		float trace = matrix.getTrace();
 		if (trace < 0.0f) {
 			if (matrix.mat[4] > matrix.mat[0]) {
-				if(matrix.mat[8] > matrix.mat[4]) {
+				if (matrix.mat[8] > matrix.mat[4]) {
 					float rrr = (float) Math.sqrt(matrix.mat[8] - matrix.mat[0] - matrix.mat[4] + 1.0f);
 					float sss = 0.5f / rrr;
 					this.x = (matrix.mat[6] + matrix.mat[2]) * sss;
@@ -110,17 +89,17 @@ public class Quaternion {
 			this.w = 0.5f * rrr;
 		}
 	}
+
 	public Quaternion(Quaternion obj) {
 		this.x = obj.x;
 		this.y = obj.y;
 		this.z = obj.z;
 		this.w = obj.w;
 	}
-	/**
+
-	 * @brief Add a vector to this one.
+	/** @brief Add a vector to this one.
 	 * @param[in] obj The vector to add to this one
-	 * @return Local reference of the vector
+	 * @return Local reference of the vector */
-	 */
 	public Quaternion add(Quaternion obj) {
 		this.x += obj.x;
 		this.y += obj.y;
@@ -128,22 +107,17 @@ public class Quaternion {
 		this.w += obj.w;
 		return this;
 	}
-	/**
+
-	 * @brief Add a vector to this one.
+	/** @brief Add a vector to this one.
 	 * @param[in] obj The vector to add to this one
-	 * @return New vector containing the value
+	 * @return New vector containing the value */
-	 */
 	public Quaternion add_new(Quaternion obj) {
-		return new Quaternion(this.x + obj.x,
+		return new Quaternion(this.x + obj.x, this.y + obj.y, this.z + obj.z, this.w + obj.w);
-		                      this.y + obj.y,
-		                      this.z + obj.z,
-		                      this.w + obj.w);
 	}
-	/**
+
-	 * @brief Subtract a vector from this one
+	/** @brief Subtract a vector from this one
 	 * @param obj The vector to subtract
-	 * @return Local reference of the vector
+	 * @return Local reference of the vector */
-	 */
 	public Quaternion less(Quaternion obj) {
 		this.x -= obj.x;
 		this.y -= obj.y;
@@ -151,22 +125,17 @@ public class Quaternion {
 		this.w -= obj.w;
 		return this;
 	}
-	/**
+
-	 * @brief Subtract a vector from this one
+	/** @brief Subtract a vector from this one
 	 * @param obj The vector to subtract
-	 * @return New quaternion containing the value
+	 * @return New quaternion containing the value */
-	 */
 	public Quaternion less_new(Quaternion obj) {
-		return new Quaternion(this.x - obj.x,
+		return new Quaternion(this.x - obj.x, this.y - obj.y, this.z - obj.z, this.w - obj.w);
-		                      this.y - obj.y,
-		                      this.z - obj.z,
-		                      this.w - obj.w);
 	}
-	/**
+
-	 * @brief Scale the quaternion
+	/** @brief Scale the quaternion
 	 * @param[in] val Scale factor
-	 * @return Local reference of the quaternion
+	 * @return Local reference of the quaternion */
-	 */
 	public Quaternion multiply(float val) {
 		this.x *= val;
 		this.y *= val;
@@ -174,22 +143,17 @@ public class Quaternion {
 		this.w *= val;
 		return this;
 	}
-	/**
+
-	 * @brief Scale the quaternion
+	/** @brief Scale the quaternion
 	 * @param[in] val Scale factor
-	 * @return New quaternion containing the value
+	 * @return New quaternion containing the value */
-	 */
 	public Quaternion multiply_new(float val) {
-		return new Quaternion(this.x * val,
+		return new Quaternion(this.x * val, this.y * val, this.z * val, this.w * val);
-		                      this.y * val,
-		                      this.z * val,
-		                      this.w * val);
 	}
-	/**
+
-	 * @brief Inversely scale the quaternion
+	/** @brief Inversely scale the quaternion
 	 * @param[in] val Scale factor to divide by.
-	 * @return Local reference of the quaternion
+	 * @return Local reference of the quaternion */
-	 */
 	public Quaternion devide(float val) {
 		if (val != 0) {
 			this.x /= val;
@@ -199,49 +163,38 @@ public class Quaternion {
 		}
 		return this;
 	}
-	/**
+
-	 * @brief Inversely scale the quaternion
+	/** @brief Inversely scale the quaternion
 	 * @param[in] val Scale factor to divide by.
-	 * @return New quaternion containing the value
+	 * @return New quaternion containing the value */
-	 */
 	public Quaternion devide_new(float val) {
 		if (val != 0) {
-			return new Quaternion(this.x / val,
+			return new Quaternion(this.x / val, this.y / val, this.z / val, this.w / val);
-			                      this.y / val,
-			                      this.z / val,
-			                      this.w / val);
 		}
 		return new Quaternion(this);
 	}
-	/**
+
-	 * @brief Return the dot product
+	/** @brief Return the dot product
 	 * @param obj The other quaternion in the dot product
-	 * @return Dot result value
+	 * @return Dot result value */
-	 */
 	public float dot(Quaternion obj) {
-		return   this.x * obj.x
+		return this.x * obj.x + this.y * obj.y + this.z * obj.z + this.w * obj.w;
-		       + this.y * obj.y
-		       + this.z * obj.z
-		       + this.w * obj.w;
 	}
-	/**
+
-	 * @brief Return the squared length of the quaternion.
+	/** @brief Return the squared length of the quaternion.
-	 * @return Squared length value.
+	 * @return Squared length value. */
-	 */
 	public float length2() {
 		return dot(this);
 	}
-	/**
+
-	 * @brief Return the length of the quaternion
+	/** @brief Return the length of the quaternion
-	 * @return Length value
+	 * @return Length value */
-	 */
 	public float length() {
 		return (float) Math.sqrt(length2());
 	}
-	/**
+
-	 * @brief Normalize this quaternion x^2 + y^2 + z^2 + w^2 = 1
+	/** @brief Normalize this quaternion x^2 + y^2 + z^2 + w^2 = 1
-	 * @return Local reference of the quaternion normalized
+	 * @return Local reference of the quaternion normalized */
-	 */
 	public Quaternion normalize() {
 		float invLength = 1.0f / length();
 		this.x *= invLength;
@@ -250,19 +203,17 @@ public class Quaternion {
 		this.w *= invLength;
 		return this;
 	}
-	/**
+
-	 * @brief Return a normalized version of this quaternion
+	/** @brief Return a normalized version of this quaternion
-	 * @return New quaternion containing the value
+	 * @return New quaternion containing the value */
-	 */
 	public Quaternion normalize_new() {
 		Quaternion tmp = new Quaternion(this);
 		tmp.normalize();
 		return tmp;
 	}
-	/**
+
-	 * @brief Normalize this quaternion x^2 + y^2 + z^2 + w^2 = 1
+	/** @brief Normalize this quaternion x^2 + y^2 + z^2 + w^2 = 1
-	 * @return Local reference of the quaternion normalized
+	 * @return Local reference of the quaternion normalized */
-	 */
 	public Quaternion safeNormalize() {
 		float lengthTmp = length();
 		if (lengthTmp == 0.0f) {
@@ -278,18 +229,16 @@ public class Quaternion {
 		this.w *= invLength;
 		return this;
 	}
-	/**
+
-	 * @brief Return a normalized version of this quaternion
+	/** @brief Return a normalized version of this quaternion
-	 * @return New quaternion containing the value
+	 * @return New quaternion containing the value */
-	 */
 	public Quaternion safeNormalize_new() {
 		Quaternion tmp = new Quaternion(this);
 		tmp.safeNormalize();
 		return tmp;
 	}
-	/**
+
-	 * @brief Set the absolute values of each element
+	/** @brief Set the absolute values of each element */
-	 */
 	public Quaternion absolute() {
 		this.x = Math.abs(this.x);
 		this.y = Math.abs(this.y);
@@ -297,105 +246,84 @@ public class Quaternion {
 		this.w = Math.abs(this.w);
 		return this;
 	}
-	/**
+
-	 * @brief Return a quaternion will the absolute values of each element
+	/** @brief Return a quaternion will the absolute values of each element
-	 * @return New quaternion with the absolute value
+	 * @return New quaternion with the absolute value */
-	 */
 	public Quaternion absolute_new() {
-		return new Quaternion( Math.abs(this.x),
+		return new Quaternion(Math.abs(this.x), Math.abs(this.y), Math.abs(this.z), Math.abs(this.w));
-				Math.abs(this.y),
-				Math.abs(this.z),
-				Math.abs(this.w));
 	}
-	/**
+
-	 * @brief Get X value
+	/** @brief Get X value
-	 * @return the x value
+	 * @return the x value */
-	 */
 	public float getX() {
 		return this.x;
 	}
-	/**
+
-	 * @brief Get Y value
+	/** @brief Get Y value
-	 * @return the y value
+	 * @return the y value */
-	 */
 	public float getY() {
 		return this.y;
 	}
-	/**
+
-	 * @brief Get Z value
+	/** @brief Get Z value
-	 * @return the z value
+	 * @return the z value */
-	 */
 	public float getZ() {
 		return this.z;
 	}
-	/**
+
-	 * @brief Get W value
+	/** @brief Get W value
-	 * @return the w value
+	 * @return the w value */
-	 */
 	public float getW() {
 		return this.w;
 	}
-	/**
+
-	 * @brief Set the x value
+	/** @brief Set the x value
-	 * @param[in] x New value
+	 * @param[in] x New value */
-	 */
 	public Quaternion setX(float x) {
 		this.x = x;
 		return this;
 	};
-	/**
+
-	 * @brief Set the y value
+	/** @brief Set the y value
-	 * @param[in] y New value
+	 * @param[in] y New value */
-	 */
 	public Quaternion setY(float y) {
-		this.y = y; 
+		this.y = y;
 		return this;
 	};
-	/**
+
-	 * @brief Set the z value
+	/** @brief Set the z value
-	 * @param[in] z New value
+	 * @param[in] z New value */
-	 */
 	public Quaternion setZ(float z) {
 		this.z = z;
 		return this;
 	};
-	/**
+
-	 * @brief Set the w value
+	/** @brief Set the w value
-	 * @param[in] w New value
+	 * @param[in] w New value */
-	 */
 	public Quaternion setW(float w) {
 		this.w = w;
 		return this;
 	}
-	/**
+
-	 * @brief Equality compare operator with an other object.
+	/** @brief Equality compare operator with an other object.
 	 * @param[in] obj Reference on the comparing object
 	 * @return true The Objects are identical
-	 * @return false The Objects are NOT identical
+	 * @return false The Objects are NOT identical */
-	 */
 	public boolean isEqual(Quaternion obj) {
-		return (    (this.w == obj.w)
+		return ((this.w == obj.w) && (this.z == obj.z) && (this.y == obj.y) && (this.x == obj.x));
-		         && (this.z == obj.z)
-		         && (this.y == obj.y)
-		         && (this.x == obj.x));
 	}
-	/**
+
-	 * @brief In-Equality compare operator with an other object.
+	/** @brief In-Equality compare operator with an other object.
 	 * @param[in] obj Reference on the comparing object
 	 * @return true The Objects are NOT identical
-	 * @return false The Objects are identical
+	 * @return false The Objects are identical */
-	 */
 	public boolean isDifferent(Quaternion obj) {
-		return (    (this.w != obj.w)
+		return ((this.w != obj.w) || (this.z != obj.z) || (this.y != obj.y) || (this.x != obj.x));
-		         || (this.z != obj.z)
-		         || (this.y != obj.y)
-		         || (this.x != obj.x));
 	}
-	/**
+
-	 * @brief Multiply this quaternion by the other.
+	/** @brief Multiply this quaternion by the other.
 	 * @param obj The other quaternion
-	 * @return Local reference of the quaternion
+	 * @return Local reference of the quaternion */
-	 */
 	public Quaternion multiply(Quaternion obj) {
 		Vector3f base = getVectorV();
 		Vector3f crossValue = base.cross(obj.getVectorV());
@@ -406,21 +334,19 @@ public class Quaternion {
 		safeNormalize();
 		return this;
 	}
-	/**
+
-	 * @brief Multiply this quaternion by the other.
+	/** @brief Multiply this quaternion by the other.
 	 * @param obj The other quaternion
-	 * @return New quaternion containing the value
+	 * @return New quaternion containing the value */
-	 */
 	public Quaternion multiply_new(Quaternion obj) {
 		Quaternion tmp = new Quaternion(this);
 		tmp.multiply(obj);
 		return tmp;
 	}
-	/**
+
-	 * @brief Operator* with a vector. This methods rotates a point given the rotation of a quaternion
+	/** @brief Operator* with a vector. This methods rotates a point given the rotation of a quaternion
-	 * @param[in] point Point to move
+	 * @param point Point to move
-	 * @return Point with the updated position
+	 * @return Point with the updated position */
-	 */
 	public Vector3f multiply(Vector3f point) {
 		Vector3f qvec = getVectorV();
 		Vector3f uv = qvec.cross(point);
@@ -429,85 +355,82 @@ public class Quaternion {
 		uuv.multiply(2.0f);
 		return uv.add(point).add(uuv);
 	}
-	/**
+
-	 * @brief Set each element to the max of the current values and the values of another Vector
+	public Vector3f multiply(float xxx, float yyy, float zzz) {
-	 * @param obj The other Vector to compare with
+		Vector3f point = new Vector3f(xxx, yyy, zzz);
-	 */
+		Vector3f qvec = getVectorV();
+		Vector3f uv = qvec.cross(point);
+		Vector3f uuv = qvec.cross(uv);
+		uv.multiply(2.0f * this.w);
+		uuv.multiply(2.0f);
+		return uv.add(point).add(uuv);
+	}
+
+	/** @brief Set each element to the max of the current values and the values of another Vector
+	 * @param obj The other Vector to compare with */
 	public void setMax(Quaternion obj) {
-		this.x = (float)Math.max(this.x, obj.x);
+		this.x = Math.max(this.x, obj.x);
-		this.y = (float)Math.max(this.y, obj.y);
+		this.y = Math.max(this.y, obj.y);
-		this.z = (float)Math.max(this.z, obj.z);
+		this.z = Math.max(this.z, obj.z);
-		this.w = (float)Math.max(this.w, obj.w);
+		this.w = Math.max(this.w, obj.w);
 	}
-	/**
+
-	 * @brief Set each element to the min of the current values and the values of another Vector
+	/** @brief Set each element to the min of the current values and the values of another Vector
-	 * @param obj The other Vector to compare with
+	 * @param obj The other Vector to compare with */
-	 */
 	public void setMin(Quaternion obj) {
-		this.x = (float)Math.min(this.x, obj.x);
+		this.x = Math.min(this.x, obj.x);
-		this.y = (float)Math.min(this.y, obj.y);
+		this.y = Math.min(this.y, obj.y);
-		this.z = (float)Math.min(this.z, obj.z);
+		this.z = Math.min(this.z, obj.z);
-		this.w = (float)Math.min(this.w, obj.w);
+		this.w = Math.min(this.w, obj.w);
 	}
-	/**
+
-	 * @brief Set Value on the quaternion
+	/** @brief Set Value on the quaternion
-	 * @param[in] xxx X value.
+	 * @param xxx X value.
-	 * @param[in] yyy Y value.
+	 * @param yyy Y value.
-	 * @param[in] zzz Z value.
+	 * @param zzz Z value.
-	 * @param[in] www W value.
+	 * @param www W value. */
-	 */
 	public void setValue(float xxx, float yyy, float zzz, float www) {
 		this.x = xxx;
 		this.y = yyy;
 		this.z = zzz;
 		this.w = www;
 	}
-	/**
+
-	 * @brief Set 0 value on all the quaternion
+	/** @brief Set 0 value on all the quaternion */
-	 */
 	public void setZero() {
-		setValue(0,0,0,0);
+		setValue(0, 0, 0, 0);
 	}
-	/**
+
-	 * @brief get a 0 value on all a quaternion
+	/** @brief get a 0 value on all a quaternion
-	 * @return a (float)Math. quaternion
+	 * @return a (float)Math. quaternion */
-	 */
 	public static Quaternion zero() {
-		return new Quaternion(0,0,0,0);
+		return new Quaternion(0, 0, 0, 0);
 	}
-	/**
+
-	 * @brief Check if the quaternion is equal to (0,0,0,0)
+	/** @brief Check if the quaternion is equal to (0,0,0,0)
 	 * @return true The value is equal to (0,0,0,0)
-	 * @return false The value is NOT equal to (0,0,0,0)
+	 * @return false The value is NOT equal to (0,0,0,0) */
-	 */
 	public boolean isZero() {
-		return    this.x == 0
+		return this.x == 0 && this.y == 0 && this.z == 0 && this.w == 0;
-		       && this.y == 0
-		       && this.z == 0
-		       && this.w == 0;
 	}
-	/**
+
-	 * @brief Set identity value at the quaternion
+	/** @brief Set identity value at the quaternion */
-	 */
 	public void setIdentity() {
-		setValue(0,0,0,1);
+		setValue(0, 0, 0, 1);
 	}
-	/**
+
-	 * @brief get an identity quaternion
+	/** @brief get an identity quaternion
-	 * @return an identity quaternion
+	 * @return an identity quaternion */
-	 */
 	public static Quaternion identity() {
-		return new Quaternion(0,0,0,1);
+		return new Quaternion(0, 0, 0, 1);
 	}
-	/**
+
-	 * @brief get x, y, z in a Vector3f
+	/** @brief get x, y, z in a Vector3f */
-	 */
 	public Vector3f getVectorV() {
 		return new Vector3f(this.x, this.y, this.z);
 	}
-	/**
+
-	 * @brief Inverse the quaternion
+	/** @brief Inverse the quaternion */
-	 */
 	public void inverse() {
 		float invLengthSquare = 1.0f / length2();
 		this.x *= -invLengthSquare;
@@ -515,159 +438,126 @@ public class Quaternion {
 		this.z *= -invLengthSquare;
 		this.w *= invLengthSquare;
 	}
-	/**
+
-	 * @brief Return the inverse of the quaternion
+	/** @brief Return the inverse of the quaternion
-	 * @return inverted quaternion
+	 * @return inverted quaternion */
-	 */
 	public Quaternion inverse_new() {
 		Quaternion tmp = new Quaternion(this);
 		tmp.inverse();
 		return tmp;
 	}
-	/**
+
-	 * @brief Return the unit quaternion
+	/** @brief Return the unit quaternion
-	 * @return Quaternion unitarised
+	 * @return Quaternion unitarised */
-	 */
 	public Quaternion getUnit() {
 		return normalize_new();
 	}
-	/**
+
-	 * @brief Conjugate the quaternion
+	/** @brief Conjugate the quaternion */
-	 */
 	public void conjugate() {
 		this.x *= -1.0f;
 		this.y *= -1.0f;
 		this.z *= -1.0f;
 	}
-	/**
+
-	 * @brief Return the conjugate of the quaternion
+	/** @brief Return the conjugate of the quaternion
-	 * @return Conjugate quaternion
+	 * @return Conjugate quaternion */
-	 */
 	public Quaternion conjugate_new() {
 		Quaternion tmp = new Quaternion(this);
 		tmp.conjugate();
 		return tmp;
 	}
-	/**
+
-	 * @brief Compute the rotation angle (in radians) and the rotation axis
+	/** @brief Compute the rotation angle (in radians) and the rotation axis
 	 * @param[out] angle Angle of the quaternion
-	 * @param[out] axis Axis of the quaternion
+	 * @param[out] axis Axis of the quaternion */
-	 */
 	public void getAngleAxis(float angle, Vector3f axis) {
 		Quaternion quaternion = getUnit();
-		angle = (float)Math.acos(quaternion.w) * 2.0f;
+		angle = (float) Math.acos(quaternion.w) * 2.0f;
 		Vector3f rotationAxis = new Vector3f(quaternion.x, quaternion.y, quaternion.z);
 		rotationAxis = rotationAxis.normalize_new();
 		axis.setValue(rotationAxis.x, rotationAxis.y, rotationAxis.z);
 	}
-	/**
+
-	 * @brief Get the orientation matrix corresponding to this quaternion
+	/** @brief Get the orientation matrix corresponding to this quaternion
-	 * @return the 3x3 transformation matrix
+	 * @return the 3x3 transformation matrix */
-	 */
 	public Matrix3f getMatrix() {
-		float nQ =   this.x*this.x
+		float nQ = this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w;
-		           + this.y*this.y
-		           + this.z*this.z
-		           + this.w*this.w;
 		float sss = 0.0f;
 		if (nQ > 0.0f) {
 			sss = 2.0f / nQ;
 		}
-		float xs = this.x*sss;
+		float xs = this.x * sss;
-		float ys = this.y*sss;
+		float ys = this.y * sss;
-		float zs = this.z*sss;
+		float zs = this.z * sss;
-		float wxs = this.w*xs;
+		float wxs = this.w * xs;
-		float wys = this.w*ys;
+		float wys = this.w * ys;
-		float wzs = this.w*zs;
+		float wzs = this.w * zs;
-		float xxs = this.x*xs;
+		float xxs = this.x * xs;
-		float xys = this.x*ys;
+		float xys = this.x * ys;
-		float xzs = this.x*zs;
+		float xzs = this.x * zs;
-		float yys = this.y*ys;
+		float yys = this.y * ys;
-		float yzs = this.y*zs;
+		float yzs = this.y * zs;
-		float zzs = this.z*zs;
+		float zzs = this.z * zs;
-		return new Matrix3f(1.0f - yys - zzs,
+		return new Matrix3f(1.0f - yys - zzs, xys - wzs, xzs + wys, xys + wzs, 1.0f - xxs - zzs, yzs - wxs, xzs - wys, yzs + wxs, 1.0f - xxs - yys);
-		                      xys - wzs,
-		                      xzs + wys,
-		                      xys + wzs,
-		                      1.0f - xxs - zzs,
-		                      yzs - wxs,
-		                      xzs - wys,
-		                      yzs + wxs,
-		                      1.0f - xxs - yys);
 	}
+
 	public Matrix4f getMatrix4() {
 
-		float nQ =   this.x*this.x
+		float nQ = this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w;
-		           + this.y*this.y
-		           + this.z*this.z
-		           + this.w*this.w;
 		float sss = 0.0f;
 		if (nQ > 0.0f) {
 			sss = 2.0f / nQ;
 		}
-		float xs = this.x*sss;
+		float xs = this.x * sss;
-		float ys = this.y*sss;
+		float ys = this.y * sss;
-		float zs = this.z*sss;
+		float zs = this.z * sss;
-		float wxs = this.w*xs;
+		float wxs = this.w * xs;
-		float wys = this.w*ys;
+		float wys = this.w * ys;
-		float wzs = this.w*zs;
+		float wzs = this.w * zs;
-		float xxs = this.x*xs;
+		float xxs = this.x * xs;
-		float xys = this.x*ys;
+		float xys = this.x * ys;
-		float xzs = this.x*zs;
+		float xzs = this.x * zs;
-		float yys = this.y*ys;
+		float yys = this.y * ys;
-		float yzs = this.y*zs;
+		float yzs = this.y * zs;
-		float zzs = this.z*zs;
+		float zzs = this.z * zs;
-		return new Matrix4f(1.0f - yys - zzs,
+		return new Matrix4f(1.0f - yys - zzs, xys - wzs, xzs + wys, 0, xys + wzs, 1.0f - xxs - zzs, yzs - wxs, 0, xzs - wys, yzs + wxs, 1.0f - xxs - yys, 0, 0, 0, 0, 1);
-		                      xys - wzs,
-		                      xzs + wys,
-		                      0,
-		                      xys + wzs,
-		                      1.0f - xxs - zzs,
-		                      yzs - wxs,
-		                      0,
-		                      xzs - wys,
-		                      yzs + wxs,
-		                      1.0f - xxs - yys,
-		                      0,
-		                      0,0,0,1);
 	}
-	/**
+
-	 * @brief Compute the spherical linear interpolation between two quaternions.
+	/** @brief Compute the spherical linear interpolation between two quaternions.
 	 * @param[in] obj1 First quaternion
 	 * @param[in] obj2 Second quaternion
-	 * @param[in] ttt linar coefficient interpolation to be such that [0..1]
+	 * @param[in] ttt linar coefficient interpolation to be such that [0..1] */
-	 */
 	public static Quaternion slerp(Quaternion obj1, Quaternion obj2, float ttt) {
-		//TKASSERT(ttt >= 0.0f  ttt <= 1.0f, "wrong intermolation");
+		// TKASSERT(ttt >= 0.0f ttt <= 1.0f, "wrong intermolation");
 		float invert = 1.0f;
 		float cosineTheta = obj1.dot(obj2);
 		if (cosineTheta < 0.0f) {
 			cosineTheta = -cosineTheta;
 			invert = -1.0f;
 		}
-		if(1-cosineTheta < 0.00001f) {
+		if (1 - cosineTheta < 0.00001f) {
-			return obj1.multiply_new(1.0f-ttt).add(obj2.multiply_new(ttt * invert));
+			return obj1.multiply_new(1.0f - ttt).add(obj2.multiply_new(ttt * invert));
 		}
-		float theta = (float)Math.acos(cosineTheta);
+		float theta = (float) Math.acos(cosineTheta);
-		float sineTheta = (float)Math.sin(theta);
+		float sineTheta = (float) Math.sin(theta);
-		float coeff1 = (float)Math.sin((1.0f-ttt)*theta) / sineTheta;
+		float coeff1 = (float) Math.sin((1.0f - ttt) * theta) / sineTheta;
-		float coeff2 = (float)Math.sin(ttt*theta) / sineTheta * invert;
+		float coeff2 = (float) Math.sin(ttt * theta) / sineTheta * invert;
 		return obj1.multiply_new(coeff1).add(obj2.multiply_new(coeff2));
 	}
-	/**
+
-	 * @brief Configure the quaternion with euler angles.
+	/** @brief Configure the quaternion with euler angles.
-	 * @param[out] angles Eular angle of the quaternion.
+	 * @param[out] angles Eular angle of the quaternion. */
-	 */
 	public void setEulerAngles(Vector3f angles) {
 		float angle = angles.x * 0.5f;
-		float sinX = (float)Math.sin(angle);
+		float sinX = (float) Math.sin(angle);
-		float cosX = (float)Math.cos(angle);
+		float cosX = (float) Math.cos(angle);
 		angle = angles.y * 0.5f;
-		float sinY = (float)Math.sin(angle);
+		float sinY = (float) Math.sin(angle);
-		float cosY = (float)Math.cos(angle);
+		float cosY = (float) Math.cos(angle);
 		angle = angles.z * 0.5f;
-		float sinZ = (float)Math.sin(angle);
+		float sinZ = (float) Math.sin(angle);
-		float cosZ = (float)Math.cos(angle);
+		float cosZ = (float) Math.cos(angle);
 		float cosYcosZ = cosY * cosZ;
 		float sinYcosZ = sinY * cosZ;
 		float cosYsinZ = cosY * sinZ;
@@ -678,21 +568,22 @@ public class Quaternion {
 		this.w = cosX * cosYcosZ + sinX * sinYsinZ;
 		normalize();
 	}
-	/**
+
-	 * @brief Clone the current Quaternion.
+	/** @brief Clone the current Quaternion.
-	 * @return New Quaternion containing the value
+	 * @return New Quaternion containing the value */
-	 */
+	@Override
 	public Quaternion clone() {
 		return new Quaternion(this);
 	}
+
 	@Override
 	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);
+		// Log.info("diff " + a + " " + b);
 		Quaternion inv = a.inverse_new();
 		return inv.multiply(b);
 	}

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

@@ -0,0 +1,14 @@
+package org.atriaSoft.gameEngine.physics;
+
+import org.atriaSoft.etk.math.Vector3f;
+
+public class ColisionPoints {
+	public Vector3f position;
+	public Vector3f force;
+
+	public ColisionPoints(Vector3f position, Vector3f force) {
+		super();
+		this.position = position;
+		this.force = force;
+	}
+}

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

@@ -2,71 +2,83 @@ package org.atriaSoft.gameEngine.physics;
 
 import org.atriaSoft.etk.Color;
 import org.atriaSoft.etk.math.Matrix4f;
-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.gameEngine.sample.LoxelEngine.LoxelApplication;
 
 public class PhysicBox extends PhysicShape {
 	// Box size property in X, Y and Z
-	private Vector3f size = new Vector3f(1,1,1);
+	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;
 		// 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))));
-		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 narrowPhaseAxisX = new Vector3f(1, 0, 0);
-	public Vector3f narrowPhaseAxisY = new Vector3f(1,0,0);
+	public Vector3f narrowPhaseAxisY = new Vector3f(1, 0, 0);
-	public Vector3f narrowPhaseAxisZ = new Vector3f(1,0,0);
+	public Vector3f narrowPhaseAxisZ = new Vector3f(1, 0, 0);
 	public Vector3f narrowPhaseHalfSize;
+
 	@Override
 	public void updateForNarrowCollision(Transform3D transformGlobal) {
-		this.narrowPhaseGlobalPos = transformGlobal.multiply(this.transform.multiply(new Vector3f(0,0,0)));
+		this.narrowPhaseGlobalPos = transformGlobal.multiply(this.transform.multiply(new Vector3f(0, 0, 0)));
-		this.narrowPhaseAxisX = transformGlobal.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 = transformGlobal.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 = transformGlobal.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);
 	}
 
 	private void renderPoint(Vector3f subPosition, Transform3D transformGlobal, ResourceColored3DObject debugDrawProperty) {
 		Matrix4f transformation = transformGlobal.getOpenGLMatrix().multiply_new(this.transform.getOpenGLMatrix()).multiply(Matrix4f.createMatrixTranslate(subPosition));
-		
+
-		debugDrawProperty.drawSquare(new Vector3f(0.1f,0.1f,0.1f), transformation, new Color(0,0,1,1));
+		debugDrawProperty.drawSquare(new Vector3f(0.1f, 0.1f, 0.1f), transformation, new Color(0, 0, 1, 1));
 	}
+
 	private void renderPoint2(Vector3f subPosition, Transform3D transformGlobal, ResourceColored3DObject debugDrawProperty) {
 		Matrix4f transformation = transformGlobal.getOpenGLMatrix().multiply_new(this.transform.getOpenGLMatrix()).multiply(Matrix4f.createMatrixTranslate(subPosition));
-		debugDrawProperty.drawSquare(new Vector3f(0.05f,0.05f,0.05f), transformation, new Color(0,1,0,1));
+		debugDrawProperty.drawSquare(new Vector3f(0.05f, 0.05f, 0.05f), transformation, new Color(0, 1, 0, 1));
-		
+
 	}
+
 	private void renderPoint3(Vector3f subPosition, Transform3D transformGlobal, ResourceColored3DObject debugDrawProperty) {
 		Matrix4f transformation = transformGlobal.getOpenGLMatrix().multiply_new(this.transform.getOpenGLMatrix()).multiply(Matrix4f.createMatrixTranslate(subPosition));
-		debugDrawProperty.drawSquare(new Vector3f(0.05f,0.05f,0.05f), transformation, new Color(1,1,0,1));
+		debugDrawProperty.drawSquare(new Vector3f(0.05f, 0.05f, 0.05f), transformation, new Color(1, 1, 0, 1));
-		
+
 	}
+
+	private void renderPoint4(Vector3f subPosition, ResourceColored3DObject debugDrawProperty) {
+		debugDrawProperty.drawSquare(new Vector3f(0.1f, 0.1f, 0.1f), Matrix4f.identity().multiply(Matrix4f.createMatrixTranslate(subPosition)), new Color(1, 0, 0, 1));
+
+	}
+
 	@Override
 	public void renderDebug(Transform3D transformGlobal, ResourceColored3DObject debugDrawProperty) {
-		debugDrawProperty.drawSquare(this.size.multiply_new(0.5f), this.transform.getOpenGLMatrix().multiply_new(transformGlobal.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));
 		Vector3f dimention = this.size.multiply_new(0.5f);
 		renderPoint2(new Vector3f(+dimention.x, +dimention.y, +dimention.z), transformGlobal, debugDrawProperty);
 		renderPoint(new Vector3f(-dimention.x, +dimention.y, +dimention.z), transformGlobal, debugDrawProperty);
@@ -76,5 +88,10 @@ public class PhysicBox extends PhysicShape {
 		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);
+		if (this.colisionPointTest != null) {
+			for (int iii = 0; iii < this.colisionPointTest.length; iii++) {
+				renderPoint4(this.colisionPointTest[iii].position, debugDrawProperty);
+			}
+		}
 	}
 }

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

@@ -6,8 +6,9 @@ import org.atriaSoft.etk.math.Vector3f;
 import org.atriaSoft.gale.resource.ResourceColored3DObject;
 
 public abstract class PhysicShape {
-//	protected Quaternion quaternion;
+	public ColisionPoints[] colisionPointTest;
-//	protected Vector3f origin;
+	// protected Quaternion quaternion;
+	// protected Vector3f origin;
 	protected Transform3D transform;
 	protected Transform3D transformGlobal;
 	protected float mass = 0;
@@ -16,55 +17,71 @@ public abstract class PhysicShape {
 	public PhysicShape(PhysicShapeType type) {
 		this.type = type;
 		this.transform = new Transform3D();
-//		this.quaternion = Quaternion.identity();
+		// this.quaternion = Quaternion.identity();
-//		this.origin = Vector3f.zero();
+		// this.origin = Vector3f.zero();
 		this.mass = 0;
 	}
+
 	public PhysicShape(PhysicShapeType type, Quaternion quaternion, Vector3f origin, float mass) {
 		this.type = type;
 		this.transform = new Transform3D(origin, quaternion);
-//		this.quaternion = quaternion;
+		// this.quaternion = quaternion;
-//		this.origin = origin;
+		// this.origin = origin;
 		this.mass = mass;
 	}
+
 	public Quaternion getQuaternionFull() {
 		return transformGlobal.getOrientation().multiply_new(transform.getOrientation());
 	}
+
 	public Quaternion getQuaternion() {
 		return transform.getOrientation();
 	}
+
 	public void setQuaternion(Quaternion quaternion) {
 		this.transform.setOrientation(quaternion);
 	}
+
 	public Vector3f getOrigin() {
 		return this.transform.getPosition();
 	}
+
 	public void setOrigin(Vector3f origin) {
 		this.transform.setPosition(origin);
 	}
+
 	public Transform3D getTransform() {
 		return transform;
 	}
+
 	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;
 	}
+
 	public void setMass(float mass) {
 		this.mass = mass;
 	}
+
 	public PhysicShapeType getType() {
 		return type;
 	}
+
 	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/gameEngine/physics/ToolCollisionOBBWithOBB.java

@@ -1,222 +1,182 @@
 package org.atriaSoft.gameEngine.physics;
 
-import org.atriaSoft.etk.math.Matrix4f;
 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
+class OBB {
-{
+	public Vector3f position;
-	public Vector3f Pos, AxisX, AxisY, AxisZ, Half_size;
+	public Vector3f axisX;
+	public Vector3f axisY;
+	public Vector3f axisZ;
+	public Vector3f halfSize;
+
 	public OBB() {
-		Pos = new Vector3f();
+		position = new Vector3f();
-		AxisX = new Vector3f();
+		axisX = new Vector3f();
-		AxisY = new Vector3f();
+		axisY = new Vector3f();
-		AxisZ = new Vector3f();
+		axisZ = new Vector3f();
-		Half_size = new Vector3f();
+		halfSize = new Vector3f();
 	}
 };
 
 public class ToolCollisionOBBWithOBB {
+	private ToolCollisionOBBWithOBB() {}
+
 	// check if there's a separating plane in between the selected axes
-	private static boolean getSeparatingPlane(Vector3f RPos, Vector3f Plane, OBB box1, OBB box2) {
+	private static boolean getSeparatingPlane(Vector3f rPos, Vector3f plane, OBB box1, OBB box2) {
-		return (Math.abs(RPos.dot(Plane)) > 
+		return (Math.abs(rPos.dot(plane)) > (Math.abs(box1.axisX.multiply_new(box1.halfSize.x).dot(plane)) + Math.abs(box1.axisY.multiply_new(box1.halfSize.y).dot(plane))
-			(Math.abs(box1.AxisX.multiply_new(box1.Half_size.x).dot(Plane)) +
+				+ Math.abs(box1.axisZ.multiply_new(box1.halfSize.z).dot(plane)) + Math.abs(box2.axisX.multiply_new(box2.halfSize.x).dot(plane))
-				Math.abs(box1.AxisY.multiply_new(box1.Half_size.y).dot(Plane)) +
+				+ Math.abs(box2.axisY.multiply_new(box2.halfSize.y).dot(plane)) + Math.abs(box2.axisZ.multiply_new(box2.halfSize.z).dot(plane))));
-				Math.abs(box1.AxisZ.multiply_new(box1.Half_size.z).dot(Plane)) +
-				Math.abs(box2.AxisX.multiply_new(box2.Half_size.x).dot(Plane)) + 
-				Math.abs(box2.AxisY.multiply_new(box2.Half_size.y).dot(Plane)) +
-				Math.abs(box2.AxisZ.multiply_new(box2.Half_size.z).dot(Plane))
-				)
-			);
 	}
 
 	// test for separating planes in all 15 axes
 	private static boolean getCollision(OBB box1, OBB box2) {
-		Vector3f RPos = box2.Pos.less_new(box1.Pos);
+		Vector3f rPos = box2.position.less_new(box1.position);
-		boolean ret = 
+		boolean ret = getSeparatingPlane(rPos, box1.axisX, box1, box2) || getSeparatingPlane(rPos, box1.axisY, box1, box2) || getSeparatingPlane(rPos, box1.axisZ, box1, box2)
-				getSeparatingPlane(RPos, box1.AxisX, box1, box2) ||
+				|| getSeparatingPlane(rPos, box2.axisX, box1, box2) || getSeparatingPlane(rPos, box2.axisY, box1, box2) || getSeparatingPlane(rPos, box2.axisZ, box1, box2)
-				getSeparatingPlane(RPos, box1.AxisY, box1, box2) ||
+				|| getSeparatingPlane(rPos, box1.axisX.cross(box2.axisX), box1, box2) || getSeparatingPlane(rPos, box1.axisX.cross(box2.axisY), box1, box2)
-				getSeparatingPlane(RPos, box1.AxisZ, box1, box2) ||
+				|| getSeparatingPlane(rPos, box1.axisX.cross(box2.axisZ), box1, box2) || getSeparatingPlane(rPos, box1.axisY.cross(box2.axisX), box1, box2)
-				getSeparatingPlane(RPos, box2.AxisX, box1, box2) ||
+				|| getSeparatingPlane(rPos, box1.axisY.cross(box2.axisY), box1, box2) || getSeparatingPlane(rPos, box1.axisY.cross(box2.axisZ), box1, box2)
-				getSeparatingPlane(RPos, box2.AxisY, box1, box2) ||
+				|| getSeparatingPlane(rPos, box1.axisZ.cross(box2.axisX), box1, box2) || getSeparatingPlane(rPos, box1.axisZ.cross(box2.axisY), box1, box2)
-				getSeparatingPlane(RPos, box2.AxisZ, box1, box2) ||
+				|| getSeparatingPlane(rPos, box1.axisZ.cross(box2.axisZ), box1, box2);
-				getSeparatingPlane(RPos, box1.AxisX.cross(box2.AxisX), box1, box2) ||
+		return !ret;
-				getSeparatingPlane(RPos, box1.AxisX.cross(box2.AxisY), box1, box2) ||
-				getSeparatingPlane(RPos, box1.AxisX.cross(box2.AxisZ), box1, box2) ||
-				getSeparatingPlane(RPos, box1.AxisY.cross(box2.AxisX), box1, box2) ||
-				getSeparatingPlane(RPos, box1.AxisY.cross(box2.AxisY), box1, box2) ||
-				getSeparatingPlane(RPos, box1.AxisY.cross(box2.AxisZ), box1, box2) ||
-				getSeparatingPlane(RPos, box1.AxisZ.cross(box2.AxisX), box1, box2) ||
-				getSeparatingPlane(RPos, box1.AxisZ.cross(box2.AxisY), box1, box2) ||
-				getSeparatingPlane(RPos, box1.AxisZ.cross(box2.AxisZ), box1, box2);
-		return ! ret;
 	}
 
 	// a quick test to see the code working
 	public static void main(String[] args) {
 		// create two obbs
-		OBB A = new OBB();
+		OBB aaa = new OBB();
-		OBB B = new OBB();
+		OBB bbb = new OBB();
-		
+
 		// set the first obb's properties
-		A.Pos = new Vector3f(0.0f, 0.0f, 0.0f); // set its center position
+		aaa.position = new Vector3f(0.0f, 0.0f, 0.0f); // set its center position
-		
+
 		// set the half size
-		A.Half_size = new Vector3f(10.0f, 1.0f, 1.0f);
+		aaa.halfSize = new Vector3f(10.0f, 1.0f, 1.0f);
-		
+
 		// set the axes orientation
-		A.AxisX = new Vector3f(1.0f, 0.0f, 0.0f);
+		aaa.axisX = new Vector3f(1.0f, 0.0f, 0.0f);
-		A.AxisY = new Vector3f(0.0f, 1.0f, 0.0f);
+		aaa.axisY = new Vector3f(0.0f, 1.0f, 0.0f);
-		A.AxisZ = new Vector3f(0.0f, 0.0f, 1.0f);
+		aaa.axisZ = new Vector3f(0.0f, 0.0f, 1.0f);
-		
+
 		// set the second obb's properties
-		B.Pos = new Vector3f(20.0f, 0.0f, 0.0f); // set its center position
+		bbb.position = new Vector3f(20.0f, 0.0f, 0.0f); // set its center position
-		
+
 		// set the half size
-		B.Half_size = new Vector3f(10.0f, 1.0f, 1.0f);
+		bbb.halfSize = new Vector3f(10.0f, 1.0f, 1.0f);
-		
+
 		// set the axes orientation
-		B.AxisX = new Vector3f(1.0f, 0.0f, 0.0f);
+		bbb.axisX = new Vector3f(1.0f, 0.0f, 0.0f);
-		B.AxisY = new Vector3f(0.0f, 1.0f, 0.0f);
+		bbb.axisY = new Vector3f(0.0f, 1.0f, 0.0f);
-		B.AxisZ = new Vector3f(0.0f, 0.0f, 1.0f);
+		bbb.axisZ = new Vector3f(0.0f, 0.0f, 1.0f);
-		
+
 		// run the code and get the result as a message
-		if (getCollision(A, B) == true) {
+		if (getCollision(aaa, bbb)) {
 			Log.info("Collision!!!");
 		} else {
 			Log.info("NO Collision!!!");
 		}
 	}
-	
+
-	
 	// check if there's a separating plane in between the selected axes
-	private static boolean getSeparatingPlane222(Vector3f RPos, Vector3f Plane, PhysicBox box1, PhysicBox box2) {
+	private static boolean getSeparatingPlane222(Vector3f rPos, Vector3f plane, PhysicBox box1, PhysicBox box2) {
-		return (Math.abs(RPos.dot(Plane)) > 
+		return (Math.abs(rPos.dot(plane)) > (Math.abs(box1.narrowPhaseAxisX.multiply_new(box1.narrowPhaseHalfSize.x).dot(plane))
-			(Math.abs(box1.narrowPhaseAxisX.multiply_new(box1.narrowPhaseHalfSize.x).dot(Plane)) +
+				+ Math.abs(box1.narrowPhaseAxisY.multiply_new(box1.narrowPhaseHalfSize.y).dot(plane)) + Math.abs(box1.narrowPhaseAxisZ.multiply_new(box1.narrowPhaseHalfSize.z).dot(plane))
-				Math.abs(box1.narrowPhaseAxisY.multiply_new(box1.narrowPhaseHalfSize.y).dot(Plane)) +
+				+ Math.abs(box2.narrowPhaseAxisX.multiply_new(box2.narrowPhaseHalfSize.x).dot(plane)) + Math.abs(box2.narrowPhaseAxisY.multiply_new(box2.narrowPhaseHalfSize.y).dot(plane))
-				Math.abs(box1.narrowPhaseAxisZ.multiply_new(box1.narrowPhaseHalfSize.z).dot(Plane)) +
+				+ Math.abs(box2.narrowPhaseAxisZ.multiply_new(box2.narrowPhaseHalfSize.z).dot(plane))));
-				Math.abs(box2.narrowPhaseAxisX.multiply_new(box2.narrowPhaseHalfSize.x).dot(Plane)) + 
-				Math.abs(box2.narrowPhaseAxisY.multiply_new(box2.narrowPhaseHalfSize.y).dot(Plane)) +
-				Math.abs(box2.narrowPhaseAxisZ.multiply_new(box2.narrowPhaseHalfSize.z).dot(Plane))
-				)
-			);
 	}
 
 	public static boolean testCollide(PhysicBox box1, PhysicBox box2) {
 
-		Vector3f RPos = box2.narrowPhaseGlobalPos.less_new(box1.narrowPhaseGlobalPos);
+		Vector3f rPos = box2.narrowPhaseGlobalPos.less_new(box1.narrowPhaseGlobalPos);
-		boolean ret = 
+		boolean ret = getSeparatingPlane222(rPos, box1.narrowPhaseAxisX, box1, box2) || getSeparatingPlane222(rPos, box1.narrowPhaseAxisY, box1, box2)
-				getSeparatingPlane222(RPos, box1.narrowPhaseAxisX, box1, box2) ||
+				|| getSeparatingPlane222(rPos, box1.narrowPhaseAxisZ, box1, box2) || getSeparatingPlane222(rPos, box2.narrowPhaseAxisX, box1, box2)
-				getSeparatingPlane222(RPos, box1.narrowPhaseAxisY, box1, box2) ||
+				|| getSeparatingPlane222(rPos, box2.narrowPhaseAxisY, box1, box2) || getSeparatingPlane222(rPos, box2.narrowPhaseAxisZ, box1, box2)
-				getSeparatingPlane222(RPos, box1.narrowPhaseAxisZ, box1, box2) ||
+				|| getSeparatingPlane222(rPos, box1.narrowPhaseAxisX.cross(box2.narrowPhaseAxisX), box1, box2)
-				getSeparatingPlane222(RPos, box2.narrowPhaseAxisX, box1, box2) ||
+				|| getSeparatingPlane222(rPos, box1.narrowPhaseAxisX.cross(box2.narrowPhaseAxisY), box1, box2)
-				getSeparatingPlane222(RPos, box2.narrowPhaseAxisY, box1, box2) ||
+				|| getSeparatingPlane222(rPos, box1.narrowPhaseAxisX.cross(box2.narrowPhaseAxisZ), box1, box2)
-				getSeparatingPlane222(RPos, box2.narrowPhaseAxisZ, box1, box2) ||
+				|| getSeparatingPlane222(rPos, box1.narrowPhaseAxisY.cross(box2.narrowPhaseAxisX), box1, box2)
-				getSeparatingPlane222(RPos, box1.narrowPhaseAxisX.cross(box2.narrowPhaseAxisX), box1, box2) ||
+				|| getSeparatingPlane222(rPos, box1.narrowPhaseAxisY.cross(box2.narrowPhaseAxisY), box1, box2)
-				getSeparatingPlane222(RPos, box1.narrowPhaseAxisX.cross(box2.narrowPhaseAxisY), box1, box2) ||
+				|| getSeparatingPlane222(rPos, box1.narrowPhaseAxisY.cross(box2.narrowPhaseAxisZ), box1, box2)
-				getSeparatingPlane222(RPos, box1.narrowPhaseAxisX.cross(box2.narrowPhaseAxisZ), box1, box2) ||
+				|| getSeparatingPlane222(rPos, box1.narrowPhaseAxisZ.cross(box2.narrowPhaseAxisX), box1, box2)
-				getSeparatingPlane222(RPos, box1.narrowPhaseAxisY.cross(box2.narrowPhaseAxisX), box1, box2) ||
+				|| getSeparatingPlane222(rPos, box1.narrowPhaseAxisZ.cross(box2.narrowPhaseAxisY), box1, box2)
-				getSeparatingPlane222(RPos, box1.narrowPhaseAxisY.cross(box2.narrowPhaseAxisY), box1, box2) ||
+				|| getSeparatingPlane222(rPos, box1.narrowPhaseAxisZ.cross(box2.narrowPhaseAxisZ), box1, box2);
-				getSeparatingPlane222(RPos, box1.narrowPhaseAxisY.cross(box2.narrowPhaseAxisZ), box1, box2) ||
+		return !ret;
-				getSeparatingPlane222(RPos, box1.narrowPhaseAxisZ.cross(box2.narrowPhaseAxisX), box1, box2) ||
-				getSeparatingPlane222(RPos, box1.narrowPhaseAxisZ.cross(box2.narrowPhaseAxisY), box1, box2) ||
-				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 ........");
+		// Log.info("Try to calculare reverse force ........");
-		Vector3f RPos1 = box1.narrowPhaseGlobalPos.less_new(box2.narrowPhaseGlobalPos);
+		Vector3f rPos1 = box1.narrowPhaseGlobalPos.less_new(box2.narrowPhaseGlobalPos);
-		Vector3f RPos2 = box2.narrowPhaseGlobalPos.less_new(box1.narrowPhaseGlobalPos);
+		Vector3f rPos2 = 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 quatTransfer1 = Quaternion.diff(quat1, quat2);
 		Quaternion quatTransfer2 = Quaternion.diff(quat2, quat1);
-		//quatTransfer.normalize();
+		// quatTransfer.normalize();
 
-		//LoxelApplication.relativeTest = quatTransfer;
+		// LoxelApplication.relativeTest = quatTransfer;
-		//Vector3f tmp = RPos.add_new(new Vector3f(0,0,14));
+		// Vector3f tmp = rPos.add_new(new Vector3f(0,0,14));
-//		LoxelApplication.relativeTestPos.getTransform().setPosition(tmp);
+		// LoxelApplication.relativeTestPos.getTransform().setPosition(tmp);
-//		LoxelApplication.relativeTestPos.getTransform().setOrientation(quatTransfer);
+		// LoxelApplication.relativeTestPos.getTransform().setOrientation(quatTransfer);
-//		LoxelApplication.boxTest.setSize(box1.getSize());
+		// LoxelApplication.boxTest.setSize(box1.getSize());
-		//Log.info("" + RPos + quatTransfer1);
+		// Log.info("" + rPos + quatTransfer1);
-//		/*res = */getCollidePointsAABBCenteredWithOBB(box1.narrowPhaseHalfSize, box2.narrowPhaseHalfSize, quatTransfer, RPos);
+		// /*res = */getCollidePointsAABBCenteredWithOBB(box1.narrowPhaseHalfSize, box2.narrowPhaseHalfSize, quatTransfer, rPos);
-		/* res = trenasfert in generic plan the new res ...*/
+		/* 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
-		//tmp = RPos.add_new(new Vector3f(0,0,14));
+		// tmp = rPos.add_new(new Vector3f(0,0,14));
-		LoxelApplication.testRpos = quat2.inverse_new().getMatrix4().multiply(RPos1);
+		LoxelApplication.testRpos = quat2.inverse_new().getMatrix4().multiply(rPos1);
 		LoxelApplication.testQTransfert = quatTransfer2;
 		LoxelApplication.box1HalfSize = box2.narrowPhaseHalfSize;
 		LoxelApplication.box2HalfSize = box1.narrowPhaseHalfSize;
-		
+
-//		LoxelApplication.relativeTestPos.getTransform().setPosition(tmp);
+		// LoxelApplication.relativeTestPos.getTransform().setPosition(tmp);
-//		LoxelApplication.relativeTestPos.getTransform().setOrientation(quatTransfer);
+		// LoxelApplication.relativeTestPos.getTransform().setOrientation(quatTransfer);
-//		LoxelApplication.boxTest.setSize(box1.getSize());
+		// LoxelApplication.boxTest.setSize(box1.getSize());
 		// foinctionne avec la box qui n'est pas orienter...
-		//getCollidePointsAABBCenteredWithOBB(box2.narrowPhaseHalfSize, box1.narrowPhaseHalfSize, quatTransfer2, RPos1);
+		// 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));
+		// getCollidePointsAABBCenteredWithOBB(box2.narrowPhaseHalfSize, box1.narrowPhaseHalfSize, quatTransfer2, quat2.multiply(rPos2));
-		/*res = */getCollidePointsAABBCenteredWithOBB(box2.narrowPhaseHalfSize, box1.narrowPhaseHalfSize, quatTransfer2, quat2.inverse_new().getMatrix4().multiply(RPos1));
+		ColisionPoints[] collide1 = getCollidePointsAABBCenteredWithOBB(box2.narrowPhaseHalfSize, box1.narrowPhaseHalfSize, quatTransfer2, quat2.inverse_new().getMatrix4().multiply(rPos1));
-		/* res = trenasfert in generic plan the new res ...*/
+		box1.colisionPointTest = collide1;
-		
+		// transfer detection point collision in global environement:
+		if (collide1 != null) {
+			for (int iii = 0; iii < collide1.length; iii++) {
+				collide1[iii].position = quat1.multiply(collide1[iii].position).add(box1.narrowPhaseGlobalPos);
+			}
+		}
+		/* res = trenasfert in generic plan the new res ... */
+
+		LoxelApplication.testRpos = quat1.inverse_new().getMatrix4().multiply(rPos2);
+		LoxelApplication.testQTransfert = quatTransfer1;
+		LoxelApplication.box1HalfSize = box1.narrowPhaseHalfSize;
+		LoxelApplication.box2HalfSize = box2.narrowPhaseHalfSize;
+		ColisionPoints[] collide2 = getCollidePointsAABBCenteredWithOBB(box1.narrowPhaseHalfSize, box2.narrowPhaseHalfSize, quatTransfer1, quat1.inverse_new().getMatrix4().multiply(rPos2));
+		box2.colisionPointTest = collide2;
+		if (collide2 != null) {
+			for (int iii = 0; iii < collide2.length; iii++) {
+				collide2[iii].position = quat2.multiply(collide2[iii].position).add(box2.narrowPhaseGlobalPos);
+			}
+		}
+
 	}
 
-	public static void getCollidePointsAABBCenteredWithOBB(Vector3f box1HalfSize, Vector3f box2HalfSize, Quaternion box2Orientation, Vector3f box2Position) {
+	public static ColisionPoints[] getCollidePointsAABBCenteredWithOBB(Vector3f box1HalfSize, Vector3f box2HalfSize, Quaternion box2Orientation, Vector3f box2Position) {
-		// point in AABB
-		// toune autour de la box ...
-//		Matrix4f tmp = box2Orientation.getMatrix4();
-//		Vector3f topBackRight = tmp.multiply_new(Matrix4f.createMatrixTranslate(new Vector3f(+ box2HalfSize.x, + box2HalfSize.y, + box2HalfSize.z))).multiply(box2Position);
-//		Vector3f topBackLeft = tmp.multiply_new(Matrix4f.createMatrixTranslate(new Vector3f(- box2HalfSize.x, + box2HalfSize.y, + box2HalfSize.z))).multiply(box2Position);
-//		Vector3f topFrontRight = tmp.multiply_new(Matrix4f.createMatrixTranslate(new Vector3f(+ box2HalfSize.x, - box2HalfSize.y, + box2HalfSize.z))).multiply(box2Position);
-//		Vector3f topFrontLeft = tmp.multiply_new(Matrix4f.createMatrixTranslate(new Vector3f(- box2HalfSize.x, - box2HalfSize.y, + box2HalfSize.z))).multiply(box2Position);
-//		Vector3f bottomBackRight = tmp.multiply_new(Matrix4f.createMatrixTranslate(new Vector3f(+ box2HalfSize.x, + box2HalfSize.y, - box2HalfSize.z))).multiply(box2Position);
-//		Vector3f bottomBackLeft = tmp.multiply_new(Matrix4f.createMatrixTranslate(new Vector3f(- box2HalfSize.x, + box2HalfSize.y, - box2HalfSize.z))).multiply(box2Position);
-//		Vector3f bottomFrontRight = tmp.multiply_new(Matrix4f.createMatrixTranslate(new Vector3f(+ box2HalfSize.x, - box2HalfSize.y, - box2HalfSize.z))).multiply(box2Position);
-//		Vector3f bottomFrontLeft = tmp.multiply_new(Matrix4f.createMatrixTranslate(new Vector3f(- box2HalfSize.x, - box2HalfSize.y, - box2HalfSize.z))).multiply(box2Position);
 
-		// toune autour de la box ...
+		// point in AABB
-//		Matrix4f tmp = box2Orientation.getMatrix4().multiply(Matrix4f.createMatrixTranslate(box2Position));
+		Vector3f topBackRight = box2Orientation.multiply(new Vector3f(+box2HalfSize.x, +box2HalfSize.y, +box2HalfSize.z)).add(box2Position);
-//		Vector3f topBackRight = tmp.multiply(new Vector3f(+ box2HalfSize.x, + box2HalfSize.y, + box2HalfSize.z));
+		Vector3f topBackLeft = box2Orientation.multiply(new Vector3f(-box2HalfSize.x, +box2HalfSize.y, +box2HalfSize.z)).add(box2Position);
-//		Vector3f topBackLeft = tmp.multiply(new Vector3f(- box2HalfSize.x, + box2HalfSize.y, + box2HalfSize.z));
+		Vector3f topFrontRight = box2Orientation.multiply(new Vector3f(+box2HalfSize.x, -box2HalfSize.y, +box2HalfSize.z)).add(box2Position);
-//		Vector3f topFrontRight = tmp.multiply(new Vector3f(+ box2HalfSize.x, - box2HalfSize.y, + box2HalfSize.z));
+		Vector3f topFrontLeft = box2Orientation.multiply(new Vector3f(-box2HalfSize.x, -box2HalfSize.y, +box2HalfSize.z)).add(box2Position);
-//		Vector3f topFrontLeft = tmp.multiply(new Vector3f(- box2HalfSize.x, - box2HalfSize.y, + box2HalfSize.z));
+		Vector3f bottomBackRight = box2Orientation.multiply(new Vector3f(+box2HalfSize.x, +box2HalfSize.y, -box2HalfSize.z)).add(box2Position);
-//		Vector3f bottomBackRight = tmp.multiply(new Vector3f(+ box2HalfSize.x, + box2HalfSize.y, - box2HalfSize.z));
+		Vector3f bottomBackLeft = box2Orientation.multiply(new Vector3f(-box2HalfSize.x, +box2HalfSize.y, -box2HalfSize.z)).add(box2Position);
-//		Vector3f bottomBackLeft = tmp.multiply(new Vector3f(- box2HalfSize.x, + box2HalfSize.y, - box2HalfSize.z));
+		Vector3f bottomFrontRight = box2Orientation.multiply(new Vector3f(+box2HalfSize.x, -box2HalfSize.y, -box2HalfSize.z)).add(box2Position);
-//		Vector3f bottomFrontRight = tmp.multiply(new Vector3f(+ box2HalfSize.x, - box2HalfSize.y, - box2HalfSize.z));
+		Vector3f bottomFrontLeft = box2Orientation.multiply(new Vector3f(-box2HalfSize.x, -box2HalfSize.y, -box2HalfSize.z)).add(box2Position);
-//		Vector3f bottomFrontLeft = tmp.multiply(new Vector3f(- box2HalfSize.x, - box2HalfSize.y, - box2HalfSize.z));
-		
-//		Matrix4f tmp = Matrix4f.createMatrixTranslate(box2Position).multiply(box2Orientation.getMatrix4());
-//		Vector3f topBackRight = tmp.multiply(new Vector3f(+ box2HalfSize.x, + box2HalfSize.y, + box2HalfSize.z));
-//		Vector3f topBackLeft = tmp.multiply(new Vector3f(- box2HalfSize.x, + box2HalfSize.y, + box2HalfSize.z));
-//		Vector3f topFrontRight = tmp.multiply(new Vector3f(+ box2HalfSize.x, - box2HalfSize.y, + box2HalfSize.z));
-//		Vector3f topFrontLeft = tmp.multiply(new Vector3f(- box2HalfSize.x, - box2HalfSize.y, + box2HalfSize.z));
-//		Vector3f bottomBackRight = tmp.multiply(new Vector3f(+ box2HalfSize.x, + box2HalfSize.y, - box2HalfSize.z));
-//		Vector3f bottomBackLeft = tmp.multiply(new Vector3f(- box2HalfSize.x, + box2HalfSize.y, - box2HalfSize.z));
-//		Vector3f bottomFrontRight = tmp.multiply(new Vector3f(+ box2HalfSize.x, - box2HalfSize.y, - box2HalfSize.z));
-//		Vector3f bottomFrontLeft = tmp.multiply(new Vector3f(- box2HalfSize.x, - box2HalfSize.y, - box2HalfSize.z));
-		
-//		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);
@@ -227,56 +187,165 @@ public class ToolCollisionOBBWithOBB {
 		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));
-		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);
+		Vector3f insideTopBackRight = pointDistanceInAABB(box1HalfSize, topBackRight);
-		boolean insideTopBackLeft   = pointInAABB(box1HalfSize, topBackLeft);
+		Vector3f insideTopBackLeft = pointDistanceInAABB(box1HalfSize, topBackLeft);
-		boolean insideTopFrontRight = pointInAABB(box1HalfSize, topFrontRight);
+		Vector3f insideTopFrontRight = pointDistanceInAABB(box1HalfSize, topFrontRight);
-		boolean insideTopFrontLeft  = pointInAABB(box1HalfSize, topFrontLeft);
+		Vector3f insideTopFrontLeft = pointDistanceInAABB(box1HalfSize, topFrontLeft);
-		boolean insideBottomBackRight  = pointInAABB(box1HalfSize, bottomBackRight);
+		Vector3f insideBottomBackRight = pointDistanceInAABB(box1HalfSize, bottomBackRight);
-		boolean insideBottomBackLeft   = pointInAABB(box1HalfSize, bottomBackLeft);
+		Vector3f insideBottomBackLeft = pointDistanceInAABB(box1HalfSize, bottomBackLeft);
-		boolean insideBottomFrontRight = pointInAABB(box1HalfSize, bottomFrontRight);
+		Vector3f insideBottomFrontRight = pointDistanceInAABB(box1HalfSize, bottomFrontRight);
-		boolean insideBottomFrontLeft  = pointInAABB(box1HalfSize, bottomFrontLeft);
+		Vector3f insideBottomFrontLeft = pointDistanceInAABB(box1HalfSize, bottomFrontLeft);
 		LoxelApplication.testPointsCollide.clear();
-		LoxelApplication.testPointsCollide.add(insideTopBackRight);
+		LoxelApplication.testPointsCollide.add(insideTopBackRight == null ? false : true);
-		LoxelApplication.testPointsCollide.add(insideTopBackLeft);
+		LoxelApplication.testPointsCollide.add(insideTopBackLeft == null ? false : true);
-		LoxelApplication.testPointsCollide.add(insideTopFrontRight);
+		LoxelApplication.testPointsCollide.add(insideTopFrontRight == null ? false : true);
-		LoxelApplication.testPointsCollide.add(insideTopFrontLeft);
+		LoxelApplication.testPointsCollide.add(insideTopFrontLeft == null ? false : true);
-		LoxelApplication.testPointsCollide.add(insideBottomBackRight);
+		LoxelApplication.testPointsCollide.add(insideBottomBackRight == null ? false : true);
-		LoxelApplication.testPointsCollide.add(insideBottomBackLeft);
+		LoxelApplication.testPointsCollide.add(insideBottomBackLeft == null ? false : true);
-		LoxelApplication.testPointsCollide.add(insideBottomFrontRight);
+		LoxelApplication.testPointsCollide.add(insideBottomFrontRight == null ? false : true);
-		LoxelApplication.testPointsCollide.add(insideBottomFrontLeft);
+		LoxelApplication.testPointsCollide.add(insideBottomFrontLeft == null ? false : true);
-		if (insideTopBackRight == true
+		int count = 0;
-				|| insideTopBackLeft == true
+		if (insideTopBackRight != null) {
-				|| insideTopFrontRight == true
+			count++;
-				|| insideTopFrontLeft == true
+		}
-				|| insideBottomBackRight == true
+		if (insideTopBackLeft != null) {
-				|| insideBottomBackLeft == true
+			count++;
-				|| insideBottomFrontRight == true
+		}
-				|| insideBottomFrontLeft == true) {
+		if (insideTopFrontRight != null) {
+			count++;
+		}
+		if (insideTopFrontLeft != null) {
+			count++;
+		}
+		if (insideBottomBackRight != null) {
+			count++;
+		}
+		if (insideBottomBackLeft != null) {
+			count++;
+		}
+		if (insideBottomFrontRight != null) {
+			count++;
+		}
+		if (insideBottomFrontLeft != null) {
+			count++;
+		}
+		ColisionPoints[] out = new ColisionPoints[count];
+		count = 0;
+		if (insideTopBackRight != null) {
+			out[count] = new ColisionPoints(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);
+			count++;
+		}
+		if (insideTopFrontRight != null) {
+			out[count] = new ColisionPoints(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);
+			count++;
+		}
+		if (insideBottomBackRight != null) {
+			out[count] = new ColisionPoints(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);
+			count++;
+		}
+		if (insideBottomFrontRight != null) {
+			out[count] = new ColisionPoints(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);
+			count++;
+		}
+		if (count != 0) {
 			// Find a point inside the BOX ...
-			Log.info("Detect point inside ... " + insideTopBackRight    + "  " + insideTopBackLeft    + "  " + insideTopFrontRight    + "  " + insideTopFrontLeft
+			Log.info("Detect point inside ... " + insideTopBackRight + "  " + insideTopBackLeft + "  " + insideTopFrontRight + "  " + insideTopFrontLeft + "  " + insideBottomBackRight + "  "
-					                     + "  " + insideBottomBackRight + "  " + insideBottomBackLeft + "  " + insideBottomFrontRight + "  " + insideBottomFrontLeft);
+					+ insideBottomBackLeft + "  " + insideBottomFrontRight + "  " + insideBottomFrontLeft);
+
+			return out;
 		}
 		// line in AABB
 		Log.info("Need to detect line inside ...");
+		return null;
 	}
-	
+
 	public static boolean pointInAABB(Vector3f halfSize, Vector3f point) {
-		if (point.x > -halfSize.x
+		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) {
-				&& point.x < halfSize.x
-				&& point.y > -halfSize.y
-				&& point.y < halfSize.y
-				&& point.z > -halfSize.z
-				&& point.z < halfSize.z) {
 			return true;
 		}
 		return false;
 	}
+
+	public static Vector3f pointDistanceInAABB(Vector3f halfSize, Vector3f point) {
+		Vector3f out = new Vector3f();
+		if (point.x < 0) {
+			if (point.x > -halfSize.x) {
+				out.x = -halfSize.x + point.x;
+			} else {
+				return null;
+			}
+		} else {
+			if (point.x < halfSize.x) {
+				out.x = halfSize.x - point.x;
+			} else {
+				return null;
+			}
+		}
+		if (point.y < 0) {
+			if (point.y > -halfSize.y) {
+				out.y = -halfSize.y + point.y;
+			} else {
+				return null;
+			}
+		} else {
+			if (point.y < halfSize.y) {
+				out.y = halfSize.y - point.y;
+			} else {
+				return null;
+			}
+		}
+		if (point.z < 0) {
+			if (point.z > -halfSize.z) {
+				out.z = -halfSize.z + point.z;
+			} else {
+				return null;
+			}
+		} else {
+			if (point.z < halfSize.z) {
+				out.z = halfSize.z - point.z;
+			} else {
+				return null;
+			}
+		}
+		if (out.x < out.y) {
+			out.y = 0;
+			if (out.x < out.z) {
+				out.z = 0;
+				return out;
+			}
+			out.x = 0;
+			return out;
+		}
+		out.x = 0;
+		if (out.y < out.z) {
+			out.z = 0;
+			return out;
+		}
+		out.y = 0;
+		return out;
+	}
 }