diff --git a/.checkstyle b/.checkstyle new file mode 100644 index 0000000..af965e0 --- /dev/null +++ b/.checkstyle @@ -0,0 +1,7 @@ + + + + + + + diff --git a/.project b/.project index 53b0976..93bdf66 100644 --- a/.project +++ b/.project @@ -10,8 +10,14 @@ + + net.sf.eclipsecs.core.CheckstyleBuilder + + + org.eclipse.jdt.core.javanature + net.sf.eclipsecs.core.CheckstyleNature diff --git a/src/guis/GuiRenderer.java b/src/guis/GuiRenderer.java index 8b468e0..a040bf9 100644 --- a/src/guis/GuiRenderer.java +++ b/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}; - quad = loader.loadToVAO(positions, 2); - shader = new GuiShader(); + float[] positions = { -1, 1, -1, -1, 1, 1, 1, -1 }; + quad = loader.loadToVAO(positions, 2); + shader = new GuiShader(); } + public void render(List 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(); } - + } diff --git a/src/org/atriaSoft/etk/math/Quaternion.java b/src/org/atriaSoft/etk/math/Quaternion.java index 0de1c26..cf426bf 100644 --- a/src/org/atriaSoft/etk/math/Quaternion.java +++ b/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() { - 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) == true - || isnan(this.w) == true) { - TKCRITICAL(" set transform: (" << this.x << "," << this.y << "," << this.z << "," << this.w << ")"); - } - } - */ - /** - * @brief Constructor from scalars. + + /* 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) == + * 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 - * @param eulerAngles list of all euleu angle - */ + + /** @brief Constructor with Euler angles (in radians) to a quaternion + * @param eulerAngles list of all euleu angle */ public Quaternion(Vector3f eulerAngles) { setEulerAngles(eulerAngles); } - /** - * @brief Create a unit quaternion from a rotation matrix - * @param matrix generic matrix - */ + + /** @brief Create a unit quaternion from a rotation 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, - this.y + obj.y, - this.z + obj.z, - this.w + obj.w); + return new Quaternion(this.x + obj.x, 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, - this.y - obj.y, - this.z - obj.z, - this.w - obj.w); + return new Quaternion(this.x - obj.x, 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, - this.y * val, - this.z * val, - this.w * val); + return new Quaternion(this.x * 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, - this.y / val, - this.z / val, - this.w / val); + return new Quaternion(this.x / 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 - + this.y * obj.y - + this.z * obj.z - + this.w * obj.w; + return this.x * obj.x + this.y * obj.y + this.z * obj.z + this.w * obj.w; } - /** - * @brief Return the squared length of the quaternion. - * @return Squared length value. - */ + + /** @brief Return the squared length of the quaternion. + * @return Squared length value. */ public float length2() { return dot(this); } - /** - * @brief Return the length of the quaternion - * @return Length value - */ + + /** @brief Return the length of the quaternion + * @return Length value */ public float length() { return (float) Math.sqrt(length2()); } - /** - * @brief Normalize this quaternion x^2 + y^2 + z^2 + w^2 = 1 - * @return Local reference of the quaternion normalized - */ + + /** @brief Normalize this quaternion x^2 + y^2 + z^2 + w^2 = 1 + * @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 - * @return New quaternion containing the value - */ + + /** @brief Return a normalized version of this quaternion + * @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 - * @return Local reference of the quaternion normalized - */ + + /** @brief Normalize this quaternion x^2 + y^2 + z^2 + w^2 = 1 + * @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 - * @return New quaternion containing the value - */ + + /** @brief Return a normalized version of this quaternion + * @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 - * @return New quaternion with the absolute value - */ + + /** @brief Return a quaternion will the absolute values of each element + * @return New quaternion with the absolute value */ public Quaternion absolute_new() { - return new Quaternion( Math.abs(this.x), - Math.abs(this.y), - Math.abs(this.z), - Math.abs(this.w)); + return new Quaternion(Math.abs(this.x), Math.abs(this.y), Math.abs(this.z), Math.abs(this.w)); } - /** - * @brief Get X value - * @return the x value - */ + + /** @brief Get X value + * @return the x value */ public float getX() { return this.x; } - /** - * @brief Get Y value - * @return the y value - */ + + /** @brief Get Y value + * @return the y value */ public float getY() { return this.y; } - /** - * @brief Get Z value - * @return the z value - */ + + /** @brief Get Z value + * @return the z value */ public float getZ() { return this.z; } - /** - * @brief Get W value - * @return the w value - */ + + /** @brief Get W value + * @return the w value */ public float getW() { return this.w; } - /** - * @brief Set the x value - * @param[in] x New value - */ + + /** @brief Set the x value + * @param[in] x New value */ public Quaternion setX(float x) { this.x = x; return this; }; - /** - * @brief Set the y value - * @param[in] y New value - */ + + /** @brief Set the y value + * @param[in] y New value */ public Quaternion setY(float y) { - this.y = y; + this.y = y; return this; }; - /** - * @brief Set the z value - * @param[in] z New value - */ + + /** @brief Set the z value + * @param[in] z New value */ public Quaternion setZ(float z) { this.z = z; return this; }; - /** - * @brief Set the w value - * @param[in] w New value - */ + + /** @brief Set the w 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) - && (this.z == obj.z) - && (this.y == obj.y) - && (this.x == obj.x)); + return ((this.w == obj.w) && (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) - || (this.z != obj.z) - || (this.y != obj.y) - || (this.x != obj.x)); + return ((this.w != obj.w) || (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 - * @param[in] point Point to move - * @return Point with the updated position - */ + + /** @brief Operator* with a vector. This methods rotates a point given the rotation of a quaternion + * @param point Point to move + * @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 - * @param obj The other Vector to compare with - */ + + public Vector3f multiply(float xxx, float yyy, float zzz) { + 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.y = (float)Math.max(this.y, obj.y); - this.z = (float)Math.max(this.z, obj.z); - this.w = (float)Math.max(this.w, obj.w); + this.x = Math.max(this.x, obj.x); + this.y = Math.max(this.y, obj.y); + this.z = Math.max(this.z, obj.z); + 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 - * @param obj The other Vector to compare with - */ + + /** @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 */ public void setMin(Quaternion obj) { - this.x = (float)Math.min(this.x, obj.x); - this.y = (float)Math.min(this.y, obj.y); - this.z = (float)Math.min(this.z, obj.z); - this.w = (float)Math.min(this.w, obj.w); + this.x = Math.min(this.x, obj.x); + this.y = Math.min(this.y, obj.y); + this.z = Math.min(this.z, obj.z); + this.w = Math.min(this.w, obj.w); } - /** - * @brief Set Value on the quaternion - * @param[in] xxx X value. - * @param[in] yyy Y value. - * @param[in] zzz Z value. - * @param[in] www W value. - */ + + /** @brief Set Value on the quaternion + * @param xxx X value. + * @param yyy Y value. + * @param zzz Z 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 - * @return a (float)Math. quaternion - */ + + /** @brief get a 0 value on all a 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 - && this.y == 0 - && this.z == 0 - && this.w == 0; + return this.x == 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 - * @return an identity quaternion - */ + + /** @brief get 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 - * @return inverted quaternion - */ + + /** @brief Return the inverse of the quaternion + * @return inverted quaternion */ public Quaternion inverse_new() { Quaternion tmp = new Quaternion(this); tmp.inverse(); return tmp; } - /** - * @brief Return the unit quaternion - * @return Quaternion unitarised - */ + + /** @brief Return the unit quaternion + * @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 - * @return Conjugate quaternion - */ + + /** @brief Return the conjugate of the 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 - * @return the 3x3 transformation matrix - */ + + /** @brief Get the orientation matrix corresponding to this quaternion + * @return the 3x3 transformation matrix */ public Matrix3f getMatrix() { - float nQ = this.x*this.x - + this.y*this.y - + this.z*this.z - + this.w*this.w; + float nQ = this.x * this.x + 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 ys = this.y*sss; - float zs = this.z*sss; - float wxs = this.w*xs; - float wys = this.w*ys; - float wzs = this.w*zs; - float xxs = this.x*xs; - float xys = this.x*ys; - float xzs = this.x*zs; - float yys = this.y*ys; - float yzs = this.y*zs; - float zzs = this.z*zs; - 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); + float xs = this.x * sss; + float ys = this.y * sss; + float zs = this.z * sss; + float wxs = this.w * xs; + float wys = this.w * ys; + float wzs = this.w * zs; + float xxs = this.x * xs; + float xys = this.x * ys; + float xzs = this.x * zs; + float yys = this.y * ys; + float yzs = this.y * zs; + float zzs = this.z * zs; + 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); } + public Matrix4f getMatrix4() { - float nQ = this.x*this.x - + this.y*this.y - + this.z*this.z - + this.w*this.w; + float nQ = this.x * this.x + 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 ys = this.y*sss; - float zs = this.z*sss; - float wxs = this.w*xs; - float wys = this.w*ys; - float wzs = this.w*zs; - float xxs = this.x*xs; - float xys = this.x*ys; - float xzs = this.x*zs; - float yys = this.y*ys; - float yzs = this.y*zs; - float zzs = this.z*zs; - 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); + float xs = this.x * sss; + float ys = this.y * sss; + float zs = this.z * sss; + float wxs = this.w * xs; + float wys = this.w * ys; + float wzs = this.w * zs; + float xxs = this.x * xs; + float xys = this.x * ys; + float xzs = this.x * zs; + float yys = this.y * ys; + float yzs = this.y * zs; + float zzs = this.z * zs; + 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); } - /** - * @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) { - return obj1.multiply_new(1.0f-ttt).add(obj2.multiply_new(ttt * invert)); + if (1 - cosineTheta < 0.00001f) { + return obj1.multiply_new(1.0f - ttt).add(obj2.multiply_new(ttt * invert)); } - float theta = (float)Math.acos(cosineTheta); - float sineTheta = (float)Math.sin(theta); - float coeff1 = (float)Math.sin((1.0f-ttt)*theta) / sineTheta; - float coeff2 = (float)Math.sin(ttt*theta) / sineTheta * invert; + float theta = (float) Math.acos(cosineTheta); + float sineTheta = (float) Math.sin(theta); + float coeff1 = (float) Math.sin((1.0f - ttt) * theta) / sineTheta; + 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. - * @param[out] angles Eular angle of the quaternion. - */ + + /** @brief Configure the quaternion with euler angles. + * @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 cosX = (float)Math.cos(angle); + float sinX = (float) Math.sin(angle); + float cosX = (float) Math.cos(angle); angle = angles.y * 0.5f; - float sinY = (float)Math.sin(angle); - float cosY = (float)Math.cos(angle); + float sinY = (float) Math.sin(angle); + float cosY = (float) Math.cos(angle); angle = angles.z * 0.5f; - float sinZ = (float)Math.sin(angle); - float cosZ = (float)Math.cos(angle); + float sinZ = (float) Math.sin(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. - * @return New Quaternion containing the value - */ + + /** @brief Clone the current Quaternion. + * @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); } diff --git a/src/org/atriaSoft/gameEngine/physics/ColisionPoints.java b/src/org/atriaSoft/gameEngine/physics/ColisionPoints.java new file mode 100644 index 0000000..c6f1645 --- /dev/null +++ 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; + } +} diff --git a/src/org/atriaSoft/gameEngine/physics/PhysicBox.java b/src/org/atriaSoft/gameEngine/physics/PhysicBox.java index f9a5fc3..482cb41 100644 --- a/src/org/atriaSoft/gameEngine/physics/PhysicBox.java +++ b/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 narrowPhaseAxisY = new Vector3f(1,0,0); - public Vector3f narrowPhaseAxisZ = new Vector3f(1,0,0); + public Vector3f narrowPhaseAxisX = new Vector3f(1, 0, 0); + public Vector3f narrowPhaseAxisY = new Vector3f(1, 0, 0); + public Vector3f narrowPhaseAxisZ = new Vector3f(1, 0, 0); public Vector3f narrowPhaseHalfSize; + @Override public void updateForNarrowCollision(Transform3D transformGlobal) { - 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.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.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.narrowPhaseAxisY = transformGlobal.multiply(this.transform.multiply(new Vector3f(0, 1, 0))).less(this.narrowPhaseGlobalPos); + this.narrowPhaseAxisZ = transformGlobal.multiply(this.transform.multiply(new Vector3f(0, 0, 1))).less(this.narrowPhaseGlobalPos); this.narrowPhaseHalfSize = this.size.multiply_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); + } + } } } diff --git a/src/org/atriaSoft/gameEngine/physics/PhysicShape.java b/src/org/atriaSoft/gameEngine/physics/PhysicShape.java index 7081cc6..ce7d741 100644 --- a/src/org/atriaSoft/gameEngine/physics/PhysicShape.java +++ b/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; -// protected Vector3f origin; + public ColisionPoints[] colisionPointTest; + // 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.origin = Vector3f.zero(); + // this.quaternion = Quaternion.identity(); + // 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.origin = origin; + // this.quaternion = quaternion; + // 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); - + } diff --git a/src/org/atriaSoft/gameEngine/physics/ToolCollisionOBBWithOBB.java b/src/org/atriaSoft/gameEngine/physics/ToolCollisionOBBWithOBB.java index bd6dfe1..8141f27 100644 --- a/src/org/atriaSoft/gameEngine/physics/ToolCollisionOBBWithOBB.java +++ b/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 -{ - public Vector3f Pos, AxisX, AxisY, AxisZ, Half_size; +class OBB { + public Vector3f position; + public Vector3f axisX; + public Vector3f axisY; + public Vector3f axisZ; + public Vector3f halfSize; + public OBB() { - Pos = new Vector3f(); - AxisX = new Vector3f(); - AxisY = new Vector3f(); - AxisZ = new Vector3f(); - Half_size = new Vector3f(); + position = new Vector3f(); + axisX = new Vector3f(); + axisY = new Vector3f(); + axisZ = 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) { - return (Math.abs(RPos.dot(Plane)) > - (Math.abs(box1.AxisX.multiply_new(box1.Half_size.x).dot(Plane)) + - Math.abs(box1.AxisY.multiply_new(box1.Half_size.y).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)) - ) - ); + private static boolean getSeparatingPlane(Vector3f rPos, Vector3f plane, OBB box1, OBB box2) { + 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.axisZ.multiply_new(box1.halfSize.z).dot(plane)) + Math.abs(box2.axisX.multiply_new(box2.halfSize.x).dot(plane)) + + Math.abs(box2.axisY.multiply_new(box2.halfSize.y).dot(plane)) + Math.abs(box2.axisZ.multiply_new(box2.halfSize.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); - boolean ret = - getSeparatingPlane(RPos, box1.AxisX, box1, box2) || - getSeparatingPlane(RPos, box1.AxisY, box1, box2) || - getSeparatingPlane(RPos, box1.AxisZ, box1, box2) || - getSeparatingPlane(RPos, box2.AxisX, box1, box2) || - getSeparatingPlane(RPos, box2.AxisY, box1, box2) || - getSeparatingPlane(RPos, box2.AxisZ, box1, box2) || - getSeparatingPlane(RPos, box1.AxisX.cross(box2.AxisX), box1, box2) || - 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; + Vector3f rPos = box2.position.less_new(box1.position); + boolean ret = getSeparatingPlane(rPos, box1.axisX, box1, box2) || getSeparatingPlane(rPos, box1.axisY, box1, box2) || getSeparatingPlane(rPos, box1.axisZ, box1, box2) + || getSeparatingPlane(rPos, box2.axisX, box1, box2) || getSeparatingPlane(rPos, box2.axisY, box1, box2) || getSeparatingPlane(rPos, box2.axisZ, box1, box2) + || getSeparatingPlane(rPos, box1.axisX.cross(box2.axisX), box1, box2) || 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 B = new OBB(); - + OBB aaa = 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); - A.AxisY = new Vector3f(0.0f, 1.0f, 0.0f); - A.AxisZ = new Vector3f(0.0f, 0.0f, 1.0f); - + aaa.axisX = new Vector3f(1.0f, 0.0f, 0.0f); + aaa.axisY = new Vector3f(0.0f, 1.0f, 0.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); - B.AxisY = new Vector3f(0.0f, 1.0f, 0.0f); - B.AxisZ = new Vector3f(0.0f, 0.0f, 1.0f); - + bbb.axisX = new Vector3f(1.0f, 0.0f, 0.0f); + bbb.axisY = new Vector3f(0.0f, 1.0f, 0.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) { - return (Math.abs(RPos.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(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)) - ) - ); + private static boolean getSeparatingPlane222(Vector3f rPos, Vector3f plane, PhysicBox box1, PhysicBox box2) { + return (Math.abs(rPos.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(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); - boolean ret = - getSeparatingPlane222(RPos, box1.narrowPhaseAxisX, box1, box2) || - getSeparatingPlane222(RPos, box1.narrowPhaseAxisY, box1, box2) || - getSeparatingPlane222(RPos, box1.narrowPhaseAxisZ, box1, box2) || - getSeparatingPlane222(RPos, box2.narrowPhaseAxisX, box1, box2) || - getSeparatingPlane222(RPos, box2.narrowPhaseAxisY, box1, box2) || - getSeparatingPlane222(RPos, box2.narrowPhaseAxisZ, box1, box2) || - getSeparatingPlane222(RPos, box1.narrowPhaseAxisX.cross(box2.narrowPhaseAxisX), box1, box2) || - getSeparatingPlane222(RPos, box1.narrowPhaseAxisX.cross(box2.narrowPhaseAxisY), box1, box2) || - getSeparatingPlane222(RPos, box1.narrowPhaseAxisX.cross(box2.narrowPhaseAxisZ), box1, box2) || - getSeparatingPlane222(RPos, box1.narrowPhaseAxisY.cross(box2.narrowPhaseAxisX), box1, box2) || - getSeparatingPlane222(RPos, box1.narrowPhaseAxisY.cross(box2.narrowPhaseAxisY), box1, box2) || - getSeparatingPlane222(RPos, box1.narrowPhaseAxisY.cross(box2.narrowPhaseAxisZ), box1, box2) || - 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; + Vector3f rPos = box2.narrowPhaseGlobalPos.less_new(box1.narrowPhaseGlobalPos); + boolean ret = getSeparatingPlane222(rPos, box1.narrowPhaseAxisX, box1, box2) || getSeparatingPlane222(rPos, box1.narrowPhaseAxisY, box1, box2) + || getSeparatingPlane222(rPos, box1.narrowPhaseAxisZ, box1, box2) || getSeparatingPlane222(rPos, box2.narrowPhaseAxisX, box1, box2) + || getSeparatingPlane222(rPos, box2.narrowPhaseAxisY, box1, box2) || getSeparatingPlane222(rPos, box2.narrowPhaseAxisZ, box1, box2) + || getSeparatingPlane222(rPos, box1.narrowPhaseAxisX.cross(box2.narrowPhaseAxisX), box1, box2) + || getSeparatingPlane222(rPos, box1.narrowPhaseAxisX.cross(box2.narrowPhaseAxisY), box1, box2) + || getSeparatingPlane222(rPos, box1.narrowPhaseAxisX.cross(box2.narrowPhaseAxisZ), box1, box2) + || getSeparatingPlane222(rPos, box1.narrowPhaseAxisY.cross(box2.narrowPhaseAxisX), box1, box2) + || getSeparatingPlane222(rPos, box1.narrowPhaseAxisY.cross(box2.narrowPhaseAxisY), box1, box2) + || getSeparatingPlane222(rPos, box1.narrowPhaseAxisY.cross(box2.narrowPhaseAxisZ), box1, box2) + || 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 ........"); - Vector3f RPos1 = box1.narrowPhaseGlobalPos.less_new(box2.narrowPhaseGlobalPos); - Vector3f RPos2 = box2.narrowPhaseGlobalPos.less_new(box1.narrowPhaseGlobalPos); + // Log.info("Try to calculare reverse force ........"); + Vector3f rPos1 = box1.narrowPhaseGlobalPos.less_new(box2.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; - //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 + quatTransfer1); -// /*res = */getCollidePointsAABBCenteredWithOBB(box1.narrowPhaseHalfSize, box2.narrowPhaseHalfSize, quatTransfer, RPos); - /* res = trenasfert in generic plan the new res ...*/ + // 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 + quatTransfer1); + // /*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 - //tmp = RPos.add_new(new Vector3f(0,0,14)); - LoxelApplication.testRpos = quat2.inverse_new().getMatrix4().multiply(RPos1); + // tmp = rPos.add_new(new Vector3f(0,0,14)); + 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().setOrientation(quatTransfer); -// LoxelApplication.boxTest.setSize(box1.getSize()); + + // LoxelApplication.relativeTestPos.getTransform().setPosition(tmp); + // LoxelApplication.relativeTestPos.getTransform().setOrientation(quatTransfer); + // 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)); - /*res = */getCollidePointsAABBCenteredWithOBB(box2.narrowPhaseHalfSize, box1.narrowPhaseHalfSize, quatTransfer2, quat2.inverse_new().getMatrix4().multiply(RPos1)); - /* res = trenasfert in generic plan the new res ...*/ - + // getCollidePointsAABBCenteredWithOBB(box2.narrowPhaseHalfSize, box1.narrowPhaseHalfSize, quatTransfer2, quat2.multiply(rPos2)); + ColisionPoints[] collide1 = getCollidePointsAABBCenteredWithOBB(box2.narrowPhaseHalfSize, box1.narrowPhaseHalfSize, quatTransfer2, quat2.inverse_new().getMatrix4().multiply(rPos1)); + 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) { - // 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); + public static ColisionPoints[] getCollidePointsAABBCenteredWithOBB(Vector3f box1HalfSize, Vector3f box2HalfSize, Quaternion box2Orientation, Vector3f box2Position) { - // toune autour de la box ... -// Matrix4f tmp = box2Orientation.getMatrix4().multiply(Matrix4f.createMatrixTranslate(box2Position)); -// 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)); - -// 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); + // point in AABB + 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)); - 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.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)); + Vector3f insideTopBackRight = pointDistanceInAABB(box1HalfSize, topBackRight); + Vector3f insideTopBackLeft = pointDistanceInAABB(box1HalfSize, topBackLeft); + Vector3f insideTopFrontRight = pointDistanceInAABB(box1HalfSize, topFrontRight); + Vector3f insideTopFrontLeft = pointDistanceInAABB(box1HalfSize, topFrontLeft); + Vector3f insideBottomBackRight = pointDistanceInAABB(box1HalfSize, bottomBackRight); + Vector3f insideBottomBackLeft = pointDistanceInAABB(box1HalfSize, bottomBackLeft); + Vector3f insideBottomFrontRight = pointDistanceInAABB(box1HalfSize, bottomFrontRight); + Vector3f insideBottomFrontLeft = pointDistanceInAABB(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) { + LoxelApplication.testPointsCollide.add(insideTopBackRight == null ? false : true); + LoxelApplication.testPointsCollide.add(insideTopBackLeft == null ? false : true); + LoxelApplication.testPointsCollide.add(insideTopFrontRight == null ? false : true); + LoxelApplication.testPointsCollide.add(insideTopFrontLeft == null ? false : true); + LoxelApplication.testPointsCollide.add(insideBottomBackRight == null ? false : true); + LoxelApplication.testPointsCollide.add(insideBottomBackLeft == null ? false : true); + LoxelApplication.testPointsCollide.add(insideBottomFrontRight == null ? false : true); + LoxelApplication.testPointsCollide.add(insideBottomFrontLeft == null ? false : true); + int count = 0; + if (insideTopBackRight != null) { + count++; + } + if (insideTopBackLeft != null) { + count++; + } + 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 - + " " + insideBottomBackRight + " " + insideBottomBackLeft + " " + insideBottomFrontRight + " " + insideBottomFrontLeft); + Log.info("Detect point inside ... " + insideTopBackRight + " " + insideTopBackLeft + " " + insideTopFrontRight + " " + insideTopFrontLeft + " " + insideBottomBackRight + " " + + 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 - && point.x < halfSize.x - && point.y > -halfSize.y - && point.y < halfSize.y - && point.z > -halfSize.z - && point.z < halfSize.z) { + 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; } + + 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; + } }