+ * This function uses the equal-to-zero test with the error {@link MathConstants#EPSILON}.
+ *
+ *
+ * @return true if the matrix is identity; false otherwise.
+ * @see MathUtil#isEpsilonZero(float)
+ * @see MathUtil#isEpsilonEqual(float, float)
+ */
+ public boolean isIdentity() {
+ //TODO Buffering the boolean value "isIdentity"
+ return MathUtil.isEpsilonEqual(this.m00, 1f, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonZero(this.m01, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonZero(this.m10, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonEqual(this.m11, 1f, MathConstants.JVM_MIN_FLOAT_EPSILON);
+ }
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/Matrix3f.java b/core/math/src/main/java/org/arakhne/afc/math/Matrix3f.java
new file mode 100644
index 000000000..263e92420
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/Matrix3f.java
@@ -0,0 +1,3496 @@
+/*
+ * $Id$
+ *
+ * Copyright (c) 2006-10, Multiagent Team, Laboratoire Systemes et Transports, Universite de Technologie de Belfort-Montbeliard.
+ * Copyright (C) 2012 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math;
+
+import java.io.Serializable;
+import java.util.Arrays;
+
+import org.arakhne.afc.math.geometry3d.continuous.Point3f;
+import org.arakhne.afc.math.geometry3d.continuous.Tuple3f;
+import org.arakhne.afc.math.geometry3d.continuous.Vector3f;
+import org.arakhne.afc.vmutil.locale.Locale;
+
+/**
+ * Is represented internally as a 3x3 floating point matrix. The mathematical
+ * representation is row major, as in traditional matrix mathematics.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class Matrix3f implements Serializable, Cloneable, MathConstants {
+
+ private static final long serialVersionUID = -7386754038391115819L;
+
+ /**
+ * The first matrix element in the first row.
+ */
+ public float m00;
+
+ /**
+ * The second matrix element in the first row.
+ */
+ public float m01;
+
+ /**
+ * The third matrix element in the first row.
+ */
+ public float m02;
+
+ /**
+ * The first matrix element in the second row.
+ */
+ public float m10;
+
+ /**
+ * The second matrix element in the second row.
+ */
+ public float m11;
+
+ /**
+ * The third matrix element in the second row.
+ */
+ public float m12;
+
+ /**
+ * The first matrix element in the third row.
+ */
+ public float m20;
+
+ /**
+ * The second matrix element in the third row.
+ */
+ public float m21;
+
+ /**
+ * The third matrix element in the third row.
+ */
+ public float m22;
+
+ /**
+ * Constructs and initializes a Matrix3f from the specified nine values.
+ *
+ * @param m00
+ * the [0][0] element
+ * @param m01
+ * the [0][1] element
+ * @param m02
+ * the [0][2] element
+ * @param m10
+ * the [1][0] element
+ * @param m11
+ * the [1][1] element
+ * @param m12
+ * the [1][2] element
+ * @param m20
+ * the [2][0] element
+ * @param m21
+ * the [2][1] element
+ * @param m22
+ * the [2][2] element
+ */
+ public Matrix3f(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22) {
+ this.m00 = m00;
+ this.m01 = m01;
+ this.m02 = m02;
+
+ this.m10 = m10;
+ this.m11 = m11;
+ this.m12 = m12;
+
+ this.m20 = m20;
+ this.m21 = m21;
+ this.m22 = m22;
+ }
+
+ /**
+ * Constructs and initializes a Matrix3f from the specified nine- element
+ * array.
+ *
+ * @param v
+ * the array of length 9 containing in order
+ */
+ public Matrix3f(float[] v) {
+ this.m00 = v[0];
+ this.m01 = v[1];
+ this.m02 = v[2];
+
+ this.m10 = v[3];
+ this.m11 = v[4];
+ this.m12 = v[5];
+
+ this.m20 = v[6];
+ this.m21 = v[7];
+ this.m22 = v[8];
+ }
+
+ /**
+ * Constructs a new matrix with the same values as the Matrix3f parameter.
+ *
+ * @param m1
+ * the source matrix
+ */
+ public Matrix3f(Matrix3f m1) {
+ this.m00 = m1.m00;
+ this.m01 = m1.m01;
+ this.m02 = m1.m02;
+
+ this.m10 = m1.m10;
+ this.m11 = m1.m11;
+ this.m12 = m1.m12;
+
+ this.m20 = m1.m20;
+ this.m21 = m1.m21;
+ this.m22 = m1.m22;
+ }
+
+ /**
+ * Constructs and initializes a Matrix3f to all zeros.
+ */
+ public Matrix3f() {
+ this.m00 = 0f;
+ this.m01 = 0f;
+ this.m02 = 0f;
+
+ this.m10 = 0f;
+ this.m11 = 0f;
+ this.m12 = 0f;
+
+ this.m20 = 0f;
+ this.m21 = 0f;
+ this.m22 = 0f;
+ }
+
+ /**
+ * Returns a string that contains the values of this Matrix3f.
+ *
+ * @return the String representation
+ */
+ @Override
+ public String toString() {
+ return this.m00 + ", " //$NON-NLS-1$
+ + this.m01 + ", " //$NON-NLS-1$
+ + this.m02 + "\n" //$NON-NLS-1$
+ + this.m10 + ", " //$NON-NLS-1$
+ + this.m11 + ", " //$NON-NLS-1$
+ + this.m12 + "\n" //$NON-NLS-1$
+ + this.m20 + ", " //$NON-NLS-1$
+ + this.m21 + ", " //$NON-NLS-1$
+ + this.m22 + "\n"; //$NON-NLS-1$
+ }
+
+ /**
+ * Sets this Matrix3f to identity.
+ */
+ public final void setIdentity() {
+ this.m00 = 1f;
+ this.m01 = 0f;
+ this.m02 = 0f;
+
+ this.m10 = 0f;
+ this.m11 = 1f;
+ this.m12 = 0f;
+
+ this.m20 = 0f;
+ this.m21 = 0f;
+ this.m22 = 1f;
+ }
+
+ /**
+ * Sets the specified element of this matrix3f to the value provided.
+ *
+ * @param row
+ * the row number to be modified (zero indexed)
+ * @param column
+ * the column number to be modified (zero indexed)
+ * @param value
+ * the new value
+ */
+ public final void setElement(int row, int column, float value) {
+ switch (row) {
+ case 0:
+ switch (column) {
+ case 0:
+ this.m00 = value;
+ break;
+ case 1:
+ this.m01 = value;
+ break;
+ case 2:
+ this.m02 = value;
+ break;
+ default:
+ throw new ArrayIndexOutOfBoundsException();
+ }
+ break;
+
+ case 1:
+ switch (column) {
+ case 0:
+ this.m10 = value;
+ break;
+ case 1:
+ this.m11 = value;
+ break;
+ case 2:
+ this.m12 = value;
+ break;
+ default:
+ throw new ArrayIndexOutOfBoundsException();
+ }
+ break;
+
+ case 2:
+ switch (column) {
+ case 0:
+ this.m20 = value;
+ break;
+ case 1:
+ this.m21 = value;
+ break;
+ case 2:
+ this.m22 = value;
+ break;
+ default:
+ throw new ArrayIndexOutOfBoundsException();
+ }
+ break;
+
+ default:
+ throw new ArrayIndexOutOfBoundsException();
+ }
+ }
+
+ /**
+ * Retrieves the value at the specified row and column of the specified
+ * matrix.
+ *
+ * @param row
+ * the row number to be retrieved (zero indexed)
+ * @param column
+ * the column number to be retrieved (zero indexed)
+ * @return the value at the indexed element.
+ */
+ public final float getElement(int row, int column) {
+ switch (row) {
+ case 0:
+ switch (column) {
+ case 0:
+ return (this.m00);
+ case 1:
+ return (this.m01);
+ case 2:
+ return (this.m02);
+ default:
+ break;
+ }
+ break;
+ case 1:
+ switch (column) {
+ case 0:
+ return (this.m10);
+ case 1:
+ return (this.m11);
+ case 2:
+ return (this.m12);
+ default:
+ break;
+ }
+ break;
+
+ case 2:
+ switch (column) {
+ case 0:
+ return (this.m20);
+ case 1:
+ return (this.m21);
+ case 2:
+ return (this.m22);
+ default:
+ break;
+ }
+ break;
+
+ default:
+ break;
+ }
+
+ throw new ArrayIndexOutOfBoundsException();
+ }
+
+ /**
+ * Copies the matrix values in the specified row into the vector parameter.
+ *
+ * @param row
+ * the matrix row
+ * @param v
+ * the vector into which the matrix row values will be copied
+ */
+ public final void getRow(int row, Vector3f v) {
+ if (row == 0) {
+ v.set(this.m00, this.m01, this.m02);
+ } else if (row == 1) {
+ v.set(this.m10, this.m11, this.m12);
+ } else if (row == 2) {
+ v.set(this.m20, this.m21, this.m22);
+ } else {
+ throw new ArrayIndexOutOfBoundsException();
+ }
+
+ }
+
+ /**
+ * Copies the matrix values in the specified row into the array parameter.
+ *
+ * @param row
+ * the matrix row
+ * @param v
+ * the array into which the matrix row values will be copied
+ */
+ public final void getRow(int row, float v[]) {
+ if (row == 0) {
+ v[0] = this.m00;
+ v[1] = this.m01;
+ v[2] = this.m02;
+ } else if (row == 1) {
+ v[0] = this.m10;
+ v[1] = this.m11;
+ v[2] = this.m12;
+ } else if (row == 2) {
+ v[0] = this.m20;
+ v[1] = this.m21;
+ v[2] = this.m22;
+ } else {
+ throw new ArrayIndexOutOfBoundsException();
+ }
+
+ }
+
+ /**
+ * Copies the matrix values in the specified column into the vector
+ * parameter.
+ *
+ * @param column
+ * the matrix column
+ * @param v
+ * the vector into which the matrix row values will be copied
+ */
+ public final void getColumn(int column, Vector3f v) {
+ if (column == 0) {
+ v.set(this.m00, this.m10, this.m20);
+ } else if (column == 1) {
+ v.set(this.m01, this.m11, this.m21);
+ } else if (column == 2) {
+ v.set(this.m02, this.m12, this.m22);
+ } else {
+ throw new ArrayIndexOutOfBoundsException();
+ }
+
+ }
+
+ /**
+ * Copies the matrix values in the specified column into the array
+ * parameter.
+ *
+ * @param column
+ * the matrix column
+ * @param v
+ * the array into which the matrix row values will be copied
+ */
+ public final void getColumn(int column, float v[]) {
+ if (column == 0) {
+ v[0] = this.m00;
+ v[1] = this.m10;
+ v[2] = this.m20;
+ } else if (column == 1) {
+ v[0] = this.m01;
+ v[1] = this.m11;
+ v[2] = this.m21;
+ } else if (column == 2) {
+ v[0] = this.m02;
+ v[1] = this.m12;
+ v[2] = this.m22;
+ } else {
+ throw new ArrayIndexOutOfBoundsException();
+ }
+
+ }
+
+ /**
+ * Sets the specified row of this Matrix3f to the 4 values provided.
+ *
+ * @param row
+ * the row number to be modified (zero indexed)
+ * @param x
+ * the first column element
+ * @param y
+ * the second column element
+ * @param z
+ * the third column element
+ */
+ public final void setRow(int row, float x, float y, float z) {
+ switch (row) {
+ case 0:
+ this.m00 = x;
+ this.m01 = y;
+ this.m02 = z;
+ break;
+
+ case 1:
+ this.m10 = x;
+ this.m11 = y;
+ this.m12 = z;
+ break;
+
+ case 2:
+ this.m20 = x;
+ this.m21 = y;
+ this.m22 = z;
+ break;
+
+ default:
+ throw new ArrayIndexOutOfBoundsException();
+ }
+ }
+
+ /**
+ * Sets the specified row of this Matrix3f to the Vector provided.
+ *
+ * @param row
+ * the row number to be modified (zero indexed)
+ * @param v
+ * the replacement row
+ */
+ public final void setRow(int row, Vector3f v) {
+ switch (row) {
+ case 0:
+ this.m00 = v.getX();
+ this.m01 = v.getY();
+ this.m02 = v.getZ();
+ break;
+
+ case 1:
+ this.m10 = v.getX();
+ this.m11 = v.getY();
+ this.m12 = v.getZ();
+ break;
+
+ case 2:
+ this.m20 = v.getX();
+ this.m21 = v.getY();
+ this.m22 = v.getZ();
+ break;
+
+ default:
+ throw new ArrayIndexOutOfBoundsException();
+ }
+ }
+
+ /**
+ * Sets the specified row of this Matrix3f to the three values provided.
+ *
+ * @param row
+ * the row number to be modified (zero indexed)
+ * @param v
+ * the replacement row
+ */
+ public final void setRow(int row, float v[]) {
+ switch (row) {
+ case 0:
+ this.m00 = v[0];
+ this.m01 = v[1];
+ this.m02 = v[2];
+ break;
+
+ case 1:
+ this.m10 = v[0];
+ this.m11 = v[1];
+ this.m12 = v[2];
+ break;
+
+ case 2:
+ this.m20 = v[0];
+ this.m21 = v[1];
+ this.m22 = v[2];
+ break;
+
+ default:
+ throw new ArrayIndexOutOfBoundsException();
+ }
+ }
+
+ /**
+ * Sets the specified column of this Matrix3f to the three values provided.
+ *
+ * @param column
+ * the column number to be modified (zero indexed)
+ * @param x
+ * the first row element
+ * @param y
+ * the second row element
+ * @param z
+ * the third row element
+ */
+ public final void setColumn(int column, float x, float y, float z) {
+ switch (column) {
+ case 0:
+ this.m00 = x;
+ this.m10 = y;
+ this.m20 = z;
+ break;
+
+ case 1:
+ this.m01 = x;
+ this.m11 = y;
+ this.m21 = z;
+ break;
+
+ case 2:
+ this.m02 = x;
+ this.m12 = y;
+ this.m22 = z;
+ break;
+
+ default:
+ throw new ArrayIndexOutOfBoundsException();
+ }
+ }
+
+ /**
+ * Sets the specified column of this Matrix3f to the vector provided.
+ *
+ * @param column
+ * the column number to be modified (zero indexed)
+ * @param v
+ * the replacement column
+ */
+ public final void setColumn(int column, Vector3f v) {
+ switch (column) {
+ case 0:
+ this.m00 = v.getX();
+ this.m10 = v.getY();
+ this.m20 = v.getZ();
+ break;
+
+ case 1:
+ this.m01 = v.getX();
+ this.m11 = v.getY();
+ this.m21 = v.getZ();
+ break;
+
+ case 2:
+ this.m02 = v.getX();
+ this.m12 = v.getY();
+ this.m22 = v.getZ();
+ break;
+
+ default:
+ throw new ArrayIndexOutOfBoundsException();
+ }
+ }
+
+ /**
+ * Sets the specified column of this Matrix3f to the three values provided.
+ *
+ * @param column
+ * the column number to be modified (zero indexed)
+ * @param v
+ * the replacement column
+ */
+ public final void setColumn(int column, float v[]) {
+ switch (column) {
+ case 0:
+ this.m00 = v[0];
+ this.m10 = v[1];
+ this.m20 = v[2];
+ break;
+
+ case 1:
+ this.m01 = v[0];
+ this.m11 = v[1];
+ this.m21 = v[2];
+ break;
+
+ case 2:
+ this.m02 = v[0];
+ this.m12 = v[1];
+ this.m22 = v[2];
+ break;
+
+ default:
+ throw new ArrayIndexOutOfBoundsException();
+ }
+ }
+
+ /**
+ * Adds a scalar to each component of this matrix.
+ *
+ * @param scalar
+ * the scalar adder
+ */
+ public final void add(float scalar) {
+ this.m00 += scalar;
+ this.m01 += scalar;
+ this.m02 += scalar;
+
+ this.m10 += scalar;
+ this.m11 += scalar;
+ this.m12 += scalar;
+
+ this.m20 += scalar;
+ this.m21 += scalar;
+ this.m22 += scalar;
+
+ }
+
+ /**
+ * Adds a scalar to each component of the matrix m1 and places the result
+ * into this. Matrix m1 is not modified.
+ *
+ * @param scalar
+ * the scalar adder
+ * @param m1
+ * the original matrix values
+ */
+ public final void add(float scalar, Matrix3f m1) {
+ this.m00 = m1.m00 + scalar;
+ this.m01 = m1.m01 + scalar;
+ this.m02 = m1.m02 + scalar;
+
+ this.m10 = m1.m10 + scalar;
+ this.m11 = m1.m11 + scalar;
+ this.m12 = m1.m12 + scalar;
+
+ this.m20 = m1.m20 + scalar;
+ this.m21 = m1.m21 + scalar;
+ this.m22 = m1.m22 + scalar;
+ }
+
+ /**
+ * Sets the value of this matrix to the matrix sum of matrices m1 and m2.
+ *
+ * @param m1
+ * the first matrix
+ * @param m2
+ * the second matrix
+ */
+ public final void add(Matrix3f m1, Matrix3f m2) {
+ this.m00 = m1.m00 + m2.m00;
+ this.m01 = m1.m01 + m2.m01;
+ this.m02 = m1.m02 + m2.m02;
+
+ this.m10 = m1.m10 + m2.m10;
+ this.m11 = m1.m11 + m2.m11;
+ this.m12 = m1.m12 + m2.m12;
+
+ this.m20 = m1.m20 + m2.m20;
+ this.m21 = m1.m21 + m2.m21;
+ this.m22 = m1.m22 + m2.m22;
+ }
+
+ /**
+ * Sets the value of this matrix to the sum of itself and matrix m1.
+ *
+ * @param m1
+ * the other matrix
+ */
+ public final void add(Matrix3f m1) {
+ this.m00 += m1.m00;
+ this.m01 += m1.m01;
+ this.m02 += m1.m02;
+
+ this.m10 += m1.m10;
+ this.m11 += m1.m11;
+ this.m12 += m1.m12;
+
+ this.m20 += m1.m20;
+ this.m21 += m1.m21;
+ this.m22 += m1.m22;
+ }
+
+ /**
+ * Sets the value of this matrix to the matrix difference of matrices m1 and
+ * m2.
+ *
+ * @param m1
+ * the first matrix
+ * @param m2
+ * the second matrix
+ */
+ public final void sub(Matrix3f m1, Matrix3f m2) {
+ this.m00 = m1.m00 - m2.m00;
+ this.m01 = m1.m01 - m2.m01;
+ this.m02 = m1.m02 - m2.m02;
+
+ this.m10 = m1.m10 - m2.m10;
+ this.m11 = m1.m11 - m2.m11;
+ this.m12 = m1.m12 - m2.m12;
+
+ this.m20 = m1.m20 - m2.m20;
+ this.m21 = m1.m21 - m2.m21;
+ this.m22 = m1.m22 - m2.m22;
+ }
+
+ /**
+ * Sets the value of this matrix to the matrix difference of itself and
+ * matrix m1 (this = this - m1).
+ *
+ * @param m1
+ * the other matrix
+ */
+ public final void sub(Matrix3f m1) {
+ this.m00 -= m1.m00;
+ this.m01 -= m1.m01;
+ this.m02 -= m1.m02;
+
+ this.m10 -= m1.m10;
+ this.m11 -= m1.m11;
+ this.m12 -= m1.m12;
+
+ this.m20 -= m1.m20;
+ this.m21 -= m1.m21;
+ this.m22 -= m1.m22;
+ }
+
+ /**
+ * Sets the value of this matrix to its transpose.
+ */
+ public final void transpose() {
+ float temp;
+
+ temp = this.m10;
+ this.m10 = this.m01;
+ this.m01 = temp;
+
+ temp = this.m20;
+ this.m20 = this.m02;
+ this.m02 = temp;
+
+ temp = this.m21;
+ this.m21 = this.m12;
+ this.m12 = temp;
+ }
+
+ /**
+ * Sets the value of this matrix to the transpose of the argument matrix.
+ *
+ * @param m1
+ * the matrix to be transposed
+ */
+ public final void transpose(Matrix3f m1) {
+ if (this != m1) {
+ this.m00 = m1.m00;
+ this.m01 = m1.m10;
+ this.m02 = m1.m20;
+
+ this.m10 = m1.m01;
+ this.m11 = m1.m11;
+ this.m12 = m1.m21;
+
+ this.m20 = m1.m02;
+ this.m21 = m1.m12;
+ this.m22 = m1.m22;
+ } else
+ this.transpose();
+ }
+
+ /**
+ * Sets the value of this matrix to the float value of the Matrix3f
+ * argument.
+ *
+ * @param m1
+ * the Matrix3f to be converted to float
+ */
+ public final void set(Matrix3f m1) {
+ this.m00 = m1.m00;
+ this.m01 = m1.m01;
+ this.m02 = m1.m02;
+
+ this.m10 = m1.m10;
+ this.m11 = m1.m11;
+ this.m12 = m1.m12;
+
+ this.m20 = m1.m20;
+ this.m21 = m1.m21;
+ this.m22 = m1.m22;
+ }
+
+ /**
+ * Sets the values in this Matrix3f equal to the row-major array parameter
+ * (ie, the first three elements of the array will be copied into the first
+ * row of this matrix, etc.).
+ *
+ * @param m
+ * the float precision array of length 9
+ */
+ public final void set(float[] m) {
+ this.m00 = m[0];
+ this.m01 = m[1];
+ this.m02 = m[2];
+
+ this.m10 = m[3];
+ this.m11 = m[4];
+ this.m12 = m[5];
+
+ this.m20 = m[6];
+ this.m21 = m[7];
+ this.m22 = m[8];
+
+ }
+
+ /**
+ * Set the components of the matrix.
+ *
+ * @param m00
+ * the [0][0] element
+ * @param m01
+ * the [0][1] element
+ * @param m02
+ * the [0][2] element
+ * @param m10
+ * the [1][0] element
+ * @param m11
+ * the [1][1] element
+ * @param m12
+ * the [1][2] element
+ * @param m20
+ * the [2][0] element
+ * @param m21
+ * the [2][1] element
+ * @param m22
+ * the [2][2] element
+ */
+ public void set(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22) {
+ this.m00 = m00;
+ this.m01 = m01;
+ this.m02 = m02;
+
+ this.m10 = m10;
+ this.m11 = m11;
+ this.m12 = m12;
+
+ this.m20 = m20;
+ this.m21 = m21;
+ this.m22 = m22;
+ }
+
+ /**
+ * Sets the value of this matrix to the matrix inverse of the passed matrix
+ * m1.
+ *
+ * @param m1
+ * the matrix to be inverted
+ */
+ public void invert(Matrix3f m1) {
+ invertGeneral(m1);
+ }
+
+ /**
+ * Inverts this matrix in place.
+ */
+ public void invert() {
+ invertGeneral(this);
+ }
+
+ /**
+ * General invert routine. Inverts m1 and places the result in "this". Note
+ * that this routine handles both the "this" version and the non-"this"
+ * version.
+ *
+ * Also note that since this routine is slow anyway, we won't worry about
+ * allocating a little bit of garbage.
+ */
+ private final void invertGeneral(Matrix3f m1) {
+ float result[] = new float[9];
+ int row_perm[] = new int[3];
+ int i;
+ float[] tmp = new float[9]; // scratch matrix
+
+ // Use LU decomposition and backsubstitution code specifically
+ // for floating-point 3x3 matrices.
+
+ // Copy source matrix to t1tmp
+ tmp[0] = m1.m00;
+ tmp[1] = m1.m01;
+ tmp[2] = m1.m02;
+
+ tmp[3] = m1.m10;
+ tmp[4] = m1.m11;
+ tmp[5] = m1.m12;
+
+ tmp[6] = m1.m20;
+ tmp[7] = m1.m21;
+ tmp[8] = m1.m22;
+
+ // Calculate LU decomposition: Is the matrix singular?
+ if (!luDecomposition(tmp, row_perm)) {
+ throw new SingularMatrixException(Locale.getString("NOT_INVERTABLE_MATRIX")); //$NON-NLS-1$
+ }
+
+ // Perform back substitution on the identity matrix
+ for (i = 0; i < 9; ++i)
+ result[i] = 0f;
+ result[0] = 1f;
+ result[4] = 1f;
+ result[8] = 1f;
+ luBacksubstitution(tmp, row_perm, result);
+
+ this.m00 = result[0];
+ this.m01 = result[1];
+ this.m02 = result[2];
+
+ this.m10 = result[3];
+ this.m11 = result[4];
+ this.m12 = result[5];
+
+ this.m20 = result[6];
+ this.m21 = result[7];
+ this.m22 = result[8];
+
+ }
+
+ /**
+ * Given a 3x3 array "matrix0", this function replaces it with the LU
+ * decomposition of a row-wise permutation of itself. The input parameters
+ * are "matrix0" and "dimen". The array "matrix0" is also an output
+ * parameter. The vector "row_perm[3]" is an output parameter that contains
+ * the row permutations resulting from partial pivoting. The output
+ * parameter "even_row_xchg" is 1 when the number of row exchanges is even,
+ * or -1 otherwise. Assumes data type is always float.
+ *
+ * This function is similar to luDecomposition, except that it is tuned
+ * specifically for 3x3 matrices.
+ *
+ * @return true if the matrix is nonsingular, or false otherwise.
+ */
+ //
+ // Reference: Press, Flannery, Teukolsky, Vetterling,
+ // _Numerical_Recipes_in_C_, Cambridge University Press,
+ // 1988, pp 40-45.
+ //
+ private static boolean luDecomposition(float[] matrix0, int[] row_perm) {
+
+ float row_scale[] = new float[3];
+
+ // Determine implicit scaling information by looping over rows
+ {
+ int i, j;
+ int ptr, rs;
+ float big, temp;
+
+ ptr = 0;
+ rs = 0;
+
+ // For each row ...
+ i = 3;
+ while (i-- != 0) {
+ big = 0f;
+
+ // For each column, find the largest element in the row
+ j = 3;
+ while (j-- != 0) {
+ temp = matrix0[ptr++];
+ temp = Math.abs(temp);
+ if (temp > big) {
+ big = temp;
+ }
+ }
+
+ // Is the matrix singular?
+ if (big == 0f) {
+ return false;
+ }
+ row_scale[rs++] = 1f / big;
+ }
+ }
+
+ {
+ int j;
+ int mtx;
+
+ mtx = 0;
+
+ // For all columns, execute Crout's method
+ for (j = 0; j < 3; ++j) {
+ int i, imax, k;
+ int target, p1, p2;
+ float sum, big, temp;
+
+ // Determine elements of upper diagonal matrix U
+ for (i = 0; i < j; ++i) {
+ target = mtx + (3 * i) + j;
+ sum = matrix0[target];
+ k = i;
+ p1 = mtx + (3 * i);
+ p2 = mtx + j;
+ while (k-- != 0) {
+ sum -= matrix0[p1] * matrix0[p2];
+ ++p1;
+ p2 += 3;
+ }
+ matrix0[target] = sum;
+ }
+
+ // Search for largest pivot element and calculate
+ // intermediate elements of lower diagonal matrix L.
+ big = 0f;
+ imax = -1;
+ for (i = j; i < 3; ++i) {
+ target = mtx + (3 * i) + j;
+ sum = matrix0[target];
+ k = j;
+ p1 = mtx + (3 * i);
+ p2 = mtx + j;
+ while (k-- != 0) {
+ sum -= matrix0[p1] * matrix0[p2];
+ ++p1;
+ p2 += 3;
+ }
+ matrix0[target] = sum;
+
+ // Is this the best pivot so far?
+ if ((temp = row_scale[i] * Math.abs(sum)) >= big) {
+ big = temp;
+ imax = i;
+ }
+ }
+
+ if (imax < 0) {
+ throw new RuntimeException();
+ }
+
+ // Is a row exchange necessary?
+ if (j != imax) {
+ // Yes: exchange rows
+ k = 3;
+ p1 = mtx + (3 * imax);
+ p2 = mtx + (3 * j);
+ while (k-- != 0) {
+ temp = matrix0[p1];
+ matrix0[p1++] = matrix0[p2];
+ matrix0[p2++] = temp;
+ }
+
+ // Record change in scale factor
+ row_scale[imax] = row_scale[j];
+ }
+
+ // Record row permutation
+ row_perm[j] = imax;
+
+ // Is the matrix singular
+ if (matrix0[(mtx + (3 * j) + j)] == 0f) {
+ return false;
+ }
+
+ // Divide elements of lower diagonal matrix L by pivot
+ if (j != (3 - 1)) {
+ temp = 1f / (matrix0[(mtx + (3 * j) + j)]);
+ target = mtx + (3 * (j + 1)) + j;
+ i = 2 - j;
+ while (i-- != 0) {
+ matrix0[target] *= temp;
+ target += 3;
+ }
+ }
+ }
+ }
+
+ return true;
+ }
+
+ /**
+ * Solves a set of linear equations. The input parameters "matrix1", and
+ * "row_perm" come from luDecompostionD3x3 and do not change here. The
+ * parameter "matrix2" is a set of column vectors assembled into a 3x3
+ * matrix of floating-point values. The procedure takes each column of
+ * "matrix2" in turn and treats it as the right-hand side of the matrix
+ * equation Ax = LUx = b. The solution vector replaces the original column
+ * of the matrix.
+ *
+ * If "matrix2" is the identity matrix, the procedure replaces its contents
+ * with the inverse of the matrix from which "matrix1" was originally
+ * derived.
+ */
+ //
+ // Reference: Press, Flannery, Teukolsky, Vetterling,
+ // _Numerical_Recipes_in_C_, Cambridge University Press,
+ // 1988, pp 44-45.
+ //
+ private static void luBacksubstitution(float[] matrix1, int[] row_perm,
+ float[] matrix2) {
+
+ int i, ii, ip, j, k;
+ int rp;
+ int cv, rv;
+
+ // rp = row_perm;
+ rp = 0;
+
+ // For each column vector of matrix2 ...
+ for (k = 0; k < 3; ++k) {
+ // cv = &(matrix2[0][k]);
+ cv = k;
+ ii = -1;
+
+ // Forward substitution
+ for (i = 0; i < 3; ++i) {
+ float sum;
+
+ ip = row_perm[rp + i];
+ sum = matrix2[cv + 3 * ip];
+ matrix2[cv + 3 * ip] = matrix2[cv + 3 * i];
+ if (ii >= 0) {
+ // rv = &(matrix1[i][0]);
+ rv = i * 3;
+ for (j = ii; j <= i - 1; ++j) {
+ sum -= matrix1[rv + j] * matrix2[cv + 3 * j];
+ }
+ } else if (sum != 0f) {
+ ii = i;
+ }
+ matrix2[cv + 3 * i] = sum;
+ }
+
+ // Backsubstitution
+ // rv = &(matrix1[3][0]);
+ rv = 2 * 3;
+ matrix2[cv + 3 * 2] /= matrix1[rv + 2];
+
+ rv -= 3;
+ matrix2[cv + 3 * 1] = (matrix2[cv + 3 * 1] - matrix1[rv + 2]
+ * matrix2[cv + 3 * 2])
+ / matrix1[rv + 1];
+
+ rv -= 3;
+ matrix2[cv + 4 * 0] = (matrix2[cv + 3 * 0] - matrix1[rv + 1]
+ * matrix2[cv + 3 * 1] - matrix1[rv + 2]
+ * matrix2[cv + 3 * 2])
+ / matrix1[rv + 0];
+
+ }
+ }
+
+ /**
+ * Computes the determinant of this matrix.
+ *
+ * @return the determinant of the matrix
+ */
+ public final float determinant() {
+ /* det(A,B,C) = det( [ x1 x2 x3 ]
+ * [ y1 y2 y3 ]
+ * [ z1 z2 z3 ] )
+ */
+ return
+ this.m00 * (this.m11 * this.m22 - this.m21 * this.m12)
+ + this.m10 * (this.m21 * this.m02 - this.m01 * this.m22)
+ + this.m20 * (this.m01 * this.m12 - this.m11 * this.m02);
+ }
+
+ /**
+ * Multiplies each element of this matrix by a scalar.
+ *
+ * @param scalar
+ * The scalar multiplier.
+ */
+ public final void mul(float scalar) {
+ this.m00 *= scalar;
+ this.m01 *= scalar;
+ this.m02 *= scalar;
+
+ this.m10 *= scalar;
+ this.m11 *= scalar;
+ this.m12 *= scalar;
+
+ this.m20 *= scalar;
+ this.m21 *= scalar;
+ this.m22 *= scalar;
+ }
+
+ /** Multiply this matrix by the given vector.
+ *
+ * @param v
+ * @return the vector resulting of this * v.
+ */
+ public Vector3f mul(Vector3f v) {
+ return new Vector3f(
+ this.m00 * v.getX() + this.m01 * v.getY() + this.m02 * v.getZ(),
+ this.m10 * v.getX() + this.m11 * v.getY() + this.m12 * v.getZ(),
+ this.m20 * v.getX() + this.m21 * v.getY() + this.m22 * v.getZ());
+ }
+
+ /** Multiply the transposing of this matrix by the given vector.
+ *
+ * @param v
+ * @return the vector resulting of transpose(this) * v.
+ */
+ public Vector3f mulTransposeLeft(Vector3f v) {
+ return new Vector3f(
+ this.m00 * v.getX() + this.m10 * v.getY() + this.m20 * v.getZ(),
+ this.m01 * v.getX() + this.m11 * v.getY() + this.m21 * v.getZ(),
+ this.m02 * v.getX() + this.m12 * v.getY() + this.m22 * v.getZ());
+ }
+
+ /**
+ * Multiplies each element of matrix m1 by a scalar and places the result
+ * into this. Matrix m1 is not modified.
+ *
+ * @param scalar
+ * the scalar multiplier
+ * @param m1
+ * the original matrix
+ */
+ public final void mul(float scalar, Matrix3f m1) {
+ this.m00 = scalar * m1.m00;
+ this.m01 = scalar * m1.m01;
+ this.m02 = scalar * m1.m02;
+
+ this.m10 = scalar * m1.m10;
+ this.m11 = scalar * m1.m11;
+ this.m12 = scalar * m1.m12;
+
+ this.m20 = scalar * m1.m20;
+ this.m21 = scalar * m1.m21;
+ this.m22 = scalar * m1.m22;
+ }
+
+ /**
+ * Sets the value of this matrix to the result of multiplying itself with
+ * matrix m1.
+ *
+ * @param m1
+ * the other matrix
+ */
+ public final void mul(Matrix3f m1) {
+ float _m00, _m01, _m02, _m10, _m11, _m12, _m20, _m21, _m22;
+
+ _m00 = this.m00 * m1.m00 + this.m01 * m1.m10 + this.m02 * m1.m20;
+ _m01 = this.m00 * m1.m01 + this.m01 * m1.m11 + this.m02 * m1.m21;
+ _m02 = this.m00 * m1.m02 + this.m01 * m1.m12 + this.m02 * m1.m22;
+
+ _m10 = this.m10 * m1.m00 + this.m11 * m1.m10 + this.m12 * m1.m20;
+ _m11 = this.m10 * m1.m01 + this.m11 * m1.m11 + this.m12 * m1.m21;
+ _m12 = this.m10 * m1.m02 + this.m11 * m1.m12 + this.m12 * m1.m22;
+
+ _m20 = this.m20 * m1.m00 + this.m21 * m1.m10 + this.m22 * m1.m20;
+ _m21 = this.m20 * m1.m01 + this.m21 * m1.m11 + this.m22 * m1.m21;
+ _m22 = this.m20 * m1.m02 + this.m21 * m1.m12 + this.m22 * m1.m22;
+
+ this.m00 = _m00;
+ this.m01 = _m01;
+ this.m02 = _m02;
+ this.m10 = _m10;
+ this.m11 = _m11;
+ this.m12 = _m12;
+ this.m20 = _m20;
+ this.m21 = _m21;
+ this.m22 = _m22;
+ }
+
+ /**
+ * Sets the value of this matrix to the result of multiplying the two
+ * argument matrices together.
+ *
+ * @param m1
+ * the first matrix
+ * @param m2
+ * the second matrix
+ */
+ public final void mul(Matrix3f m1, Matrix3f m2) {
+ if (this != m1 && this != m2) {
+ this.m00 = m1.m00 * m2.m00 + m1.m01 * m2.m10 + m1.m02 * m2.m20;
+ this.m01 = m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21;
+ this.m02 = m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22;
+
+ this.m10 = m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20;
+ this.m11 = m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21;
+ this.m12 = m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22;
+
+ this.m20 = m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20;
+ this.m21 = m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21;
+ this.m22 = m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22;
+ } else {
+ float _m00, _m01, _m02, _m10, _m11, _m12, _m20, _m21, _m22; // vars for temp
+ // result matrix
+
+ _m00 = m1.m00 * m2.m00 + m1.m01 * m2.m10 + m1.m02 * m2.m20;
+ _m01 = m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21;
+ _m02 = m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22;
+
+ _m10 = m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20;
+ _m11 = m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21;
+ _m12 = m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22;
+
+ _m20 = m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20;
+ _m21 = m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21;
+ _m22 = m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22;
+
+ this.m00 = _m00;
+ this.m01 = _m01;
+ this.m02 = _m02;
+ this.m10 = _m10;
+ this.m11 = _m11;
+ this.m12 = _m12;
+ this.m20 = _m20;
+ this.m21 = _m21;
+ this.m22 = _m22;
+ }
+ }
+
+ /**
+ * Multiplies this matrix by matrix m1, does an SVD normalization of the
+ * result, and places the result back into this matrix this =
+ * SVDnorm(this*m1).
+ *
+ * @param m1
+ * the matrix on the right hand side of the multiplication
+ */
+ public final void mulNormalize(Matrix3f m1) {
+
+ float[] tmp = new float[9]; // scratch matrix
+ float[] tmp_rot = new float[9]; // scratch matrix
+ float[] tmp_scale = new float[3]; // scratch matrix
+
+ tmp[0] = this.m00 * m1.m00 + this.m01 * m1.m10 + this.m02 * m1.m20;
+ tmp[1] = this.m00 * m1.m01 + this.m01 * m1.m11 + this.m02 * m1.m21;
+ tmp[2] = this.m00 * m1.m02 + this.m01 * m1.m12 + this.m02 * m1.m22;
+
+ tmp[3] = this.m10 * m1.m00 + this.m11 * m1.m10 + this.m12 * m1.m20;
+ tmp[4] = this.m10 * m1.m01 + this.m11 * m1.m11 + this.m12 * m1.m21;
+ tmp[5] = this.m10 * m1.m02 + this.m11 * m1.m12 + this.m12 * m1.m22;
+
+ tmp[6] = this.m20 * m1.m00 + this.m21 * m1.m10 + this.m22 * m1.m20;
+ tmp[7] = this.m20 * m1.m01 + this.m21 * m1.m11 + this.m22 * m1.m21;
+ tmp[8] = this.m20 * m1.m02 + this.m21 * m1.m12 + this.m22 * m1.m22;
+
+ compute_svd(tmp, tmp_scale, tmp_rot);
+
+ this.m00 = tmp_rot[0];
+ this.m01 = tmp_rot[1];
+ this.m02 = tmp_rot[2];
+
+ this.m10 = tmp_rot[3];
+ this.m11 = tmp_rot[4];
+ this.m12 = tmp_rot[5];
+
+ this.m20 = tmp_rot[6];
+ this.m21 = tmp_rot[7];
+ this.m22 = tmp_rot[8];
+
+ }
+
+ /**
+ * Multiplies matrix m1 by matrix m2, does an SVD normalization of the
+ * result, and places the result into this matrix this = SVDnorm(m1*m2).
+ *
+ * @param m1
+ * the matrix on the left hand side of the multiplication
+ * @param m2
+ * the matrix on the right hand side of the multiplication
+ */
+ public final void mulNormalize(Matrix3f m1, Matrix3f m2) {
+
+ float[] tmp = new float[9]; // scratch matrix
+ float[] tmp_rot = new float[9]; // scratch matrix
+ float[] tmp_scale = new float[3]; // scratch matrix
+
+ tmp[0] = m1.m00 * m2.m00 + m1.m01 * m2.m10 + m1.m02 * m2.m20;
+ tmp[1] = m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21;
+ tmp[2] = m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22;
+
+ tmp[3] = m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20;
+ tmp[4] = m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21;
+ tmp[5] = m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22;
+
+ tmp[6] = m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20;
+ tmp[7] = m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21;
+ tmp[8] = m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22;
+
+ compute_svd(tmp, tmp_scale, tmp_rot);
+
+ this.m00 = tmp_rot[0];
+ this.m01 = tmp_rot[1];
+ this.m02 = tmp_rot[2];
+
+ this.m10 = tmp_rot[3];
+ this.m11 = tmp_rot[4];
+ this.m12 = tmp_rot[5];
+
+ this.m20 = tmp_rot[6];
+ this.m21 = tmp_rot[7];
+ this.m22 = tmp_rot[8];
+
+ }
+
+ /**
+ * Multiplies the transpose of matrix m1 times the transpose of matrix m2,
+ * and places the result into this.
+ *
+ * @param m1
+ * the matrix on the left hand side of the multiplication
+ * @param m2
+ * the matrix on the right hand side of the multiplication
+ */
+ public final void mulTransposeBoth(Matrix3f m1, Matrix3f m2) {
+ if (this != m1 && this != m2) {
+ this.m00 = m1.m00 * m2.m00 + m1.m10 * m2.m01 + m1.m20 * m2.m02;
+ this.m01 = m1.m00 * m2.m10 + m1.m10 * m2.m11 + m1.m20 * m2.m12;
+ this.m02 = m1.m00 * m2.m20 + m1.m10 * m2.m21 + m1.m20 * m2.m22;
+
+ this.m10 = m1.m01 * m2.m00 + m1.m11 * m2.m01 + m1.m21 * m2.m02;
+ this.m11 = m1.m01 * m2.m10 + m1.m11 * m2.m11 + m1.m21 * m2.m12;
+ this.m12 = m1.m01 * m2.m20 + m1.m11 * m2.m21 + m1.m21 * m2.m22;
+
+ this.m20 = m1.m02 * m2.m00 + m1.m12 * m2.m01 + m1.m22 * m2.m02;
+ this.m21 = m1.m02 * m2.m10 + m1.m12 * m2.m11 + m1.m22 * m2.m12;
+ this.m22 = m1.m02 * m2.m20 + m1.m12 * m2.m21 + m1.m22 * m2.m22;
+ } else {
+ float _m00, _m01, _m02, _m10, _m11, _m12, _m20, _m21, _m22; // vars for temp
+ // result matrix
+
+ _m00 = m1.m00 * m2.m00 + m1.m10 * m2.m01 + m1.m20 * m2.m02;
+ _m01 = m1.m00 * m2.m10 + m1.m10 * m2.m11 + m1.m20 * m2.m12;
+ _m02 = m1.m00 * m2.m20 + m1.m10 * m2.m21 + m1.m20 * m2.m22;
+
+ _m10 = m1.m01 * m2.m00 + m1.m11 * m2.m01 + m1.m21 * m2.m02;
+ _m11 = m1.m01 * m2.m10 + m1.m11 * m2.m11 + m1.m21 * m2.m12;
+ _m12 = m1.m01 * m2.m20 + m1.m11 * m2.m21 + m1.m21 * m2.m22;
+
+ _m20 = m1.m02 * m2.m00 + m1.m12 * m2.m01 + m1.m22 * m2.m02;
+ _m21 = m1.m02 * m2.m10 + m1.m12 * m2.m11 + m1.m22 * m2.m12;
+ _m22 = m1.m02 * m2.m20 + m1.m12 * m2.m21 + m1.m22 * m2.m22;
+
+ this.m00 = _m00;
+ this.m01 = _m01;
+ this.m02 = _m02;
+ this.m10 = _m10;
+ this.m11 = _m11;
+ this.m12 = _m12;
+ this.m20 = _m20;
+ this.m21 = _m21;
+ this.m22 = _m22;
+ }
+
+ }
+
+ /**
+ * Multiplies matrix m1 times the transpose of matrix m2, and places the
+ * result into this.
+ *
+ * @param m1
+ * the matrix on the left hand side of the multiplication
+ * @param m2
+ * the matrix on the right hand side of the multiplication
+ */
+ public final void mulTransposeRight(Matrix3f m1, Matrix3f m2) {
+ if (this != m1 && this != m2) {
+ this.m00 = m1.m00 * m2.m00 + m1.m01 * m2.m01 + m1.m02 * m2.m02;
+ this.m01 = m1.m00 * m2.m10 + m1.m01 * m2.m11 + m1.m02 * m2.m12;
+ this.m02 = m1.m00 * m2.m20 + m1.m01 * m2.m21 + m1.m02 * m2.m22;
+
+ this.m10 = m1.m10 * m2.m00 + m1.m11 * m2.m01 + m1.m12 * m2.m02;
+ this.m11 = m1.m10 * m2.m10 + m1.m11 * m2.m11 + m1.m12 * m2.m12;
+ this.m12 = m1.m10 * m2.m20 + m1.m11 * m2.m21 + m1.m12 * m2.m22;
+
+ this.m20 = m1.m20 * m2.m00 + m1.m21 * m2.m01 + m1.m22 * m2.m02;
+ this.m21 = m1.m20 * m2.m10 + m1.m21 * m2.m11 + m1.m22 * m2.m12;
+ this.m22 = m1.m20 * m2.m20 + m1.m21 * m2.m21 + m1.m22 * m2.m22;
+ } else {
+ float _m00, _m01, _m02, _m10, _m11, _m12, _m20, _m21, _m22; // vars for temp
+ // result matrix
+
+ _m00 = m1.m00 * m2.m00 + m1.m01 * m2.m01 + m1.m02 * m2.m02;
+ _m01 = m1.m00 * m2.m10 + m1.m01 * m2.m11 + m1.m02 * m2.m12;
+ _m02 = m1.m00 * m2.m20 + m1.m01 * m2.m21 + m1.m02 * m2.m22;
+
+ _m10 = m1.m10 * m2.m00 + m1.m11 * m2.m01 + m1.m12 * m2.m02;
+ _m11 = m1.m10 * m2.m10 + m1.m11 * m2.m11 + m1.m12 * m2.m12;
+ _m12 = m1.m10 * m2.m20 + m1.m11 * m2.m21 + m1.m12 * m2.m22;
+
+ _m20 = m1.m20 * m2.m00 + m1.m21 * m2.m01 + m1.m22 * m2.m02;
+ _m21 = m1.m20 * m2.m10 + m1.m21 * m2.m11 + m1.m22 * m2.m12;
+ _m22 = m1.m20 * m2.m20 + m1.m21 * m2.m21 + m1.m22 * m2.m22;
+
+ this.m00 = _m00;
+ this.m01 = _m01;
+ this.m02 = _m02;
+ this.m10 = _m10;
+ this.m11 = _m11;
+ this.m12 = _m12;
+ this.m20 = _m20;
+ this.m21 = _m21;
+ this.m22 = _m22;
+ }
+ }
+
+ /**
+ * Multiplies the transpose of matrix m1 times matrix m2, and places the
+ * result into this.
+ *
+ * @param m1
+ * the matrix on the left hand side of the multiplication
+ * @param m2
+ * the matrix on the right hand side of the multiplication
+ */
+ public final void mulTransposeLeft(Matrix3f m1, Matrix3f m2) {
+ if (this != m1 && this != m2) {
+ this.m00 = m1.m00 * m2.m00 + m1.m10 * m2.m10 + m1.m20 * m2.m20;
+ this.m01 = m1.m00 * m2.m01 + m1.m10 * m2.m11 + m1.m20 * m2.m21;
+ this.m02 = m1.m00 * m2.m02 + m1.m10 * m2.m12 + m1.m20 * m2.m22;
+
+ this.m10 = m1.m01 * m2.m00 + m1.m11 * m2.m10 + m1.m21 * m2.m20;
+ this.m11 = m1.m01 * m2.m01 + m1.m11 * m2.m11 + m1.m21 * m2.m21;
+ this.m12 = m1.m01 * m2.m02 + m1.m11 * m2.m12 + m1.m21 * m2.m22;
+
+ this.m20 = m1.m02 * m2.m00 + m1.m12 * m2.m10 + m1.m22 * m2.m20;
+ this.m21 = m1.m02 * m2.m01 + m1.m12 * m2.m11 + m1.m22 * m2.m21;
+ this.m22 = m1.m02 * m2.m02 + m1.m12 * m2.m12 + m1.m22 * m2.m22;
+ } else {
+ float _m00, _m01, _m02, _m10, _m11, _m12, _m20, _m21, _m22; // vars for temp
+ // result matrix
+
+ _m00 = m1.m00 * m2.m00 + m1.m10 * m2.m10 + m1.m20 * m2.m20;
+ _m01 = m1.m00 * m2.m01 + m1.m10 * m2.m11 + m1.m20 * m2.m21;
+ _m02 = m1.m00 * m2.m02 + m1.m10 * m2.m12 + m1.m20 * m2.m22;
+
+ _m10 = m1.m01 * m2.m00 + m1.m11 * m2.m10 + m1.m21 * m2.m20;
+ _m11 = m1.m01 * m2.m01 + m1.m11 * m2.m11 + m1.m21 * m2.m21;
+ _m12 = m1.m01 * m2.m02 + m1.m11 * m2.m12 + m1.m21 * m2.m22;
+
+ _m20 = m1.m02 * m2.m00 + m1.m12 * m2.m10 + m1.m22 * m2.m20;
+ _m21 = m1.m02 * m2.m01 + m1.m12 * m2.m11 + m1.m22 * m2.m21;
+ _m22 = m1.m02 * m2.m02 + m1.m12 * m2.m12 + m1.m22 * m2.m22;
+
+ this.m00 = _m00;
+ this.m01 = _m01;
+ this.m02 = _m02;
+ this.m10 = _m10;
+ this.m11 = _m11;
+ this.m12 = _m12;
+ this.m20 = _m20;
+ this.m21 = _m21;
+ this.m22 = _m22;
+ }
+ }
+
+ /**
+ * Perform singular value decomposition normalization of matrix m1 and place
+ * the normalized values into this.
+ *
+ * @param m1
+ * Provides the matrix values to be normalized
+ */
+ public final void normalize(Matrix3f m1) {
+
+ float[] tmp = new float[9]; // scratch matrix
+ float[] tmp_rot = new float[9]; // scratch matrix
+ float[] tmp_scale = new float[3]; // scratch matrix
+
+ tmp[0] = m1.m00;
+ tmp[1] = m1.m01;
+ tmp[2] = m1.m02;
+
+ tmp[3] = m1.m10;
+ tmp[4] = m1.m11;
+ tmp[5] = m1.m12;
+
+ tmp[6] = m1.m20;
+ tmp[7] = m1.m21;
+ tmp[8] = m1.m22;
+
+ compute_svd(tmp, tmp_scale, tmp_rot);
+
+ this.m00 = tmp_rot[0];
+ this.m01 = tmp_rot[1];
+ this.m02 = tmp_rot[2];
+
+ this.m10 = tmp_rot[3];
+ this.m11 = tmp_rot[4];
+ this.m12 = tmp_rot[5];
+
+ this.m20 = tmp_rot[6];
+ this.m21 = tmp_rot[7];
+ this.m22 = tmp_rot[8];
+ }
+
+ /**
+ * Perform cross product normalization of this matrix.
+ */
+
+ public final void normalizeCP() {
+ float mag = 1f / (float)Math.sqrt(this.m00 * this.m00 + this.m10 * this.m10 + this.m20 * this.m20);
+ this.m00 = this.m00 * mag;
+ this.m10 = this.m10 * mag;
+ this.m20 = this.m20 * mag;
+
+ mag = 1f / (float)Math.sqrt(this.m01 * this.m01 + this.m11 * this.m11 + this.m21 * this.m21);
+ this.m01 = this.m01 * mag;
+ this.m11 = this.m11 * mag;
+ this.m21 = this.m21 * mag;
+
+ this.m02 = this.m10 * this.m21 - this.m11 * this.m20;
+ this.m12 = this.m01 * this.m20 - this.m00 * this.m21;
+ this.m22 = this.m00 * this.m11 - this.m01 * this.m10;
+ }
+
+ /**
+ * Perform cross product normalization of matrix m1 and place the normalized
+ * values into this.
+ *
+ * @param m1
+ * Provides the matrix values to be normalized
+ */
+ public final void normalizeCP(Matrix3f m1) {
+ float mag = 1f / (float)Math.sqrt(m1.m00 * m1.m00 + m1.m10 * m1.m10 + m1.m20
+ * m1.m20);
+ this.m00 = m1.m00 * mag;
+ this.m10 = m1.m10 * mag;
+ this.m20 = m1.m20 * mag;
+
+ mag = 1f / (float)Math.sqrt(m1.m01 * m1.m01 + m1.m11 * m1.m11 + m1.m21
+ * m1.m21);
+ this.m01 = m1.m01 * mag;
+ this.m11 = m1.m11 * mag;
+ this.m21 = m1.m21 * mag;
+
+ this.m02 = this.m10 * this.m21 - this.m11 * this.m20;
+ this.m12 = this.m01 * this.m20 - this.m00 * this.m21;
+ this.m22 = this.m00 * this.m11 - this.m01 * this.m10;
+ }
+
+ /**
+ * Returns true if all of the data members of Matrix3f m1 are equal to the
+ * corresponding data members in this Matrix3f.
+ *
+ * @param m1
+ * the matrix with which the comparison is made
+ * @return true or false
+ */
+ public boolean equals(Matrix3f m1) {
+ try {
+ return (this.m00 == m1.m00 && this.m01 == m1.m01
+ && this.m02 == m1.m02 && this.m10 == m1.m10
+ && this.m11 == m1.m11 && this.m12 == m1.m12
+ && this.m20 == m1.m20 && this.m21 == m1.m21 && this.m22 == m1.m22);
+ } catch (NullPointerException e2) {
+ return false;
+ }
+
+ }
+
+ /**
+ * Returns true if the Object t1 is of type Matrix3f and all of the data
+ * members of t1 are equal to the corresponding data members in this
+ * Matrix3f.
+ *
+ * @param t1
+ * the matrix with which the comparison is made
+ * @return true or false
+ */
+ @Override
+ public boolean equals(Object t1) {
+ try {
+ Matrix3f m2 = (Matrix3f) t1;
+ return (this.m00 == m2.m00 && this.m01 == m2.m01
+ && this.m02 == m2.m02 && this.m10 == m2.m10
+ && this.m11 == m2.m11 && this.m12 == m2.m12
+ && this.m20 == m2.m20 && this.m21 == m2.m21 && this.m22 == m2.m22);
+ } catch (ClassCastException e1) {
+ return false;
+ } catch (NullPointerException e2) {
+ return false;
+ }
+
+ }
+
+ /**
+ * Returns true if the L-infinite distance between this matrix and matrix m1
+ * is less than or equal to the epsilon parameter, otherwise returns false.
+ * The L-infinite distance is equal to MAX[i=0,1,2 ; j=0,1,2 ;
+ * abs(this.m(i,j) - m1.m(i,j)]
+ *
+ * @param m1
+ * the matrix to be compared to this matrix
+ * @param epsilon
+ * the threshold value
+ * @return true if this matrix is equals to the specified matrix at epsilon.
+ */
+ public boolean epsilonEquals(Matrix3f m1, float epsilon) {
+ float diff;
+
+ diff = this.m00 - m1.m00;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ diff = this.m01 - m1.m01;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ diff = this.m02 - m1.m02;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ diff = this.m10 - m1.m10;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ diff = this.m11 - m1.m11;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ diff = this.m12 - m1.m12;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ diff = this.m20 - m1.m20;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ diff = this.m21 - m1.m21;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ diff = this.m22 - m1.m22;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ return true;
+ }
+
+ /**
+ * Returns a hash code value based on the data values in this object. Two
+ * different Matrix3f objects with identical data values (i.e.,
+ * Matrix3f.equals returns true) will return the same hash code value. Two
+ * objects with different data members may return the same hash value,
+ * although this is not likely.
+ *
+ * @return the integer hash code value
+ */
+ @Override
+ public int hashCode() {
+ long bits = 1L;
+ bits = 31L * bits + doubleToLongBits(this.m00);
+ bits = 31L * bits + doubleToLongBits(this.m01);
+ bits = 31L * bits + doubleToLongBits(this.m02);
+ bits = 31L * bits + doubleToLongBits(this.m10);
+ bits = 31L * bits + doubleToLongBits(this.m11);
+ bits = 31L * bits + doubleToLongBits(this.m12);
+ bits = 31L * bits + doubleToLongBits(this.m20);
+ bits = 31L * bits + doubleToLongBits(this.m21);
+ bits = 31L * bits + doubleToLongBits(this.m22);
+ return (int) (bits ^ (bits >> 32));
+ }
+
+ private static long doubleToLongBits(float d) {
+ // Check for +0 or -0
+ if (d == 0f) {
+ return 0L;
+ }
+ return Float.floatToIntBits(d);
+ }
+
+ /**
+ * Sets this matrix to all zeros.
+ */
+ public final void setZero() {
+ this.m00 = 0f;
+ this.m01 = 0f;
+ this.m02 = 0f;
+
+ this.m10 = 0f;
+ this.m11 = 0f;
+ this.m12 = 0f;
+
+ this.m20 = 0f;
+ this.m21 = 0f;
+ this.m22 = 0f;
+ }
+
+ /**
+ * Sets this matrix as diagonal.
+ *
+ * @param m00
+ * the first element of the diagonal
+ * @param m11
+ * the second element of the diagonal
+ * @param m22
+ * the third element of the diagonal
+ */
+ public final void setDiagonal(float m00, float m11, float m22) {
+ this.m00 = m00;
+ this.m01 = 0f;
+ this.m02 = 0f;
+ this.m10 = 0f;
+ this.m11 = m11;
+ this.m12 = 0f;
+ this.m20 = 0f;
+ this.m21 = 0f;
+ this.m22 = m22;
+ }
+
+ /**
+ * Negates the value of this matrix: this = -this.
+ */
+ public final void negate() {
+ this.m00 = -this.m00;
+ this.m01 = -this.m01;
+ this.m02 = -this.m02;
+
+ this.m10 = -this.m10;
+ this.m11 = -this.m11;
+ this.m12 = -this.m12;
+
+ this.m20 = -this.m20;
+ this.m21 = -this.m21;
+ this.m22 = -this.m22;
+
+ }
+
+ /**
+ * Sets the value of this matrix equal to the negation of of the Matrix3f
+ * parameter.
+ *
+ * @param m1
+ * the source matrix
+ */
+ public final void negate(Matrix3f m1) {
+ this.m00 = -m1.m00;
+ this.m01 = -m1.m01;
+ this.m02 = -m1.m02;
+
+ this.m10 = -m1.m10;
+ this.m11 = -m1.m11;
+ this.m12 = -m1.m12;
+
+ this.m20 = -m1.m20;
+ this.m21 = -m1.m21;
+ this.m22 = -m1.m22;
+ }
+
+ private static boolean epsilonEquals(float a, float b) {
+ return Math.abs(a-b) <= MathConstants.JVM_MIN_FLOAT_EPSILON;
+ }
+
+ private static void compute_svd(float[] m, float[] outScale, float[] outRot) {
+ int i;
+ float g;
+ float[] u1 = new float[9];
+ float[] v1 = new float[9];
+ float[] t1 = new float[9];
+ float[] t2 = new float[9];
+
+ float[] tmp = t1;
+ float[] single_values = t2;
+
+ float[] rot = new float[9];
+ float[] e = new float[3];
+ float[] scales = new float[3];
+
+ int negCnt = 0;
+ float c1, c2, c3, c4;
+ float s1, s2, s3, s4;
+
+ for (i = 0; i < 9; ++i)
+ rot[i] = m[i];
+
+ // u1
+
+ if (m[3] * m[3] < JVM_MIN_FLOAT_EPSILON) {
+ u1[0] = 1f;
+ u1[1] = 0f;
+ u1[2] = 0f;
+ u1[3] = 0f;
+ u1[4] = 1f;
+ u1[5] = 0f;
+ u1[6] = 0f;
+ u1[7] = 0f;
+ u1[8] = 1f;
+ } else if (m[0] * m[0] < JVM_MIN_FLOAT_EPSILON) {
+ tmp[0] = m[0];
+ tmp[1] = m[1];
+ tmp[2] = m[2];
+ m[0] = m[3];
+ m[1] = m[4];
+ m[2] = m[5];
+
+ m[3] = -tmp[0]; // zero
+ m[4] = -tmp[1];
+ m[5] = -tmp[2];
+
+ u1[0] = 0f;
+ u1[1] = 1f;
+ u1[2] = 0f;
+ u1[3] = -1f;
+ u1[4] = 0f;
+ u1[5] = 0f;
+ u1[6] = 0f;
+ u1[7] = 0f;
+ u1[8] = 1f;
+ } else {
+ g = 1f / (float)Math.sqrt(m[0] * m[0] + m[3] * m[3]);
+ c1 = m[0] * g;
+ s1 = m[3] * g;
+ tmp[0] = c1 * m[0] + s1 * m[3];
+ tmp[1] = c1 * m[1] + s1 * m[4];
+ tmp[2] = c1 * m[2] + s1 * m[5];
+
+ m[3] = -s1 * m[0] + c1 * m[3]; // zero
+ m[4] = -s1 * m[1] + c1 * m[4];
+ m[5] = -s1 * m[2] + c1 * m[5];
+
+ m[0] = tmp[0];
+ m[1] = tmp[1];
+ m[2] = tmp[2];
+ u1[0] = c1;
+ u1[1] = s1;
+ u1[2] = 0f;
+ u1[3] = -s1;
+ u1[4] = c1;
+ u1[5] = 0f;
+ u1[6] = 0f;
+ u1[7] = 0f;
+ u1[8] = 1f;
+ }
+
+ // u2
+
+ if (m[6] * m[6] < JVM_MIN_FLOAT_EPSILON) {
+ //
+ } else if (m[0] * m[0] < JVM_MIN_FLOAT_EPSILON) {
+ tmp[0] = m[0];
+ tmp[1] = m[1];
+ tmp[2] = m[2];
+ m[0] = m[6];
+ m[1] = m[7];
+ m[2] = m[8];
+
+ m[6] = -tmp[0]; // zero
+ m[7] = -tmp[1];
+ m[8] = -tmp[2];
+
+ tmp[0] = u1[0];
+ tmp[1] = u1[1];
+ tmp[2] = u1[2];
+ u1[0] = u1[6];
+ u1[1] = u1[7];
+ u1[2] = u1[8];
+
+ u1[6] = -tmp[0]; // zero
+ u1[7] = -tmp[1];
+ u1[8] = -tmp[2];
+ } else {
+ g = 1f / (float)Math.sqrt(m[0] * m[0] + m[6] * m[6]);
+ c2 = m[0] * g;
+ s2 = m[6] * g;
+ tmp[0] = c2 * m[0] + s2 * m[6];
+ tmp[1] = c2 * m[1] + s2 * m[7];
+ tmp[2] = c2 * m[2] + s2 * m[8];
+
+ m[6] = -s2 * m[0] + c2 * m[6];
+ m[7] = -s2 * m[1] + c2 * m[7];
+ m[8] = -s2 * m[2] + c2 * m[8];
+ m[0] = tmp[0];
+ m[1] = tmp[1];
+ m[2] = tmp[2];
+
+ tmp[0] = c2 * u1[0];
+ tmp[1] = c2 * u1[1];
+ u1[2] = s2;
+
+ tmp[6] = -u1[0] * s2;
+ tmp[7] = -u1[1] * s2;
+ u1[8] = c2;
+ u1[0] = tmp[0];
+ u1[1] = tmp[1];
+ u1[6] = tmp[6];
+ u1[7] = tmp[7];
+ }
+
+ // v1
+
+ if (m[2] * m[2] < JVM_MIN_FLOAT_EPSILON) {
+ v1[0] = 1f;
+ v1[1] = 0f;
+ v1[2] = 0f;
+ v1[3] = 0f;
+ v1[4] = 1f;
+ v1[5] = 0f;
+ v1[6] = 0f;
+ v1[7] = 0f;
+ v1[8] = 1f;
+ } else if (m[1] * m[1] < JVM_MIN_FLOAT_EPSILON) {
+ tmp[2] = m[2];
+ tmp[5] = m[5];
+ tmp[8] = m[8];
+ m[2] = -m[1];
+ m[5] = -m[4];
+ m[8] = -m[7];
+
+ m[1] = tmp[2]; // zero
+ m[4] = tmp[5];
+ m[7] = tmp[8];
+
+ v1[0] = 1f;
+ v1[1] = 0f;
+ v1[2] = 0f;
+ v1[3] = 0f;
+ v1[4] = 0f;
+ v1[5] = -1f;
+ v1[6] = 0f;
+ v1[7] = 1f;
+ v1[8] = 0f;
+ } else {
+ g = 1f / (float)Math.sqrt(m[1] * m[1] + m[2] * m[2]);
+ c3 = m[1] * g;
+ s3 = m[2] * g;
+ tmp[1] = c3 * m[1] + s3 * m[2]; // can assign to m[1]?
+ m[2] = -s3 * m[1] + c3 * m[2]; // zero
+ m[1] = tmp[1];
+
+ tmp[4] = c3 * m[4] + s3 * m[5];
+ m[5] = -s3 * m[4] + c3 * m[5];
+ m[4] = tmp[4];
+
+ tmp[7] = c3 * m[7] + s3 * m[8];
+ m[8] = -s3 * m[7] + c3 * m[8];
+ m[7] = tmp[7];
+
+ v1[0] = 1f;
+ v1[1] = 0f;
+ v1[2] = 0f;
+ v1[3] = 0f;
+ v1[4] = c3;
+ v1[5] = -s3;
+ v1[6] = 0f;
+ v1[7] = s3;
+ v1[8] = c3;
+ }
+
+ // u3
+
+ if (m[7] * m[7] < JVM_MIN_FLOAT_EPSILON) {
+ //
+ } else if (m[4] * m[4] < JVM_MIN_FLOAT_EPSILON) {
+ tmp[3] = m[3];
+ tmp[4] = m[4];
+ tmp[5] = m[5];
+ m[3] = m[6]; // zero
+ m[4] = m[7];
+ m[5] = m[8];
+
+ m[6] = -tmp[3]; // zero
+ m[7] = -tmp[4]; // zero
+ m[8] = -tmp[5];
+
+ tmp[3] = u1[3];
+ tmp[4] = u1[4];
+ tmp[5] = u1[5];
+ u1[3] = u1[6];
+ u1[4] = u1[7];
+ u1[5] = u1[8];
+
+ u1[6] = -tmp[3]; // zero
+ u1[7] = -tmp[4];
+ u1[8] = -tmp[5];
+
+ } else {
+ g = 1f / (float)Math.sqrt(m[4] * m[4] + m[7] * m[7]);
+ c4 = m[4] * g;
+ s4 = m[7] * g;
+ tmp[3] = c4 * m[3] + s4 * m[6];
+ m[6] = -s4 * m[3] + c4 * m[6]; // zero
+ m[3] = tmp[3];
+
+ tmp[4] = c4 * m[4] + s4 * m[7];
+ m[7] = -s4 * m[4] + c4 * m[7];
+ m[4] = tmp[4];
+
+ tmp[5] = c4 * m[5] + s4 * m[8];
+ m[8] = -s4 * m[5] + c4 * m[8];
+ m[5] = tmp[5];
+
+ tmp[3] = c4 * u1[3] + s4 * u1[6];
+ u1[6] = -s4 * u1[3] + c4 * u1[6];
+ u1[3] = tmp[3];
+
+ tmp[4] = c4 * u1[4] + s4 * u1[7];
+ u1[7] = -s4 * u1[4] + c4 * u1[7];
+ u1[4] = tmp[4];
+
+ tmp[5] = c4 * u1[5] + s4 * u1[8];
+ u1[8] = -s4 * u1[5] + c4 * u1[8];
+ u1[5] = tmp[5];
+ }
+
+ single_values[0] = m[0];
+ single_values[1] = m[4];
+ single_values[2] = m[8];
+ e[0] = m[1];
+ e[1] = m[5];
+
+ if (e[0] * e[0] < JVM_MIN_FLOAT_EPSILON && e[1] * e[1] < JVM_MIN_FLOAT_EPSILON) {
+ //
+ } else {
+ compute_qr(single_values, e, u1, v1);
+ }
+
+ scales[0] = single_values[0];
+ scales[1] = single_values[1];
+ scales[2] = single_values[2];
+
+ // Do some optimization here. If scale is unity, simply return the
+ // rotation matrix.
+ if (epsilonEquals(Math.abs(scales[0]), 1f)
+ && epsilonEquals(Math.abs(scales[1]), 1f)
+ && epsilonEquals(Math.abs(scales[2]), 1f)) {
+ // System.out.println("Scale components almost to 1f");
+
+ for (i = 0; i < 3; ++i)
+ if (scales[i] < 0f)
+ ++negCnt;
+
+ if ((negCnt == 0) || (negCnt == 2)) {
+ // System.out.println("Optimize!!");
+ outScale[0] = outScale[1] = outScale[2] = 1f;
+ for (i = 0; i < 9; ++i)
+ outRot[i] = rot[i];
+
+ return;
+ }
+ }
+
+ transpose_mat(u1, t1);
+ transpose_mat(v1, t2);
+
+ svdReorder(m, t1, t2, scales, outRot, outScale);
+
+ }
+
+ private static void svdReorder(float[] m, float[] t1, float[] t2,
+ float[] scales, float[] outRot, float[] outScale) {
+
+ int[] out = new int[3];
+ int in0, in1, in2, index, i;
+ float[] mag = new float[3];
+ float[] rot = new float[9];
+
+ // check for rotation information in the scales
+ if (scales[0] < 0f) { // move the rotation info to rotation matrix
+ scales[0] = -scales[0];
+ t2[0] = -t2[0];
+ t2[1] = -t2[1];
+ t2[2] = -t2[2];
+ }
+ if (scales[1] < 0f) { // move the rotation info to rotation matrix
+ scales[1] = -scales[1];
+ t2[3] = -t2[3];
+ t2[4] = -t2[4];
+ t2[5] = -t2[5];
+ }
+ if (scales[2] < 0f) { // move the rotation info to rotation matrix
+ scales[2] = -scales[2];
+ t2[6] = -t2[6];
+ t2[7] = -t2[7];
+ t2[8] = -t2[8];
+ }
+
+ mat_mul(t1, t2, rot);
+
+ // check for equal scales case and do not reorder
+ if (epsilonEquals(Math.abs(scales[0]), Math.abs(scales[1]))
+ && epsilonEquals(Math.abs(scales[1]), Math.abs(scales[2]))) {
+ for (i = 0; i < 9; ++i) {
+ outRot[i] = rot[i];
+ }
+ for (i = 0; i < 3; ++i) {
+ outScale[i] = scales[i];
+ }
+
+ } else {
+
+ // sort the order of the results of SVD
+ if (scales[0] > scales[1]) {
+ if (scales[0] > scales[2]) {
+ if (scales[2] > scales[1]) {
+ out[0] = 0;
+ out[1] = 2;
+ out[2] = 1; // xzy
+ } else {
+ out[0] = 0;
+ out[1] = 1;
+ out[2] = 2; // xyz
+ }
+ } else {
+ out[0] = 2;
+ out[1] = 0;
+ out[2] = 1; // zxy
+ }
+ } else { // y > x
+ if (scales[1] > scales[2]) {
+ if (scales[2] > scales[0]) {
+ out[0] = 1;
+ out[1] = 2;
+ out[2] = 0; // yzx
+ } else {
+ out[0] = 1;
+ out[1] = 0;
+ out[2] = 2; // yxz
+ }
+ } else {
+ out[0] = 2;
+ out[1] = 1;
+ out[2] = 0; // zyx
+ }
+ }
+
+ /*
+ * System.out.println("\nscales="+scales[0]+" "+scales[1]+" "+scales[
+ * 2]); System.out.println("\nrot="+rot[0]+" "+rot[1]+" "+rot[2]);
+ * System.out.println("rot="+rot[3]+" "+rot[4]+" "+rot[5]);
+ * System.out.println("rot="+rot[6]+" "+rot[7]+" "+rot[8]);
+ */
+
+ // sort the order of the input matrix
+ mag[0] = (m[0] * m[0] + m[1] * m[1] + m[2] * m[2]);
+ mag[1] = (m[3] * m[3] + m[4] * m[4] + m[5] * m[5]);
+ mag[2] = (m[6] * m[6] + m[7] * m[7] + m[8] * m[8]);
+
+ if (mag[0] > mag[1]) {
+ if (mag[0] > mag[2]) {
+ if (mag[2] > mag[1]) {
+ // 0 - 2 - 1
+ in0 = 0;
+ in2 = 1;
+ in1 = 2;// xzy
+ } else {
+ // 0 - 1 - 2
+ in0 = 0;
+ in1 = 1;
+ in2 = 2; // xyz
+ }
+ } else {
+ // 2 - 0 - 1
+ in2 = 0;
+ in0 = 1;
+ in1 = 2; // zxy
+ }
+ } else { // y > x 1>0
+ if (mag[1] > mag[2]) {
+ if (mag[2] > mag[0]) {
+ // 1 - 2 - 0
+ in1 = 0;
+ in2 = 1;
+ in0 = 2; // yzx
+ } else {
+ // 1 - 0 - 2
+ in1 = 0;
+ in0 = 1;
+ in2 = 2; // yxz
+ }
+ } else {
+ // 2 - 1 - 0
+ in2 = 0;
+ in1 = 1;
+ in0 = 2; // zyx
+ }
+ }
+
+ index = out[in0];
+ outScale[0] = scales[index];
+
+ index = out[in1];
+ outScale[1] = scales[index];
+
+ index = out[in2];
+ outScale[2] = scales[index];
+
+ index = out[in0];
+ outRot[0] = rot[index];
+
+ index = out[in0] + 3;
+ outRot[0 + 3] = rot[index];
+
+ index = out[in0] + 6;
+ outRot[0 + 6] = rot[index];
+
+ index = out[in1];
+ outRot[1] = rot[index];
+
+ index = out[in1] + 3;
+ outRot[1 + 3] = rot[index];
+
+ index = out[in1] + 6;
+ outRot[1 + 6] = rot[index];
+
+ index = out[in2];
+ outRot[2] = rot[index];
+
+ index = out[in2] + 3;
+ outRot[2 + 3] = rot[index];
+
+ index = out[in2] + 6;
+ outRot[2 + 6] = rot[index];
+ }
+ }
+
+ private static int compute_qr(float[] s, float[] e, float[] u, float[] v) {
+
+ int k;
+ boolean converged;
+ float shift, r;
+ float[] cosl = new float[2];
+ float[] cosr = new float[2];
+ float[] sinl = new float[2];
+ float[] sinr = new float[2];
+ float[] m = new float[9];
+
+ float utemp, vtemp;
+ float f, g;
+
+ final int MAX_INTERATIONS = 10;
+ final float CONVERGE_TOL = 4.89E-15f;
+
+ float c_b48 = 1f;
+ //int first;
+ converged = false;
+
+ //first = 1;
+
+ if (Math.abs(e[1]) < CONVERGE_TOL || Math.abs(e[0]) < CONVERGE_TOL)
+ converged = true;
+
+ for (k = 0; k < MAX_INTERATIONS && !converged; ++k) {
+ shift = compute_shift(s[1], e[1], s[2]);
+ f = (Math.abs(s[0]) - shift) * (d_sign(c_b48, s[0]) + shift / s[0]);
+ g = e[0];
+ r = compute_rot(f, g, sinr, cosr, 0/*, first*/);
+ f = cosr[0] * s[0] + sinr[0] * e[0];
+ e[0] = cosr[0] * e[0] - sinr[0] * s[0];
+ g = sinr[0] * s[1];
+ s[1] = cosr[0] * s[1];
+
+ r = compute_rot(f, g, sinl, cosl, 0/*, first*/);
+ //first = 0;
+ s[0] = r;
+ f = cosl[0] * e[0] + sinl[0] * s[1];
+ s[1] = cosl[0] * s[1] - sinl[0] * e[0];
+ g = sinl[0] * e[1];
+ e[1] = cosl[0] * e[1];
+
+ r = compute_rot(f, g, sinr, cosr, 1/*, first*/);
+ e[0] = r;
+ f = cosr[1] * s[1] + sinr[1] * e[1];
+ e[1] = cosr[1] * e[1] - sinr[1] * s[1];
+ g = sinr[1] * s[2];
+ s[2] = cosr[1] * s[2];
+
+ r = compute_rot(f, g, sinl, cosl, 1/*, first*/);
+ s[1] = r;
+ f = cosl[1] * e[1] + sinl[1] * s[2];
+ s[2] = cosl[1] * s[2] - sinl[1] * e[1];
+ e[1] = f;
+
+ // update u matrices
+ utemp = u[0];
+ u[0] = cosl[0] * utemp + sinl[0] * u[3];
+ u[3] = -sinl[0] * utemp + cosl[0] * u[3];
+ utemp = u[1];
+ u[1] = cosl[0] * utemp + sinl[0] * u[4];
+ u[4] = -sinl[0] * utemp + cosl[0] * u[4];
+ utemp = u[2];
+ u[2] = cosl[0] * utemp + sinl[0] * u[5];
+ u[5] = -sinl[0] * utemp + cosl[0] * u[5];
+
+ utemp = u[3];
+ u[3] = cosl[1] * utemp + sinl[1] * u[6];
+ u[6] = -sinl[1] * utemp + cosl[1] * u[6];
+ utemp = u[4];
+ u[4] = cosl[1] * utemp + sinl[1] * u[7];
+ u[7] = -sinl[1] * utemp + cosl[1] * u[7];
+ utemp = u[5];
+ u[5] = cosl[1] * utemp + sinl[1] * u[8];
+ u[8] = -sinl[1] * utemp + cosl[1] * u[8];
+
+ // update v matrices
+
+ vtemp = v[0];
+ v[0] = cosr[0] * vtemp + sinr[0] * v[1];
+ v[1] = -sinr[0] * vtemp + cosr[0] * v[1];
+ vtemp = v[3];
+ v[3] = cosr[0] * vtemp + sinr[0] * v[4];
+ v[4] = -sinr[0] * vtemp + cosr[0] * v[4];
+ vtemp = v[6];
+ v[6] = cosr[0] * vtemp + sinr[0] * v[7];
+ v[7] = -sinr[0] * vtemp + cosr[0] * v[7];
+
+ vtemp = v[1];
+ v[1] = cosr[1] * vtemp + sinr[1] * v[2];
+ v[2] = -sinr[1] * vtemp + cosr[1] * v[2];
+ vtemp = v[4];
+ v[4] = cosr[1] * vtemp + sinr[1] * v[5];
+ v[5] = -sinr[1] * vtemp + cosr[1] * v[5];
+ vtemp = v[7];
+ v[7] = cosr[1] * vtemp + sinr[1] * v[8];
+ v[8] = -sinr[1] * vtemp + cosr[1] * v[8];
+
+ m[0] = s[0];
+ m[1] = e[0];
+ m[2] = 0f;
+ m[3] = 0f;
+ m[4] = s[1];
+ m[5] = e[1];
+ m[6] = 0f;
+ m[7] = 0f;
+ m[8] = s[2];
+
+ if (Math.abs(e[1]) < CONVERGE_TOL || Math.abs(e[0]) < CONVERGE_TOL)
+ converged = true;
+ }
+
+ if (Math.abs(e[1]) < CONVERGE_TOL) {
+ compute_2X2(s[0], e[0], s[1], s, sinl, cosl, sinr, cosr, 0);
+
+ utemp = u[0];
+ u[0] = cosl[0] * utemp + sinl[0] * u[3];
+ u[3] = -sinl[0] * utemp + cosl[0] * u[3];
+ utemp = u[1];
+ u[1] = cosl[0] * utemp + sinl[0] * u[4];
+ u[4] = -sinl[0] * utemp + cosl[0] * u[4];
+ utemp = u[2];
+ u[2] = cosl[0] * utemp + sinl[0] * u[5];
+ u[5] = -sinl[0] * utemp + cosl[0] * u[5];
+
+ // update v matrices
+
+ vtemp = v[0];
+ v[0] = cosr[0] * vtemp + sinr[0] * v[1];
+ v[1] = -sinr[0] * vtemp + cosr[0] * v[1];
+ vtemp = v[3];
+ v[3] = cosr[0] * vtemp + sinr[0] * v[4];
+ v[4] = -sinr[0] * vtemp + cosr[0] * v[4];
+ vtemp = v[6];
+ v[6] = cosr[0] * vtemp + sinr[0] * v[7];
+ v[7] = -sinr[0] * vtemp + cosr[0] * v[7];
+ } else {
+ compute_2X2(s[1], e[1], s[2], s, sinl, cosl, sinr, cosr, 1);
+
+ utemp = u[3];
+ u[3] = cosl[0] * utemp + sinl[0] * u[6];
+ u[6] = -sinl[0] * utemp + cosl[0] * u[6];
+ utemp = u[4];
+ u[4] = cosl[0] * utemp + sinl[0] * u[7];
+ u[7] = -sinl[0] * utemp + cosl[0] * u[7];
+ utemp = u[5];
+ u[5] = cosl[0] * utemp + sinl[0] * u[8];
+ u[8] = -sinl[0] * utemp + cosl[0] * u[8];
+
+ // update v matrices
+
+ vtemp = v[1];
+ v[1] = cosr[0] * vtemp + sinr[0] * v[2];
+ v[2] = -sinr[0] * vtemp + cosr[0] * v[2];
+ vtemp = v[4];
+ v[4] = cosr[0] * vtemp + sinr[0] * v[5];
+ v[5] = -sinr[0] * vtemp + cosr[0] * v[5];
+ vtemp = v[7];
+ v[7] = cosr[0] * vtemp + sinr[0] * v[8];
+ v[8] = -sinr[0] * vtemp + cosr[0] * v[8];
+ }
+
+ return (0);
+ }
+
+ private static float d_sign(float a, float b) {
+ float x;
+ x = (a >= 0 ? a : -a);
+ return (b >= 0 ? x : -x);
+ }
+
+ private static float compute_shift(float f, float g, float h) {
+ float d__1, d__2;
+ float fhmn, fhmx, c, fa, ga, ha, as, at, au;
+ float ssmin;
+
+ fa = Math.abs(f);
+ ga = Math.abs(g);
+ ha = Math.abs(h);
+ fhmn = Math.min(fa, ha);
+ fhmx = Math.max(fa, ha);
+ if (fhmn == 0f) {
+ ssmin = 0f;
+ if (fhmx == 0f) {
+ //
+ } else {
+ d__1 = Math.min(fhmx, ga) / Math.max(fhmx, ga);
+ }
+ } else {
+ if (ga < fhmx) {
+ as = fhmn / fhmx + 1f;
+ at = (fhmx - fhmn) / fhmx;
+ d__1 = ga / fhmx;
+ au = d__1 * d__1;
+ c = 2f / ((float)Math.sqrt(as * as + au) + (float)Math.sqrt(at * at + au));
+ ssmin = fhmn * c;
+ } else {
+ au = fhmx / ga;
+ if (au == 0f) {
+ ssmin = fhmn * fhmx / ga;
+ } else {
+ as = fhmn / fhmx + 1f;
+ at = (fhmx - fhmn) / fhmx;
+ d__1 = as * au;
+ d__2 = at * au;
+ c = 1f / ((float)Math.sqrt(d__1 * d__1 + 1f) + (float)Math.sqrt(d__2
+ * d__2 + 1f));
+ ssmin = fhmn * c * au;
+ ssmin += ssmin;
+ }
+ }
+ }
+
+ return (ssmin);
+ }
+
+ private static int compute_2X2(float f, float g, float h,
+ float[] single_values, float[] snl, float[] csl, float[] snr,
+ float[] csr, int index) {
+
+ float c_b3 = 2f;
+ float c_b4 = 1f;
+
+ float d__1;
+ int pmax;
+ float temp;
+ boolean swap;
+ float a, d, l, m, r, s, t, tsign, fa, ga, ha;
+ float ft, gt, ht, mm;
+ boolean gasmal;
+ float tt, clt, crt, slt, srt;
+ float ssmin, ssmax;
+
+ ssmax = single_values[0];
+ ssmin = single_values[1];
+ clt = 0f;
+ crt = 0f;
+ slt = 0f;
+ srt = 0f;
+ tsign = 0f;
+
+ ft = f;
+ fa = Math.abs(ft);
+ ht = h;
+ ha = Math.abs(h);
+
+ pmax = 1;
+ if (ha > fa)
+ swap = true;
+ else
+ swap = false;
+
+ if (swap) {
+ pmax = 3;
+ temp = ft;
+ ft = ht;
+ ht = temp;
+ temp = fa;
+ fa = ha;
+ ha = temp;
+
+ }
+ gt = g;
+ ga = Math.abs(gt);
+ if (ga == 0f) {
+
+ single_values[1] = ha;
+ single_values[0] = fa;
+ clt = 1f;
+ crt = 1f;
+ slt = 0f;
+ srt = 0f;
+ } else {
+ gasmal = true;
+
+ if (ga > fa) {
+ pmax = 2;
+ if (fa / ga < JVM_MIN_FLOAT_EPSILON) {
+
+ gasmal = false;
+ ssmax = ga;
+ if (ha > 1.) {
+ ssmin = fa / (ga / ha);
+ } else {
+ ssmin = fa / ga * ha;
+ }
+ clt = 1f;
+ slt = ht / gt;
+ srt = 1f;
+ crt = ft / gt;
+ }
+ }
+ if (gasmal) {
+
+ d = fa - ha;
+ if (d == fa) {
+
+ l = 1f;
+ } else {
+ l = d / fa;
+ }
+
+ m = gt / ft;
+
+ t = 2f - l;
+
+ mm = m * m;
+ tt = t * t;
+ s = (float)Math.sqrt(tt + mm);
+
+ if (l == 0f) {
+ r = Math.abs(m);
+ } else {
+ r = (float)Math.sqrt(l * l + mm);
+ }
+
+ a = (s + r) * .5f;
+
+ if (ga > fa) {
+ pmax = 2;
+ if (fa / ga < JVM_MIN_FLOAT_EPSILON) {
+
+ gasmal = false;
+ ssmax = ga;
+ if (ha > 1.) {
+ ssmin = fa / (ga / ha);
+ } else {
+ ssmin = fa / ga * ha;
+ }
+ clt = 1f;
+ slt = ht / gt;
+ srt = 1f;
+ crt = ft / gt;
+ }
+ }
+ if (gasmal) {
+
+ d = fa - ha;
+ if (d == fa) {
+
+ l = 1f;
+ } else {
+ l = d / fa;
+ }
+
+ m = gt / ft;
+
+ t = 2f - l;
+
+ mm = m * m;
+ tt = t * t;
+ s = (float)Math.sqrt(tt + mm);
+
+ if (l == 0f) {
+ r = Math.abs(m);
+ } else {
+ r = (float)Math.sqrt(l * l + mm);
+ }
+
+ a = (s + r) * .5f;
+
+ ssmin = ha / a;
+ ssmax = fa * a;
+ if (mm == 0f) {
+
+ if (l == 0f) {
+ t = d_sign(c_b3, ft) * d_sign(c_b4, gt);
+ } else {
+ t = gt / d_sign(d, ft) + m / t;
+ }
+ } else {
+ t = (m / (s + t) + m / (r + l)) * (a + 1f);
+ }
+ l = (float)Math.sqrt(t * t + 4f);
+ crt = 2f / l;
+ srt = t / l;
+ clt = (crt + srt * m) / a;
+ slt = ht / ft * srt / a;
+ }
+ }
+ if (swap) {
+ csl[0] = srt;
+ snl[0] = crt;
+ csr[0] = slt;
+ snr[0] = clt;
+ } else {
+ csl[0] = clt;
+ snl[0] = slt;
+ csr[0] = crt;
+ snr[0] = srt;
+ }
+
+ if (pmax == 1) {
+ tsign = d_sign(c_b4, csr[0]) * d_sign(c_b4, csl[0])
+ * d_sign(c_b4, f);
+ }
+ if (pmax == 2) {
+ tsign = d_sign(c_b4, snr[0]) * d_sign(c_b4, csl[0])
+ * d_sign(c_b4, g);
+ }
+ if (pmax == 3) {
+ tsign = d_sign(c_b4, snr[0]) * d_sign(c_b4, snl[0])
+ * d_sign(c_b4, h);
+ }
+ single_values[index] = d_sign(ssmax, tsign);
+ d__1 = tsign * d_sign(c_b4, f) * d_sign(c_b4, h);
+ single_values[index + 1] = d_sign(ssmin, d__1);
+
+ }
+ return 0;
+ }
+
+ private static float compute_rot(float f, float g, float[] sin, float[] cos,
+ int index) {
+ float cs, sn;
+ int i;
+ float scale;
+ int count;
+ float f1, g1;
+ float r;
+ final double safmn2 = 2.002083095183101E-146;
+ final double safmx2 = 4.994797680505588E+145;
+
+ if (g == 0f) {
+ cs = 1f;
+ sn = 0f;
+ r = f;
+ } else if (f == 0f) {
+ cs = 0f;
+ sn = 1f;
+ r = g;
+ } else {
+ f1 = f;
+ g1 = g;
+ scale = Math.max(Math.abs(f1), Math.abs(g1));
+ if (scale >= safmx2) {
+ count = 0;
+ while (scale >= safmx2) {
+ ++count;
+ f1 *= safmn2;
+ g1 *= safmn2;
+ scale = Math.max(Math.abs(f1), Math.abs(g1));
+ }
+ r = (float)Math.sqrt(f1 * f1 + g1 * g1);
+ cs = f1 / r;
+ sn = g1 / r;
+ for (i = 1; i <= count; ++i) {
+ r *= safmx2;
+ }
+ } else if (scale <= safmn2) {
+ count = 0;
+ while (scale <= safmn2) {
+ ++count;
+ f1 *= safmx2;
+ g1 *= safmx2;
+ scale = Math.max(Math.abs(f1), Math.abs(g1));
+ }
+ r = (float)Math.sqrt(f1 * f1 + g1 * g1);
+ cs = f1 / r;
+ sn = g1 / r;
+ for (i = 1; i <= count; ++i) {
+ r *= safmn2;
+ }
+ } else {
+ r = (float)Math.sqrt(f1 * f1 + g1 * g1);
+ cs = f1 / r;
+ sn = g1 / r;
+ }
+ if (Math.abs(f) > Math.abs(g) && cs < 0f) {
+ cs = -cs;
+ sn = -sn;
+ r = -r;
+ }
+ }
+ sin[index] = sn;
+ cos[index] = cs;
+ return r;
+
+ }
+
+ private static void mat_mul(float[] m1, float[] m2, float[] m3) {
+ int i;
+ float[] tmp = new float[9];
+
+ tmp[0] = m1[0] * m2[0] + m1[1] * m2[3] + m1[2] * m2[6];
+ tmp[1] = m1[0] * m2[1] + m1[1] * m2[4] + m1[2] * m2[7];
+ tmp[2] = m1[0] * m2[2] + m1[1] * m2[5] + m1[2] * m2[8];
+
+ tmp[3] = m1[3] * m2[0] + m1[4] * m2[3] + m1[5] * m2[6];
+ tmp[4] = m1[3] * m2[1] + m1[4] * m2[4] + m1[5] * m2[7];
+ tmp[5] = m1[3] * m2[2] + m1[4] * m2[5] + m1[5] * m2[8];
+
+ tmp[6] = m1[6] * m2[0] + m1[7] * m2[3] + m1[8] * m2[6];
+ tmp[7] = m1[6] * m2[1] + m1[7] * m2[4] + m1[8] * m2[7];
+ tmp[8] = m1[6] * m2[2] + m1[7] * m2[5] + m1[8] * m2[8];
+
+ for (i = 0; i < 9; ++i) {
+ m3[i] = tmp[i];
+ }
+ }
+
+ private static void transpose_mat(float[] in, float[] out) {
+ out[0] = in[0];
+ out[1] = in[3];
+ out[2] = in[6];
+
+ out[3] = in[1];
+ out[4] = in[4];
+ out[5] = in[7];
+
+ out[6] = in[2];
+ out[7] = in[5];
+ out[8] = in[8];
+ }
+
+ /**
+ * Creates a new object of the same class as this object.
+ *
+ * @return a clone of this instance.
+ * @exception OutOfMemoryError
+ * if there is not enough memory.
+ * @see java.lang.Cloneable
+ */
+ @Override
+ public Matrix3f clone() {
+ Matrix3f m1 = null;
+ try {
+ m1 = (Matrix3f) super.clone();
+ } catch (CloneNotSupportedException e) {
+ // this shouldn't happen, since we are Cloneable
+ throw new InternalError();
+ }
+
+ // Also need to create new tmp arrays (no need to actually clone them)
+ return m1;
+ }
+
+ /**
+ * Get the first matrix element in the first row.
+ *
+ * @return Returns the m00f
+ * @since vecmath 1.5
+ */
+ public final float getM00() {
+ return this.m00;
+ }
+
+ /**
+ * Set the first matrix element in the first row.
+ *
+ * @param m00
+ * The m00 to set.
+ *
+ * @since vecmath 1.5
+ */
+ public final void setM00(float m00) {
+ this.m00 = m00;
+ }
+
+ /**
+ * Get the second matrix element in the first row.
+ *
+ * @return Returns the m01.
+ *
+ * @since vecmath 1.5
+ */
+ public final float getM01() {
+ return this.m01;
+ }
+
+ /**
+ * Set the second matrix element in the first row.
+ *
+ * @param m01
+ * The m01 to set.
+ *
+ * @since vecmath 1.5
+ */
+ public final void setM01(float m01) {
+ this.m01 = m01;
+ }
+
+ /**
+ * Get the third matrix element in the first row.
+ *
+ * @return Returns the m02.
+ *
+ * @since vecmath 1.5
+ */
+ public final float getM02() {
+ return this.m02;
+ }
+
+ /**
+ * Set the third matrix element in the first row.
+ *
+ * @param m02
+ * The m02 to set.
+ *
+ * @since vecmath 1.5
+ */
+ public final void setM02(float m02) {
+ this.m02 = m02;
+ }
+
+ /**
+ * Get first matrix element in the second row.
+ *
+ * @return Returns the m10f
+ *
+ * @since vecmath 1.5
+ */
+ public final float getM10() {
+ return this.m10;
+ }
+
+ /**
+ * Set first matrix element in the second row.
+ *
+ * @param m10
+ * The m10 to set.
+ *
+ * @since vecmath 1.5
+ */
+ public final void setM10(float m10) {
+ this.m10 = m10;
+ }
+
+ /**
+ * Get second matrix element in the second row.
+ *
+ * @return Returns the m11.
+ *
+ * @since vecmath 1.5
+ */
+ public final float getM11() {
+ return this.m11;
+ }
+
+ /**
+ * Set the second matrix element in the second row.
+ *
+ * @param m11
+ * The m11 to set.
+ *
+ * @since vecmath 1.5
+ */
+ public final void setM11(float m11) {
+ this.m11 = m11;
+ }
+
+ /**
+ * Get the third matrix element in the second row.
+ *
+ * @return Returns the m12.
+ *
+ * @since vecmath 1.5
+ */
+ public final float getM12() {
+ return this.m12;
+ }
+
+ /**
+ * Set the third matrix element in the second row.
+ *
+ * @param m12
+ * The m12 to set.
+ *
+ * @since vecmath 1.5
+ */
+ public final void setM12(float m12) {
+ this.m12 = m12;
+ }
+
+ /**
+ * Get the first matrix element in the third row.
+ *
+ * @return Returns the m20
+ *
+ * @since vecmath 1.5
+ */
+ public final float getM20() {
+ return this.m20;
+ }
+
+ /**
+ * Set the first matrix element in the third row.
+ *
+ * @param m20
+ * The m20 to set.
+ *
+ * @since vecmath 1.5
+ */
+ public final void setM20(float m20) {
+ this.m20 = m20;
+ }
+
+ /**
+ * Get the second matrix element in the third row.
+ *
+ * @return Returns the m21.
+ *
+ * @since vecmath 1.5
+ */
+ public final float getM21() {
+ return this.m21;
+ }
+
+ /**
+ * Set the second matrix element in the third row.
+ *
+ * @param m21
+ * The m21 to set.
+ *
+ * @since vecmath 1.5
+ */
+ public final void setM21(float m21) {
+ this.m21 = m21;
+ }
+
+ /**
+ * Get the third matrix element in the third row .
+ *
+ * @return Returns the m22.
+ *
+ * @since vecmath 1.5
+ */
+ public final float getM22() {
+ return this.m22;
+ }
+
+ /**
+ * Set the third matrix element in the third row.
+ *
+ * @param m22
+ * The m22 to set.
+ *
+ * @since vecmath 1.5
+ */
+ public final void setM22(float m22) {
+ this.m22 = m22;
+ }
+
+ /**
+ * Sets the scale component of the current matrix by factoring out the
+ * current scale (by doing an SVD) and multiplying by the new scale.
+ *
+ * @param scale
+ * the new scale amount
+ */
+ public final void setScale(float scale) {
+
+ float[] tmp_rot = new float[9]; // scratch matrix
+ float[] tmp_scale = new float[3]; // scratch matrix
+
+ getScaleRotate(tmp_scale, tmp_rot);
+
+ this.m00 = tmp_rot[0] * scale;
+ this.m01 = tmp_rot[1] * scale;
+ this.m02 = tmp_rot[2] * scale;
+
+ this.m10 = tmp_rot[3] * scale;
+ this.m11 = tmp_rot[4] * scale;
+ this.m12 = tmp_rot[5] * scale;
+
+ this.m20 = tmp_rot[6] * scale;
+ this.m21 = tmp_rot[7] * scale;
+ this.m22 = tmp_rot[8] * scale;
+ }
+
+ /**
+ * Performs an SVD normalization of this matrix to calculate and return the
+ * uniform scale factor. If the matrix has non-uniform scale factors, the
+ * largest of the x, y scale factors will be returned. This matrix is
+ * not modified.
+ *
+ * @return the scale factor of this matrix
+ */
+ public final float getScale() {
+
+ float[] tmp_scale = new float[3]; // scratch matrix
+ float[] tmp_rot = new float[9]; // scratch matrix
+ getScaleRotate(tmp_scale, tmp_rot);
+
+ return (MathUtil.max(tmp_scale));
+
+ }
+
+ /**
+ * perform SVD (if necessary to get rotational component)
+ */
+ private void getScaleRotate(float scales[], float rots[]) {
+
+ float[] tmp = new float[9]; // scratch matrix
+
+ tmp[0] = this.m00;
+ tmp[1] = this.m01;
+ tmp[2] = this.m02;
+
+ tmp[3] = this.m10;
+ tmp[4] = this.m11;
+ tmp[5] = this.m12;
+
+ tmp[6] = this.m20;
+ tmp[7] = this.m21;
+ tmp[8] = this.m22;
+ compute_svd(tmp, scales, rots);
+
+ return;
+ }
+
+ /**
+ * Performs singular value decomposition normalization of this matrix.
+ */
+ public final void normalize() {
+ float[] tmp_rot = new float[9]; // scratch matrix
+ float[] tmp_scale = new float[3]; // scratch matrix
+
+ getScaleRotate(tmp_scale, tmp_rot);
+
+ this.m00 = tmp_rot[0];
+ this.m01 = tmp_rot[1];
+ this.m02 = tmp_rot[2];
+
+ this.m10 = tmp_rot[3];
+ this.m11 = tmp_rot[4];
+ this.m12 = tmp_rot[5];
+
+ this.m20 = tmp_rot[6];
+ this.m21 = tmp_rot[7];
+ this.m22 = tmp_rot[8];
+
+ }
+
+ /** Set this matrix with the covariance matrix's elements for the given
+ * set of tuples.
+ *
+ * @param tuples
+ * @return the mean of the tuples.
+ */
+ public final Vector3f cov(Vector3f... tuples) {
+ return cov(Arrays.asList(tuples));
+ }
+
+ /** Set this matrix with the covariance matrix's elements for the given
+ * set of tuples.
+ *
+ * @param tuples
+ * @return the mean of the tuples.
+ */
+ public final Vector3f cov(Point3f... tuples) {
+ return cov(Arrays.asList(tuples));
+ }
+
+ /** Set this matrix with the covariance matrix's elements for the given
+ * set of tuples.
+ *
+ * @param tuples
+ * @return the mean of the tuples.
+ */
+ public Vector3f cov(Iterable> tuples) {
+ setZero();
+
+ // Compute the mean m
+ Vector3f m = new Vector3f();
+ int count = 0;
+ for(Tuple3f p : tuples) {
+ m.add(p.getX(), p.getY(), p.getZ());
+ ++count;
+ }
+
+ if (count==0) return null;
+
+ m.scale(1f/count);
+
+ // Compute the covariance term [Gottshalk2000]
+ // c_ij = sum(p'_i * p'_j) / n
+ // c_ij = sum((p_i - m_i) * (p_j - m_j)) / n
+ for(Tuple3f p : tuples) {
+ this.m00 += (p.getX() - m.getX()) * (p.getX() - m.getX());
+ this.m01 += (p.getX() - m.getX()) * (p.getY() - m.getY()); // same as m10
+ this.m02 += (p.getX() - m.getX()) * (p.getZ() - m.getZ()); // same as m20
+ //cov.m10 += (p.getY() - m.getY()) * (p.getX() - m.getX()); // same as m01
+ this.m11 += (p.getY() - m.getY()) * (p.getY() - m.getY());
+ this.m12 += (p.getY() - m.getY()) * (p.getZ() - m.getZ()); // same as m21
+ //cov.m20 += (p.getZ() - m.getZ()) * (p.getX() - m.getX()); // same as m02
+ //cov.m21 += (p.getZ() - m.getZ()) * (p.getY() - m.getY()); // same as m12
+ this.m22 += (p.getZ() - m.getZ()) * (p.getZ() - m.getZ());
+ }
+
+ this.m00 /= count;
+ this.m01 /= count;
+ this.m02 /= count;
+ this.m10 = this.m01;
+ this.m11 /= count;
+ this.m12 /= count;
+ this.m20 = this.m02;
+ this.m21 = this.m12;
+ this.m22 /= count;
+
+ return m;
+ }
+
+ /** Replies if the matrix is symmetric.
+ *
+ * @return true if the matrix is symmetric, otherwise
+ * false
+ */
+ public boolean isSymmetric() {
+ return this.m01 == this.m10
+ && this.m02 == this.m20
+ && this.m12 == this.m21;
+ }
+
+ /**
+ * Compute the eigenvectors of the given symmetric matrix
+ * according to the Jacobi Cyclic Method.
+ *
+ * Given the n x n real symmetric matrix A, the routine
+ * Jacobi_Cyclic_Method calculates the eigenvalues and
+ * eigenvectors of A by successively sweeping through the
+ * matrix A annihilating off-diagonal non-zero elements
+ * by a rotation of the row and column in which the
+ * non-zero element occurs.
+ *
+ * The Jacobi procedure for finding the eigenvalues and eigenvectors of a
+ * symmetric matrix A is based on finding a similarity transformation
+ * which diagonalizes A. The similarity transformation is given by a
+ * product of a sequence of orthogonal (rotation) matrices each of which
+ * annihilates an off-diagonal element and its transpose. The rotation
+ * effects only the rows and columns containing the off-diagonal element
+ * and its transpose, i.e. if a[i][j] is an off-diagonal element, then
+ * the orthogonal transformation rotates rows a[i][] and a[j][], and
+ * equivalently it rotates columns a[][i] and a[][j], so that a[i][j] = 0
+ * and a[j][i] = 0.
+ * The cyclic Jacobi method considers the off-diagonal elements in the
+ * following order: (0,1),(0,2),...,(0,n-1),(1,2),...,(n-2,n-1). If the
+ * the magnitude of the off-diagonal element is greater than a treshold,
+ * then a rotation is performed to annihilate that off-diagnonal element.
+ * The process described above is called a sweep. After a sweep has been
+ * completed, the threshold is lowered and another sweep is performed
+ * with the new threshold. This process is completed until the final
+ * sweep is performed with the final threshold.
+ * The orthogonal transformation which annihilates the matrix element
+ * a[k][m], k != m, is Q = q[i][j], where q[i][j] = 0 if i != j, i,j != k
+ * i,j != m and q[i][j] = 1 if i = j, i,j != k, i,j != m, q[k][k] =
+ * q[m][m] = cos(phi), q[k][m] = -sin(phi), and q[m][k] = sin(phi), where
+ * the angle phi is determined by requiring a[k][m] -> 0. This condition
+ * on the angle phi is equivalent to
+ * cot(2 phi) = 0.5 * (a[k][k] - a[m][m]) / a[k][m]
+ * Since tan(2 phi) = 2 tan(phi) / (1.0 - tan(phi)^2),
+ * tan(phi)^2 + 2cot(2 phi) * tan(phi) - 1 = 0.
+ * Solving for tan(phi), choosing the solution with smallest magnitude,
+ * tan(phi) = - cot(2 phi) + sgn(cot(2 phi)) sqrt(cot(2phi)^2 + 1).
+ * Then cos(phi)^2 = 1 / (1 + tan(phi)^2) and sin(phi)^2 = 1 - cos(phi)^2.
+ * Finally by taking the sqrts and assigning the sign to the sin the same
+ * as that of the tan, the orthogonal transformation Q is determined.
+ * Let A" be the matrix obtained from the matrix A by applying the
+ * similarity transformation Q, since Q is orthogonal, A" = Q'AQ, where Q'
+ * is the transpose of Q (which is the same as the inverse of Q). Then
+ * a"[i][j] = Q'[i][p] a[p][q] Q[q][j] = Q[p][i] a[p][q] Q[q][j],
+ * where repeated indices are summed over.
+ * If i is not equal to either k or m, then Q[i][j] is the Kronecker
+ * delta. So if both i and j are not equal to either k or m,
+ * a"[i][j] = a[i][j].
+ * If i = k, j = k,
+ * a"[k][k] = a[k][k]*cos(phi)^2 + a[k][m]*sin(2 phi) + a[m][m]*sin(phi)^2
+ * If i = k, j = m,
+ * a"[k][m] = a"[m][k] = 0 = a[k][m]*cos(2 phi) + 0.5 * (a[m][m] - a[k][k])*sin(2 phi)
+ * If i = k, j != k or m,
+ * a"[k][j] = a"[j][k] = a[k][j] * cos(phi) + a[m][j] * sin(phi)
+ * If i = m, j = k, a"[m][k] = 0
+ * If i = m, j = m,
+ * a"[m][m] = a[m][m]*cos(phi)^2 - a[k][m]*sin(2 phi) + a[k][k]*sin(phi)^2
+ * If i= m, j != k or m,
+ * a"[m][j] = a"[j][m] = a[m][j] * cos(phi) - a[k][j] * sin(phi)
+ *
+ * If X is the matrix of normalized eigenvectors stored so that the ith
+ * column corresponds to the ith eigenvalue, then AX = X Lamda, where
+ * Lambda is the diagonal matrix with the ith eigenvalue stored at
+ * Lambda[i][i], i.e. X'AX = Lambda and X is orthogonal, the eigenvectors
+ * are normalized and orthogonal. So, X = Q1 Q2 ... Qs, where Qi is
+ * the ith orthogonal matrix, i.e. X can be recursively approximated by
+ * the recursion relation X" = X Q, where Q is the orthogonal matrix and
+ * the initial estimate for X is the identity matrix.
+ * If j = k, then x"[i][k] = x[i][k] * cos(phi) + x[i][m] * sin(phi),
+ * if j = m, then x"[i][m] = x[i][m] * cos(phi) - x[i][k] * sin(phi), and
+ * if j != k and j != m, then x"[i][j] = x[i][j].
+ *
+ * @param eigenVectors are the matrix of vectors to fill. Eigen vectors are the
+ * columns of the matrix.
+ * @return the eigenvalues which are corresponding to the eigenVectors columns.
+ * @see "Mathematics for 3D Game Programming and Computer Graphics, 2nd edition; pp.437."
+ */
+ public float[] eigenVectorsOfSymmetricMatrix(Matrix3f eigenVectors) {
+ // Copy values up to the diagonal
+ float m11 = getElement(0,0);
+ float m12 = getElement(0,1);
+ float m13 = getElement(0,2);
+ float m22 = getElement(1,1);
+ float m23 = getElement(1,2);
+ float m33 = getElement(2,2);
+
+ eigenVectors.setIdentity();
+
+ boolean sweepsConsumed = true;
+ int i;
+ float u, u2, u2p1, t, c, s;
+ float tmp, ri0, ri1, ri2;
+
+ for(int a=0; a
+ * This function uses the equal-to-zero test with the error {@link MathConstants#JVM_MIN_FLOAT_EPSILON}.
+ *
+ * @return true if the matrix is identity; false otherwise.
+ * @see MathUtil#isEpsilonZero(float)
+ * @see MathUtil#isEpsilonEqual(float, float)
+ */
+ public boolean isIdentity() {
+ //TODO Buffering the boolean value "isIdentity"
+ return MathUtil.isEpsilonEqual(this.m00, 1f, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonZero(this.m01, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonZero(this.m02, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonZero(this.m10, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonEqual(this.m11, 1f, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonZero(this.m12, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonZero(this.m20, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonZero(this.m21, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonEqual(this.m22, 1f, MathConstants.JVM_MIN_FLOAT_EPSILON);
+ }
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/Matrix4f.java b/core/math/src/main/java/org/arakhne/afc/math/Matrix4f.java
new file mode 100644
index 000000000..fec697f71
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/Matrix4f.java
@@ -0,0 +1,1960 @@
+/*
+ * $Id$
+ *
+ * Copyright (c) 2006-10, Multiagent Team, Laboratoire Systemes et Transports, Universite de Technologie de Belfort-Montbeliard.
+ * Copyright (C) 2012 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math;
+
+import java.io.Serializable;
+
+/**
+ * Is represented internally as a 4x4 floating point matrix. The mathematical
+ * representation is row major, as in traditional matrix mathematics.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class Matrix4f implements Serializable, Cloneable, MathConstants {
+
+ private static final long serialVersionUID = 7216873052550769543L;
+
+ /**
+ * The first matrix element in the first row.
+ */
+ public float m00;
+
+ /**
+ * The second matrix element in the first row.
+ */
+ public float m01;
+
+ /**
+ * The third matrix element in the first row.
+ */
+ public float m02;
+
+ /**
+ * The fourth matrix element in the first row.
+ */
+ public float m03;
+
+ /**
+ * The first matrix element in the second row.
+ */
+ public float m10;
+
+ /**
+ * The second matrix element in the second row.
+ */
+ public float m11;
+
+ /**
+ * The third matrix element in the second row.
+ */
+ public float m12;
+
+ /**
+ * The fourth matrix element in the second row.
+ */
+ public float m13;
+
+ /**
+ * The first matrix element in the third row.
+ */
+ public float m20;
+
+ /**
+ * The second matrix element in the third row.
+ */
+ public float m21;
+
+ /**
+ * The third matrix element in the third row.
+ */
+ public float m22;
+
+ /**
+ * The fourth matrix element in the third row.
+ */
+ public float m23;
+
+ /**
+ * The first matrix element in the fourth row.
+ */
+ public float m30;
+
+ /**
+ * The second matrix element in the fourth row.
+ */
+ public float m31;
+
+ /**
+ * The third matrix element in the fourth row.
+ */
+ public float m32;
+
+ /**
+ * The fourth matrix element in the fourth row.
+ */
+ public float m33;
+
+ /**
+ * Constructs and initializes a Matrix4f from the specified nine values.
+ *
+ * @param m00
+ * the [0][0] element
+ * @param m01
+ * the [0][1] element
+ * @param m02
+ * the [0][2] element
+ * @param m03
+ * the [0][3] element
+ * @param m10
+ * the [1][0] element
+ * @param m11
+ * the [1][1] element
+ * @param m12
+ * the [1][2] element
+ * @param m13
+ * the [1][3] element
+ * @param m20
+ * the [2][0] element
+ * @param m21
+ * the [2][1] element
+ * @param m22
+ * the [2][2] element
+ * @param m23
+ * the [2][3] element
+ * @param m30
+ * the [3][0] element
+ * @param m31
+ * the [3][1] element
+ * @param m32
+ * the [3][2] element
+ * @param m33
+ * the [3][3] element
+ */
+ public Matrix4f(float m00, float m01, float m02, float m03, float m10, float m11, float m12, float m13, float m20, float m21, float m22, float m23, float m30, float m31, float m32, float m33) {
+ this.m00 = m00;
+ this.m01 = m01;
+ this.m02 = m02;
+ this.m03 = m03;
+
+ this.m10 = m10;
+ this.m11 = m11;
+ this.m12 = m12;
+ this.m13 = m13;
+
+ this.m20 = m20;
+ this.m21 = m21;
+ this.m22 = m22;
+ this.m23 = m23;
+
+ this.m30 = m30;
+ this.m31 = m31;
+ this.m32 = m32;
+ this.m33 = m33;
+ }
+
+ /**
+ * Constructs and initializes a Matrix4f from the specified sixteen- element
+ * array.
+ *
+ * @param v
+ * the array of length 16 containing in order
+ */
+ public Matrix4f(float[] v) {
+ this.m00 = v[0];
+ this.m01 = v[1];
+ this.m02 = v[2];
+ this.m03 = v[3];
+
+ this.m10 = v[4];
+ this.m11 = v[5];
+ this.m12 = v[6];
+ this.m13 = v[7];
+
+ this.m20 = v[8];
+ this.m21 = v[9];
+ this.m22 = v[10];
+ this.m23 = v[11];
+
+ this.m30 = v[12];
+ this.m31 = v[13];
+ this.m32 = v[14];
+ this.m33 = v[15];
+ }
+
+ /**
+ * Constructs a new matrix with the same values as the Matrix4f parameter.
+ *
+ * @param m1
+ * the source matrix
+ */
+ public Matrix4f(Matrix4f m1) {
+ this.m00 = m1.m00;
+ this.m01 = m1.m01;
+ this.m02 = m1.m02;
+ this.m03 = m1.m03;
+
+ this.m10 = m1.m10;
+ this.m11 = m1.m11;
+ this.m12 = m1.m12;
+ this.m13 = m1.m13;
+
+ this.m20 = m1.m20;
+ this.m21 = m1.m21;
+ this.m22 = m1.m22;
+ this.m23 = m1.m23;
+
+ this.m30 = m1.m30;
+ this.m31 = m1.m31;
+ this.m32 = m1.m32;
+ this.m33 = m1.m33;
+ }
+
+ /**
+ * Constructs and initializes a Matrix4f to all zeros.
+ */
+ public Matrix4f() {
+ this.m00 = 0f;
+ this.m01 = 0f;
+ this.m02 = 0f;
+ this.m03 = 0f;
+
+ this.m10 = 0f;
+ this.m11 = 0f;
+ this.m12 = 0f;
+ this.m13 = 0f;
+
+ this.m20 = 0f;
+ this.m21 = 0f;
+ this.m22 = 0f;
+ this.m23 = 0f;
+
+ this.m30 = 0f;
+ this.m31 = 0f;
+ this.m32 = 0f;
+ this.m33 = 0f;
+ }
+
+ /**
+ * Returns a string that contains the values of this Matrix4f.
+ *
+ * @return the String representation
+ */
+ @Override
+ public String toString() {
+ return this.m00 + ", " //$NON-NLS-1$
+ + this.m01 + ", " //$NON-NLS-1$
+ + this.m02 + ", " //$NON-NLS-1$
+ + this.m03 + "\n" //$NON-NLS-1$
+ + this.m10 + ", " //$NON-NLS-1$
+ + this.m11 + ", " //$NON-NLS-1$
+ + this.m12 + ", " //$NON-NLS-1$
+ + this.m13 + "\n" //$NON-NLS-1$
+ + this.m20 + ", " //$NON-NLS-1$
+ + this.m21 + ", " //$NON-NLS-1$
+ + this.m22 + ", " //$NON-NLS-1$
+ + this.m23 + "\n" //$NON-NLS-1$
+ + this.m30 + ", " //$NON-NLS-1$
+ + this.m31 + ", " //$NON-NLS-1$
+ + this.m32 + ", " //$NON-NLS-1$
+ + this.m33 + "\n"; //$NON-NLS-1$
+ }
+
+ /**
+ * Sets this Matrix4f to identity.
+ */
+ public final void setIdentity() {
+ this.m00 = 1f;
+ this.m01 = 0f;
+ this.m02 = 0f;
+ this.m03 = 0f;
+
+ this.m10 = 0f;
+ this.m11 = 1f;
+ this.m12 = 0f;
+ this.m13 = 0f;
+
+ this.m20 = 0f;
+ this.m21 = 0f;
+ this.m22 = 1f;
+ this.m23 = 0f;
+
+ this.m30 = 0f;
+ this.m31 = 0f;
+ this.m32 = 0f;
+ this.m33 = 1f;
+ }
+
+ /**
+ * Sets the specified element of this matrix3f to the value provided.
+ *
+ * @param row
+ * the row number to be modified (zero indexed)
+ * @param column
+ * the column number to be modified (zero indexed)
+ * @param value
+ * the new value
+ */
+ public final void setElement(int row, int column, float value) {
+ switch (row) {
+ case 0:
+ switch (column) {
+ case 0:
+ this.m00 = value;
+ break;
+ case 1:
+ this.m01 = value;
+ break;
+ case 2:
+ this.m02 = value;
+ break;
+ case 3:
+ this.m03 = value;
+ break;
+ default:
+ throw new ArrayIndexOutOfBoundsException();
+ }
+ break;
+
+ case 1:
+ switch (column) {
+ case 0:
+ this.m10 = value;
+ break;
+ case 1:
+ this.m11 = value;
+ break;
+ case 2:
+ this.m12 = value;
+ break;
+ case 3:
+ this.m13 = value;
+ break;
+ default:
+ throw new ArrayIndexOutOfBoundsException();
+ }
+ break;
+
+ case 2:
+ switch (column) {
+ case 0:
+ this.m20 = value;
+ break;
+ case 1:
+ this.m21 = value;
+ break;
+ case 2:
+ this.m22 = value;
+ break;
+ case 3:
+ this.m23 = value;
+ break;
+ default:
+ throw new ArrayIndexOutOfBoundsException();
+ }
+ break;
+
+ case 3:
+ switch (column) {
+ case 0:
+ this.m30 = value;
+ break;
+ case 1:
+ this.m31 = value;
+ break;
+ case 2:
+ this.m32 = value;
+ break;
+ case 3:
+ this.m33 = value;
+ break;
+ default:
+ throw new ArrayIndexOutOfBoundsException();
+ }
+ break;
+
+ default:
+ throw new ArrayIndexOutOfBoundsException();
+ }
+ }
+
+ /**
+ * Retrieves the value at the specified row and column of the specified
+ * matrix.
+ *
+ * @param row
+ * the row number to be retrieved (zero indexed)
+ * @param column
+ * the column number to be retrieved (zero indexed)
+ * @return the value at the indexed element.
+ */
+ public final float getElement(int row, int column) {
+ switch (row) {
+ case 0:
+ switch (column) {
+ case 0:
+ return (this.m00);
+ case 1:
+ return (this.m01);
+ case 2:
+ return (this.m02);
+ case 3:
+ return (this.m03);
+ default:
+ break;
+ }
+ break;
+ case 1:
+ switch (column) {
+ case 0:
+ return (this.m10);
+ case 1:
+ return (this.m11);
+ case 2:
+ return (this.m12);
+ case 3:
+ return (this.m13);
+ default:
+ break;
+ }
+ break;
+
+ case 2:
+ switch (column) {
+ case 0:
+ return (this.m20);
+ case 1:
+ return (this.m21);
+ case 2:
+ return (this.m22);
+ case 3:
+ return (this.m23);
+ default:
+ break;
+ }
+ break;
+
+ case 3:
+ switch (column) {
+ case 0:
+ return (this.m30);
+ case 1:
+ return (this.m31);
+ case 2:
+ return (this.m32);
+ case 3:
+ return (this.m33);
+ default:
+ break;
+ }
+ break;
+
+ default:
+ break;
+ }
+
+ throw new ArrayIndexOutOfBoundsException();
+ }
+
+ /**
+ * Copies the matrix values in the specified row into the array parameter.
+ *
+ * @param row
+ * the matrix row
+ * @param v
+ * the array into which the matrix row values will be copied
+ */
+ public final void getRow(int row, float v[]) {
+ if (row == 0) {
+ v[0] = this.m00;
+ v[1] = this.m01;
+ v[2] = this.m02;
+ v[3] = this.m03;
+ } else if (row == 1) {
+ v[0] = this.m10;
+ v[1] = this.m11;
+ v[2] = this.m12;
+ v[3] = this.m13;
+ } else if (row == 2) {
+ v[0] = this.m20;
+ v[1] = this.m21;
+ v[2] = this.m22;
+ v[3] = this.m23;
+ } else if (row == 3) {
+ v[0] = this.m30;
+ v[1] = this.m31;
+ v[2] = this.m32;
+ v[3] = this.m33;
+ } else {
+ throw new ArrayIndexOutOfBoundsException();
+ }
+
+ }
+
+ /**
+ * Copies the matrix values in the specified column into the array
+ * parameter.
+ *
+ * @param column
+ * the matrix column
+ * @param v
+ * the array into which the matrix row values will be copied
+ */
+ public final void getColumn(int column, float v[]) {
+ if (column == 0) {
+ v[0] = this.m00;
+ v[1] = this.m10;
+ v[2] = this.m20;
+ v[3] = this.m30;
+ } else if (column == 1) {
+ v[0] = this.m01;
+ v[1] = this.m11;
+ v[2] = this.m21;
+ v[3] = this.m31;
+ } else if (column == 2) {
+ v[0] = this.m02;
+ v[1] = this.m12;
+ v[2] = this.m22;
+ v[3] = this.m32;
+ } else if (column == 3) {
+ v[0] = this.m03;
+ v[1] = this.m13;
+ v[2] = this.m23;
+ v[3] = this.m33;
+ } else {
+ throw new ArrayIndexOutOfBoundsException();
+ }
+
+ }
+
+ /**
+ * Sets the specified row of this Matrix4f to the 4 values provided.
+ *
+ * @param row
+ * the row number to be modified (zero indexed)
+ * @param a
+ * the first column element
+ * @param b
+ * the second column element
+ * @param c
+ * the third column element
+ * @param d
+ * the fourth column element
+ */
+ public final void setRow(int row, float a, float b, float c, float d) {
+ switch (row) {
+ case 0:
+ this.m00 = a;
+ this.m01 = b;
+ this.m02 = c;
+ this.m03 = d;
+ break;
+
+ case 1:
+ this.m10 = a;
+ this.m11 = b;
+ this.m12 = c;
+ this.m13 = d;
+ break;
+
+ case 2:
+ this.m20 = a;
+ this.m21 = b;
+ this.m22 = c;
+ this.m23 = d;
+ break;
+
+ case 3:
+ this.m30 = a;
+ this.m31 = b;
+ this.m32 = c;
+ this.m33 = d;
+ break;
+
+ default:
+ throw new ArrayIndexOutOfBoundsException();
+ }
+ }
+
+ /**
+ * Sets the specified row of this Matrix4f to the three values provided.
+ *
+ * @param row
+ * the row number to be modified (zero indexed)
+ * @param v
+ * the replacement row
+ */
+ public final void setRow(int row, float v[]) {
+ switch (row) {
+ case 0:
+ this.m00 = v[0];
+ this.m01 = v[1];
+ this.m02 = v[2];
+ this.m03 = v[3];
+ break;
+
+ case 1:
+ this.m10 = v[0];
+ this.m11 = v[1];
+ this.m12 = v[2];
+ this.m13 = v[3];
+ break;
+
+ case 2:
+ this.m20 = v[0];
+ this.m21 = v[1];
+ this.m22 = v[2];
+ this.m23 = v[3];
+ break;
+
+ case 3:
+ this.m30 = v[0];
+ this.m31 = v[1];
+ this.m32 = v[2];
+ this.m33 = v[3];
+ break;
+
+ default:
+ throw new ArrayIndexOutOfBoundsException();
+ }
+ }
+
+ /**
+ * Sets the specified column of this Matrix4f to the three values provided.
+ *
+ * @param column
+ * the column number to be modified (zero indexed)
+ * @param a
+ * the first row element
+ * @param b
+ * the second row element
+ * @param c
+ * the third row element
+ * @param d
+ * the fourth row element
+ */
+ public final void setColumn(int column, float a, float b, float c, float d) {
+ switch (column) {
+ case 0:
+ this.m00 = a;
+ this.m10 = b;
+ this.m20 = c;
+ this.m30 = d;
+ break;
+
+ case 1:
+ this.m01 = a;
+ this.m11 = b;
+ this.m21 = c;
+ this.m31 = d;
+ break;
+
+ case 2:
+ this.m02 = a;
+ this.m12 = b;
+ this.m22 = c;
+ this.m32 = d;
+ break;
+
+ case 3:
+ this.m03 = a;
+ this.m13 = b;
+ this.m23 = c;
+ this.m33 = d;
+ break;
+
+ default:
+ throw new ArrayIndexOutOfBoundsException();
+ }
+ }
+
+ /**
+ * Sets the specified column of this Matrix4f to the three values provided.
+ *
+ * @param column
+ * the column number to be modified (zero indexed)
+ * @param v
+ * the replacement column
+ */
+ public final void setColumn(int column, float v[]) {
+ switch (column) {
+ case 0:
+ this.m00 = v[0];
+ this.m10 = v[1];
+ this.m20 = v[2];
+ this.m30 = v[3];
+ break;
+
+ case 1:
+ this.m01 = v[0];
+ this.m11 = v[1];
+ this.m21 = v[2];
+ this.m31 = v[3];
+ break;
+
+ case 2:
+ this.m02 = v[0];
+ this.m12 = v[1];
+ this.m22 = v[2];
+ this.m32 = v[3];
+ break;
+
+ case 3:
+ this.m03 = v[0];
+ this.m13 = v[1];
+ this.m23 = v[2];
+ this.m33 = v[3];
+ break;
+
+ default:
+ throw new ArrayIndexOutOfBoundsException();
+ }
+ }
+
+ /**
+ * Adds a scalar to each component of this matrix.
+ *
+ * @param scalar
+ * the scalar adder
+ */
+ public final void add(float scalar) {
+ this.m00 += scalar;
+ this.m01 += scalar;
+ this.m02 += scalar;
+ this.m03 += scalar;
+
+ this.m10 += scalar;
+ this.m11 += scalar;
+ this.m12 += scalar;
+ this.m13 += scalar;
+
+ this.m20 += scalar;
+ this.m21 += scalar;
+ this.m22 += scalar;
+ this.m23 += scalar;
+
+ this.m30 += scalar;
+ this.m31 += scalar;
+ this.m32 += scalar;
+ this.m33 += scalar;
+ }
+
+ /**
+ * Adds a scalar to each component of the matrix m1 and places the result
+ * into this. Matrix m1 is not modified.
+ *
+ * @param scalar
+ * the scalar adder
+ * @param m1
+ * the original matrix values
+ */
+ public final void add(float scalar, Matrix4f m1) {
+ this.m00 = m1.m00 + scalar;
+ this.m01 = m1.m01 + scalar;
+ this.m02 = m1.m02 + scalar;
+ this.m03 = m1.m03 + scalar;
+
+ this.m10 = m1.m10 + scalar;
+ this.m11 = m1.m11 + scalar;
+ this.m12 = m1.m12 + scalar;
+ this.m13 = m1.m12 + scalar;
+
+ this.m20 = m1.m20 + scalar;
+ this.m21 = m1.m21 + scalar;
+ this.m22 = m1.m22 + scalar;
+ this.m23 = m1.m23 + scalar;
+
+ this.m30 = m1.m30 + scalar;
+ this.m31 = m1.m31 + scalar;
+ this.m32 = m1.m32 + scalar;
+ this.m33 = m1.m33 + scalar;
+ }
+
+ /**
+ * Sets the value of this matrix to the matrix sum of matrices m1 and m2.
+ *
+ * @param m1
+ * the first matrix
+ * @param m2
+ * the second matrix
+ */
+ public final void add(Matrix4f m1, Matrix4f m2) {
+ this.m00 = m1.m00 + m2.m00;
+ this.m01 = m1.m01 + m2.m01;
+ this.m02 = m1.m02 + m2.m02;
+ this.m03 = m1.m03 + m2.m03;
+
+ this.m10 = m1.m10 + m2.m10;
+ this.m11 = m1.m11 + m2.m11;
+ this.m12 = m1.m12 + m2.m12;
+ this.m13 = m1.m13 + m2.m13;
+
+ this.m20 = m1.m20 + m2.m20;
+ this.m21 = m1.m21 + m2.m21;
+ this.m22 = m1.m22 + m2.m22;
+ this.m23 = m1.m23 + m2.m23;
+
+ this.m30 = m1.m30 + m2.m30;
+ this.m31 = m1.m31 + m2.m31;
+ this.m32 = m1.m32 + m2.m32;
+ this.m33 = m1.m33 + m2.m33;
+ }
+
+ /**
+ * Sets the value of this matrix to the sum of itself and matrix m1.
+ *
+ * @param m1
+ * the other matrix
+ */
+ public final void add(Matrix4f m1) {
+ this.m00 += m1.m00;
+ this.m01 += m1.m01;
+ this.m02 += m1.m02;
+ this.m03 += m1.m03;
+
+ this.m10 += m1.m10;
+ this.m11 += m1.m11;
+ this.m12 += m1.m12;
+ this.m13 += m1.m13;
+
+ this.m20 += m1.m20;
+ this.m21 += m1.m21;
+ this.m22 += m1.m22;
+ this.m23 += m1.m23;
+
+ this.m30 += m1.m30;
+ this.m31 += m1.m31;
+ this.m32 += m1.m32;
+ this.m33 += m1.m33;
+ }
+
+ /**
+ * Sets the value of this matrix to the matrix difference of matrices m1 and
+ * m2.
+ *
+ * @param m1
+ * the first matrix
+ * @param m2
+ * the second matrix
+ */
+ public final void sub(Matrix4f m1, Matrix4f m2) {
+ this.m00 = m1.m00 - m2.m00;
+ this.m01 = m1.m01 - m2.m01;
+ this.m02 = m1.m02 - m2.m02;
+ this.m03 = m1.m03 - m2.m03;
+
+ this.m10 = m1.m10 - m2.m10;
+ this.m11 = m1.m11 - m2.m11;
+ this.m12 = m1.m12 - m2.m12;
+ this.m13 = m1.m13 - m2.m13;
+
+ this.m20 = m1.m20 - m2.m20;
+ this.m21 = m1.m21 - m2.m21;
+ this.m22 = m1.m22 - m2.m22;
+ this.m23 = m1.m23 - m2.m23;
+
+ this.m30 = m1.m30 - m2.m30;
+ this.m31 = m1.m31 - m2.m31;
+ this.m32 = m1.m32 - m2.m32;
+ this.m33 = m1.m33 - m2.m33;
+ }
+
+ /**
+ * Sets the value of this matrix to the matrix difference of itself and
+ * matrix m1 (this = this - m1).
+ *
+ * @param m1
+ * the other matrix
+ */
+ public final void sub(Matrix4f m1) {
+ this.m00 -= m1.m00;
+ this.m01 -= m1.m01;
+ this.m02 -= m1.m02;
+ this.m03 -= m1.m03;
+
+ this.m10 -= m1.m10;
+ this.m11 -= m1.m11;
+ this.m12 -= m1.m12;
+ this.m13 -= m1.m13;
+
+ this.m20 -= m1.m20;
+ this.m21 -= m1.m21;
+ this.m22 -= m1.m22;
+ this.m23 -= m1.m23;
+
+ this.m30 -= m1.m30;
+ this.m31 -= m1.m31;
+ this.m32 -= m1.m32;
+ this.m33 -= m1.m33;
+ }
+
+ /**
+ * Sets the value of this matrix to its transpose.
+ */
+ public final void transpose() {
+ float temp;
+
+ temp = this.m10;
+ this.m10 = this.m01;
+ this.m01 = temp;
+
+ temp = this.m20;
+ this.m20 = this.m02;
+ this.m02 = temp;
+
+ temp = this.m30;
+ this.m30 = this.m03;
+ this.m03 = temp;
+
+ temp = this.m12;
+ this.m12 = this.m21;
+ this.m21 = temp;
+
+ temp = this.m13;
+ this.m13 = this.m31;
+ this.m31 = temp;
+
+ temp = this.m23;
+ this.m23 = this.m32;
+ this.m32 = temp;
+ }
+
+ /**
+ * Sets the value of this matrix to the transpose of the argument matrix.
+ *
+ * @param m1
+ * the matrix to be transposed
+ */
+ public final void transpose(Matrix4f m1) {
+ if (this != m1) {
+ this.m00 = m1.m00;
+ this.m01 = m1.m10;
+ this.m02 = m1.m20;
+ this.m03 = m1.m30;
+
+ this.m10 = m1.m01;
+ this.m11 = m1.m11;
+ this.m12 = m1.m21;
+ this.m13 = m1.m31;
+
+ this.m20 = m1.m02;
+ this.m21 = m1.m12;
+ this.m22 = m1.m22;
+ this.m23 = m1.m32;
+
+ this.m30 = m1.m03;
+ this.m31 = m1.m13;
+ this.m32 = m1.m23;
+ this.m33 = m1.m33;
+ }
+ else {
+ this.transpose();
+ }
+ }
+
+ /**
+ * Sets the value of this matrix to the float value of the Matrix3f
+ * argument.
+ *
+ * @param m1
+ * the Matrix4f to be converted to float
+ */
+ public final void set(Matrix4f m1) {
+ this.m00 = m1.m00;
+ this.m01 = m1.m01;
+ this.m02 = m1.m02;
+ this.m03 = m1.m03;
+
+ this.m10 = m1.m10;
+ this.m11 = m1.m11;
+ this.m12 = m1.m12;
+ this.m13 = m1.m13;
+
+ this.m20 = m1.m20;
+ this.m21 = m1.m21;
+ this.m22 = m1.m22;
+ this.m23 = m1.m23;
+
+ this.m30 = m1.m30;
+ this.m31 = m1.m31;
+ this.m32 = m1.m32;
+ this.m33 = m1.m33;
+ }
+
+ /**
+ * Sets the values in this Matrix4f equal to the row-major array parameter
+ * (ie, the first four elements of the array will be copied into the first
+ * row of this matrix, etc.).
+ *
+ * @param m
+ * the float precision array of length 16
+ */
+ public final void set(float[] m) {
+ this.m00 = m[0];
+ this.m01 = m[1];
+ this.m02 = m[2];
+ this.m03 = m[3];
+
+ this.m10 = m[4];
+ this.m11 = m[5];
+ this.m12 = m[6];
+ this.m13 = m[7];
+
+ this.m20 = m[8];
+ this.m21 = m[9];
+ this.m22 = m[10];
+ this.m23 = m[11];
+
+ this.m30 = m[12];
+ this.m31 = m[13];
+ this.m32 = m[14];
+ this.m33 = m[15];
+ }
+
+ /**
+ * Set the components of the matrix.
+ *
+ * @param m00
+ * the [0][0] element
+ * @param m01
+ * the [0][1] element
+ * @param m02
+ * the [0][2] element
+ * @param m03
+ * the [0][3] element
+ * @param m10
+ * the [1][0] element
+ * @param m11
+ * the [1][1] element
+ * @param m12
+ * the [1][2] element
+ * @param m13
+ * the [1][3] element
+ * @param m20
+ * the [2][0] element
+ * @param m21
+ * the [2][1] element
+ * @param m22
+ * the [2][2] element
+ * @param m23
+ * the [2][3] element
+ * @param m30
+ * the [3][0] element
+ * @param m31
+ * the [3][1] element
+ * @param m32
+ * the [3][2] element
+ * @param m33
+ * the [3][3] element
+ */
+ public void set(float m00, float m01, float m02, float m03, float m10, float m11, float m12, float m13, float m20, float m21, float m22, float m23, float m30, float m31, float m32, float m33) {
+ this.m00 = m00;
+ this.m01 = m01;
+ this.m02 = m02;
+ this.m03 = m03;
+
+ this.m10 = m10;
+ this.m11 = m11;
+ this.m12 = m12;
+ this.m13 = m13;
+
+ this.m20 = m20;
+ this.m21 = m21;
+ this.m22 = m22;
+ this.m23 = m23;
+
+ this.m30 = m30;
+ this.m31 = m31;
+ this.m32 = m32;
+ this.m33 = m33;
+ }
+
+ /**
+ * Computes the determinant of this matrix.
+ *
+ * @return the determinant of the matrix
+ */
+ public final float determinant() {
+ float det1 = this.m22 * this.m33 - this.m23 * this.m32;
+ float det2 = this.m12 * this.m33 - this.m13 * this.m32;
+ float det3 = this.m12 * this.m23 - this.m13 * this.m22;
+ float det4 = this.m02 * this.m33 - this.m03 * this.m32;
+ float det5 = this.m02 * this.m23 - this.m03 * this.m22;
+ float det6 = this.m02 * this.m13 - this.m03 * this.m12;
+ return
+ this.m00 * (
+ this.m11 * det1 +
+ this.m21 * det2 +
+ this.m31 * det3
+ ) +
+ this.m10 * (
+ this.m01 * det1 +
+ this.m21 * det4 +
+ this.m31 * det5
+ ) +
+ this.m20 * (
+ this.m01 * det2 +
+ this.m11 * det4 +
+ this.m31 * det6
+ ) +
+ this.m30 * (
+ this.m01 * det3 +
+ this.m11 * det5 +
+ this.m21 * det6
+ );
+ }
+
+ /**
+ * Multiplies each element of this matrix by a scalar.
+ *
+ * @param scalar
+ * The scalar multiplier.
+ */
+ public final void mul(float scalar) {
+ this.m00 *= scalar;
+ this.m01 *= scalar;
+ this.m02 *= scalar;
+ this.m03 *= scalar;
+
+ this.m10 *= scalar;
+ this.m11 *= scalar;
+ this.m12 *= scalar;
+ this.m13 *= scalar;
+
+ this.m20 *= scalar;
+ this.m21 *= scalar;
+ this.m22 *= scalar;
+ this.m23 *= scalar;
+
+ this.m30 *= scalar;
+ this.m31 *= scalar;
+ this.m32 *= scalar;
+ this.m33 *= scalar;
+ }
+
+ /**
+ * Multiplies each element of matrix m1 by a scalar and places the result
+ * into this. Matrix m1 is not modified.
+ *
+ * @param scalar
+ * the scalar multiplier
+ * @param m1
+ * the original matrix
+ */
+ public final void mul(float scalar, Matrix4f m1) {
+ this.m00 = scalar * m1.m00;
+ this.m01 = scalar * m1.m01;
+ this.m02 = scalar * m1.m02;
+ this.m03 = scalar * m1.m03;
+
+ this.m10 = scalar * m1.m10;
+ this.m11 = scalar * m1.m11;
+ this.m12 = scalar * m1.m12;
+ this.m13 = scalar * m1.m13;
+
+ this.m20 = scalar * m1.m20;
+ this.m21 = scalar * m1.m21;
+ this.m22 = scalar * m1.m22;
+ this.m23 = scalar * m1.m23;
+
+ this.m30 = scalar * m1.m30;
+ this.m31 = scalar * m1.m31;
+ this.m32 = scalar * m1.m32;
+ this.m33 = scalar * m1.m33;
+ }
+
+ /**
+ * Sets the value of this matrix to the result of multiplying itself with
+ * matrix m1.
+ *
+ * @param m1
+ * the other matrix
+ */
+ public final void mul(Matrix4f m1) {
+ float _m00, _m01, _m02, _m03, _m10, _m11, _m12, _m13, _m20, _m21, _m22, _m23, _m30, _m31, _m32, _m33;
+
+ _m00 = this.m00 * m1.m00 + this.m01 * m1.m10 + this.m02 * m1.m20 + this.m03 * m1.m30;
+ _m01 = this.m00 * m1.m01 + this.m01 * m1.m11 + this.m02 * m1.m21 + this.m03 * m1.m31;
+ _m02 = this.m00 * m1.m02 + this.m01 * m1.m12 + this.m02 * m1.m22 + this.m03 * m1.m32;
+ _m03 = this.m00 * m1.m03 + this.m01 * m1.m13 + this.m02 * m1.m23 + this.m03 * m1.m33;
+
+ _m10 = this.m10 * m1.m00 + this.m11 * m1.m10 + this.m12 * m1.m20 + this.m13 * m1.m30;
+ _m11 = this.m10 * m1.m01 + this.m11 * m1.m11 + this.m12 * m1.m21 + this.m13 * m1.m31;
+ _m12 = this.m10 * m1.m02 + this.m11 * m1.m12 + this.m12 * m1.m22 + this.m13 * m1.m32;
+ _m13 = this.m10 * m1.m03 + this.m11 * m1.m13 + this.m12 * m1.m23 + this.m13 * m1.m33;
+
+ _m20 = this.m20 * m1.m00 + this.m21 * m1.m10 + this.m22 * m1.m20 + this.m23 * m1.m30;
+ _m21 = this.m20 * m1.m01 + this.m21 * m1.m11 + this.m22 * m1.m21 + this.m23 * m1.m31;
+ _m22 = this.m20 * m1.m02 + this.m21 * m1.m12 + this.m22 * m1.m22 + this.m23 * m1.m32;
+ _m23 = this.m20 * m1.m03 + this.m21 * m1.m13 + this.m22 * m1.m23 + this.m23 * m1.m33;
+
+ _m30 = this.m30 * m1.m00 + this.m31 * m1.m10 + this.m32 * m1.m20 + this.m33 * m1.m30;
+ _m31 = this.m30 * m1.m01 + this.m31 * m1.m11 + this.m32 * m1.m21 + this.m33 * m1.m31;
+ _m32 = this.m30 * m1.m02 + this.m31 * m1.m12 + this.m32 * m1.m22 + this.m33 * m1.m32;
+ _m33 = this.m30 * m1.m03 + this.m31 * m1.m13 + this.m32 * m1.m23 + this.m33 * m1.m33;
+
+ this.m00 = _m00;
+ this.m01 = _m01;
+ this.m02 = _m02;
+ this.m03 = _m03;
+ this.m10 = _m10;
+ this.m11 = _m11;
+ this.m12 = _m12;
+ this.m13 = _m13;
+ this.m20 = _m20;
+ this.m21 = _m21;
+ this.m22 = _m22;
+ this.m23 = _m23;
+ this.m30 = _m30;
+ this.m31 = _m31;
+ this.m32 = _m32;
+ this.m33 = _m33;
+ }
+
+ /**
+ * Sets the value of this matrix to the result of multiplying the two
+ * argument matrices together.
+ *
+ * @param m1
+ * the first matrix
+ * @param m2
+ * the second matrix
+ */
+ public final void mul(Matrix4f m1, Matrix4f m2) {
+ if (this != m1 && this != m2) {
+ this.m00 = m1.m00 * m2.m00 + m1.m01 * m2.m10 + m1.m02 * m2.m20 + m1.m03 * m2.m30;
+ this.m01 = m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21 + m1.m03 * m2.m31;
+ this.m02 = m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22 + m1.m03 * m2.m32;
+ this.m03 = m1.m00 * m2.m03 + m1.m01 * m2.m13 + m1.m02 * m2.m23 + m1.m03 * m2.m33;
+
+ this.m10 = m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20 + m1.m13 * m2.m30;
+ this.m11 = m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21 + m1.m13 * m2.m31;
+ this.m12 = m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22 + m1.m13 * m2.m32;
+ this.m13 = m1.m10 * m2.m03 + m1.m11 * m2.m13 + m1.m12 * m2.m23 + m1.m13 * m2.m33;
+
+ this.m20 = m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20 + m1.m23 * m2.m30;
+ this.m21 = m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21 + m1.m23 * m2.m31;
+ this.m22 = m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22 + m1.m23 * m2.m32;
+ this.m23 = m1.m20 * m2.m03 + m1.m21 * m2.m13 + m1.m22 * m2.m23 + m1.m23 * m2.m33;
+
+ this.m30 = m1.m30 * m2.m00 + m1.m31 * m2.m10 + m1.m32 * m2.m20 + m1.m33 * m2.m30;
+ this.m31 = m1.m30 * m2.m01 + m1.m31 * m2.m11 + m1.m32 * m2.m21 + m1.m33 * m2.m31;
+ this.m32 = m1.m30 * m2.m02 + m1.m31 * m2.m12 + m1.m32 * m2.m22 + m1.m33 * m2.m32;
+ this.m33 = m1.m30 * m2.m03 + m1.m31 * m2.m13 + m1.m32 * m2.m23 + m1.m33 * m2.m33;
+ } else {
+ float _m00, _m01, _m02, _m03, _m10, _m11, _m12, _m13, _m20, _m21, _m22, _m23, _m30, _m31, _m32, _m33; // vars for temp
+ // result matrix
+
+ _m00 = m1.m00 * m2.m00 + m1.m01 * m2.m10 + m1.m02 * m2.m20 + m1.m03 * m2.m30;
+ _m01 = m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21 + m1.m03 * m2.m31;
+ _m02 = m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22 + m1.m03 * m2.m32;
+ _m03 = m1.m00 * m2.m03 + m1.m01 * m2.m13 + m1.m02 * m2.m23 + m1.m03 * m2.m33;
+
+ _m10 = m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20 + m1.m13 * m2.m30;
+ _m11 = m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21 + m1.m13 * m2.m31;
+ _m12 = m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22 + m1.m13 * m2.m32;
+ _m13 = m1.m10 * m2.m03 + m1.m11 * m2.m13 + m1.m12 * m2.m23 + m1.m13 * m2.m33;
+
+ _m20 = m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20 + m1.m23 * m2.m30;
+ _m21 = m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21 + m1.m23 * m2.m31;
+ _m22 = m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22 + m1.m23 * m2.m32;
+ _m23 = m1.m20 * m2.m03 + m1.m21 * m2.m13 + m1.m22 * m2.m23 + m1.m23 * m2.m33;
+
+ _m30 = m1.m30 * m2.m00 + m1.m31 * m2.m10 + m1.m32 * m2.m20 + m1.m33 * m2.m30;
+ _m31 = m1.m30 * m2.m01 + m1.m31 * m2.m11 + m1.m32 * m2.m21 + m1.m33 * m2.m31;
+ _m32 = m1.m30 * m2.m02 + m1.m31 * m2.m12 + m1.m32 * m2.m22 + m1.m33 * m2.m32;
+ _m33 = m1.m30 * m2.m03 + m1.m31 * m2.m13 + m1.m32 * m2.m23 + m1.m33 * m2.m33;
+
+ this.m00 = _m00;
+ this.m01 = _m01;
+ this.m02 = _m02;
+ this.m03 = _m03;
+ this.m10 = _m10;
+ this.m11 = _m11;
+ this.m12 = _m12;
+ this.m13 = _m13;
+ this.m20 = _m20;
+ this.m21 = _m21;
+ this.m22 = _m22;
+ this.m23 = _m23;
+ this.m30 = _m30;
+ this.m31 = _m31;
+ this.m32 = _m32;
+ this.m33 = _m33;
+ }
+ }
+
+ /**
+ * Returns true if all of the data members of Matrix4f m1 are equal to the
+ * corresponding data members in this Matrix4f.
+ *
+ * @param m1
+ * the matrix with which the comparison is made
+ * @return true or false
+ */
+ public boolean equals(Matrix4f m1) {
+ try {
+ return (this.m00 == m1.m00 && this.m01 == m1.m01
+ && this.m02 == m1.m02 && this.m03 == m1.m03
+ && this.m10 == m1.m10 && this.m11 == m1.m11
+ && this.m12 == m1.m12 && this.m13 == m1.m13
+ && this.m20 == m1.m20 && this.m21 == m1.m21
+ && this.m22 == m1.m22 && this.m23 == m1.m23
+ && this.m30 == m1.m30 && this.m31 == m1.m31
+ && this.m32 == m1.m32 && this.m33 == m1.m33);
+ } catch (NullPointerException e2) {
+ return false;
+ }
+
+ }
+
+ /**
+ * Returns true if the Object t1 is of type Matrix4f and all of the data
+ * members of t1 are equal to the corresponding data members in this
+ * Matrix4f.
+ *
+ * @param t1
+ * the matrix with which the comparison is made
+ * @return true or false
+ */
+ @Override
+ public boolean equals(Object t1) {
+ try {
+ Matrix4f m2 = (Matrix4f) t1;
+ return (this.m00 == m2.m00 && this.m01 == m2.m01
+ && this.m02 == m2.m02 && this.m03 == m2.m03
+ && this.m10 == m2.m10 && this.m11 == m2.m11
+ && this.m12 == m2.m12 && this.m13 == m2.m13
+ && this.m20 == m2.m20 && this.m21 == m2.m21
+ && this.m22 == m2.m22 && this.m23 == m2.m23
+ && this.m30 == m2.m30 && this.m31 == m2.m31
+ && this.m32 == m2.m32 && this.m33 == m2.m33);
+ } catch (ClassCastException e1) {
+ return false;
+ } catch (NullPointerException e2) {
+ return false;
+ }
+
+ }
+
+ /**
+ * Returns true if the L-infinite distance between this matrix and matrix m1
+ * is less than or equal to the epsilon parameter, otherwise returns false.
+ * The L-infinite distance is equal to MAX[i=0,1,2,3 ; j=0,1,2,3 ;
+ * abs(this.m(i,j) - m1.m(i,j)]
+ *
+ * @param m1
+ * the matrix to be compared to this matrix
+ * @param epsilon
+ * the threshold value
+ * @return true if this matrix is equals to the specified matrix at epsilon.
+ */
+ public boolean epsilonEquals(Matrix4f m1, float epsilon) {
+ float diff;
+
+ diff = this.m00 - m1.m00;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ diff = this.m01 - m1.m01;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ diff = this.m02 - m1.m02;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ diff = this.m03 - m1.m03;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ diff = this.m10 - m1.m10;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ diff = this.m11 - m1.m11;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ diff = this.m12 - m1.m12;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ diff = this.m13 - m1.m13;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ diff = this.m20 - m1.m20;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ diff = this.m21 - m1.m21;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ diff = this.m22 - m1.m22;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ diff = this.m23 - m1.m23;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ diff = this.m30 - m1.m30;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ diff = this.m31 - m1.m31;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ diff = this.m32 - m1.m32;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ diff = this.m33 - m1.m33;
+ if ((diff < 0 ? -diff : diff) > epsilon)
+ return false;
+
+ return true;
+ }
+
+ /**
+ * Returns a hash code value based on the data values in this object. Two
+ * different Matrix4f objects with identical data values (i.e.,
+ * Matrix4f.equals returns true) will return the same hash code value. Two
+ * objects with different data members may return the same hash value,
+ * although this is not likely.
+ *
+ * @return the integer hash code value
+ */
+ @Override
+ public int hashCode() {
+ long bits = 1L;
+ bits = 31L * bits + floatToIntBits(this.m00);
+ bits = 31L * bits + floatToIntBits(this.m01);
+ bits = 31L * bits + floatToIntBits(this.m02);
+ bits = 31L * bits + floatToIntBits(this.m03);
+ bits = 31L * bits + floatToIntBits(this.m10);
+ bits = 31L * bits + floatToIntBits(this.m11);
+ bits = 31L * bits + floatToIntBits(this.m12);
+ bits = 31L * bits + floatToIntBits(this.m13);
+ bits = 31L * bits + floatToIntBits(this.m20);
+ bits = 31L * bits + floatToIntBits(this.m21);
+ bits = 31L * bits + floatToIntBits(this.m22);
+ bits = 31L * bits + floatToIntBits(this.m23);
+ bits = 31L * bits + floatToIntBits(this.m30);
+ bits = 31L * bits + floatToIntBits(this.m31);
+ bits = 31L * bits + floatToIntBits(this.m32);
+ bits = 31L * bits + floatToIntBits(this.m33);
+ return (int) (bits ^ (bits >> 32));
+ }
+
+ private static int floatToIntBits(float d) {
+ // Check for +0 or -0
+ if (d == 0f) {
+ return 0;
+ }
+ return Float.floatToIntBits(d);
+ }
+
+ /**
+ * Sets this matrix to all zeros.
+ */
+ public final void setZero() {
+ this.m00 = 0f;
+ this.m01 = 0f;
+ this.m02 = 0f;
+ this.m03 = 0f;
+
+ this.m10 = 0f;
+ this.m11 = 0f;
+ this.m12 = 0f;
+ this.m13 = 0f;
+
+ this.m20 = 0f;
+ this.m21 = 0f;
+ this.m22 = 0f;
+ this.m23 = 0f;
+
+ this.m30 = 0f;
+ this.m31 = 0f;
+ this.m32 = 0f;
+ this.m33 = 0f;
+ }
+
+ /**
+ * Sets this matrix as diagonal.
+ *
+ * @param m00
+ * the first element of the diagonal
+ * @param m11
+ * the second element of the diagonal
+ * @param m22
+ * the third element of the diagonal
+ * @param m33
+ * the fourth element of the diagonal
+ */
+ public final void setDiagonal(float m00, float m11, float m22, float m33) {
+ this.m00 = m00;
+ this.m01 = 0f;
+ this.m02 = 0f;
+ this.m03 = 0f;
+ this.m10 = 0f;
+ this.m11 = m11;
+ this.m12 = 0f;
+ this.m13 = 0f;
+ this.m20 = 0f;
+ this.m21 = 0f;
+ this.m22 = m22;
+ this.m23 = 0f;
+ this.m30 = 0f;
+ this.m31 = 0f;
+ this.m32 = 0f;
+ this.m33 = m33;
+ }
+
+ /**
+ * Negates the value of this matrix: this = -this.
+ */
+ public final void negate() {
+ this.m00 = -this.m00;
+ this.m01 = -this.m01;
+ this.m02 = -this.m02;
+ this.m03 = -this.m03;
+
+ this.m10 = -this.m10;
+ this.m11 = -this.m11;
+ this.m12 = -this.m12;
+ this.m13 = -this.m13;
+
+ this.m20 = -this.m20;
+ this.m21 = -this.m21;
+ this.m22 = -this.m22;
+ this.m23 = -this.m23;
+
+ this.m30 = -this.m30;
+ this.m31 = -this.m31;
+ this.m32 = -this.m32;
+ this.m33 = -this.m33;
+ }
+
+ /**
+ * Sets the value of this matrix equal to the negation of of the Matrix4f
+ * parameter.
+ *
+ * @param m1
+ * the source matrix
+ */
+ public final void negate(Matrix4f m1) {
+ this.m00 = -m1.m00;
+ this.m01 = -m1.m01;
+ this.m02 = -m1.m02;
+ this.m03 = -m1.m03;
+
+ this.m10 = -m1.m10;
+ this.m11 = -m1.m11;
+ this.m12 = -m1.m12;
+ this.m13 = -m1.m13;
+
+ this.m20 = -m1.m20;
+ this.m21 = -m1.m21;
+ this.m22 = -m1.m22;
+ this.m23 = -m1.m23;
+
+ this.m30 = -m1.m30;
+ this.m31 = -m1.m31;
+ this.m32 = -m1.m32;
+ this.m33 = -m1.m33;
+ }
+
+ /**
+ * Creates a new object of the same class as this object.
+ *
+ * @return a clone of this instance.
+ * @exception OutOfMemoryError
+ * if there is not enough memory.
+ * @see java.lang.Cloneable
+ */
+ @Override
+ public Matrix4f clone() {
+ Matrix4f m1 = null;
+ try {
+ m1 = (Matrix4f) super.clone();
+ } catch (CloneNotSupportedException e) {
+ // this shouldn't happen, since we are Cloneable
+ throw new InternalError();
+ }
+
+ // Also need to create new tmp arrays (no need to actually clone them)
+ return m1;
+ }
+
+ /**
+ * Get the first matrix element in the first row.
+ *
+ * @return Returns the m00f
+ */
+ public final float getM00() {
+ return this.m00;
+ }
+
+ /**
+ * Set the first matrix element in the first row.
+ *
+ * @param m00
+ * The m00 to set.
+ */
+ public final void setM00(float m00) {
+ this.m00 = m00;
+ }
+
+ /**
+ * Get the second matrix element in the first row.
+ *
+ * @return Returns the m01.
+ */
+ public final float getM01() {
+ return this.m01;
+ }
+
+ /**
+ * Set the second matrix element in the first row.
+ *
+ * @param m01
+ * The m01 to set.
+ */
+ public final void setM01(float m01) {
+ this.m01 = m01;
+ }
+
+ /**
+ * Get the third matrix element in the first row.
+ *
+ * @return Returns the m02.
+ */
+ public final float getM02() {
+ return this.m02;
+ }
+
+ /**
+ * Set the third matrix element in the first row.
+ *
+ * @param m02
+ * The m02 to set.
+ */
+ public final void setM02(float m02) {
+ this.m02 = m02;
+ }
+
+ /**
+ * Get the fourth matrix element in the first row.
+ *
+ * @return Returns the m03.
+ */
+ public final float getM03() {
+ return this.m03;
+ }
+
+ /**
+ * Set the fourth matrix element in the first row.
+ *
+ * @param m03
+ * The m03 to set.
+ */
+ public final void setM03(float m03) {
+ this.m03 = m03;
+ }
+
+ /**
+ * Get first matrix element in the second row.
+ *
+ * @return Returns the m10
+ */
+ public final float getM10() {
+ return this.m10;
+ }
+
+ /**
+ * Set first matrix element in the second row.
+ *
+ * @param m10
+ * The m10 to set.
+ */
+ public final void setM10(float m10) {
+ this.m10 = m10;
+ }
+
+ /**
+ * Get second matrix element in the second row.
+ *
+ * @return Returns the m11.
+ */
+ public final float getM11() {
+ return this.m11;
+ }
+
+ /**
+ * Set the second matrix element in the second row.
+ *
+ * @param m11
+ * The m11 to set.
+ */
+ public final void setM11(float m11) {
+ this.m11 = m11;
+ }
+
+ /**
+ * Get the third matrix element in the second row.
+ *
+ * @return Returns the m12.
+ */
+ public final float getM12() {
+ return this.m12;
+ }
+
+ /**
+ * Set the third matrix element in the second row.
+ *
+ * @param m12
+ * The m12 to set.
+ */
+ public final void setM12(float m12) {
+ this.m12 = m12;
+ }
+
+ /**
+ * Get the fourth matrix element in the second row.
+ *
+ * @return Returns the m13.
+ */
+ public final float getM13() {
+ return this.m13;
+ }
+
+ /**
+ * Set the fourth matrix element in the second row.
+ *
+ * @param m13
+ * The m13 to set.
+ */
+ public final void setM13(float m13) {
+ this.m13 = m13;
+ }
+
+ /**
+ * Get the first matrix element in the third row.
+ *
+ * @return Returns the m20
+ */
+ public final float getM20() {
+ return this.m20;
+ }
+
+ /**
+ * Set the first matrix element in the third row.
+ *
+ * @param m20
+ * The m20 to set.
+ */
+ public final void setM20(float m20) {
+ this.m20 = m20;
+ }
+
+ /**
+ * Get the second matrix element in the third row.
+ *
+ * @return Returns the m21.
+ */
+ public final float getM21() {
+ return this.m21;
+ }
+
+ /**
+ * Set the second matrix element in the third row.
+ *
+ * @param m21
+ * The m21 to set.
+ */
+ public final void setM21(float m21) {
+ this.m21 = m21;
+ }
+
+ /**
+ * Get the third matrix element in the third row .
+ *
+ * @return Returns the m22.
+ */
+ public final float getM22() {
+ return this.m22;
+ }
+
+ /**
+ * Set the third matrix element in the third row.
+ *
+ * @param m22
+ * The m22 to set.
+ */
+ public final void setM22(float m22) {
+ this.m22 = m22;
+ }
+
+ /**
+ * Get the fourth matrix element in the third row .
+ *
+ * @return Returns the m23.
+ */
+ public final float getM23() {
+ return this.m23;
+ }
+
+ /**
+ * Set the fourth matrix element in the third row.
+ *
+ * @param m23
+ * The m23 to set.
+ */
+ public final void setM23(float m23) {
+ this.m23 = m23;
+ }
+
+ /**
+ * Get the first matrix element in the fourth row.
+ *
+ * @return Returns the m30
+ */
+ public final float getM30() {
+ return this.m30;
+ }
+
+ /**
+ * Set the first matrix element in the fourth row.
+ *
+ * @param m30
+ * The m30 to set.
+ */
+ public final void setM30(float m30) {
+ this.m30 = m30;
+ }
+
+ /**
+ * Get the second matrix element in the fourth row.
+ *
+ * @return Returns the m31.
+ */
+ public final float getM31() {
+ return this.m31;
+ }
+
+ /**
+ * Set the second matrix element in the fourth row.
+ *
+ * @param m31
+ * The m31 to set.
+ */
+ public final void setM31(float m31) {
+ this.m31 = m31;
+ }
+
+ /**
+ * Get the third matrix element in the fourth row .
+ *
+ * @return Returns the m32.
+ */
+ public final float getM32() {
+ return this.m32;
+ }
+
+ /**
+ * Set the third matrix element in the fourth row.
+ *
+ * @param m32
+ * The m32 to set.
+ */
+ public final void setM32(float m32) {
+ this.m32 = m32;
+ }
+
+ /**
+ * Get the fourth matrix element in the fourth row .
+ *
+ * @return Returns the m33.
+ */
+ public final float getM33() {
+ return this.m33;
+ }
+
+ /**
+ * Set the fourth matrix element in the fourth row.
+ *
+ * @param m33
+ * The m33 to set.
+ */
+ public final void setM33(float m33) {
+ this.m33 = m33;
+ }
+
+ /** Replies if the matrix is symmetric.
+ *
+ * @return true if the matrix is symmetric, otherwise
+ * false
+ */
+ public boolean isSymmetric() {
+ return this.m01 == this.m10
+ && this.m02 == this.m20
+ && this.m03 == this.m03
+ && this.m12 == this.m21
+ && this.m13 == this.m31
+ && this.m23 == this.m32;
+ }
+
+ /** Replies if the matrix is identity.
+ *
+ * This function uses the equal-to-zero test with the error {@link MathConstants#EPSILON}.
+ *
+ * @return true if the matrix is identity; false otherwise.
+ * @see MathUtil#isEpsilonZero(float)
+ * @see MathUtil#isEpsilonEqual(float, float)
+ */
+ public boolean isIdentity() {
+ //TODO Buffering the boolean value "isIdentity"
+ return MathUtil.isEpsilonEqual(this.m00, 1f, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonZero(this.m01, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonZero(this.m02, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonZero(this.m03, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonZero(this.m10, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonEqual(this.m11, 1f, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonZero(this.m12, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonZero(this.m13, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonZero(this.m20, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonZero(this.m21, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonEqual(this.m22, 1f, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonZero(this.m23, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonZero(this.m30, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonZero(this.m31, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonZero(this.m32, MathConstants.JVM_MIN_FLOAT_EPSILON)
+ && MathUtil.isEpsilonEqual(this.m33, 1f, MathConstants.JVM_MIN_FLOAT_EPSILON);
+ }
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/SingularMatrixException.java b/core/math/src/main/java/org/arakhne/afc/math/SingularMatrixException.java
new file mode 100644
index 000000000..5e95105e1
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/SingularMatrixException.java
@@ -0,0 +1,63 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2011 Janus Core Developers
+ * Copyright (C) 2012 Stéphane GALLAND
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+package org.arakhne.afc.math;
+
+
+
+/** Exception for all the singular matrices.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class SingularMatrixException extends RuntimeException {
+
+ private static final long serialVersionUID = 2834240107372614319L;
+
+ /**
+ */
+ public SingularMatrixException() {
+ //
+ }
+
+ /**
+ * @param message
+ */
+ public SingularMatrixException(String message) {
+ super(message);
+ }
+
+ /**
+ * @param cause
+ */
+ public SingularMatrixException(Throwable cause) {
+ super(cause);
+ }
+
+ /**
+ * @param message
+ * @param cause
+ */
+ public SingularMatrixException(String message, Throwable cause) {
+ super(message, cause);
+ }
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry/ClassifierUtil.java b/core/math/src/main/java/org/arakhne/afc/math/geometry/ClassifierUtil.java
new file mode 100644
index 000000000..be7fda01f
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry/ClassifierUtil.java
@@ -0,0 +1,1437 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Christophe BOHRHAUER.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry;
+
+import org.arakhne.afc.math.MathConstants;//TODO Faire les JUNIT
+import org.arakhne.afc.math.MathUtil;
+import org.arakhne.afc.math.geometry2d.continuous.Point2f;
+import org.arakhne.afc.math.geometry3d.continuous.Point3f;
+/**
+ * Several intersection functions.
+ *
+ * Algo inspired from Mathematics for 3D Game Programming and Computer Graphics (MGPCG)
+ * and from 3D Game Engine Design (GED)
+ * and from Real Time Collision Detection (RTCD).
+ *
+ * @author $Author: cbohrhauer$
+ * @author $Author: sgalland$
+ * @author $Author: ngaud$
+ * @author $Author: jdemange$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public final class ClassifierUtil implements MathConstants{
+
+ private ClassifierUtil() {
+ //
+ }
+
+ /**
+ * Classify a sphere against an axis-aligned box.
+ *
+ * This function assumes:
+ *
+ * sphere_radius >= 0lowerx <= upperxlowery <= upperylowerz <= upperz
+ *
+ * @param sphereCenterx is the X coordinate of the sphere center.
+ * @param sphereCentery is the Y coordinate of the sphere center.
+ * @param sphereCenterz is the Z coordinate of the sphere center.
+ * @param radius the radius of the sphere.
+ * @param lowerx is the X coordinate of the lowest point of the box.
+ * @param lowery is the Y coordinate of the lowest point of the box.
+ * @param lowerz is the Z coordinate of the lowest point of the box.
+ * @param upperx is the X coordinate of the uppermost point of the box.
+ * @param uppery is the Y coordinate of the uppermost point of the box.
+ * @param upperz is the Z coordinate of the uppermost point of the box.
+ * @return the value {@link IntersectionType#INSIDE} if the sphere is inside the box;
+ * {@link IntersectionType#OUTSIDE} if the sphere is outside the box;
+ * {@link IntersectionType#ENCLOSING} if the sphere is enclosing the box;
+ * {@link IntersectionType#SPANNING} otherwise.
+ */
+ public static IntersectionType classifiesSolidSphereSolidAlignedBox(
+ float sphereCenterx, float sphereCentery, float sphereCenterz, float radius,
+ float lowerx, float lowery, float lowerz,
+ float upperx, float uppery, float upperz) {
+
+ // Assumptions
+ assert(radius >= 0.);
+ assert(lowerx <= upperx);
+ assert(lowery <= uppery);
+ assert(lowerz <= upperz);
+
+ // Compute the distance betwen the sphere center and
+ // the closest point of the box
+
+ // Compute the distance between the sphere center and
+ // the farest point of the box
+
+ float dmin; // distance between the sphere center and the nearest point of the box.
+ float dmax; // distance between the sphere center and the farest point of the box.
+ float a,b; // tmp value
+
+ boolean sphereInsideOnAllAxis = false;
+
+ dmin = dmax = 0.f;
+
+ if (sphereCenterxupperx) {
+ a = sphereCenterx - upperx;
+ dmin = a*a;
+ a = sphereCenterx - lowerx;
+ dmax = a*a;
+ }
+ else {
+ a = sphereCenterx-lowerx;
+ b = upperx-sphereCenterx;
+ if (a>=b) {
+ sphereInsideOnAllAxis = (radiusuppery) {
+ a = sphereCentery - uppery;
+ dmin += a*a;
+ a = sphereCentery - lowery;
+ dmax += a*a;
+ }
+ else {
+ a = sphereCentery-lowery;
+ b = uppery-sphereCentery;
+ if (a>=b) {
+ sphereInsideOnAllAxis &= (radiusupperz) {
+ a = sphereCenterz - upperz;
+ dmin += a*a;
+ a = sphereCenterz - lowerz;
+ dmax += a*a;
+ }
+ else {
+ a = sphereCenterz-lowerz;
+ b = upperz-sphereCenterz;
+ if (a>=b) {
+ sphereInsideOnAllAxis &= (radiusdmax) return IntersectionType.ENCLOSING;
+ }
+ else {
+ // Sphere center is outside the box.
+ if (sr<=dmin) return IntersectionType.OUTSIDE;
+ if (sr>dmax) return IntersectionType.ENCLOSING;
+ }
+ return IntersectionType.SPANNING;
+ }
+
+ /**
+ * Classify a circle against an minimum bounding rectangle.
+ *
+ * This function assumes:
+ *
+ * radius >= 0lowerx <= upperxlowery <= uppery
+ *
+ *
+ * @param circleCenterx is the X coordinate of the circle center.
+ * @param circleCentery is the Y coordinate of the circle center.
+ * @param radius the radius of the circle.
+ * @param lowerx is the X coordinate of the lowest point of the box.
+ * @param lowery is the Y coordinate of the lowest point of the box.
+ * @param upperx is the X coordinate of the uppermost point of the box.
+ * @param uppery is the Y coordinate of the uppermost point of the box.
+ * @return the value {@link IntersectionType#INSIDE} if the sphere is inside the rectangle;
+ * {@link IntersectionType#OUTSIDE} if the circle is outside the rectangle;
+ * {@link IntersectionType#ENCLOSING} if the circle is enclosing the rectangle;
+ * {@link IntersectionType#SPANNING} otherwise.
+ */
+ public static IntersectionType classifiesSolidCircleSolidAlignedRectangle(
+ float circleCenterx, float circleCentery, float radius,
+ float lowerx, float lowery, float upperx, float uppery) {
+ // Assumptions
+ assert(radius >= 0.);
+ assert(lowerx <= upperx);
+ assert(lowery <= uppery);
+
+ // Compute the distance between the circle center and
+ // the closest point of the box
+
+ // Compute the distance between the circle center and
+ // the farthest point of the box
+
+ float dmin; // distance between the circle center and the nearest point of the box.
+ float dmax; // distance between the circle center and the farthest point of the box.
+ float a,b; // tmp value
+
+ boolean circleInsideOnAllAxis = false;
+
+ dmin = dmax = 0.f;
+
+ if (circleCenterxupperx) {
+ a = circleCenterx - upperx;
+ dmin = a*a;
+ a = circleCenterx - lowerx;
+ dmax = a*a;
+ }
+ else {
+ a = circleCenterx-lowerx;
+ b = upperx-circleCenterx;
+ if (a>=b) {
+ circleInsideOnAllAxis = (radiusuppery) {
+ a = circleCentery - uppery;
+ dmin += a*a;
+ a = circleCentery - lowery;
+ dmax += a*a;
+ }
+ else {
+ a = circleCentery-lowery;
+ b = uppery-circleCentery;
+ if (a>=b) {
+ circleInsideOnAllAxis &= (radiusdmax) return IntersectionType.ENCLOSING;
+ }
+ else {
+ // circle center is outside the box.
+ if (sr<=dmin) return IntersectionType.OUTSIDE;
+ if (sr>dmax) return IntersectionType.ENCLOSING;
+ }
+ return IntersectionType.SPANNING;
+ }
+
+ /**
+ * Classifies two 1D segments.
+ *
+ * This function is assuming that l1 is lower
+ * or equal to u1 and l2 is lower
+ * or equal to u2.
+ *
+ * @param l1 the min coordinate of the first segment
+ * @param u1 the max coordinate of the first segment
+ * @param l2 the min coordinate of the second segment
+ * @param u2 the max coordinate of the second segment
+ * @return the value {@link IntersectionType#INSIDE} if the first segment is inside
+ * the second segment; {@link IntersectionType#OUTSIDE} if the first segment is
+ * outside the second segment; {@link IntersectionType#ENCLOSING} if the
+ * first segment is enclosing the second segment;
+ * {@link IntersectionType#SPANNING} otherwise.
+ */
+ public static IntersectionType classifiesAlignedSegments(float l1, float u1, float l2, float u2) {
+ assert(l1<=u1);
+ assert(l2<=u2);
+ if (l1u2) return IntersectionType.ENCLOSING;
+ return IntersectionType.SPANNING;
+ }
+ if (u2u1) return IntersectionType.INSIDE;
+ return IntersectionType.SPANNING;
+ }
+
+ /**
+ * Classifies two 2D axis-aligned rectangles.
+ *
+ * This function is assuming that lx1 is lower
+ * or equal to ux1, ly1 is lower
+ * or equal to uy1, and so on.
+ *
+ * @param lx1 the X coordinate of the lowest point of the first rectangle.
+ * @param ly1 the Y coordinate of the lowest point of the first rectangle.
+ * @param ux1 the X coordinate of the uppermost point of the first rectangle.
+ * @param uy1 the Y coordinate of the uppermost point of the first rectangle.
+ * @param lx2 the X coordinate of the lowest point of the second rectangle.
+ * @param ly2 the Y coordinate of the lowest point of the second rectangle.
+ * @param ux2 the X coordinate of the uppermost point of the second rectangle.
+ * @param uy2 the Y coordinate of the uppermost point of the second rectangle.
+ * @return the value {@link IntersectionType#INSIDE} if the first rectangle is inside
+ * the second rectangle; {@link IntersectionType#OUTSIDE} if the first rectangle is
+ * outside the second rectangle; {@link IntersectionType#ENCLOSING} if the
+ * first rectangle is enclosing the second rectangle;
+ * {@link IntersectionType#ENCLOSING} if the first rectangle is the same as the second rectangle;
+ * {@link IntersectionType#SPANNING} otherwise.
+ */
+ public static IntersectionType classifiesAlignedRectangles(float lx1, float ly1, float ux1, float uy1, float lx2, float ly2, float ux2, float uy2) {
+ assert(lx1<=ux1);
+ assert(ly1<=uy1);
+ assert(lx2<=ux2);
+ assert(ly2<=uy2);
+
+ IntersectionType inter;
+
+ if (lx1lx2) {
+ if (ux2<=lx1) return IntersectionType.OUTSIDE;
+ if (ux1<=ux2) inter = IntersectionType.INSIDE;
+ else inter = IntersectionType.SPANNING;
+ } else {
+ if (ux1==ux2) inter = IntersectionType.SAME;
+ else if (ux1ly2) {
+ if (uy2<=ly1) return IntersectionType.OUTSIDE;
+ if (uy1<=uy2) return inter.and(IntersectionType.INSIDE);
+ return inter.and(IntersectionType.SPANNING);
+ } else {
+ if (uy1==uy2) return inter.and(IntersectionType.SAME);
+ else if (uy1
+ * This function is assuming that lower1x is lower
+ * or equal to upper1x, lower1y is lower
+ * or equal to upper1y, and so on.
+ *
+ * @param lower1x is the X coordinate of the lowest point of the first box.
+ * @param lower1y is the Y coordinate of the lowest point of the first box.
+ * @param lower1z is the Z coordinate of the lowest point of the first box.
+ * @param upper1x is the X coordinate of the uppermost point of the first box.
+ * @param upper1y is the Y coordinate of the uppermost point of the first box.
+ * @param upper1z is the Z coordinate of the uppermost point of the first box.
+ * @param lower2x is the X coordinate of the lowest point of the second box.
+ * @param lower2y is the Y coordinate of the lowest point of the second box.
+ * @param lower2z is the Z coordinate of the lowest point of the second box.
+ * @param upper2x is the X coordinate of the uppermost point of the second box.
+ * @param upper2y is the Y coordinate of the uppermost point of the second box.
+ * @param upper2z is the Z coordinate of the uppermost point of the second box.
+ * @return the value {@link IntersectionType#INSIDE} if the first box is inside
+ * the second box; {@link IntersectionType#OUTSIDE} if the first box is
+ * outside the second box; {@link IntersectionType#ENCLOSING} if the
+ * first box is enclosing the second box;
+ * {@link IntersectionType#SPANNING} otherwise.
+ */
+ public static IntersectionType classifiesAlignedBoxes(
+ float lower1x, float lower1y, float lower1z,
+ float upper1x, float upper1y, float upper1z,
+ float lower2x, float lower2y, float lower2z,
+ float upper2x, float upper2y, float upper2z) {
+ assert(lower1x<=upper1x);
+ assert(lower1y<=upper1y);
+ assert(lower1z<=upper1z);
+ assert(lower2x<=upper2x);
+ assert(lower2y<=upper2y);
+ assert(lower2z<=upper2z);
+
+ IntersectionType inter;
+
+ if (lower1xlower2x) {
+ if (upper2x<=lower1x) return IntersectionType.OUTSIDE;
+ if (upper1x<=upper2x) inter = IntersectionType.INSIDE;
+ else inter = IntersectionType.SPANNING;
+ } else {
+ if (upper1x==upper2x) inter = IntersectionType.SAME;
+ else if (upper1xlower2y) {
+ if (upper2y<=lower1y) return IntersectionType.OUTSIDE;
+ if (upper1y<=upper2y) inter = inter.and(IntersectionType.INSIDE);
+ else inter = inter.and(IntersectionType.SPANNING);
+ } else {
+ if (upper1y==upper2y) inter = inter.and(IntersectionType.SAME);
+ else if (upper1ylower2z) {
+ if (upper2z<=lower1z) return IntersectionType.OUTSIDE;
+ if (upper1z<=upper2z) return inter.and(IntersectionType.INSIDE);
+ return inter.and(IntersectionType.SPANNING);
+ }
+ if (upper1z==upper2z) return inter.and(IntersectionType.SAME);
+ else if (upper1z
+ * This function is assuming that lower1x is lower
+ * or equal to upper1x, and lower1y is lower
+ * or equal to upper1y.
+ *
+ * This function is implemented in the best efficient way according
+ * to the priority of the intersections types which are deduced
+ * from {@link IntersectionType#and(IntersectionType, IntersectionType)}:
+ * a) if one axis is {@code OUTSIDE} the boxes are {@code OUTSIDE}, b) if
+ * one axis is {@code SPANNING} the boxes are {@code SPANNING}, c) otherwise
+ * the "and" rule is applied.
+ *
+ * @param lower1x is the X coordinate of the lowest point of the first rectangle.
+ * @param lower1y is the Y coordinate of the lowest point of the first rectangle.
+ * @param upper1x is the X coordinate of the uppermost point of the first rectangle.
+ * @param upper1y is the Y coordinate of the uppermost point of the first rectangle.
+ * @param lower2x is the X coordinate of the lowest point of the second rectangle.
+ * @param lower2y is the Y coordinate of the lowest point of the second rectangle.
+ * @param upper2x is the X coordinate of the uppermost point of the second rectangle.
+ * @param upper2y is the Y coordinate of the uppermost point of the second rectangle.
+ * @return the value {@link IntersectionType#INSIDE} if the first rectangle is inside
+ * the second rectangle; {@link IntersectionType#OUTSIDE} if the first rectangle is
+ * outside the second rectangle; {@link IntersectionType#ENCLOSING} if the
+ * first rectangle is enclosing the second rectangle;
+ * {@link IntersectionType#SPANNING} otherwise.
+ */
+ public static IntersectionType classifiesAlignedRectangles(
+ float lower1x, float lower1y, float lower1z,
+ float upper1x, float upper1y, float upper1z,
+ float lower2x, float lower2y, float lower2z,
+ float upper2x, float upper2y, float upper2z) {
+ assert(lower1x<=upper1x);
+ assert(lower1y<=upper1y);
+ assert(lower2x<=upper2x);
+ assert(lower2y<=upper2y);
+
+ IntersectionType inter;
+
+ if (lower1xlower2x) {
+ if (upper2x<=lower1x) return IntersectionType.OUTSIDE;
+ if (upper1x<=upper2x) inter = IntersectionType.INSIDE;
+ else inter = IntersectionType.SPANNING;
+ } else {
+ if (upper1x==upper2x) inter = IntersectionType.SAME;
+ else if (upper1xlower2y) {
+ if (upper2y<=lower1y) return IntersectionType.OUTSIDE;
+ if (upper1y<=upper2y) return inter.and(IntersectionType.INSIDE);
+ return inter.and(IntersectionType.SPANNING);
+ }
+ if (upper1y==upper2y) return inter.and(IntersectionType.SAME);
+ else if (upper1y
+ * The extents are assumed to be positive or zero.
+ *
+ * @param sphereCenterx is the X coordinate of the sphere center.
+ * @param sphereCentery is the Y coordinate of the sphere center.
+ * @param sphereCenterz is the Z coordinate of the sphere center.
+ * @param radius is the radius of the sphere.
+ * @param boxCenterx is the X coordinate of the box center.
+ * @param boxCentery is the Y coordinate of the box center.
+ * @param boxCenterz is the Z coordinate of the box center.
+ * @param boxAxis1x is the X coordinate of the Axis1 unit vector.
+ * @param boxAxis1y is the Y coordinate of the Axis1 unit vector.
+ * @param boxAxis1z is the Z coordinate of the Axis1 unit vector.
+ * @param boxAxis2x is the X coordinate of the Axis2 unit vector.
+ * @param boxAxis2y is the Y coordinate of the Axis2 unit vector.
+ * @param boxAxis2z is the Z coordinate of the Axis2 unit vector.
+ * @param boxAxis3x is the X coordinate of the Axis3 unit vector.
+ * @param boxAxis3y is the Y coordinate of the Axis3 unit vector.
+ * @param boxAxis3z is the Z coordinate of the Axis3 unit vector.
+ * @param boxExtentAxis1 is the 'Axis1' size of the OBB.
+ * @param boxExtentAxis2 is the 'Axis2' size of the OBB.
+ * @param boxExtentAxis3 is the 'Axis3' size of the OBB.
+ * @param epsilon the accuracy parameter (distance) must be the same unit of measurement as others parameters
+ * @return the value {@link IntersectionType#INSIDE} if the sphere is inside
+ * the box; {@link IntersectionType#OUTSIDE} if the sphere is
+ * outside the box; {@link IntersectionType#ENCLOSING} if the
+ * sphere is enclosing the box;
+ * {@link IntersectionType#SPANNING} otherwise.
+ */
+ public static IntersectionType classifiesSolidSphereOrientedBox(
+ float sphereCenterx, float sphereCentery, float sphereCenterz, float sphereRadius,
+ float boxCenterx, float boxCentery, float boxCenterz,
+ float boxAxis1x, float boxAxis1y, float boxAxis1z,
+ float boxAxis2x, float boxAxis2y, float boxAxis2z,
+ float boxAxis3x, float boxAxis3y, float boxAxis3z,
+ float boxExtentAxis1, float boxExtentAxis2, float boxExtentAxis3,float epsilon) {
+ assert(sphereRadius>=0);
+ assert(boxExtentAxis1>=0);
+ assert(boxExtentAxis2>=0);
+ assert(boxExtentAxis3>=0);
+
+ // Find points on OBB closest and farest to sphere center
+ Point3f closest = new Point3f();
+ Point3f farest = new Point3f();
+ GeometryUtil.closestFarthestPointsOBBPoint(
+ boxCenterx, boxCentery, boxCenterz,
+ boxAxis1x, boxAxis1y, boxAxis1z,
+ boxAxis2x, boxAxis2y, boxAxis2z,
+ boxAxis3x, boxAxis3y, boxAxis3z,
+ boxExtentAxis1, boxExtentAxis2, boxExtentAxis3,
+ sphereCenterx, sphereCentery, sphereCenterz,
+ closest,
+ farest);
+
+ // Sphere and OBB intersect if the (squared) distance from sphere
+ // center to point p is less than the (squared) sphere radius
+ float squaredRadius = sphereRadius * sphereRadius;
+
+ // Compute (squared) distance to closest point
+ if (GeometryUtil.distanceSquaredPointPoint(farest.getX(),farest.getY(), farest.getZ(), sphereCenterx, sphereCentery, sphereCenterz)squaredRadius+epsilon) return IntersectionType.OUTSIDE;
+
+ // If the sphere center is inside the box and the
+ // radius is inside the box's extents, then the
+ // sphere is inside the box.
+
+ if (MathUtil.isEpsilonZero(d, epsilon)) {
+
+ float vx= sphereCenterx - boxCenterx;
+ float vy= sphereCentery - boxCentery;
+ float vz= sphereCenterz - boxCenterz;
+
+ float d1,d2,d3;
+ d1 = Math.abs(MathUtil.dotProduct(boxAxis1x, boxAxis1y, boxAxis1z, vx, vy, vz));
+ d2 = Math.abs(MathUtil.dotProduct(boxAxis2x, boxAxis2y, boxAxis2z, vx, vy, vz));
+ d3 = Math.abs(MathUtil.dotProduct(boxAxis3x, boxAxis3y, boxAxis3z, vx, vy, vz));
+
+ if(d1+sphereRadius > boxExtentAxis1 || d2+sphereRadius > boxExtentAxis2 || d3+sphereRadius > boxExtentAxis3)
+ return IntersectionType.SPANNING;
+ return IntersectionType.INSIDE;
+ }
+
+ return IntersectionType.SPANNING;
+
+ }
+
+ /**
+ * Classifies a circle against an oriented rectangle.
+ *
+ * The extents are assumed to be positive or zero just as circleRadius.
+ *
+ * @param circleCenterx is the X coordinate of the circle center.
+ * @param circleCentery is the Y coordinate of the circle center.
+ * @param radius is the radius of the circle.
+ * @param centerx is the X coordinate of the rectangle center.
+ * @param centery is the Y coordinate of the rectangle center.
+ * @param rectangleAxis1x is the X coordinate of the Axis1 unit vector.
+ * @param rectangleAxis1y is the Y coordinate of the Axis1 unit vector.
+ * @param rectangleAxis2x is the X coordinate of the Axis2 unit vector.
+ * @param rectangleAxis2y is the Y coordinate of the Axis2 unit vector.
+ * @param rectangleExtentAxis1 is the 'Axis1' size of the OBR.
+ * @param rectangleExtentAxis2 is the 'Axis2' size of the OBR.
+ * @param epsilon the accuracy parameter (distance) must be the same unit of measurement as others parameters
+ * @return the value {@link IntersectionType#INSIDE} if the circle is inside
+ * the rectangle; {@link IntersectionType#OUTSIDE} if the circle is
+ * outside the rectangle; {@link IntersectionType#ENCLOSING} if the
+ * circle is enclosing the rectangle;
+ * {@link IntersectionType#SPANNING} otherwise.
+ */
+ public static IntersectionType classifiesSolidCircleOrientedRectangle(
+ float circleCenterx, float circleCentery, float circleRadius,
+ float centerx, float centery,
+ float rectangleAxis1x, float rectangleAxis1y,
+ float rectangleAxis2x, float rectangleAxis2y,
+ float rectangleAxis3x, float rectangleAxis3y,
+ float rectangleExtentAxis1, float rectangleExtentAxis2,float epsilon) {
+ assert(circleRadius>=0);
+ assert(rectangleExtentAxis1>=0);
+ assert(rectangleExtentAxis2>=0);
+
+ // Find points on OBB closest and farest to sphere center
+ Point2f closest = new Point2f();
+ Point2f farest = new Point2f();
+ GeometryUtil.closestFarthestPointsOBRPoint(
+ circleCenterx, circleCentery,
+ centerx, centery,
+ rectangleAxis1x, rectangleAxis1y,
+ rectangleAxis2x, rectangleAxis2y,
+ rectangleExtentAxis1, rectangleExtentAxis2,
+ closest,
+ farest);
+
+ // Sphere and OBB intersect if the (squared) distance from sphere
+ // center to point p is less than the (squared) sphere radius
+ float squaredRadius = circleRadius * circleRadius;
+
+ // Compute (squared) distance to closest point
+ if (GeometryUtil.distanceSquaredPointPoint(farest.getX(),farest.getY(), circleCenterx, circleCentery)squaredRadius+epsilon) return IntersectionType.OUTSIDE;
+
+ // If the sphere center is inside the rectangle and the
+ // radius is inside the rectangle's extents, then the
+ // sphere is inside the rectangle.
+
+ if (MathUtil.isEpsilonZero(d, epsilon)) {
+
+ float vx= circleCenterx - centerx;
+ float vy= circleCentery - centery;
+
+ float d1,d2;
+ d1 = Math.abs(MathUtil.dotProduct(rectangleAxis1x, rectangleAxis1y, vx, vy));
+ d2 = Math.abs(MathUtil.dotProduct(rectangleAxis2x, rectangleAxis2y, vx, vy));
+
+ if(d1+circleRadius > rectangleExtentAxis1 || d2+circleRadius > rectangleExtentAxis2)
+ return IntersectionType.SPANNING;
+ return IntersectionType.INSIDE;
+ }
+
+ return IntersectionType.SPANNING;
+
+ }
+
+ /**
+ * Classifies the OBB's axis according to {@code |T.L|}, {@code ra} and {@rb}
+ * where {@code T} is the vector between the OBB's centers, {@code L} is
+ * the separation vector, {@code ra} is the size of the projection of the first
+ * OBB on L, and {@code rb} is the size of the projection of the second
+ * OBB on L.
+ *
+ * This function is also working fine for 2D classifications.
+ *
+ * @param tl is {@code |T.L|}
+ * @param ra is the size of the projection of the first OBB on L
+ * @param rb is the size of the projection of the second OBB on L
+ * @param type is the intersection type previously detected for the other axis.
+ * @return the type of intersection
+ * @see Gamasutra OBB intersection test
+ * @see #classifiesOrientedBoxes(float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float)
+ * @see #classifiesOrientedRectangles(float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float)
+ */
+ private static IntersectionType classifiesOrientedBoxAxis(float tl, float ra, float rb, IntersectionType type) {
+ // Special case: same center, same radius.
+ if (tl==0. && ra==rb) return type;
+ // A and B do not overlap.
+ if (tl > ra+rb) return IntersectionType.OUTSIDE;
+
+ IntersectionType t;
+
+ // A is enclosing B
+ if (tl+rb < ra) t = IntersectionType.ENCLOSING;
+ // A is inside B
+ else if (tl+ra < rb) t = IntersectionType.INSIDE;
+ // A and B do overlap
+ else return IntersectionType.SPANNING;
+
+ return (type==null) ? t : IntersectionType.and(type, t);
+ }
+
+ /**
+ * Classifies two oriented boxes.
+ *
+ * The extents are assumed to be positive or zero.
+ * The lengths of the given arrays are assumed to be 3.
+ *
+ * This function uses the "separating axis theorem" which states that
+ * for any two OBBs (AABB is a special case of OBB)
+ * that do not touch, a separating axis can be found.
+ *
+ * This function uses an general intersection test between two OBB.
+ * If the first box is expected to be an AAB, please use the
+ * optimized algorithm given by
+ * {@link #classifiesAlignedBoxOrientedBox(float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float)}.
+ *
+ * @param center1x is the X coordinate of the box1 center.
+ * @param center1y is the Y coordinate of the box1 center.
+ * @param center1z is the Z coordinate of the box1 center.
+ * @param box1Axis1x is the X coordinate of the Axis1 unit vector.
+ * @param box1Axis1y is the Y coordinate of the Axis1 unit vector.
+ * @param box1Axis1z is the Z coordinate of the Axis1 unit vector.
+ * @param box1Axis2x is the X coordinate of the Axis2 unit vector.
+ * @param box1Axis2y is the Y coordinate of the Axis2 unit vector.
+ * @param box1Axis2z is the Z coordinate of the Axis2 unit vector.
+ * @param box1Axis3x is the X coordinate of the Axis3 unit vector.
+ * @param box1Axis3y is the Y coordinate of the Axis3 unit vector.
+ * @param box1Axis3z is the Z coordinate of the Axis3 unit vector.
+ * @param box1ExtentAxis1 is the 'Axis1' size of the box1.
+ * @param box1ExtentAxis2 is the 'Axis2' size of the box1.
+ * @param box1ExtentAxis3 is the 'Axis3' size of the box1.
+ * @param center2x is the X coordinate of the box2 center.
+ * @param center2y is the Y coordinate of the box2 center.
+ * @param center2z is the Z coordinate of the box2 center.
+ * @param box2Axis1x is the X coordinate of the Axis1 unit vector.
+ * @param box2Axis1y is the Y coordinate of the Axis1 unit vector.
+ * @param box2Axis1z is the Z coordinate of the Axis1 unit vector.
+ * @param box2Axis2x is the X coordinate of the Axis2 unit vector.
+ * @param box2Axis2y is the Y coordinate of the Axis2 unit vector.
+ * @param box2Axis2z is the Z coordinate of the Axis2 unit vector.
+ * @param box2Axis3x is the X coordinate of the Axis3 unit vector.
+ * @param box2Axis3y is the Y coordinate of the Axis3 unit vector.
+ * @param box2Axis3z is the Z coordinate of the Axis3 unit vector.
+ * @param box2ExtentAxis1 is the 'Axis1' size of the box2.
+ * @param box2ExtentAxis2 is the 'Axis2' size of the box2.
+ * @param box2ExtentAxis3 is the 'Axis3' size of the box2.
+ * @return the value {@link IntersectionType#INSIDE} if the first box is inside
+ * the second box; {@link IntersectionType#OUTSIDE} if the first box is
+ * outside the second box; {@link IntersectionType#ENCLOSING} if the
+ * first box is enclosing the second box;
+ * {@link IntersectionType#SPANNING} otherwise.
+ * @see "RTCD pages 102-105"
+ * @see Gamasutra OBB intersection test
+ */
+ public static IntersectionType classifiesOrientedBoxes(
+ float center1x, float center1y, float center1z,
+ float box1Axis1x, float box1Axis1y, float box1Axis1z,
+ float box1Axis2x, float box1Axis2y, float box1Axis2z,
+ float box1Axis3x, float box1Axis3y, float box1Axis3z,
+ float box1ExtentAxis1, float box1ExtentAxis2, float box1ExtentAxis3,
+ float center2x, float center2y, float center2z,
+ float box2Axis1x, float box2Axis1y, float box2Axis1z,
+ float box2Axis2x, float box2Axis2y, float box2Axis2z,
+ float box2Axis3x, float box2Axis3y, float box2Axis3z,
+ float box2ExtentAxis1, float box2ExtentAxis2, float box2ExtentAxis3) {
+
+ assert(box1ExtentAxis1>=0);
+ assert(box1ExtentAxis2>=0);
+ assert(box1ExtentAxis3>=0);
+ assert(box2ExtentAxis1>=0);
+ assert(box2ExtentAxis2>=0);
+ assert(box2ExtentAxis3>=0);
+
+ //translation, in parent frame
+ //translation, in parent frame
+ float vx, vy, vz;
+ vx = center2x - center1x;
+ vy = center2y - center1y;
+ vz = center2z - center1z;
+
+ //translation, in A's frame
+ float tx,ty,tz;
+ tx = MathUtil.dotProduct(vx, vy, vz, box1Axis1x, box1Axis1y, box1Axis1z);
+ ty = MathUtil.dotProduct(vx, vy, vz, box1Axis2x, box1Axis2y, box1Axis2z);
+ tz = MathUtil.dotProduct(vx, vy, vz, box1Axis3x, box1Axis3y, box1Axis3z);
+
+ //B's basis with respect to A's local frame
+ float R_1_1, R_1_2, R_1_3,
+ R_2_1, R_2_2, R_2_3,
+ R_3_1, R_3_2, R_3_3,
+
+ absR_1_1, absR_1_2, absR_1_3,
+ absR_2_1, absR_2_2, absR_2_3,
+ absR_3_1, absR_3_2, absR_3_3;
+
+ R_1_1 = MathUtil.dotProduct(box1Axis1x, box1Axis1y,box1Axis1z,box2Axis1x, box2Axis1y,box2Axis1z); absR_1_1 = (R_1_1 < 0) ? -R_1_1 : R_1_1;
+ R_1_2 = MathUtil.dotProduct(box1Axis1x, box1Axis1y,box1Axis1z,box2Axis2x, box2Axis2y,box2Axis2z); absR_1_2 = (R_1_2 < 0) ? -R_1_2 : R_1_2;
+ R_1_3 = MathUtil.dotProduct(box1Axis1x, box1Axis1y,box1Axis1z,box2Axis3x, box2Axis3y,box2Axis3z); absR_1_3 = (R_1_3 < 0) ? -R_1_3 : R_1_3;
+ R_2_1 = MathUtil.dotProduct(box1Axis2x, box1Axis2y,box1Axis2z,box2Axis1x, box2Axis1y,box2Axis1z); absR_2_1 = (R_2_1 < 0) ? -R_2_1 : R_2_1;
+ R_2_2 = MathUtil.dotProduct(box1Axis2x, box1Axis2y,box1Axis2z,box2Axis2x, box2Axis2y,box2Axis2z); absR_2_2 = (R_2_2 < 0) ? -R_2_2 : R_2_2;
+ R_2_3 = MathUtil.dotProduct(box1Axis2x, box1Axis2y,box1Axis2z,box2Axis3x, box2Axis3y,box2Axis3z); absR_2_3 = (R_2_3 < 0) ? -R_2_3 : R_2_3;
+ R_3_1 = MathUtil.dotProduct(box1Axis3x, box1Axis3y,box1Axis3z,box2Axis1x, box2Axis1y,box2Axis1z); absR_3_1 = (R_3_1 < 0) ? -R_3_1 : R_3_1;
+ R_3_2 = MathUtil.dotProduct(box1Axis3x, box1Axis3y,box1Axis3z,box2Axis2x, box2Axis2y,box2Axis2z); absR_3_2 = (R_3_2 < 0) ? -R_3_2 : R_3_2;
+ R_3_3 = MathUtil.dotProduct(box1Axis3x, box1Axis3y,box1Axis3z,box2Axis3x, box2Axis3y,box2Axis3z); absR_3_3 = (R_3_3 < 0) ? -R_3_3 : R_3_3;
+
+
+ // ALGORITHM: Use the separating axis test for all 15 potential
+ // separating axes. If a separating axis could not be found, the two
+ // boxes overlap.
+ float ra, rb, t;
+ IntersectionType type = null;
+
+
+ ra = box1ExtentAxis1;
+ rb = box2ExtentAxis1*absR_1_1+ box2ExtentAxis2*absR_1_2 + box2ExtentAxis3*absR_1_3;
+ t = Math.abs(tx);
+ type = classifiesOrientedBoxAxis(t, ra, rb, type);
+ if (type==IntersectionType.OUTSIDE) return type;
+
+ ra = box1ExtentAxis2;
+ rb = box2ExtentAxis1*absR_2_1+ box2ExtentAxis2*absR_2_2 + box2ExtentAxis3*absR_3_3;
+ t = Math.abs(ty);
+ type = classifiesOrientedBoxAxis(t, ra, rb, type);
+ if (type==IntersectionType.OUTSIDE) return type;
+
+ ra = box1ExtentAxis3;
+ rb = box2ExtentAxis1*absR_3_1+ box2ExtentAxis2*absR_3_2 + box2ExtentAxis3*absR_3_3;
+ t = Math.abs(tz);
+ type = classifiesOrientedBoxAxis(t, ra, rb, type);
+ if (type==IntersectionType.OUTSIDE) return type;
+
+ //B's basis vectors
+ ra = box1ExtentAxis1*absR_1_1+ box1ExtentAxis2*absR_2_1 + box1ExtentAxis3*absR_3_1;
+ rb = box2ExtentAxis1;
+ t = Math.abs( tx*R_1_1 + ty*R_2_1 + tz*R_3_1 );
+ type = classifiesOrientedBoxAxis(t, ra, rb, type);
+ if (type==IntersectionType.OUTSIDE) return type;
+
+ ra = box1ExtentAxis1*absR_1_2+ box1ExtentAxis2*absR_2_2 + box1ExtentAxis3*absR_3_2;
+ rb = box2ExtentAxis2;
+ t = Math.abs( tx*R_1_2 + ty*R_2_2 + tz*R_3_2 );
+ type = classifiesOrientedBoxAxis(t, ra, rb, type);
+ if (type==IntersectionType.OUTSIDE) return type;
+
+ ra = box1ExtentAxis1*absR_1_3+ box1ExtentAxis2*absR_2_3 + box1ExtentAxis3*absR_3_3;
+ rb = box2ExtentAxis3;
+ t = Math.abs( tx*R_1_3 + ty*R_2_3 + tz*R_3_3 );
+ type = classifiesOrientedBoxAxis(t, ra, rb, type);
+ if (type==IntersectionType.OUTSIDE) return type;
+
+ //9 cross products
+
+ //L = A0 x B0
+ ra = box1ExtentAxis1*absR_3_1 + box1ExtentAxis3*absR_2_1;
+ rb = box1ExtentAxis3*absR_1_3 + box1ExtentAxis3*absR_1_2;
+ t = Math.abs( tz*R_2_1 - ty*R_3_1 );
+ type = classifiesOrientedBoxAxis(t, ra, rb, type);
+ if (type==IntersectionType.OUTSIDE) return type;
+
+
+ ra = box1ExtentAxis2*absR_3_1 + box1ExtentAxis3*absR_2_1;
+ rb = box1ExtentAxis3*absR_1_3 + box1ExtentAxis3*absR_1_2;
+ t = Math.abs( tz*R_2_1 - ty*R_3_1 );
+ type = classifiesOrientedBoxAxis(t, ra, rb, type);
+ if (type==IntersectionType.OUTSIDE) return type;
+
+ //L = A0 x B1
+ ra = box1ExtentAxis2*absR_3_2 + box1ExtentAxis3*absR_2_2;
+ rb = box1ExtentAxis3*absR_1_3 + box1ExtentAxis3*absR_1_1;
+ t = Math.abs( tz*R_2_2 - ty*R_3_2 );
+ type = classifiesOrientedBoxAxis(t, ra, rb, type);
+ if (type==IntersectionType.OUTSIDE) return type;
+
+ //L = A0 x B2
+ ra = box1ExtentAxis2*absR_3_3 + box1ExtentAxis3*absR_2_3;
+ rb = box1ExtentAxis3*absR_1_2 + box1ExtentAxis3*absR_1_1;
+ t = Math.abs( tz*R_2_3 - ty*R_3_3 );
+ type = classifiesOrientedBoxAxis(t, ra, rb, type);
+ if (type==IntersectionType.OUTSIDE) return type;
+
+ //L = A1 x B0
+ ra = box1ExtentAxis1*absR_3_1 + box1ExtentAxis3*absR_1_1;
+ rb = box1ExtentAxis3*absR_2_3 + box1ExtentAxis3*absR_2_2;
+ t = Math.abs( tx*R_3_1 - tz*R_1_1 );
+ type = classifiesOrientedBoxAxis(t, ra, rb, type);
+ if (type==IntersectionType.OUTSIDE) return type;
+
+ //L = A1 x B1
+ ra = box1ExtentAxis1*absR_3_2 + box1ExtentAxis3*absR_1_2;
+ rb = box1ExtentAxis3*absR_2_3 + box1ExtentAxis3*absR_2_1;
+ t = Math.abs( tx*R_3_2 - tz*R_1_2 );
+ type = classifiesOrientedBoxAxis(t, ra, rb, type);
+ if (type==IntersectionType.OUTSIDE) return type;
+
+ //L = A1 x B2
+ ra = box1ExtentAxis1*absR_3_3 + box1ExtentAxis3*absR_1_3;
+ rb = box1ExtentAxis3*absR_2_2 + box1ExtentAxis3*absR_2_1;
+ t = Math.abs( tx*R_3_3 - tz*R_1_3 );
+ type = classifiesOrientedBoxAxis(t, ra, rb, type);
+ if (type==IntersectionType.OUTSIDE) return type;
+
+ //L = A2 x B0
+ ra = box1ExtentAxis1*absR_2_1 + box1ExtentAxis2*absR_1_1;
+ rb = box1ExtentAxis3*absR_3_3 + box1ExtentAxis3*absR_3_2;
+ t = Math.abs( ty*R_1_1 - tx*R_2_1 );
+ type = classifiesOrientedBoxAxis(t, ra, rb, type);
+ if (type==IntersectionType.OUTSIDE) return type;
+
+ //L = A2 x B1
+ ra = box1ExtentAxis1*absR_2_2 + box1ExtentAxis2*absR_1_2;
+ rb = box1ExtentAxis3*absR_3_3 + box1ExtentAxis3*absR_3_1;
+ t = Math.abs( ty*R_1_2 - tx*R_2_2 );
+ type = classifiesOrientedBoxAxis(t, ra, rb, type);
+ if (type==IntersectionType.OUTSIDE) return type;
+
+ //L = A2 x B2
+ ra = box1ExtentAxis1*absR_2_3 + box1ExtentAxis2*absR_1_3;
+ rb = box1ExtentAxis3*absR_3_2 + box1ExtentAxis3*absR_3_1;
+ t = Math.abs( ty*R_1_3 - tx*R_2_3 );
+ type = classifiesOrientedBoxAxis(t, ra, rb, type);
+ if (type==IntersectionType.OUTSIDE) return type;
+
+ /*no separating axis found, the two boxes overlap */
+
+ return type;
+ }
+
+ /**
+ * Classifies two oriented rectangles.
+ *
+ * The extents are assumed to be positive or zero.
+ * The lengths of the given arrays are assumed to be 2.
+ *
+ * This function uses the "separating axis theorem" which states that
+ * for any two OBRs (MBRis a special case of OBR)
+ * that do not touch, a separating axis can be found.
+ *
+ * This function uses an general intersection test between two OBR.
+ * If the first box is expected to be an AAR, please use the
+ * optimized algorithm given by
+ * {@link #classifiesAlignedRectangleOrientedRectangle(float, float, float, float, float, float, float, float, float, float, float, float)}.
+ *
+ * Basic Algorithm:
+ * <>
+ * To be able to decide whether two polygons are intersecting (touching each other)
+ * we can use the following basic facts:
+ *
+ * - If two convex polygons are not intersecting, there exists a line that passes between them.
+ * - Such a line only exists if one of the sides of one of the polygons forms such a line.
+ *
+ *
+ * The first statement is easy. Since the polygons are both convex, you'll be able to draw a line
+ * with one polygon on one side and the other polygon on the other side unless they are intersecting.
+ * The second is slightly less intuitive.
+ *
+ * Unless the closest sided of the polygons are parallel to each other, the
+ * point where they get closest to each other is the point where a corner of one
+ * polygon gets closest to a side of the other polygon. This side will then
+ * form a separating axis between the polygons. If the sides are parallel,
+ * they both are separating axes.
+ *
+ * How does this concretely help us decide whether polygon A and B intersect?
+ * We just go over each side of each polygon and check whether it forms a
+ * separating axis. To do this we'll be using some basic vector math to
+ * squash all the points of both polygons onto a line that is perpendicular
+ * to the potential separating line.
+ *
+ * Now the whole problem is conveniently 1-dimensional. We can determine a region
+ * in which the points for each polygon lie, and this line is a separating axis
+ * if these regions do not overlap.
+ *
+ * If, after checking each line from both polygons, no separating axis was
+ * found, it has been proven that the polygons intersect and something has
+ * to be done about it.
+ *
+ * @param center1x is the X coordinate of the rect1 center.
+ * @param center1y is the Y coordinate of the rect1 center.
+ * @param rect1Axis1x is the X coordinate of the Axis1 unit vector.
+ * @param rect1Axis1y is the Y coordinate of the Axis1 unit vector.
+ * @param rect1Axis2x is the X coordinate of the Axis2 unit vector.
+ * @param rect1Axis2y is the Y coordinate of the Axis2 unit vector.
+ * @param rect1ExtentAxis1 is the 'Axis1' size of the rect1.
+ * @param rect1ExtentAxis2 is the 'Axis2' size of the rect1.
+ * @param center2x is the X coordinate of the rect2 center.
+ * @param center2y is the Y coordinate of the rect2 center.
+ * @param rect2Axis1x is the X coordinate of the Axis1 unit vector.
+ * @param rect2Axis1y is the Y coordinate of the Axis1 unit vector.
+ * @param rect2Axis2x is the X coordinate of the Axis2 unit vector.
+ * @param rect2Axis2y is the Y coordinate of the Axis2 unit vector.
+ * @param rect2ExtentAxis1 is the 'Axis1' size of the rect2.
+ * @param rect2ExtentAxis2 is the 'Axis2' size of the rect2.
+ * @return the value {@link IntersectionType#INSIDE} if the first rectangle is inside
+ * the second rectangle; {@link IntersectionType#OUTSIDE} if the first rectangle is
+ * outside the second rectangle; {@link IntersectionType#ENCLOSING} if the
+ * first rectangle is enclosing the second rectangle;
+ * {@link IntersectionType#SPANNING} otherwise.
+ * @see "RTCD pages 102-105"
+ * @see Gamasutra OBB intersection test
+ */
+ public static IntersectionType classifiesOrientedRectangles(
+ float center1x, float center1y, float rect1Axis1x, float rect1Axis1y, float rect1Axis2x, float rect1Axis2y, float rect1ExtentAxis1, float rect1ExtentAxis2,
+ float center2x, float center2y, float rect2Axis1x, float rect2Axis1y, float rect2Axis2x, float rect2Axis2y, float rect2ExtentAxis1, float rect2ExtentAxis2){
+ assert(rect1ExtentAxis1>=0);
+ assert(rect1ExtentAxis2>=0);
+ assert(rect2ExtentAxis1>=0);
+ assert(rect2ExtentAxis2>=0);
+
+ float tx, ty;
+ tx = center2x - center1x;
+ ty = center2y - center1y;
+
+ //B's basis with respect to A's local frame
+ float R_1_1, R_1_2,
+ R_2_1, R_2_2,
+
+ absR_1_1, absR_1_2,
+ absR_2_1, absR_2_2;
+
+ R_1_1 = MathUtil.dotProduct(rect1Axis1x, rect1Axis1y,rect2Axis1x, rect2Axis1y); absR_1_1 = (R_1_1 < 0) ? -R_1_1 : R_1_1;
+ R_1_2 = MathUtil.dotProduct(rect1Axis1x, rect1Axis1x,rect2Axis2x, rect2Axis2y); absR_1_2 = (R_1_2 < 0) ? -R_1_2 : R_1_2;
+ R_2_1 = MathUtil.dotProduct(rect1Axis2x, rect1Axis2y,rect2Axis1x, rect2Axis1y); absR_2_1 = (R_2_1 < 0) ? -R_2_1 : R_2_1;
+ R_2_2 = MathUtil.dotProduct(rect1Axis2x, rect1Axis2y,rect2Axis2x, rect2Axis2y); absR_2_2 = (R_2_2 < 0) ? -R_2_2 : R_2_2;
+
+ float ra, rb, t;
+ IntersectionType type = null;
+
+ //L = A0
+ ra = rect1ExtentAxis1;
+ rb = rect2ExtentAxis1*absR_1_1 + rect2ExtentAxis2*absR_1_2;
+ t = Math.abs(tx);
+ type = classifiesOrientedBoxAxis(t, ra, rb, type);
+ if (type==IntersectionType.OUTSIDE) return type;
+
+ //L = A1
+ ra = rect1ExtentAxis2;
+ rb = rect2ExtentAxis1*absR_2_1 + rect2ExtentAxis2*absR_2_2;
+ t = Math.abs(ty);
+ type = classifiesOrientedBoxAxis(t, ra, rb, type);
+ if (type==IntersectionType.OUTSIDE) return type;
+
+ //L = B0
+ ra = rect1ExtentAxis1*absR_1_1 + rect1ExtentAxis2*absR_2_1;
+ rb = rect2ExtentAxis1;
+ t = tx*absR_1_1 + ty*absR_2_1;
+ type = classifiesOrientedBoxAxis(t, ra, rb, type);
+ if (type==IntersectionType.OUTSIDE) return type;
+
+ //L = B1
+ ra = rect1ExtentAxis1*absR_1_2 + rect1ExtentAxis2*absR_2_2;
+ rb = rect2ExtentAxis2;
+ t = tx*absR_1_2 + ty*absR_2_2;
+ type = classifiesOrientedBoxAxis(t, ra, rb, type);
+ if (type==IntersectionType.OUTSIDE) return type;
+
+ //L = A0 x B0, ra = rb = t = 0, discarted
+
+ //L = A0 x B1, ra = rb = t = 0, discarted
+
+ //L = A0 x B2
+ ra = rect1ExtentAxis2;
+ rb = rect2ExtentAxis1*absR_1_2 + rect2ExtentAxis2*absR_1_1;
+ t = Math.abs( ty );
+ type = classifiesOrientedBoxAxis(t, ra, rb, type);
+ if (type==IntersectionType.OUTSIDE) return type;
+
+ //L = A1 x B0, ra = rb = t = 0, discarted
+
+ //L = A1 x B1, ra = rb = t = 0, discarted
+
+ //L = A1 x B2
+ ra = rect1ExtentAxis1;
+ rb = rect2ExtentAxis1*absR_2_2 + rect2ExtentAxis2*absR_2_1;
+ t = Math.abs( tx );
+ type = classifiesOrientedBoxAxis(t, ra, rb, type);
+ if (type==IntersectionType.OUTSIDE) return type;
+
+ //L = A2 x B0
+ ra = rect1ExtentAxis1*absR_2_1 + rect1ExtentAxis2*absR_1_1;
+ rb = rect2ExtentAxis2;
+ t = Math.abs( ty*R_1_1 - tx*R_2_1 );
+ type = classifiesOrientedBoxAxis(t, ra, rb, type);
+ if (type==IntersectionType.OUTSIDE) return type;
+
+ //L = A2 x B1
+ ra = rect1ExtentAxis1*absR_2_2 + rect1ExtentAxis2*absR_1_2;
+ rb = rect2ExtentAxis1;
+ t = Math.abs( ty*R_1_2 - tx*R_2_2 );
+ type = classifiesOrientedBoxAxis(t, ra, rb, type);
+ if (type==IntersectionType.OUTSIDE) return type;
+
+ //L = A2 x B2, ra = rb = t = 0, discarted
+
+ /*no separating axis found, the two boxes overlap */
+
+ return type;
+ }
+
+ /**
+ * Classifies an axis-aligned box and an oriented box.
+ *
+ * This function is assuming that lx1 is lower
+ * or equal to ux1, ly1 is lower
+ * or equal to uy1, and so on.
+ * The extents are assumed to be positive or zero.
+ * The lengths of the given arrays are assumed to be 3.
+ *
+ * This function uses the "separating axis theorem" which states that
+ * for any two OBBs (AABB is a special case of OBB)
+ * that do not touch, a separating axis can be found.
+ *
+ * This function uses an optimized algorithm for AABB as first parameter.
+ * The general intersection type between two OBB is given by
+ * {@link #classifiesOrientedBoxes(float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float))}
+ *
+ * @param lowerx is the X coordinate of the lowest point of the box.
+ * @param lowery is the Y coordinate of the lowest point of the box.
+ * @param lowerz is the Z coordinate of the lowest point of the box.
+ * @param upperx is the X coordinate of the uppermost point of the box.
+ * @param uppery is the Y coordinate of the uppermost point of the box.
+ * @param upperz is the Z coordinate of the uppermost point of the box.
+ * @param centerx is the X coordinate of the box center.
+ * @param centery is the Y coordinate of the box center.
+ * @param centerz is the Z coordinate of the box center.
+ * @param axis1x is the X coordinate of the axis1 unit vector.
+ * @param axis1y is the Y coordinate of the axis1 unit vector.
+ * @param axis1z is the Z coordinate of the axis1 unit vector.
+ * @param axis2x is the X coordinate of the axis2 unit vector.
+ * @param axis2y is the Y coordinate of the axis2 unit vector.
+ * @param axis2z is the Z coordinate of the axis2 unit vector.
+ * @param axis3x is the X coordinate of the axis3 unit vector.
+ * @param axis3y is the Y coordinate of the axis3 unit vector.
+ * @param axis3z is the Z coordinate of the axis3 unit vector.
+ * @param Extentaxis1 is the 'axis1' size of the OBB.
+ * @param Extentaxis2 is the 'axis2' size of the OBB.
+ * @param Extentaxis3 is the 'axis3' size of the OBB.
+ * @return the value {@link IntersectionType#INSIDE} if the first box is inside
+ * the second box; {@link IntersectionType#OUTSIDE} if the first box is
+ * outside the second box; {@link IntersectionType#ENCLOSING} if the
+ * first box is enclosing the second box;
+ * {@link IntersectionType#SPANNING} otherwise.
+ * @see "RTCD pages 102-105"
+ * @see OBB collision detection on Gamasutra.com
+ */
+ public static IntersectionType classifiesAlignedBoxOrientedBox(
+ float lowerx,float lowery,float lowerz,
+ float upperx,float uppery,float upperz,
+ float centerx,float centery,float centerz,
+ float axis1x, float axis1y, float axis1z,
+ float axis2x, float axis2y, float axis2z,
+ float axis3x, float axis3y, float axis3z,
+ float extentAxis1, float extentAxis2, float extentAxis3) {
+ assert(lowerx<=upperx);
+ assert(lowery<=uppery);
+ assert(lowerz<=upperz);
+ assert(extentAxis1>=0);
+ assert(extentAxis2>=0);
+ assert(extentAxis3>=0);
+
+ float aabbCenterx,aabbCentery,aabbCenterz;
+ aabbCenterx = (upperx+lowerx)/2.f;
+ aabbCentery = (uppery+lowery)/2.f;
+ aabbCenterz = (upperz+lowerz)/2.f;
+
+ return classifiesOrientedBoxes(
+ aabbCenterx, aabbCentery, aabbCenterz,
+ 1,0,0, //Axis 1
+ 0,1,0, //Axis 2
+ 0,0,1, //Axis 3
+ upperx - aabbCenterx, uppery - aabbCentery, upperz - aabbCenterz,
+ centerx, centery, centerz,
+ axis1x, axis1y, axis1z,
+ axis2x, axis2y, axis2z,
+ axis3x, axis3y, axis3z,
+ extentAxis1, extentAxis2, extentAxis3);
+ }
+
+ /**
+ * Classifies an minimum bounding rectangle and an oriented rectangle.
+ *
+ * This function is assuming that lx1 is lower
+ * or equal to ux1, and ly1 is lower
+ * or equal to uy1.
+ * The extents are assumed to be positive or zero.
+ * The lengths of the given arrays are assumed to be 2.
+ *
+ * This function uses the "separating axis theorem" which states that
+ * for any two OBRs (AABB is a special case of OBR)
+ * that do not touch, a separating axis can be found.
+ *
+ * This function uses an optimized algorithm for AABB as first parameter.
+ * The general intersection type between two OBR is given by
+ * {@link #classifiesOrientedRectangles(float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float)}
+ *
+ * @param lowerx is the X coordinate of the lowest point of the rectangle.
+ * @param lowery is the Y coordinate of the lowest point of the rectangle.
+ * @param upperx is the X coordinate of the uppermost point of the rectangle.
+ * @param uppery is the Y coordinate of the uppermost point of the rectangle.
+ * @param centerx is the X coordinate of the rectangle center.
+ * @param centery is the Y coordinate of the rectangle center.
+ * @param axis1x is the X coordinate of the axis1 unit vector.
+ * @param axis1y is the Y coordinate of the axis1 unit vector.
+ * @param axis1z is the Z coordinate of the axis1 unit vector.
+ * @param axis2x is the X coordinate of the axis2 unit vector.
+ * @param axis2y is the Y coordinate of the axis2 unit vector.
+ * @param Extentaxis1 is the 'axis1' size of the OBR.
+ * @param Extentaxis2 is the 'axis2' size of the OBR.
+ * @return the value {@link IntersectionType#INSIDE} if the first rectangle is inside
+ * the second rectangle; {@link IntersectionType#OUTSIDE} if the first rectangle is
+ * outside the second rectangle; {@link IntersectionType#ENCLOSING} if the
+ * first rectangle is enclosing the second rectangle;
+ * {@link IntersectionType#SPANNING} otherwise.
+ * @see "RTCD pages 102-105"
+ * @see OBB collision detection on Gamasutra.com
+ */
+ public static IntersectionType classifiesAlignedRectangleOrientedRectangle(
+ float lowerx,float lowery, float upperx,float uppery,
+ float centerx,float centery, float axis1x, float axis1y, float axis2x, float axis2y, float extentAxis1, float extentAxis2) {
+ assert(lowerx<=upperx);
+ assert(lowery<=uppery);
+ assert(extentAxis1>=0);
+ assert(extentAxis2>=0);
+
+ float mbrCenterx, mbrCentery;
+
+ mbrCenterx = (upperx+lowerx)/2.f;
+ mbrCentery = (uppery+lowery)/2.f;
+
+ return classifiesOrientedRectangles(
+ mbrCenterx, mbrCentery, 1, 0, 0, 1, upperx - mbrCenterx, uppery - mbrCentery,
+ centerx, centery, axis1x, axis1y, axis2x, axis2y, extentAxis1, extentAxis2);
+ }
+
+ /**
+ * Replies if the specified sphere intersects the specified capsule.
+ * @param sphereCenterx is the X coordinate of the sphere center.
+ * @param sphereCentery is the Y coordinate of the sphere center.
+ * @param sphereCenterz is the Z coordinate of the sphere center.
+ * @param radius is the radius of the sphere.
+ * @param capsuleAx is the X coordinate of medial line segment start point of the capsule
+ * @param capsuleAy is the Y coordinate of medial line segment start point of the capsule
+ * @param capsuleAz is the Z coordinate of medial line segment start point of the capsule
+ * @param capsuleBx is the X coordinate of medial line segment end point of the capsule
+ * @param capsuleBy is the Y coordinate of medial line segment end point of the capsule
+ * @param capsuleBz is the Z coordinate of medial line segment end point of the capsule
+ * @param capsuleRadius - radius of the capsule
+ * @return the value {@link IntersectionType#INSIDE} if the capsule is inside
+ * the sphere; {@link IntersectionType#OUTSIDE} if the capsule is
+ * outside the sphere; {@link IntersectionType#ENCLOSING} if the
+ * capsule is enclosing the sphere;
+ */
+ public static IntersectionType classifySphereCapsule(
+ float sphereCenterx, float sphereCentery, float sphereCenterz, float sphereRadius,
+ float capsuleAx, float capsuleAy, float capsuleAz,
+ float capsuleBx, float capsuleBy, float capsuleBz, float capsuleRadius) {
+ // Computedistance between sphere center and capsule line segment
+
+ float dist2 = GeometryUtil.distanceSquaredPointSegment(sphereCenterx, sphereCentery, sphereCenterz, capsuleAx, capsuleAy, capsuleAz, capsuleBx, capsuleBy, capsuleBz);
+
+ // If distance smaller than sum of radii, they collide
+ float fullRadius = sphereRadius + capsuleRadius;
+
+ if (dist2 < fullRadius) {
+
+ float d1 = GeometryUtil.distanceSquaredPointPoint(capsuleAx, capsuleAy, capsuleAz, sphereCenterx, sphereCenterx, sphereCenterx);
+ float d2 = GeometryUtil.distanceSquaredPointPoint(capsuleBx, capsuleBy, capsuleBz, sphereCenterx, sphereCenterx, sphereCenterx);
+
+ if((dist2+sphereRadius) <= capsuleRadius) {
+ return IntersectionType.ENCLOSING;
+ }
+ else if (sphereRadius*sphereRadius >= (Math.max(d1,d2)+capsuleRadius)) {
+ return IntersectionType.INSIDE;
+ }
+ return IntersectionType.SPANNING;
+ }
+ return IntersectionType.OUTSIDE;
+ }
+
+ /**
+ * Compute intersection between an OBB and a capsule
+ * @param centerx is the X coordinate of the box center.
+ * @param centery is the Y coordinate of the box center.
+ * @param centerz is the Z coordinate of the box center.
+ * @param axis1x is the X coordinate of the axis1 unit vector.
+ * @param axis1y is the Y coordinate of the axis1 unit vector.
+ * @param axis1z is the Z coordinate of the axis1 unit vector.
+ * @param axis2x is the X coordinate of the axis2 unit vector.
+ * @param axis2y is the Y coordinate of the axis2 unit vector.
+ * @param axis2z is the Z coordinate of the axis2 unit vector.
+ * @param axis3x is the X coordinate of the axis3 unit vector.
+ * @param axis3y is the Y coordinate of the axis3 unit vector.
+ * @param axis3z is the Z coordinate of the axis3 unit vector.
+ * @param Extentaxis1 is the 'axis1' size of the OBB.
+ * @param Extentaxis2 is the 'axis2' size of the OBB.
+ * @param Extentaxis3 is the 'axis3' size of the OBB.
+ * @param capsuleAx is the X coordinate of medial line segment start point of the capsule
+ * @param capsuleAy is the Y coordinate of medial line segment start point of the capsule
+ * @param capsuleAz is the Z coordinate of medial line segment start point of the capsule
+ * @param capsuleBx is the X coordinate of medial line segment end point of the capsule
+ * @param capsuleBy is the Y coordinate of medial line segment end point of the capsule
+ * @param capsuleBz is the Z coordinate of medial line segment end point of the capsule
+ * @param capsuleRadius - radius of the capsule
+ * @param epsilon the accuracy parameter (distance) must be the same unit of measurement as others parameters
+ * @return the value {@link IntersectionType#INSIDE} if the capsule is inside
+ * the box; {@link IntersectionType#OUTSIDE} if the capsule is
+ * outside the box; {@link IntersectionType#ENCLOSING} if the
+ * capsule is enclosing the box;
+ */
+ public static IntersectionType classifyOrientedBoxCapsule(
+ float centerx,float centery,float centerz,
+ float axis1x, float axis1y, float axis1z, float axis2x, float axis2y, float axis2z, float axis3x, float axis3y, float axis3z,
+ float extentAxis1, float extentAxis2, float extentAxis3,
+ float capsule1Ax, float capsule1Ay, float capsule1Az, float capsule1Bx, float capsule1By, float capsule1Bz, float capsule1Radius,float epsilon) {
+
+
+ Point3f closestFromA = new Point3f();
+ Point3f closestFromB = new Point3f();
+ Point3f farestFromA = new Point3f();
+ Point3f farestFromB = new Point3f();
+ GeometryUtil.closestFarthestPointsOBBPoint(
+ centerx, centery, centerz,axis1x, axis1y, axis1z, axis2x, axis2y, axis2z, axis3x, axis3y, axis3z,extentAxis1, extentAxis2, extentAxis3,
+ capsule1Ax, capsule1Ay, capsule1Az,
+ closestFromA, farestFromA);
+ GeometryUtil.closestFarthestPointsOBBPoint(
+ centerx, centery, centerz, axis1x, axis1y, axis1z, axis2x, axis2y, axis2z, axis3x, axis3y, axis3z, extentAxis1, extentAxis2, extentAxis3,
+ capsule1Bx, capsule1By, capsule1Bz,
+ closestFromB, farestFromB);
+
+ float distanceToNearest = GeometryUtil.distanceSegmentSegment(capsule1Ax, capsule1Ay, capsule1Bx, capsule1By, closestFromA.getX(), closestFromA.getY(), closestFromB.getX(), closestFromB.getY(), epsilon);
+
+ if (distanceToNearest > capsule1Radius) {
+ return IntersectionType.OUTSIDE;
+ }
+
+ float distanceToFarest = GeometryUtil.distanceSegmentSegment(capsule1Ax, capsule1Ay, capsule1Bx, capsule1By, farestFromA.getX(), farestFromA.getY(), farestFromB.getX(), farestFromB.getY(), epsilon);
+ if(distanceToFarest <= capsule1Radius) {
+ return IntersectionType.ENCLOSING;
+ }
+
+ IntersectionType onSphereA = ClassifierUtil.classifiesSolidSphereOrientedBox(
+ capsule1Ax, capsule1Ay, capsule1Az, capsule1Radius,
+ centerx, centery, centerz, axis1x, axis1y, axis1z, axis2x, axis2y, axis2z, axis3x, axis3y, axis3z, extentAxis1, extentAxis2, extentAxis3, epsilon);
+ IntersectionType onSphereB = ClassifierUtil.classifiesSolidSphereOrientedBox(
+ capsule1Bx, capsule1By, capsule1Bz, capsule1Radius,
+ centerx, centery, centerz, axis1x, axis1y, axis1z, axis2x, axis2y, axis2z, axis3x, axis3y, axis3z, extentAxis1, extentAxis2, extentAxis3, epsilon);
+
+ if(onSphereA.equals(IntersectionType.INSIDE)&&onSphereB.equals(IntersectionType.INSIDE)) {
+ return IntersectionType.INSIDE;
+
+ }
+ else if(onSphereA.equals(IntersectionType.INSIDE) || onSphereB.equals(IntersectionType.INSIDE)) {
+ return IntersectionType.SPANNING;
+ }
+ else if(onSphereA.equals(IntersectionType.ENCLOSING) || onSphereB.equals(IntersectionType.ENCLOSING)) {
+ return IntersectionType.ENCLOSING;
+ }
+
+ return IntersectionType.SPANNING;
+ }
+
+ /**
+ * Compute intersection between a point and a capsule
+ * @param px is the X coordinate of the point to test.
+ * @param py is the Y coordinate of the point to test.
+ * @param pz is the Z coordinate of the point to test.
+ * @param capsuleAx is the X coordinate of medial line segment start point of the capsule
+ * @param capsuleAy is the Y coordinate of medial line segment start point of the capsule
+ * @param capsuleAz is the Z coordinate of medial line segment start point of the capsule
+ * @param capsuleBx is the X coordinate of medial line segment end point of the capsule
+ * @param capsuleBy is the Y coordinate of medial line segment end point of the capsule
+ * @param capsuleBz is the Z coordinate of medial line segment end point of the capsule
+ * @param capsuleRadius - radius of the capsule
+ * @return the value {@link IntersectionType#INSIDE} if the point is inside
+ * the capsule; {@link IntersectionType#OUTSIDE} if the point is
+ * outside the capsule; {@link IntersectionType#ENCLOSING} is not defined here;
+ */
+ public static IntersectionType classifyPointCapsule(
+ float px, float py, float pz,
+ float capsule1Ax, float capsule1Ay, float capsule1Az, float capsule1Bx, float capsule1By, float capsule1Bz, float capsule1Radius) {
+
+ float distPointToCapsuleSegment = GeometryUtil.distancePointSegment(px,py,pz,capsule1Ax,capsule1Ay,capsule1Az,capsule1Bx,capsule1By,capsule1Bz);
+ if (distPointToCapsuleSegment > capsule1Radius) {
+ return IntersectionType.OUTSIDE;
+ } else if (distPointToCapsuleSegment == capsule1Radius) {
+ return IntersectionType.SPANNING;
+ }else {
+
+ return IntersectionType.INSIDE;
+ }
+ }
+}
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry/IntersectionClassifier.java b/core/math/src/main/java/org/arakhne/afc/math/geometry/IntersectionClassifier.java
new file mode 100644
index 000000000..28c4bb6dc
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry/IntersectionClassifier.java
@@ -0,0 +1,98 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Christophe BOHRHAUER.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry;
+
+/**
+ * This interface describes an object that permits to classify intersection
+ * between objects.
+ *
+ * @param is the type of the object.
+ * @param is the type of the points.
+ * @author $Author: cbohrhauer$d
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public interface IntersectionClassifier, P> {
+
+ /**
+ * Classify a box with respect to the classifier.
+ *
+ * @param box
+ * @return the value {@link IntersectionType#INSIDE} if the box is inside this classifier;
+ * {@link IntersectionType#OUTSIDE} if the box is outside this classifier;
+ * {@link IntersectionType#ENCLOSING} if the box is enclosing this classifier;
+ * {@link IntersectionType#SPANNING} otherwhise.
+ */
+ public IntersectionType classifies(IC box);
+
+ /**
+ * Replies if the specified box intersects this classifier.
+ *
+ * @param box
+ * @return true if the box is intersecting this classifier,
+ * otherwise false
+ */
+ public boolean intersects(IC box);
+
+ /**
+ * Classify a point.
+ *
+ * @param p is a point
+ * @return the value {@link IntersectionType#INSIDE} if the point is inside this classifier;
+ * {@link IntersectionType#OUTSIDE} if the point is outside this classifier;
+ * {@link IntersectionType#SPANNING} if the point is on the classifier.
+ */
+ public IntersectionType classifies(P p);
+
+ /**
+ * Replies if the specified point intersects this classifier.
+ *
+ * @param p is a point
+ * @return true if an intersection exists, otherwise false
+ */
+ public boolean intersects(P p);
+
+ /**
+ * Classify an area/box.
+ *
+ * @param l is the lower point of the box
+ * @param u is the upper point of the box
+ * @return the value {@link IntersectionType#INSIDE} if the box is inside this classifier;
+ * {@link IntersectionType#OUTSIDE} if the box is outside this classifier;
+ * {@link IntersectionType#ENCLOSING} if the box is enclosing this classifier;
+ * {@link IntersectionType#SPANNING} otherwhise.
+ */
+ public IntersectionType classifies(P l, P u);
+
+ /**
+ * Replies if the specified area intersects this classifier.
+ *
+ * @param l is the lower point of the box
+ * @param u is the upper point of the box
+ * @return the value {@link IntersectionType#INSIDE} if the box is inside this classifier;
+ * {@link IntersectionType#OUTSIDE} if the box is outside this classifier;
+ * {@link IntersectionType#ENCLOSING} if the box is enclosing this classifier;
+ * {@link IntersectionType#SPANNING} otherwhise.
+ */
+ public boolean intersects(P l, P u);
+
+}
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry/IntersectionClassifier2D.java b/core/math/src/main/java/org/arakhne/afc/math/geometry/IntersectionClassifier2D.java
new file mode 100644
index 000000000..967657592
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry/IntersectionClassifier2D.java
@@ -0,0 +1,80 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Christophe BOHRHAUER.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry;
+
+import org.arakhne.afc.math.geometry2d.continuous.Point2f;
+import org.arakhne.afc.math.geometry2d.continuous.Vector2f;
+
+/**
+ * This interface describes an object that permits to classify intersection
+ * between objects in a 2D space.
+ *
+ * @param is the type of the object.
+ * @author $Author: cbohrhauer$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public interface IntersectionClassifier2D> extends IntersectionClassifier {
+
+ /**
+ * Classify a circle.
+ *
+ * @param c is the center point of the circle.
+ * @param r is the radius of the circle.
+ * @return the value {@link IntersectionType#INSIDE} if the v is inside this classifier;
+ * {@link IntersectionType#OUTSIDE} if the v is outside this classifier;
+ * {@link IntersectionType#SPANNING} if the v is on this classifier.
+ */
+ public IntersectionType classifies(Point2f c, float r);
+
+ /**
+ * Replies if the specified axis-aligned box intersects this classifier.
+ *
+ * @param c is the center point of the circle.
+ * @param r is the radius of the circle.
+ * @return true if an intersection exists, otherwise false
+ */
+ public boolean intersects(Point2f c, float r);
+
+ /**
+ * Classify an oriented bounding box.
+ *
+ * @param center is the center point of the oriented bounding box.
+ * @param axis are the unit vectors for the three axis of the orientied bounding box.
+ * @param extent are the sizes of the oriented bounding box along all the three box's axis.
+ * @return the value {@link IntersectionType#INSIDE} if the v is inside this classifier;
+ * {@link IntersectionType#OUTSIDE} if the v is outside this classifier;
+ * {@link IntersectionType#SPANNING} if the v is on this classifier.
+ */
+ public IntersectionType classifies(Point2f center, Vector2f[] axis, float[] extent);
+
+ /**
+ * Replies if the specified oriented bounding box intersects this classifier.
+ *
+ * @param center is the center point of the oriented bounding box.
+ * @param axis are the unit vectors for the three axis of the orientied bounding box.
+ * @param extent are the sizes of the oriented bounding box along all the three box's axis.
+ * @return true if an intersection exists, otherwise false
+ */
+ public boolean intersects(Point2f center, Vector2f[] axis, float[] extent);
+
+}
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry/IntersectionClassifier3D.java b/core/math/src/main/java/org/arakhne/afc/math/geometry/IntersectionClassifier3D.java
new file mode 100644
index 000000000..59eea2172
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry/IntersectionClassifier3D.java
@@ -0,0 +1,112 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Christophe BOHRHAUER.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry;
+
+import org.arakhne.afc.math.geometry3d.continuous.PlanarClassificationType;
+import org.arakhne.afc.math.geometry3d.continuous.Plane;
+import org.arakhne.afc.math.geometry3d.continuous.Point3f;
+import org.arakhne.afc.math.geometry3d.continuous.Vector3f;
+
+/**
+ * This interface describes an object that permits to classify intersection
+ * between objects in a 3D space.
+ *
+ * @param is the type of the object.
+ * @author $Author: sgalland$
+ * @author $Author: ngaud$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public interface IntersectionClassifier3D> extends IntersectionClassifier {
+
+ /**
+ * Classify a sphere.
+ *
+ * @param c is the center point of the sphere.
+ * @param r is the radius of the sphere.
+ * @return the value {@link IntersectionType#INSIDE} if the v is inside this classifier;
+ * {@link IntersectionType#OUTSIDE} if the v is outside this classifier;
+ * {@link IntersectionType#SPANNING} if the v is on this classifier.
+ */
+ public IntersectionType classifies(Point3f c, float r);
+
+ /**
+ * Replies if the specified sphere intersects this classifier.
+ *
+ * @param c is the center point of the circle.
+ * @param r is the radius of the circle.
+ * @return true if an intersection exists, otherwise false
+ */
+ public boolean intersects(Point3f c, float r);
+
+ /**
+ * Classify an plane.
+ *
+ * @param plane is the other plane to classify
+ * @return the value {@link IntersectionType#OUTSIDE} if the plane does not intersecting this classifier;
+ * {@link IntersectionType#SPANNING} if the plane intersecting this classifier.
+ */
+ public IntersectionType classifies(Plane plane);
+
+ /**
+ * Classify this bounds according to the specified plane.
+ *
+ * @param plane is the other plane to classify on
+ * @return the value {@link PlanarClassificationType#IN_FRONT_OF} if this classifier does not intersecting
+ * the plane and is located in the front side of the plane;
+ * {@link PlanarClassificationType#BEHIND} if this classifier does not intersecting
+ * the plane and is located in the behind side of the plane;
+ * {@link PlanarClassificationType#COINCIDENT} if this classifier intersecting the plane.
+ */
+ public PlanarClassificationType classifiesAgainst(Plane plane);
+
+ /**
+ * Replies if the specified plane intersects this classifier.
+ *
+ * @param plane is the other plane to test
+ * @return true if an intersection exists, otherwise false
+ */
+ public boolean intersects(Plane plane);
+
+ /**
+ * Classify an oriented bounding box.
+ *
+ * @param center is the center point of the oriented bounding box.
+ * @param axis are the unit vectors for the three axis of the orientied bounding box.
+ * @param extent are the sizes of the oriented bounding box along all the three box's axis.
+ * @return the value {@link IntersectionType#INSIDE} if the v is inside this classifier;
+ * {@link IntersectionType#OUTSIDE} if the v is outside this classifier;
+ * {@link IntersectionType#SPANNING} if the v is on this classifier.
+ */
+ public IntersectionType classifies(Point3f center, Vector3f[] axis, float[] extent);
+
+ /**
+ * Replies if the specified oriented bounding box intersects this classifier.
+ *
+ * @param center is the center point of the oriented bounding box.
+ * @param axis are the unit vectors for the three axis of the orientied bounding box.
+ * @param extent are the sizes of the oriented bounding box along all the three box's axis.
+ * @return true if an intersection exists, otherwise false
+ */
+ public boolean intersects(Point3f center, Vector3f[] axis, float[] extent);
+
+}
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry/IntersectionType.java b/core/math/src/main/java/org/arakhne/afc/math/geometry/IntersectionType.java
new file mode 100644
index 000000000..5c09cfe0c
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry/IntersectionType.java
@@ -0,0 +1,323 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Christophe BOHRHAUER.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry;
+
+/**
+ * This enumeration describes a intersection classification.
+ *
+ * The operation intersection is not commutative. So,
+ * classify(A,B) could not provides the
+ * same intersection classification as
+ * classify(B,A).
+ *
+ * The call classify(A,B) replies the following values:
+ *
+ * INSIDE: A is entirely inside B,OUTSIDE: A and B have not intersection,SPANNING: A and B have an intersection but
+ * A is not entirely inside B nor B is not
+ * entirely inside A,ENCLOSING: B is entirely inside A,
+ *
+ * @author $Author: sgalland$
+ * @author $Author: ngaud$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public enum IntersectionType {
+
+ /**
+ * The second object is the same as the first one.
+ */
+ SAME,
+ /**
+ * The second object is entirely inside the first one.
+ */
+ ENCLOSING,
+ /**
+ * The first object is entirely inside the second one.
+ */
+ INSIDE,
+ /**
+ * The first object is intersecting the second one.
+ */
+ SPANNING,
+ /**
+ * The first object is entirely outside the second one.
+ */
+ OUTSIDE;
+
+ /** Invert the intersection classification.
+ *
+ *
+ * | t | result |
+ *
+ * INSIDE | ENCLOSING |
+ *
+ * OUTSIDE | OUTSIDE |
+ *
+ * SPANNING | SPANNING |
+ *
+ * ENCLOSING | INSIDE |
+ *
+ * SAME | SAME |
+ *
+ *
+ *
+ * @param t
+ * @return the inverted classification
+ */
+ public static IntersectionType invert(IntersectionType t) {
+ switch (t) {
+ case INSIDE:
+ return ENCLOSING;
+ case OUTSIDE:
+ return OUTSIDE;
+ case SPANNING:
+ return SPANNING;
+ case ENCLOSING:
+ return INSIDE;
+ default:
+ return t;
+ }
+ }
+
+ /** Invert the intersection classification.
+ *
+ *
+ * | t | result |
+ *
+ * INSIDE | ENCLOSING |
+ *
+ * OUTSIDE | OUTSIDE |
+ *
+ * SPANNING | SPANNING |
+ *
+ * ENCLOSING | INSIDE |
+ *
+ * SAME | SAME |
+ *
+ *
+ *
+ * @return the inverted classification
+ */
+ public IntersectionType invert() {
+ return invert(this);
+ }
+
+ /** Compute the OR-combinaison of two intersection types.
+ * It could be used to compute the intersection type of a global object
+ * that is composed of two sub objects for which we have the classitification
+ * respectively. This operator replies a positive intersection if at least
+ * one of the sub object intersections is positive.
+ *
+ *
+ * | f1 | f2 | result |
+ *
+ * INSIDE | INSIDE | INSIDE |
+ * INSIDE | OUTSIDE | INSIDE |
+ * INSIDE | SPANNING | INSIDE |
+ * INSIDE | ENCLOSING | INSIDE |
+ * INSIDE | SAME | INSIDE |
+ *
+ * OUTSIDE | INSIDE | INSIDE |
+ * OUTSIDE | OUTSIDE | OUTSIDE |
+ * OUTSIDE | SPANNING | SPANNING |
+ * OUTSIDE | ENCLOSING | SPANNING |
+ * OUTSIDE | SAME | SPANNING |
+ *
+ * SPANNING | INSIDE | INSIDE |
+ * SPANNING | OUTSIDE | SPANNING |
+ * SPANNING | SPANNING | SPANNING |
+ * SPANNING | ENCLOSING | SPANNING |
+ * SPANNING | SAME | SPANNING |
+ *
+ * ENCLOSING | INSIDE | INSIDE |
+ * ENCLOSING | OUTSIDE | SPANNING |
+ * ENCLOSING | SPANNING | SPANNING |
+ * ENCLOSING | ENCLOSING | ENCLOSING |
+ * ENCLOSING | SAME | SPANNING |
+ *
+ * SAME | INSIDE | INSIDE |
+ * SAME | OUTSIDE | SPANNING |
+ * SAME | SPANNING | SPANNING |
+ * SAME | ENCLOSING | SPANNING |
+ * SAME | SAME | SAME |
+ *
+ *
+ *
+ * @param f1 is the classification of E against F1.
+ * @param f2 is the classification of E against F2.
+ * @return the classification of E against the whole object composed of F1 and F2.
+ */
+ public static IntersectionType or(IntersectionType f1, IntersectionType f2) {
+ if (f1==f2) return f1;
+ if (f1==INSIDE || f2==INSIDE) return INSIDE;
+ return SPANNING;
+ }
+
+ /** Compute the OR-combinaison of two intersection types.
+ * It could be used to compute the intersection type of a global object
+ * that is composed of two sub objects for which we have the classitification
+ * respectively. This operator replies a positive intersection if at least
+ * one of the sub object intersections is positive.
+ *
+ *
+ * | this | f2 | result |
+ *
+ * INSIDE | INSIDE | INSIDE |
+ * INSIDE | OUTSIDE | INSIDE |
+ * INSIDE | SPANNING | INSIDE |
+ * INSIDE | ENCLOSING | INSIDE |
+ * INSIDE | SAME | INSIDE |
+ *
+ * OUTSIDE | INSIDE | INSIDE |
+ * OUTSIDE | OUTSIDE | OUTSIDE |
+ * OUTSIDE | SPANNING | SPANNING |
+ * OUTSIDE | ENCLOSING | SPANNING |
+ * OUTSIDE | SAME | SPANNING |
+ *
+ * SPANNING | INSIDE | INSIDE |
+ * SPANNING | OUTSIDE | SPANNING |
+ * SPANNING | SPANNING | SPANNING |
+ * SPANNING | ENCLOSING | SPANNING |
+ * SPANNING | SAME | SPANNING |
+ *
+ * ENCLOSING | INSIDE | INSIDE |
+ * ENCLOSING | OUTSIDE | SPANNING |
+ * ENCLOSING | SPANNING | SPANNING |
+ * ENCLOSING | ENCLOSING | ENCLOSING |
+ * ENCLOSING | SAME | ENCLOSING |
+ *
+ * SAME | INSIDE | INSIDE |
+ * SAME | OUTSIDE | SPANNING |
+ * SAME | SPANNING | SPANNING |
+ * SAME | ENCLOSING | ENCLOSING |
+ * SAME | SAME | SAME |
+ *
+ *
+ *
+ * @param f2 is the classification of E against F2.
+ * @return the classification of E against the whole object composed of F1 and F2.
+ */
+ public IntersectionType or(IntersectionType f2) {
+ return or(this,f2);
+ }
+
+ /** Compute the AND-combinaison of two intersection types.
+ * It could be used to compute the intersection type of a global 2D object
+ * when we know the classification against each of the two sides of the global 2D object.
+ *
+ *
+ * | t | c | result |
+ *
+ * INSIDE | INSIDE | INSIDE |
+ * INSIDE | OUTSIDE | OUTSIDE |
+ * INSIDE | SPANNING | SPANNING |
+ * INSIDE | ENCLOSING | SPANNING |
+ * INSIDE | SAME | INSIDE |
+ *
+ * OUTSIDE | INSIDE | OUTSIDE |
+ * OUTSIDE | OUTSIDE | OUTSIDE |
+ * OUTSIDE | SPANNING | OUTSIDE |
+ * OUTSIDE | ENCLOSING | OUTSIDE |
+ * OUTSIDE | SAME | OUTSIDE |
+ *
+ * SPANNING | INSIDE | SPANNING |
+ * SPANNING | OUTSIDE | OUTSIDE |
+ * SPANNING | SPANNING | SPANNING |
+ * SPANNING | ENCLOSING | SPANNING |
+ * SPANNING | SAME | SPANNING |
+ *
+ * ENCLOSING | INSIDE | SPANNING |
+ * ENCLOSING | OUTSIDE | OUTSIDE |
+ * ENCLOSING | SPANNING | SPANNING |
+ * ENCLOSING | ENCLOSING | ENCLOSING |
+ * ENCLOSING | SAME | ENCLOSING |
+ *
+ * SAME | INSIDE | INSIDE |
+ * SAME | OUTSIDE | OUTSIDE |
+ * SAME | SPANNING | SPANNING |
+ * SAME | ENCLOSING | ENCLOSING |
+ * SAME | SAME | SAME |
+ *
+ *
+ *
+ * @param t
+ * @param c
+ * @return the result of the intersection.
+ */
+ public static IntersectionType and(IntersectionType t, IntersectionType c) {
+ if (t==c) return t;
+ if ((t==INSIDE && c==ENCLOSING)||
+ (t==ENCLOSING && c==INSIDE)) return SPANNING;
+ return (t.ordinal()>c.ordinal()) ? t : c;
+ }
+
+ /** Compute the AND-combinaison of two intersection types.
+ * It could be used to compute the intersection type of a global 2D object
+ * when we know the classification against each of the two sides of the global 2D object.
+ *
+ *
+ * | this | c | result |
+ *
+ * INSIDE | INSIDE | INSIDE |
+ * INSIDE | OUTSIDE | OUTSIDE |
+ * INSIDE | SPANNING | SPANNING |
+ * INSIDE | ENCLOSING | SPANNING |
+ * INSIDE | SAME | SPANNING |
+ *
+ * OUTSIDE | INSIDE | OUTSIDE |
+ * OUTSIDE | OUTSIDE | OUTSIDE |
+ * OUTSIDE | SPANNING | OUTSIDE |
+ * OUTSIDE | ENCLOSING | OUTSIDE |
+ * OUTSIDE | SAME | OUTSIDE |
+ *
+ * SPANNING | INSIDE | SPANNING |
+ * SPANNING | OUTSIDE | OUTSIDE |
+ * SPANNING | SPANNING | SPANNING |
+ * SPANNING | ENCLOSING | SPANNING |
+ * SPANNING | SAME | SPANNING |
+ *
+ * ENCLOSING | INSIDE | SPANNING |
+ * ENCLOSING | OUTSIDE | OUTSIDE |
+ * ENCLOSING | SPANNING | SPANNING |
+ * ENCLOSING | ENCLOSING | ENCLOSING |
+ * ENCLOSING | SAME | ENCLOSING |
+ *
+ * SAME | INSIDE | INSIDE |
+ * SAME | OUTSIDE | OUTSIDE |
+ * SAME | SPANNING | SPANNING |
+ * SAME | ENCLOSING | ENCLOSING |
+ * SAME | SAME | SAME |
+ *
+ *
+ *
+ * @param c
+ * @return the result of the intersection.
+ */
+ public IntersectionType and(IntersectionType c) {
+ return and(this,c);
+ }
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry/IntersectionUtil.java b/core/math/src/main/java/org/arakhne/afc/math/geometry/IntersectionUtil.java
new file mode 100644
index 000000000..7448b3fad
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry/IntersectionUtil.java
@@ -0,0 +1,2343 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Christophe BOHRHAUER.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry;
+
+import org.arakhne.afc.math.MathConstants;
+import org.arakhne.afc.math.MathUtil;
+import org.arakhne.afc.math.geometry2d.continuous.Point2f;
+import org.arakhne.afc.math.geometry3d.continuous.Point3f;
+
+
+/**
+ * Several intersection functions.
+ *
+ * Algo inspired from Mathematics for 3D Game Programming and Computer Graphics (MGPCG)
+ * and from 3D Game Engine Design (GED)
+ * and from Real Time Collision Detection (RTCD).
+ *
+ * @author $Author: cbohrhauer$
+ * @author $Author: sgalland$
+ * @author $Author: ngaud$
+ * @author $Author: jdemange$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public final class IntersectionUtil implements MathConstants{//TODO faire les JUnit
+
+ private IntersectionUtil() {
+ //
+ }
+
+ /** Replies if the specified box intersects the specified sphere.
+ *
+ * A Simple Method for Box-Sphere Intersection Testing by Jim Arvo
+ * from "Graphics Gems", Academic Press, 1990
+ *
+ * This routine tests for intersection between an 3-dimensional
+ * axis-aligned box and an 3-dimensional sphere. The algorithm type
+ * argument indicates whether the objects are to be regarded as
+ * surfaces or solids.
+ *
+ * @param sphereCenter are the coordinates of the sphere center.
+ * @param radius is the radius of the sphere.
+ * @param lower coordinates of the lowest point of the box.
+ * @param upper coordinates of the uppermost point of the box.
+ * @return true if intersecting, otherwise false
+ */
+ public static boolean intersectsHollowSphereHollowAlignedBox(float sphereCenterx, float sphereCentery, float sphereCenterz, float radius,
+ float lowerx, float lowery, float lowerz,
+ float upperx, float uppery, float upperz) {
+ float r2 = radius*radius;
+ float a, b, dmin, dmax;
+ boolean face;
+
+ dmin = 0;
+ dmax = 0;
+
+ face = false;
+
+ // X
+ a = sphereCenterx - lowerx;
+ a = a*a;
+
+ b = sphereCenterx - upperx;
+ b = b*b;
+
+ dmax += Math.max(a, b);
+
+ if( sphereCenterx < lowerx ) {
+ face = true;
+ dmin += a;
+ }
+ else if( sphereCenterx > upperx ) {
+ face = true;
+ dmin += b;
+ }
+ else if( Math.min(a, b) <= r2 ) {
+ face = true;
+ }
+
+ // Y
+ a = sphereCentery - lowery;
+ a = a*a;
+
+ b = sphereCentery - uppery;
+ b = b*b;
+
+ dmax += Math.max(a, b);
+
+ if( sphereCentery < lowery ) {
+ face = true;
+ dmin += a;
+ }
+ else if( sphereCentery > uppery ) {
+ face = true;
+ dmin += b;
+ }
+ else if( Math.min(a, b) <= r2 ) {
+ face = true;
+ }
+
+ // Z
+ a = sphereCenterz - lowerz;
+ a = a*a;
+
+ b = sphereCenterz - upperz;
+ b = b*b;
+
+ dmax += Math.max(a, b);
+
+ if( sphereCenterz < lowerz ) {
+ face = true;
+ dmin += a;
+ }
+ else if( sphereCenterz > upperz ) {
+ face = true;
+ dmin += b;
+ }
+ else if( Math.min(a, b) <= r2 ) {
+ face = true;
+ }
+
+ return (face && ( dmin <= r2 ) && ( r2 <= dmax));
+ }
+
+ /** Replies if the specified box intersects the specified sphere.
+ *
+ * A Simple Method for Box-Sphere Intersection Testing by Jim Arvo
+ * from "Graphics Gems", Academic Press, 1990
+ *
+ * This routine tests for intersection between an 3-dimensional
+ * axis-aligned box and an 3-dimensional sphere. The algorithm type
+ * argument indicates whether the objects are to be regarded as
+ * surfaces or solids.
+ *
+ * @param sphereCenter are the coordinates of the sphere center.
+ * @param radius is the radius of the sphere.
+ * @param lower coordinates of the lowest point of the box.
+ * @param upper coordinates of the uppermost point of the box.
+ * @return true if intersecting, otherwise false
+ */
+ public static boolean intersectsSolidSphereHollowAlignedBox(float sphereCenterx, float sphereCentery, float sphereCenterz, float radius,
+ float lowerx, float lowery, float lowerz,
+ float upperx, float uppery, float upperz) {
+ float r2 = radius*radius;
+ float a, dmin;
+ boolean face;
+
+ dmin = 0;
+
+ face = false;
+
+ // X
+ if ( sphereCenterx < lowerx ) {
+ face = true;
+ a = sphereCenterx - lowerx;
+ dmin += a*a;
+ }
+ else if ( sphereCenterx > upperx ) {
+ face = true;
+ a = sphereCenterx - upperx;
+ dmin += a*a;
+ }
+ else if ( sphereCenterx - lowerx <= radius ) face = true;
+ else if ( upperx - sphereCenterx <= radius ) face = true;
+
+ // Y
+ if ( sphereCentery < lowery ) {
+ face = true;
+ a = sphereCentery - lowery;
+ dmin += a*a;
+ }
+ else if ( sphereCentery > uppery ) {
+ face = true;
+ a = sphereCentery - uppery;
+ dmin += a*a;
+ }
+ else if ( sphereCentery - lowery <= radius ) face = true;
+ else if ( uppery - sphereCentery <= radius ) face = true;
+
+ // Z
+ if ( sphereCenterz < lowerz ) {
+ face = true;
+ a = sphereCenterz - lowerz;
+ dmin += a*a;
+ }
+ else if ( sphereCenterz > upperz ) {
+ face = true;
+ a = sphereCenterz - upperz;
+ dmin += a*a;
+ }
+ else if ( sphereCenterz - lowerz <= radius ) face = true;
+ else if ( upperz - sphereCenterz <= radius ) face = true;
+
+ return ( face && ( dmin <= r2 ) );
+ }
+
+ /** Replies if the specified box intersects the specified sphere.
+ *
+ * A Simple Method for Box-Sphere Intersection Testing by Jim Arvo
+ * from "Graphics Gems", Academic Press, 1990
+ *
+ * This routine tests for intersection between an 3-dimensional
+ * axis-aligned box and an 3-dimensional sphere. The algorithm type
+ * argument indicates whether the objects are to be regarded as
+ * surfaces or solids.
+ *
+ * @param sphereCenter are the coordinates of the sphere center.
+ * @param radius is the radius of the sphere.
+ * @param lower coordinates of the lowest point of the box.
+ * @param upper coordinates of the uppermost point of the box.
+ * @return true if intersecting, otherwise false
+ */
+ public static boolean intersectsHollowSphereSolidAlignedBox(float sphereCenterx, float sphereCentery, float sphereCenterz, float radius,
+ float lowerx, float lowery, float lowerz,
+ float upperx, float uppery, float upperz) {
+ float r2 = radius*radius;
+ float a, b, dmin, dmax;
+
+ dmax = 0;
+ dmin = 0;
+
+ // X
+ a = sphereCenterx - lowerx;
+ a = a*a;
+
+ b = sphereCenterx - upperx;
+ b = b*b;
+
+ dmax += Math.max(a, b);
+ if( sphereCenterx < lowerx ) dmin += a;
+ else if( sphereCenterx > upperx ) dmin += b;
+
+ // Y
+ a = sphereCentery - lowery;
+ a = a*a;
+
+ b = sphereCentery - uppery;
+ b = b*b;
+
+ dmax += Math.max(a, b);
+ if( sphereCentery < lowery ) dmin += a;
+ else if( sphereCentery > uppery ) dmin += b;
+
+ // Z
+ a = sphereCenterz - lowerz;
+ a = a*a;
+
+ b = sphereCenterz - upperz;
+ b = b*b;
+
+ dmax += Math.max(a, b);
+ if( sphereCenterz < lowerz ) dmin += a;
+ else if( sphereCenterz > upperz ) dmin += b;
+
+ return ( dmin <= r2 && r2 <= dmax );
+ }
+
+ /** Replies if the specified box intersects the specified sphere.
+ *
+ * A Simple Method for Box-Sphere Intersection Testing by Jim Arvo
+ * from "Graphics Gems", Academic Press, 1990
+ *
+ * This routine tests for intersection between an 3-dimensional
+ * axis-aligned box and an 3-dimensional sphere. The algorithm type
+ * argument indicates whether the objects are to be regarded as
+ * surfaces or solids.
+ *
+ * @param sphereCenter are the coordinates of the sphere center.
+ * @param radius is the radius of the sphere.
+ * @param lower coordinates of the lowest point of the box.
+ * @param upper coordinates of the uppermost point of the box.
+ * @return true if intersecting, otherwise false
+ */
+ public static boolean intersectsSolidSphereSolidAlignedBox(float sphereCenterx, float sphereCentery, float sphereCenterz, float radius,
+ float lowerx, float lowery, float lowerz,
+ float upperx, float uppery, float upperz) {
+ float r2 = radius*radius;
+ float a, dmin;
+
+ dmin = 0;
+
+ // X
+ if( sphereCenterx < lowerx ) {
+ a = sphereCenterx - lowerx;
+ dmin += a*a;
+ }
+ else if( sphereCenterx > upperx ) {
+ a = sphereCenterx - upperx;
+ dmin += a * a;
+ }
+
+ // Y
+ if( sphereCentery < lowery ) {
+ a = sphereCentery - lowery;
+ dmin += a*a;
+ }
+ else if( sphereCentery > uppery ) {
+ a = sphereCentery - uppery;
+ dmin += a * a;
+ }
+
+ // Z
+ if( sphereCenterz < lowerz ) {
+ a = sphereCenterz - lowerz;
+ dmin += a*a;
+ }
+ else if( sphereCenterz > upperz ) {
+ a = sphereCenterz - upperz;
+ dmin += a * a;
+ }
+
+ return ( dmin <= r2 );
+ }
+
+ /** Replies if the specified rectangle intersects the specified circle.
+ *
+ * A Simple Method for Box-Sphere Intersection Testing by Jim Arvo
+ * from "Graphics Gems", Academic Press, 1990
+ *
+ * This routine tests for intersection between an 2-dimensional
+ * axis-aligned rectangle and an 2-dimensional circle. The algorithm type
+ * argument indicates whether the objects are to be regarded as
+ * surfaces or solids.
+ *
+ * @param circleCenter are the coordinates of the circle center.
+ * @param radius is the radius of the circle.
+ * @param lower coordinates of the lowest point of the rectangle.
+ * @param upper coordinates of the uppermost point of the rectangle.
+ * @return true if intersecting, otherwise false
+ */
+ public static boolean intersectsSolidCircleSolidAlignedRectangle(float circleCenterx, float circleCentery, float circleCenterz, float radius,
+ float lowerx, float lowery, float lowerz,
+ float upperx, float uppery, float upperz) {
+ float r2 = radius*radius;
+ float a, dmin;
+
+ dmin = 0;
+
+ // X
+ if( circleCenterx < lowerx ) {
+ a = circleCenterx - lowerx;
+ dmin += a*a;
+ }
+ else if( circleCenterx > upperx ) {
+ a = circleCenterx - upperx;
+ dmin += a * a;
+ }
+
+ // Y
+ if( circleCentery < lowery ) {
+ a = circleCentery - lowery;
+ dmin += a*a;
+ }
+ else if( circleCentery > uppery ) {
+ a = circleCentery - uppery;
+ dmin += a * a;
+ }
+
+ return ( dmin < r2 );
+ }
+
+ /**
+ * Tests if the line segment from {@code (x1,y1)} to
+ * {@code (x2,y2)} intersects the line segment
+ * from {@code (x3,y3)} to {@code (x4,y4)}.
+ *
+ * @param x1 the X coordinate of the start point of the
+ * first specified line segment
+ * @param y1 the Y coordinate of the start point of the
+ * first specified line segment
+ * @param x2 the X coordinate of the end point of the
+ * specified line segment
+ * @param y2 the Y coordinate of the end point of the
+ * first specified line segments
+ * @param x3 the X coordinate of the start point of the
+ * first specified line segment
+ * @param y3 the Y coordinate of the start point of the
+ * first specified line segment
+ * @param x4 the X coordinate of the end point of the
+ * specified line segment
+ * @param y4 the Y coordinate of the end point of the
+ * first specified line segment
+ * @param epsilon the accuracy parameter (a distance here) must be the same unit of measurement as others parameters
+ * @return true if this line segments intersect each
+ * other; false otherwise.
+ * @since 3.0
+ */
+ public static boolean intersectsSegments(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4, float epsilon) {
+ return ((GeometryUtil.ccw(x1, y1, x2, y2, x3, y3, epsilon) *
+ GeometryUtil.ccw(x1, y1, x2, y2, x4, y4, epsilon) <= 0)
+ && (GeometryUtil.ccw(x3, y3, x4, y4, x1, y1, epsilon) *
+ GeometryUtil.ccw(x3, y3, x4, y4, x2, y2, epsilon) <= 0));
+ }
+
+ /**
+ * Tests if the line from {@code (x1,y1)} to
+ * {@code (x2,y2)} intersects the line segment
+ * from {@code (x3,y3)} to {@code (x4,y4)}.
+ *
+ * @param x1 the X coordinate of the start point of the line
+ * @param y1 the Y coordinate of the start point of the line
+ * @param x2 the X coordinate of the end point of the line
+ * @param y2 the Y coordinate of the end point of the line
+ * @param x3 the X coordinate of the start point of the line segment
+ * @param y3 the Y coordinate of the start point of the line segment
+ * @param x4 the X coordinate of the end point of the line segment
+ * @param y4 the Y coordinate of the end point of the line segment
+ * @param epsilon the accuracy parameter (distance) must be the same unit of measurement as others parameters
+ * @return true if this line intersects the line segment,
+ * false otherwise.
+ * @since 3.0
+ */
+ public static boolean intersectsLineSegment(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4, float epsilon) {
+ return (GeometryUtil.ccw(x1, y1, x2, y2, x3, y3, epsilon) *
+ GeometryUtil.ccw(x1, y1, x2, y2, x4, y4, epsilon) <= 0);
+ }
+
+ /**
+ * Tests if the line from {@code (x1,y1)} to
+ * {@code (x2,y2)} intersects the line
+ * from {@code (x3,y3)} to {@code (x4,y4)}.
+ *
+ * If lines are colinear, this function replied false.
+ *
+ * @param x1 the X coordinate of the start point of the first line
+ * @param y1 the Y coordinate of the start point of the first line
+ * @param x2 the X coordinate of the end point of the first line
+ * @param y2 the Y coordinate of the end point of the first line
+ * @param x3 the X coordinate of the start point of the second line
+ * @param y3 the Y coordinate of the start point of the second line
+ * @param x4 the X coordinate of the end point of the second line
+ * @param y4 the Y coordinate of the end point of the second line
+ * @param epsilon the accuracy parameter (distance) must be the same unit of measurement as others parameters
+ * @return true if this line segments intersect each
+ * other; false otherwise.
+ * @since 3.0
+ */
+ public static boolean intersectsLines(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4, float epsilon) {
+ if (GeometryUtil.isParallelLines(x1, y1, x2, y2, x3, y3, x4, y4, epsilon)) {
+ return GeometryUtil.isCollinearPoints(x1, y1, x2, y2, x3, y3, epsilon);
+ }
+ return true;
+ }
+
+ /**
+ * Tests if the point {@code (px,py)}
+ * lies inside a 2D triangle
+ * given by {@code (x1,y1)}, {@code (x2,y2)}
+ * and {@code (x3,y3)} points.
+ *
+ * Caution: Tests are "epsiloned."
+ *
+ * Trigonometric Method (Slowest)
+ *
+ * A common way to check if a point is in a triangle is to
+ * find the vectors connecting the point to each of the
+ * triangle's three vertices and sum the angles between
+ * those vectors. If the sum of the angles is 2*pi
+ * then the point is inside the triangle, otherwise it
+ * is not. It works, but it is very slow.
+ *
+ *
+ * 
+ *
+ * The advantage of the method above is that it's very simple to understand so that once
+ * you read it you should be able to remember it forever and code it up at
+ * any time without having to refer back to anything.
+ *
+ * There's another method that is also as easy conceptually but executes faster.
+ * The downside is there's a little more math involved, but once you see
+ * it worked out it should be no problem.
+ *
+ * Barycenric Method (Fastest)
+ *
+ * So remember that the three points of the triangle define a plane in space.
+ * Pick one of the points and we can consider all other locations on the plane
+ * as relative to that point. Let's select A -- it'll be our origin on the
+ * plane. Now what we need are basis vectors so we can give coordinate
+ * values to all the locations on the plane.
+ * We'll pick the two edges of the triangle that touch A,
+ * (C - A) and (B - A).
+ * Now we can get to any point on the plane just by starting at A
+ * and walking some distance along (C - A) and then from there walking
+ * some more in the direction (B - A).
+ *
+ *
+ *
+ * With that in mind we can now describe any point on the plane as:
+ * P = A + u * (C - A) + v * (B - A)
+ *
+ * Notice now that if u or v < 0 then we've walked in the wrong direction
+ * and must be outside the triangle. Also if u or v > 1 then we've
+ * walked too far in a direction and are outside the triangle.
+ * Finally if u + v > 1 then we've crossed the edge BC again leaving the triangle.
+ *
+ * Given u and v we can easily calculate the point P with the above
+ * equation, but how can we go in the reverse direction and calculate
+ * u and v from a given point P?
+ * P = A + u * (C - A) + v * (B - A) // Original equation
+ * (P - A) = u * (C - A) + v * (B - A) // Subtract A from both sides
+ * v2 = u * v0 + v * v1 // Substitute v0, v1, v2 for less writing
+ *
+ * We have two unknowns (u and v) so we need two equations to solve
+ * for them. Dot both sides by v0 to get one and dot both sides by
+ * v1 to get a second.
+ * (v2) . v0 = (u * v0 + v * v1) . v0
+ * (v2) . v1 = (u * v0 + v * v1) . v1
+ *
+ * Distribute v0 and v1
+ * v2 . v0 = u * (v0 . v0) + v * (v1 . v0)
+ * v2 . v1 = u * (v0 . v1) + v * (v1 . v1)
+ *
+ * Now we have two equations and two unknowns and can solve one
+ * equation for one variable and substitute into the other. Or
+ * fire up GNU Octave and save some handwriting.
+ * Solve[v2.v0 == {u(v0.v0) + v(v1.v0), v2.v1 == u(v0.v1) + v(v1.v1)}, {u, v}]
+ * u = ((v1.v1)(v2.v0)-(v1.v0)(v2.v1)) / ((v0.v0)(v1.v1) - (v0.v1)(v1.v0))
+ * v = ((v0.v0)(v2.v1)-(v0.v1)(v2.v0)) / ((v0.v0)(v1.v1) - (v0.v1)(v1.v0))
+ *
+ * @param px the X coordinate of the point
+ * @param py the Y coordinate of the point
+ * @param ax the X coordinate of the first point of the triangle
+ * @param ay the Y coordinate of the first point of the triangle
+ * @param bx the X coordinate of the second point of the triangle
+ * @param by the Y coordinate of the second point of the triangle
+ * @param cx the X coordinate of the third point of the triangle
+ * @param cy the Y coordinate of the third point of the triangle
+ * @param epsilon the accuracy parameter (distance) must be the same unit of measurement as others parameters
+ * @return true if the points is coplanar to the triangle and
+ * lies inside it, otherwise false
+ * @since 3.0
+ */
+ public static boolean intersectsPointTriangle(
+ float px, float py,
+ float ax, float ay,
+ float bx, float by,
+ float cx, float cy, float epsilon) {
+ /* The comment code is the trivial trigonometric implementation:
+ float vx1 = x1 - px;
+ float vy1 = y1 - py;
+ float vx2 = x2 - px;
+ float vy2 = y2 - py;
+ float vx3 = x3 - px;
+ float vy3 = y3 - py;
+
+ float angle;
+
+ angle = Math.acos(MathUtil.dotProduct(vx1, vy1, vx2, vy2));
+ angle += Math.acos(MathUtil.dotProduct(vx2, vy2, vx3, vy3));
+ angle += Math.acos(MathUtil.dotProduct(vx3, vy3, vx1, vy1));
+
+ return MathUtil.epsilonEqualsRadian(angle, MathConstants.TWO_PI);*/
+
+ //
+ // Compute vectors
+ //
+ // v0 = C - A
+ float v0x = cx - ax;
+ float v0y = cy - ay;
+ // v1 = B - A
+ float v1x = bx - ax;
+ float v1y = by - ay;
+ // v2 = P - A
+ float v2x = px - ax;
+ float v2y = py - ay;
+
+ //
+ // Compute dot products
+ //
+ // dot01 = dot(v0, v0)
+ float dot00 = MathUtil.dotProduct(v0x, v0y, v0x, v0y);
+ // dot01 = dot(v0, v1)
+ float dot01 = MathUtil.dotProduct(v0x, v0y, v1x, v1y);
+ // dot02 = dot(v0, v2)
+ float dot02 = MathUtil.dotProduct(v0x, v0y, v2x, v2y);
+ // dot11 = dot(v1, v1)
+ float dot11 = MathUtil.dotProduct(v1x, v1y, v1x, v1y);
+ // dot12 = dot(v1, v2)
+ float dot12 = MathUtil.dotProduct(v1x, v1y, v2x, v2y);
+
+ //
+ // Compute barycentric coordinates
+ //
+ float invDenom = 1.f / (dot00 * dot11 - dot01 * dot01);
+ float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
+ float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
+
+ // Check if point is in triangle
+ return (MathUtil.epsilonCompareTo(u, 0.f, epsilon) >= 0)
+ && (MathUtil.epsilonCompareTo(v, 0.f, epsilon) >= 0)
+ && (MathUtil.epsilonCompareTo(u + v, 1.f, epsilon) <= 0);
+ }
+
+ /**
+ * Tests if the point {@code (px,py,pz)}
+ * lies inside a 3D triangle
+ * given by {@code (x1,y1,z1)}, {@code (x2,y2,z2)}
+ * and {@code (x3,y3,z3)} points.
+ *
+ * Caution: Tests are "epsiloned."
+ *
+ * Parameter forceCoplanar has a deep influence on the function
+ * result. It indicates if coplanarity test must be done or not.
+ * Following table explains this influence:
+ *
+ *
+ *
+ * | Point is coplanar? |
+ * Point projection on plane is inside triangle? |
+ * forceCoplanar |
+ * intersectsPointTrangle() Result |
+ *
+ *
+ *
+ * true |
+ * true |
+ * true |
+ * true |
+ *
+ *
+ * true |
+ * true |
+ * false |
+ * true |
+ *
+ *
+ * true |
+ * false |
+ * true |
+ * false |
+ *
+ *
+ * true |
+ * false |
+ * false |
+ * false |
+ *
+ *
+ * false |
+ * true |
+ * true |
+ * false |
+ *
+ *
+ * false |
+ * true |
+ * false |
+ * true |
+ *
+ *
+ * false |
+ * false |
+ * true |
+ * false |
+ *
+ *
+ * false |
+ * false |
+ * false |
+ * false |
+ *
+ *
+ *
+ * Trigonometric Method (Slowest)
+ *
+ * A common way to check if a point is in a triangle is to
+ * find the vectors connecting the point to each of the
+ * triangle's three vertices and sum the angles between
+ * those vectors. If the sum of the angles is 2*pi
+ * then the point is inside the triangle, otherwise it
+ * is not. It works, but it is very slow.
+ *
+ *
+ *
+ *
+ * The advantage of the method above is that it's very simple to understand so that once
+ * you read it you should be able to remember it forever and code it up at
+ * any time without having to refer back to anything.
+ *
+ * Barycenric Method (Fastest)
+ *
+ * There's another method that is also as easy conceptually but executes faster.
+ * The downside is there's a little more math involved, but once you see
+ * it worked out it should be no problem.
+ *
+ * So remember that the three points of the triangle define a plane in space.
+ * Pick one of the points and we can consider all other locations on the plane
+ * as relative to that point. Let's select A -- it'll be our origin on the
+ * plane. Now what we need are basis vectors so we can give coordinate
+ * values to all the locations on the plane.
+ * We'll pick the two edges of the triangle that touch A,
+ * (C - A) and (B - A).
+ * Now we can get to any point on the plane just by starting at A
+ * and walking some distance along (C - A) and then from there walking
+ * some more in the direction (B - A).
+ *
+ *
+ *
+ * With that in mind we can now describe any point on the plane as:
+ * P = A + u * (C - A) + v * (B - A)
+ *
+ * Notice now that if u or v < 0 then we've walked in the wrong direction
+ * and must be outside the triangle. Also if u or v > 1 then we've
+ * walked too far in a direction and are outside the triangle.
+ * Finally if u + v > 1 then we've crossed the edge BC again leaving the triangle.
+ *
+ * Given u and v we can easily calculate the point P with the above
+ * equation, but how can we go in the reverse direction and calculate
+ * u and v from a given point P?
+ * P = A + u * (C - A) + v * (B - A) // Original equation
+ * (P - A) = u * (C - A) + v * (B - A) // Subtract A from both sides
+ * v2 = u * v0 + v * v1 // Substitute v0, v1, v2 for less writing
+ *
+ * We have two unknowns (u and v) so we need two equations to solve
+ * for them. Dot both sides by v0 to get one and dot both sides by
+ * v1 to get a second.
+ * (v2) . v0 = (u * v0 + v * v1) . v0
+ * (v2) . v1 = (u * v0 + v * v1) . v1
+ *
+ * Distribute v0 and v1
+ * v2 . v0 = u * (v0 . v0) + v * (v1 . v0)
+ * v2 . v1 = u * (v0 . v1) + v * (v1 . v1)
+ *
+ * Now we have two equations and two unknowns and can solve one
+ * equation for one variable and substitute into the other. Or
+ * fire up GNU Octave and save some handwriting.
+ * Solve[v2.v0 == {u(v0.v0) + v(v1.v0), v2.v1 == u(v0.v1) + v(v1.v1)}, {u, v}]
+ * u = ((v1.v1)(v2.v0)-(v1.v0)(v2.v1)) / ((v0.v0)(v1.v1) - (v0.v1)(v1.v0))
+ * v = ((v0.v0)(v2.v1)-(v0.v1)(v2.v0)) / ((v0.v0)(v1.v1) - (v0.v1)(v1.v0))
+ *
+ * @param px the X coordinate of the point
+ * @param py the Y coordinate of the point
+ * @param pz the Z coordinate of the point
+ * @param ax the X coordinate of the first point of the triangle
+ * @param ay the Y coordinate of the first point of the triangle
+ * @param az the Z coordinate of the first point of the triangle
+ * @param bx the X coordinate of the second point of the triangle
+ * @param by the Y coordinate of the second point of the triangle
+ * @param bz the Z coordinate of the second point of the triangle
+ * @param cx the X coordinate of the third point of the triangle
+ * @param cy the Y coordinate of the third point of the triangle
+ * @param cz the Z coordinate of the third point of the triangle
+ * @param forceCoplanar is true to force to test
+ * if the given point is coplanar to the triangle, false
+ * to not consider coplanarity of the point.
+ * @param epsilon the accuracy parameter (distance) must be the same unit of measurement as others parameters
+ * @return true if the points is coplanar - or not,
+ * depending on forceCoplanar - to the triangle and
+ * lies inside it, otherwise false
+ * @since 3.0
+ */
+ public static boolean intersectsPointTriangle(
+ float px, float py, float pz,
+ float ax, float ay, float az,
+ float bx, float by, float bz,
+ float cx, float cy, float cz,
+ boolean forceCoplanar, float epsilon) {
+
+ //
+ // Compute vectors
+ //
+ // v0 = C - A
+ float v0x = cx - ax;
+ float v0y = cy - ay;
+ float v0z = cz - az;
+ // v1 = B - A
+ float v1x = bx - ax;
+ float v1y = by - ay;
+ float v1z = bz - az;
+ // v2 = P - A
+ float v2x = px - ax;
+ float v2y = py - ay;
+ float v2z = pz - az;
+
+ //
+ // Compute dot products
+ //
+ // dot01 = dot(v0, v0)
+ float dot00 = MathUtil.dotProduct(v0x, v0y, v0z, v0x, v0y, v0z);
+ // dot01 = dot(v0, v1)
+ float dot01 = MathUtil.dotProduct(v0x, v0y, v0z, v1x, v1y, v1z);
+ // dot02 = dot(v0, v2)
+ float dot02 = MathUtil.dotProduct(v0x, v0y, v0z, v2x, v2y, v2z);
+ // dot11 = dot(v1, v1)
+ float dot11 = MathUtil.dotProduct(v1x, v1y, v1z, v1x, v1y, v1z);
+ // dot12 = dot(v1, v2)
+ float dot12 = MathUtil.dotProduct(v1x, v1y, v1z, v2x, v2y, v2z);
+
+ //
+ // Compute barycentric coordinates
+ //
+ float invDenom = 1.f / (dot00 * dot11 - dot01 * dot01);
+ float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
+ float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
+
+ // Check if point is in triangle
+ if ((MathUtil.epsilonCompareTo(u, 0.f, epsilon) >= 0)
+ && (MathUtil.epsilonCompareTo(v, 0.f, epsilon) >= 0)
+ && (MathUtil.epsilonCompareTo(u + v, 1.f, epsilon) <= 0)) {
+ if (forceCoplanar) {
+ // Triangle's plane equation:
+ // nx = ay * (bz - cz) + by * (cz - az) + cy * (az - bz)
+ // ny = az * (bx - cx) + bz * (cx - ax) + cz * (ax - bx)
+ // nz = ax * (by - cy) + bx * (cy - ay) + cx * (ay - by)
+ // d = - (nx * ax + ny * ay + nz * az)
+
+ // Result dot* variables to prevent memory allocation
+ dot00 = ay * (bz - cz) + by * v0z - cy * v1z;
+ dot01 = az * (bx - cx) + bz * v0x - cz * v1x;
+ dot02 = ax * (by - cy) + bx * v0y - cx * v1y;
+ dot11 = - (dot00 * ax + dot01 * ay + dot02 * az);
+ dot12 = dot00 * px + dot01 * py + dot02 * pz + dot11;
+
+ return MathUtil.isEpsilonZero(dot12,0.f);
+ }
+ return true;
+ }
+ return false;
+ }
+
+ /**
+ * Tests if the point {@code (px,py,pz)}
+ * lies inside a 3D segment
+ * given by {@code (x1,y1,z1)} and {@code (x2,y2,z2)}
+ * points.
+ *
+ * This function projects the point on the 3D line and tests if the projection
+ * is lying on the segment. To force the point to be on the segment, see below.
+ *
+ * Parameter forceCollinear has a deep influence on the function
+ * result. It indicates if collinear test must be done or not.
+ * Following table explains this influence:
+ *
+ *
+ *
+ * | Point is collinear? |
+ * Point projection on line is inside segment? |
+ * forceCollinear |
+ * intersectsPointSegment() Result |
+ *
+ *
+ *
+ * true |
+ * true |
+ * true |
+ * true |
+ *
+ *
+ * true |
+ * true |
+ * false |
+ * true |
+ *
+ *
+ * true |
+ * false |
+ * true |
+ * false |
+ *
+ *
+ * true |
+ * false |
+ * false |
+ * false |
+ *
+ *
+ * false |
+ * true |
+ * true |
+ * false |
+ *
+ *
+ * false |
+ * true |
+ * false |
+ * true |
+ *
+ *
+ * false |
+ * false |
+ * true |
+ * false |
+ *
+ *
+ * false |
+ * false |
+ * false |
+ * false |
+ *
+ *
+ *
+ * @param px the X coordinate of the point
+ * @param py the Y coordinate of the point
+ * @param pz the Z coordinate of the point
+ * @param ax the X coordinate of the first point of the segment
+ * @param ay the Y coordinate of the first point of the segment
+ * @param az the Z coordinate of the first point of the segment
+ * @param bx the X coordinate of the second point of the segment
+ * @param by the Y coordinate of the second point of the segment
+ * @param bz the Z coordinate of the second point of the segment
+ * @param forceCollinear is true to force to test
+ * if the given point is collinear to the segment, false
+ * to not consider collinearity of the point.
+ * @param epsilon the accuracy parameter (distance) must be the same unit of measurement as others parameters
+ * @return true if the points is on segment,
+ * otherwise false
+ * @since 3.0
+ */
+ public static boolean intersectsPointSegment(
+ float px, float py, float pz,
+ float ax, float ay, float az,
+ float bx, float by, float bz,
+ boolean forceCollinear, float epsilon) {
+ float ratio = GeometryUtil.getPointProjectionFactorOnSegment(px, py, pz, ax, ay, az, bx, by, bz); //Todo : create this
+
+ if (ratio>=0. && ratio<=1.) {
+ if (forceCollinear) {
+ return GeometryUtil.isCollinearPoints(
+ ax, ay, az,
+ bx, by, bz,
+ px, py, pz, epsilon);
+ }
+ return true;
+ }
+
+ return false;
+ }
+
+ /**
+ * Tests if the point {@code (px,py)}
+ * appromativaly lies inside a 2D segment
+ * given by {@code (x1,y1)} and {@code (x2,y2)}
+ * points.
+ *
+ * This function uses {@link MathUtil#epsilonEqualsZero(float)}
+ * to approximate ownership of the given point.
+ *
+ * @param px the X coordinate of the point
+ * @param py the Y coordinate of the point
+ * @param ax the X coordinate of the first point of the segment
+ * @param ay the Y coordinate of the first point of the segment
+ * @param bx the X coordinate of the second point of the segment
+ * @param by the Y coordinate of the second point of the segment
+ * @param epsilon the accuracy parameter (distance) must be the same unit of measurement as others parameters
+ * @return true if the points is on segment,
+ * otherwise false
+ * @since 3.0
+ */
+ public static boolean intersectsPointSegment(
+ float px, float py,
+ float ax, float ay,
+ float bx, float by, float epsilon) {
+ return GeometryUtil.ccw(ax, ay, bx, by, px, py, epsilon)==0;
+ }
+
+ /**
+ * Tests if the two 1D segments are intersecting.
+ *
+ * This function is assuming that l1 is lower
+ * or equal to u1 and l2 is lower
+ * or equal to u2.
+ *
+ * @param l1 the min coordinate of the first segment
+ * @param u1 the max coordinate of the first segment
+ * @param l2 the min coordinate of the second segment
+ * @param u2 the max coordinate of the second segment
+ * @return true if the two 1D segments intersect each
+ * other; false otherwise.
+ */
+ public static boolean intersectsAlignedSegments(float l1, float u1, float l2, float u2) {
+ assert(l1<=u1);
+ assert(l2<=u2);
+ if (l1=l2;
+ return u2>=l1;
+ }
+
+ /**
+ * Tests if the two 2D axis-aligned rectangles are intersecting.
+ *
+ * This function is assuming that lx1 is lower
+ * or equal to ux1, ly1 is lower
+ * or equal to uy1, and so on.
+ *
+ * @param lx1 the X coordinate of the lowest point of the first rectangle.
+ * @param ly1 the Y coordinate of the lowest point of the first rectangle.
+ * @param ux1 the X coordinate of the uppermost point of the first rectangle.
+ * @param uy1 the Y coordinate of the uppermost point of the first rectangle.
+ * @param lx2 the X coordinate of the lowest point of the second rectangle.
+ * @param ly2 the Y coordinate of the lowest point of the second rectangle.
+ * @param ux2 the X coordinate of the uppermost point of the second rectangle.
+ * @param uy2 the Y coordinate of the uppermost point of the second rectangle.
+ * @return true if the two 2D rectangles intersect each
+ * other; false otherwise.
+ */
+ public static boolean intersectsAlignedRectangles(float lx1, float ly1, float ux1, float uy1, float lx2, float ly2, float ux2, float uy2) {
+ assert(lx1<=ux1);
+ assert(ly1<=uy1);
+ assert(lx2<=ux2);
+ assert(ly2<=uy2);
+
+ boolean intersects;
+ if (lx1lx2;
+ else intersects = ux2>lx1;
+
+ if (intersects) {
+ if (ly1ly2;
+ else intersects = uy2>ly1;
+ }
+
+ return intersects;
+ }
+
+ /**
+ * Tests if the two 3D axis-aligned boxes are intersecting.
+ *
+ * This function is assuming that lx1 is lower
+ * or equal to ux1, ly1 is lower
+ * or equal to uy1, and so on.
+ *
+ * @param lower1 coordinates of the lowest point of the first box.
+ * @param upper1 coordinates of the uppermost point of the first box.
+ * @param lower2 coordinates of the lowest point of the second box.
+ * @param upper2 coordinates of the uppermost point of the second box.
+ * @return true if the two 3D boxes intersect each
+ * other; false otherwise.
+ */
+ public static boolean intersectsAlignedBoxes(float lower1x, float lower1y, float lower1z, float upper1x, float upper1y, float upper1z,
+ float lower2x, float lower2y, float lower2z, float upper2x, float upper2y, float upper2z) {
+ assert(lower1x<=upper1x);
+ assert(lower1y<=upper1y);
+ assert(lower1z<=upper1z);
+ assert(lower2x<=upper2x);
+ assert(lower2y<=upper2y);
+ assert(lower2z<=upper2z);
+
+ boolean intersects;
+ if (lower1xlower2x;
+ else intersects = upper2x>lower1x;
+
+ if (intersects) {
+ if (lower1ylower2y;
+ else intersects = upper2y>lower1y;
+
+ if (intersects) {
+ if (lower1zlower2z;
+ else intersects = upper2z>lower1z;
+ }
+ }
+
+ return intersects;
+ }
+
+ /**
+ * Tests if the two 2D minimum bounding rectangles are intersecting.
+ *
+ * This function is assuming that lx1 is lower
+ * or equal to ux1, and ly1 is lower
+ * or equal to uy1.
+ *
+ * @param lower1 coordinates of the lowest point of the first rectangle.
+ * @param upper1 coordinates of the uppermost point of the first rectangle.
+ * @param lower2 coordinates of the lowest point of the second rectangle.
+ * @param upper2 coordinates of the uppermost point of the second rectangle.
+ * @return true if the two 2D rectangles intersect each
+ * other; false otherwise.
+ */
+ public static boolean intersectsAlignedRectangles(float lower1x, float lower1y, float lower1z, float upper1x, float upper1y, float upper1z,
+ float lower2x, float lower2y, float lower2z, float upper2x, float upper2y, float upper2z){
+ assert(lower1x<=upper1x);
+ assert(lower1y<=upper1y);
+ assert(lower2x<=upper2x);
+ assert(lower2y<=upper2y);
+
+ boolean intersects;
+ if (lower1xlower2x;
+ else intersects = upper2x>lower1x;
+
+ if (intersects) {
+ if (lower1ylower2y;
+ else intersects = upper2y>lower1y;
+ }
+
+ return intersects;
+ }
+
+ /** Replies if the specified box intersects the specified sphere.
+ *
+ * @param sphereCenter are the coordinates of the sphere center.
+ * @param sphereRadius is the radius of the sphere.
+ * @param boxCenter is the center point of the oriented box.
+ * @param boxAxis are the unit vectors of the oriented box axis.
+ * @param boxExtent are the sizes of the oriented box.
+ * @param epsilon the accuracy parameter (distance) must be the same unit of measurement as others parameters
+ * @return true if intersecting, otherwise false
+ */
+ public static boolean intersectsSolidSphereOrientedBox(float sphereCenterx, float sphereCentery, float sphereCenterz, float sphereRadius,
+ float boxCenterx, float boxCentery, float boxCenterz,
+ float boxAxis1x, float boxAxis1y, float boxAxis1z,
+ float boxAxis2x, float boxAxis2y, float boxAxis2z,
+ float boxAxis3x, float boxAxis3y, float boxAxis3z,
+ float boxExtentAxis1, float boxExtentAxis2, float boxExtentAxis3, float epsilon) {
+ // Find points on OBB closest and farest to sphere center
+ Point3f closest = new Point3f();
+ Point3f farest = new Point3f();
+
+ GeometryUtil.closestFarthestPointsOBBPoint( boxCenterx, boxCentery, boxCenterz,
+ boxAxis1x, boxAxis1y, boxAxis1z,
+ boxAxis2x, boxAxis2y, boxAxis2z,
+ boxAxis3x, boxAxis3y, boxAxis3z,
+ boxExtentAxis1, boxExtentAxis2, boxExtentAxis3,
+ sphereCenterx, sphereCentery, sphereCenterz,
+ closest,
+ farest);
+
+ // Sphere and OBB intersect if the (squared) distance from sphere
+ // center to point p is less than the (squared) sphere radius
+ float squaredRadius = sphereRadius * sphereRadius;
+
+ if (GeometryUtil.distanceSquaredPointPoint(sphereCenterx, sphereCentery, sphereCenterz,
+ closest.getX(), closest.getY(), closest.getZ())>squaredRadius+epsilon) return false;
+
+ return true;
+ }
+
+ /** Replies if the specified rectangle intersects the specified circle.
+ *
+ * @param circleCenter are the coordinates of the circle center.
+ * @param circleRadius is the radius of the circle.
+ * @param obrCenter is the center point of the OBR.
+ * @param obrAxis are the unit vectors of the OBR axis.
+ * @param obrExtent are the sizes of the OBR.
+ * @param epsilon the accuracy parameter (distance) must be the same unit of measurement as others parameters
+ * @return true if intersecting, otherwise false
+ */
+ public static boolean intersectsSolidCircleOrientedRectangle(float circleCenterx, float circleCentery, float circleRadius,
+ float obrCenterx, float obrCentery,
+ float obrAxis1x, float obrAxis1y,
+ float obrAxis2x, float obrAxis2y,
+ float obrExtentAxis1, float obrExtentAxis2, float epsilon) {
+ // Find points on OBR closest and farest to sphere center
+ Point2f closest = new Point2f();
+ Point2f farest = new Point2f();
+
+ GeometryUtil.closestFarthestPointsOBRPoint( circleCenterx, circleCentery,
+ obrCenterx, obrCentery,
+ obrAxis1x, obrAxis1y,
+ obrAxis2x, obrAxis2y,
+ obrExtentAxis1, obrExtentAxis2,
+ closest, farest);
+
+ // Circle and OBR intersect if the (squared) distance from sphere
+ // center to point p is less than the (squared) sphere radius
+ float squaredRadius = circleRadius * circleRadius;
+
+ if (GeometryUtil.distanceSquaredPointPoint(obrCenterx, obrCentery,
+ closest.getX(), closest.getY())>squaredRadius+epsilon) return false;
+
+ return true;
+ }
+
+ /** Replies if the specified boxes intersect.
+ *
+ * The extents are assumed to be positive or zero.
+ * The lengths of the given arrays are assumed to be 3.
+ *
+ * This function uses the "separating axis theorem" which states that
+ * for any two OBBs (AABB is a special case of OBB)
+ * that do not touch, a separating axis can be found.
+ *
+ * This function uses an general intersection test between two OBB.
+ * If the first box is expected to be an AAB, please use the
+ * optimized algorithm given by
+ * {@link #intersectsAlignedBoxOrientedBox(float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float)}.
+ *
+ * @param center1 is the center point of the first oriented box.
+ * @param axis1 are the unit vectors of the first oriented box axis.
+ * @param extent1 are the sizes of the first oriented box.
+ * @param center2 is the center point of the second oriented box.
+ * @param axis2 are the unit vectors of the second oriented box axis.
+ * @param extent2 are the sizes of the second oriented box.
+ * @return true if intersecting, otherwise false
+ * @see "RTCD pages 102-105"
+ * @see OBB collision detection on Gamasutra.com
+ */
+ public static boolean intersectsOrientedBoxeOrientedBoxe(float center1x, float center1y, float center1z,
+ float box1Axis1x, float box1Axis1y, float box1Axis1z,
+ float box1Axis2x, float box1Axis2y, float box1Axis2z,
+ float box1Axis3x, float box1Axis3y, float box1Axis3z,
+ float box1ExtentAxis1, float box1ExtentAxis2, float box1ExtentAxis3,
+ float center2x, float center2y, float center2z,
+ float box2Axis1x, float box2Axis1y, float box2Axis1z,
+ float box2Axis2x, float box2Axis2y, float box2Axis2z,
+ float box2Axis3x, float box2Axis3y, float box2Axis3z,
+ float box2ExtentAxis1, float box2ExtentAxis2, float box2ExtentAxis3){
+
+ //translation, in parent frame
+ float vx, vy, vz;
+ vx = center2x - center1x;
+ vy = center2y - center1y;
+ vz = center2z - center1z;
+
+ //translation, in A's frame
+ float tx,ty,tz;
+ tx = MathUtil.dotProduct(vx, vy, vz, box1Axis1x, box1Axis1y, box1Axis1z);
+ ty = MathUtil.dotProduct(vx, vy, vz, box1Axis2x, box1Axis2y, box1Axis2z);
+ tz = MathUtil.dotProduct(vx, vy, vz, box1Axis3x, box1Axis3y, box1Axis3z);
+
+ //B's basis with respect to A's local frame
+ float R_1_1, R_1_2, R_1_3,
+ R_2_1, R_2_2, R_2_3,
+ R_3_1, R_3_2, R_3_3,
+
+ absR_1_1, absR_1_2, absR_1_3,
+ absR_2_1, absR_2_2, absR_2_3,
+ absR_3_1, absR_3_2, absR_3_3;
+
+ R_1_1 = MathUtil.dotProduct(box1Axis1x, box1Axis1y,box1Axis1z,box2Axis1x, box2Axis1y,box2Axis1z); absR_1_1 = (R_1_1 < 0) ? -R_1_1 : R_1_1;
+ R_1_2 = MathUtil.dotProduct(box1Axis1x, box1Axis1y,box1Axis1z,box2Axis2x, box2Axis2y,box2Axis2z); absR_1_2 = (R_1_2 < 0) ? -R_1_2 : R_1_2;
+ R_1_3 = MathUtil.dotProduct(box1Axis1x, box1Axis1y,box1Axis1z,box2Axis3x, box2Axis3y,box2Axis3z); absR_1_3 = (R_1_3 < 0) ? -R_1_3 : R_1_3;
+ R_2_1 = MathUtil.dotProduct(box1Axis2x, box1Axis2y,box1Axis2z,box2Axis1x, box2Axis1y,box2Axis1z); absR_2_1 = (R_2_1 < 0) ? -R_2_1 : R_2_1;
+ R_2_2 = MathUtil.dotProduct(box1Axis2x, box1Axis2y,box1Axis2z,box2Axis2x, box2Axis2y,box2Axis2z); absR_2_2 = (R_2_2 < 0) ? -R_2_2 : R_2_2;
+ R_2_3 = MathUtil.dotProduct(box1Axis2x, box1Axis2y,box1Axis2z,box2Axis3x, box2Axis3y,box2Axis3z); absR_2_3 = (R_2_3 < 0) ? -R_2_3 : R_2_3;
+ R_3_1 = MathUtil.dotProduct(box1Axis3x, box1Axis3y,box1Axis3z,box2Axis1x, box2Axis1y,box2Axis1z); absR_3_1 = (R_3_1 < 0) ? -R_3_1 : R_3_1;
+ R_3_2 = MathUtil.dotProduct(box1Axis3x, box1Axis3y,box1Axis3z,box2Axis2x, box2Axis2y,box2Axis2z); absR_3_2 = (R_3_2 < 0) ? -R_3_2 : R_3_2;
+ R_3_3 = MathUtil.dotProduct(box1Axis3x, box1Axis3y,box1Axis3z,box2Axis3x, box2Axis3y,box2Axis3z); absR_3_3 = (R_3_3 < 0) ? -R_3_3 : R_3_3;
+
+ // ALGORITHM: Use the separating axis test for all 15 potential
+ // separating axes. If a separating axis could not be found, the two
+ // boxes overlap.
+ float ra, rb, t;
+
+ ra = box1ExtentAxis1;
+ rb = box2ExtentAxis1*absR_1_1+ box2ExtentAxis2*absR_1_2 + box2ExtentAxis3*absR_1_3;
+ t = Math.abs(tx);
+ if (t > ra + rb) return false;
+
+ ra = box1ExtentAxis2;
+ rb = box2ExtentAxis1*absR_2_1+ box2ExtentAxis2*absR_2_2 + box2ExtentAxis3*absR_3_3;
+ t = Math.abs(ty);
+ if (t > ra + rb) return false;
+
+ ra = box1ExtentAxis3;
+ rb = box2ExtentAxis1*absR_3_1+ box2ExtentAxis2*absR_3_2 + box2ExtentAxis3*absR_3_3;
+ t = Math.abs(tz);
+ if (t > ra + rb) return false;
+
+ //B's basis vectors
+ ra = box1ExtentAxis1*absR_1_1+ box1ExtentAxis2*absR_2_1 + box1ExtentAxis3*absR_3_1;
+ rb = box2ExtentAxis1;
+ t = Math.abs( tx*R_1_1 + ty*R_2_1 + tz*R_3_1 );
+ if (t > ra + rb) return false;
+
+ ra = box1ExtentAxis1*absR_1_2+ box1ExtentAxis2*absR_2_2 + box1ExtentAxis3*absR_3_2;
+ rb = box2ExtentAxis2;
+ t = Math.abs( tx*R_1_2 + ty*R_2_2 + tz*R_3_2 );
+ if (t > ra + rb) return false;
+
+ ra = box1ExtentAxis1*absR_1_3+ box1ExtentAxis2*absR_2_3 + box1ExtentAxis3*absR_3_3;
+ rb = box2ExtentAxis3;
+ t = Math.abs( tx*R_1_3 + ty*R_2_3 + tz*R_3_3 );
+ if (t > ra + rb) return false;
+
+ //9 cross products
+
+ //L = A0 x B0
+ ra = box1ExtentAxis1*absR_3_1 + box1ExtentAxis3*absR_2_1;
+ rb = box1ExtentAxis3*absR_1_3 + box1ExtentAxis3*absR_1_2;
+ t = Math.abs( tz*R_2_1 - ty*R_3_1 );
+ if (t > ra + rb) return false;
+
+
+ ra = box1ExtentAxis2*absR_3_1 + box1ExtentAxis3*absR_2_1;
+ rb = box1ExtentAxis3*absR_1_3 + box1ExtentAxis3*absR_1_2;
+ t = Math.abs( tz*R_2_1 - ty*R_3_1 );
+ if (t > ra + rb) return false;
+
+ //L = A0 x B1
+ ra = box1ExtentAxis2*absR_3_2 + box1ExtentAxis3*absR_2_2;
+ rb = box1ExtentAxis3*absR_1_3 + box1ExtentAxis3*absR_1_1;
+ t = Math.abs( tz*R_2_2 - ty*R_3_2 );
+ if (t > ra + rb) return false;
+
+ //L = A0 x B2
+ ra = box1ExtentAxis2*absR_3_3 + box1ExtentAxis3*absR_2_3;
+ rb = box1ExtentAxis3*absR_1_2 + box1ExtentAxis3*absR_1_1;
+ t = Math.abs( tz*R_2_3 - ty*R_3_3 );
+ if (t > ra + rb) return false;
+
+ //L = A1 x B0
+ ra = box1ExtentAxis1*absR_3_1 + box1ExtentAxis3*absR_1_1;
+ rb = box1ExtentAxis3*absR_2_3 + box1ExtentAxis3*absR_2_2;
+ t = Math.abs( tx*R_3_1 - tz*R_1_1 );
+ if (t > ra + rb) return false;
+
+ //L = A1 x B1
+ ra = box1ExtentAxis1*absR_3_2 + box1ExtentAxis3*absR_1_2;
+ rb = box1ExtentAxis3*absR_2_3 + box1ExtentAxis3*absR_2_1;
+ t = Math.abs( tx*R_3_2 - tz*R_1_2 );
+ if (t > ra + rb) return false;
+
+ //L = A1 x B2
+ ra = box1ExtentAxis1*absR_3_3 + box1ExtentAxis3*absR_1_3;
+ rb = box1ExtentAxis3*absR_2_2 + box1ExtentAxis3*absR_2_1;
+ t = Math.abs( tx*R_3_3 - tz*R_1_3 );
+ if (t > ra + rb) return false;
+
+ //L = A2 x B0
+ ra = box1ExtentAxis1*absR_2_1 + box1ExtentAxis2*absR_1_1;
+ rb = box1ExtentAxis3*absR_3_3 + box1ExtentAxis3*absR_3_2;
+ t = Math.abs( ty*R_1_1 - tx*R_2_1 );
+ if (t > ra + rb) return false;
+
+ //L = A2 x B1
+ ra = box1ExtentAxis1*absR_2_2 + box1ExtentAxis2*absR_1_2;
+ rb = box1ExtentAxis3*absR_3_3 + box1ExtentAxis3*absR_3_1;
+ t = Math.abs( ty*R_1_2 - tx*R_2_2 );
+ if (t > ra + rb) return false;
+
+ //L = A2 x B2
+ ra = box1ExtentAxis1*absR_2_3 + box1ExtentAxis2*absR_1_3;
+ rb = box1ExtentAxis3*absR_3_2 + box1ExtentAxis3*absR_3_1;
+ t = Math.abs( ty*R_1_3 - tx*R_2_3 );
+ if (t > ra + rb) return false;
+
+ /*no separating axis found, the two boxes overlap */
+ return true;
+
+ }
+
+ /** Replies if the specified rectangles intersect.
+ *
+ * The extents are assumed to be positive or zero.
+ * The lengths of the given arrays are assumed to be 2.
+ *
+ * This function uses the "separating axis theorem" which states that
+ * for any two OBBs (AABB is a special case of OBB)
+ * that do not touch, a separating axis can be found.
+ *
+ * This function uses an general intersection test between two OBR.
+ * If the first box is expected to be an MBR, please use the
+ * optimized algorithm given by
+ * {@link #intersectsAlignedRectangleOrientedRectangle(float, float, float, float, float, float, float, float, float, float, float, float)}.
+ *
+ * @param center1 is the center point of the first OBR.
+ * @param axis1 are the unit vectors of the first OBR axis.
+ * @param extent1 are the sizes of the first OBR.
+ * @param center2 is the center point of the second OBR.
+ * @param axis2 are the unit vectors of the second OBR .
+ * @param extent2 are the sizes of the second OBR.
+ * @return true if intersecting, otherwise false
+ * @see "RTCD pages 102-105"
+ * @see Intersection between two oriented boudning rectangles
+ */
+ public static boolean intersectsOrientedRectangleOrientedRectangle(
+ float center1x, float center1y, float rect1Axis1x, float rect1Axis1y, float rect1Axis2x, float rect1Axis2y, float rect1ExtentAxis1, float rect1ExtentAxis2,
+ float center2x, float center2y, float rect2Axis1x, float rect2Axis1y, float rect2Axis2x, float rect2Axis2y, float rect2ExtentAxis1, float rect2ExtentAxis2){
+ assert(rect1ExtentAxis1>=0);
+ assert(rect1ExtentAxis2>=0);
+ assert(rect2ExtentAxis1>=0);
+ assert(rect2ExtentAxis2>=0);
+
+ float tx, ty;
+ tx = center2x - center1x;
+ ty = center2y - center1y;
+
+ float scaledRect1Axis1x, scaledRect1Axis1y, scaledRect1Axis2x, scaledRect1Axis2y,
+ scaledRect2Axis1x, scaledRect2Axis1y, scaledRect2Axis2x, scaledRect2Axis2y;
+
+ scaledRect1Axis1x = rect1Axis1x * rect1ExtentAxis1;
+ scaledRect1Axis1y = rect1Axis1y * rect1ExtentAxis1;
+ scaledRect1Axis2x = rect1Axis2x * rect1ExtentAxis2;
+ scaledRect1Axis2y = rect1Axis2y * rect1ExtentAxis2;
+ scaledRect2Axis1x = rect2Axis1x * rect2ExtentAxis1;
+ scaledRect2Axis1y = rect2Axis1y * rect2ExtentAxis1;
+ scaledRect2Axis2x = rect2Axis2x * rect2ExtentAxis2;
+ scaledRect2Axis2y = rect2Axis2y * rect2ExtentAxis2;
+
+ //Let A the first box and B the second one
+ //L = Ax
+ if (Math.abs(MathUtil.dotProduct(tx, ty, rect1Axis1x, rect1Axis1y)) >
+ rect1ExtentAxis1 +
+ Math.abs(MathUtil.dotProduct(scaledRect2Axis1x, scaledRect2Axis1y, rect1Axis1x, rect1Axis1y)) +
+ Math.abs(MathUtil.dotProduct(scaledRect2Axis2x, scaledRect2Axis2y, rect1Axis1x, rect1Axis1y))
+ )
+ return false;
+
+ //L = Ay
+ if (Math.abs(MathUtil.dotProduct(tx, ty, rect1Axis2x, rect1Axis2y)) >
+ rect1ExtentAxis2 +
+ Math.abs(MathUtil.dotProduct(scaledRect2Axis1x, scaledRect2Axis1y, rect1Axis2x, rect1Axis2y)) +
+ Math.abs(MathUtil.dotProduct(scaledRect2Axis2x, scaledRect2Axis2y, rect1Axis2x, rect1Axis2y))
+ )
+ return false;
+
+ //L=Bx
+ if (Math.abs(MathUtil.dotProduct(tx, ty, rect2Axis1x, rect2Axis1y)) >
+ rect2ExtentAxis1 +
+ Math.abs(MathUtil.dotProduct(scaledRect1Axis1x, scaledRect1Axis1y, rect2Axis1x, rect2Axis1y)) +
+ Math.abs(MathUtil.dotProduct(scaledRect1Axis2x, scaledRect1Axis2y, rect2Axis1x, rect2Axis1y))
+ )
+ return false;
+
+ //L=By
+ if (Math.abs(MathUtil.dotProduct(tx, ty, rect2Axis2x, rect2Axis2y)) >
+ rect2ExtentAxis2 +
+ Math.abs(MathUtil.dotProduct(scaledRect1Axis1x, scaledRect1Axis1y, rect2Axis2x, rect2Axis2y)) +
+ Math.abs(MathUtil.dotProduct(scaledRect1Axis2x, scaledRect1Axis2y, rect2Axis2x, rect2Axis2y))
+ )
+ return false;
+
+ /*no separating axis found, the two boxes overlap */
+ return true;
+ }
+
+ /** Replies if the specified boxes intersect.
+ *
+ * This function is assuming that lx1 is lower
+ * or equal to ux1, ly1 is lower
+ * or equal to uy1, and so on.
+ * The extents are assumed to be positive or zero.
+ * The lengths of the given arrays are assumed to be 3.
+ *
+ * This function uses the "separating axis theorem" which states that
+ * for any two OBBs (AABB is a special case of OBB)
+ * that do not touch, a separating axis can be found.
+ *
+ * This function uses an optimized algorithm for AABB as first parameter.
+ * The general intersection type between two OBB is given by
+ * {@link #intersectsOrientedBoxeOrientedBoxe(float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float)}
+ *
+ * @param lower coordinates of the lowest point of the first AABB box.
+ * @param upper coordinates of the uppermost point of the first AABB box.
+ * @param center is the center point of the second oriented box.
+ * @param axis are the unit vectors of the second oriented box axis.
+ * @param extent are the sizes of the second oriented box.
+ * @return true if intersecting, otherwise false
+ * @see "RTCD pages 102-105"
+ * @see OBB collision detection on Gamasutra.com
+ */
+ public static boolean intersectsAlignedBoxOrientedBox(
+ float lowerx,float lowery,float lowerz,
+ float upperx,float uppery,float upperz,
+ float centerx,float centery,float centerz,
+ float axis1x, float axis1y, float axis1z,
+ float axis2x, float axis2y, float axis2z,
+ float axis3x, float axis3y, float axis3z,
+ float extentAxis1, float extentAxis2, float extentAxis3) {
+ assert(lowerx<=upperx);
+ assert(lowery<=uppery);
+ assert(lowerz<=upperz);
+ assert(extentAxis1>=0);
+ assert(extentAxis2>=0);
+ assert(extentAxis3>=0);
+
+ float aabbCenterx,aabbCentery,aabbCenterz;
+ aabbCenterx = (upperx+lowerx)/2.f;
+ aabbCentery = (uppery+lowery)/2.f;
+ aabbCenterz = (upperz+lowerz)/2.f;
+
+
+ return intersectsOrientedBoxeOrientedBoxe(
+ aabbCenterx, aabbCentery, aabbCenterz,
+ 1,0,0, //Axis 1
+ 0,1,0, //Axis 2
+ 0,0,1, //Axis 3
+ upperx - aabbCenterx, uppery - aabbCentery, upperz - aabbCenterz,
+ centerx, centery, centerz,
+ axis1x, axis1y, axis1z,
+ axis2x, axis2y, axis2z,
+ axis3x, axis3y, axis3z,
+ extentAxis1, extentAxis2, extentAxis3);
+ }
+
+ /** Replies if the specified rectangles intersect.
+ *
+ * This function is assuming that lx1 is lower
+ * or equal to ux1, and ly1 is lower
+ * or equal to uy1.
+ * The extents are assumed to be positive or zero.
+ * The lengths of the given arrays are assumed to be 2.
+ *
+ * This function uses the "separating axis theorem" which states that
+ * for any two OBBs (AABB is a special case of OBB)
+ * that do not touch, a separating axis can be found.
+ *
+ * This function uses an optimized algorithm for AABB as first parameter.
+ * The general intersection type between two OBB is given by
+ * {@link #intersectsOrientedBoxeOrientedBoxe(float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float, float)}
+ *
+ * @param lower coordinates of the lowest point of the first MBR.
+ * @param upper coordinates of the uppermost point of the first MBR.
+ * @param center is the center point of the second OBR.
+ * @param axis are the unit vectors of the second OBR axis.
+ * @param extent are the sizes of the second OBR.
+ * @return true if intersecting, otherwise false
+ * @see "RTCD pages 102-105"
+ * @see OBB collision detection on Gamasutra.com
+ */
+ public static boolean intersectsAlignedRectangleOrientedRectangle(
+ float lowerx,float lowery, float upperx,float uppery,
+ float centerx,float centery, float axis1x, float axis1y, float axis2x, float axis2y, float extentAxis1, float extentAxis2) {
+ assert(lowerx<=upperx);
+ assert(lowery<=uppery);
+ assert(extentAxis1>=0);
+ assert(extentAxis2>=0);
+
+ float mbrCenterx, mbrCentery;
+
+ mbrCenterx = (upperx+lowerx)/2.f;
+ mbrCentery = (uppery+lowery)/2.f;
+
+ return intersectsOrientedRectangleOrientedRectangle(
+ mbrCenterx, mbrCentery, 1, 0, 0, 1, upperx - mbrCenterx, uppery - mbrCentery,
+ centerx, centery, axis1x, axis1y, axis2x, axis2y, extentAxis1, extentAxis2);
+ }
+
+
+
+ /**
+ * Replies if the specified sphere intersects the specified capsule.
+ * @param sphereCenter - center of the sphere
+ * @param sphereRadius - radius of the sphere
+ * @param capsuleA - Medial line segment start point of the capsule
+ * @param capsuleB - Medial line segment end point of the capsule
+ * @param capsuleRadius - radius of the capsule
+ * @return true if intersecting, otherwise false
+ */
+ public static boolean intersectsSphereCapsule(float sphereCenterx, float sphereCentery, float sphereCenterz, float sphereRadius,
+ float capsuleAx, float capsuleAy, float capsuleAz,
+ float capsuleBx, float capsuleBy, float capsuleBz, float capsuleRadius){
+ // Compute (squared) distance between sphere center and capsule line segment
+
+ float dist2 = GeometryUtil.distanceSquaredPointSegment(sphereCenterx, sphereCentery, sphereCenterz, capsuleAx, capsuleAy, capsuleAz, capsuleBx, capsuleBy, capsuleBz);
+
+ // If (squared) distance smaller than (squared) sum of radii, they collide
+ float radius = sphereRadius + capsuleRadius;
+ return dist2 < radius * radius;
+ }
+
+ /**
+ * Replies if the specified capsules intersect
+ * @param capsule1A - Medial line segment start point of the first capsule
+ * @param capsule1B - Medial line segment end point of the first capsule
+ * @param capsule1Radius - radius of the first capsule
+ * @param capsule2A - Medial line segment start point of the second capsule
+ * @param capsule2B - Medial line segment end point of the second capsule
+ * @param capsule2Radius - radius of the second capsule
+ * @return true if intersecting, otherwise false
+ */
+ public static boolean intersectsCapsuleCapsule(
+ float capsule1Ax, float capsule1Ay, float capsule1Az, float capsule1Bx, float capsule1By, float capsule1Bz, float capsule1Radius,
+ float capsule2Ax, float capsule2Ay, float capsule2Az, float capsule2Bx, float capsule2By, float capsule2Bz, float capsule2Radius) {
+
+ float dist2 = GeometryUtil.closestPointsSegmentSegment(
+ capsule1Ax, capsule1Ay, capsule1Az, capsule1Bx, capsule1By, capsule1Bz,
+ capsule2Ax, capsule2Ay, capsule2Az, capsule2Bx, capsule2By, capsule2Bz,
+ null,null);
+
+ // If (squared) distance smaller than (squared) sum of radii, they collide
+ float radius = capsule1Radius + capsule2Radius;
+ return dist2 <= radius * radius;
+ }
+
+ /**
+ * Compute intersection between an OBB and a capsule
+ * @param center is the center point of the oriented box.
+ * @param axis are the unit vectors of the oriented box axis.
+ * @param extent are the sizes of the oriented box.
+ * @param capsule1A - capsule medial line segment start point
+ * @param capsule1B - capsule medial line segment end point
+ * @param capsule1Radius - capsule radius
+ * @param epsilon the accuracy parameter (distance) must be the same unit of measurement as others parameters
+ * @return true if intersecting, otherwise false
+ */
+ public static boolean intersectsOrientedBoxCapsule(
+ float centerx,float centery,float centerz,
+ float axis1x, float axis1y, float axis1z, float axis2x, float axis2y, float axis2z, float axis3x, float axis3y, float axis3z,
+ float extentAxis1, float extentAxis2, float extentAxis3,
+ float capsule1Ax, float capsule1Ay, float capsule1Az, float capsule1Bx, float capsule1By, float capsule1Bz, float capsule1Radius, float epsilon) {
+
+ Point3f closestFromA = new Point3f();
+ Point3f closestFromB = new Point3f();
+
+ GeometryUtil.closestFarthestPointsOBBPoint(
+ centerx, centery, centerz,axis1x, axis1y, axis1z, axis2x, axis2y, axis2z, axis3x, axis3y, axis3z,extentAxis1, extentAxis2, extentAxis3,
+ capsule1Ax, capsule1Ay, capsule1Az,
+ closestFromA, null);
+ GeometryUtil.closestFarthestPointsOBBPoint(
+ centerx, centery, centerz, axis1x, axis1y, axis1z, axis2x, axis2y, axis2z, axis3x, axis3y, axis3z, extentAxis1, extentAxis2, extentAxis3,
+ capsule1Bx, capsule1By, capsule1Bz,
+ closestFromB,null);
+
+ float distance = GeometryUtil.distanceSegmentSegment(capsule1Ax, capsule1Ay, capsule1Bx, capsule1By, closestFromA.getX(), closestFromA.getY(), closestFromB.getX(), closestFromB.getY(), epsilon);
+
+ return (distance <= capsule1Radius);
+ }
+
+ /**
+ * Compute intersection between a point and a capsule
+ * @param p - the point to test
+ * @param capsule1A - capsule medial line segment start point
+ * @param capsule1B - capsule medial line segment end point
+ * @param capsule1Radius - capsule radius
+ * @return true if intersecting, otherwise false
+ */
+ public static boolean intersectsPointCapsule(
+ float px, float py, float pz,
+ float capsule1Ax, float capsule1Ay, float capsule1Az, float capsule1Bx, float capsule1By, float capsule1Bz, float capsule1Radius) {
+
+
+ float distPointToCapsuleSegment = GeometryUtil.distancePointSegment(px,py,pz,capsule1Ax,capsule1Ay,capsule1Az,capsule1Bx,capsule1By,capsule1Bz);
+ return (distPointToCapsuleSegment <= capsule1Radius);
+ }
+
+ /** Replies if two circles are intersecting.
+ *
+ * @param x1 is the center of the first circle
+ * @param y1 is the center of the first circle
+ * @param radius1 is the radius of the first circle
+ * @param x2 is the center of the second circle
+ * @param y2 is the center of the second circle
+ * @param radius2 is the radius of the second circle
+ * @return true if the two shapes are intersecting; otherwise
+ * false
+ */
+ public static boolean intersectsCircleCircle(float x1, float y1, float radius1, float x2, float y2, float radius2) {
+ float r = radius1+radius2;
+ return GeometryUtil.distanceSquaredPointPoint(x1, y1, x2, y2) < (r*r);
+ }
+
+ /** Replies if a circle and a rectangle are intersecting.
+ *
+ * @param x1 is the center of the circle
+ * @param y1 is the center of the circle
+ * @param radius is the radius of the circle
+ * @param x2 is the first corner of the rectangle.
+ * @param y2 is the first corner of the rectangle.
+ * @param x3 is the second corner of the rectangle.
+ * @param y3 is the second corner of the rectangle.
+ * @return true if the two shapes are intersecting; otherwise
+ * false
+ */
+ public static boolean intersectsCircleRectangle(float x1, float y1, float radius, float x2, float y2, float x3, float y3) {
+ float dx;
+ if (x1x3) {
+ dx = x1 - x3;
+ }
+ else {
+ dx = 0f;
+ }
+ float dy;
+ if (y1y3) {
+ dy = y1 - y3;
+ }
+ else {
+ dy = 0f;
+ }
+ return (dx*dx+dy*dy) < (radius*radius);
+ }
+
+ /** Replies if a circle and a line are intersecting.
+ *
+ * @param x1 is the center of the circle
+ * @param y1 is the center of the circle
+ * @param radius is the radius of the circle
+ * @param x2 is the first point of the line.
+ * @param y2 is the first point of the line.
+ * @param x3 is the second point of the line.
+ * @param y3 is the second point of the line.
+ * @return true if the two shapes are intersecting; otherwise
+ * false
+ */
+ public static boolean intersectsCircleLine(float x1, float y1, float radius, float x2, float y2, float x3, float y3) {
+ float d = GeometryUtil.distanceSquaredPointLine(x1, y1, x2, y2, x3, y3);
+ return d<(radius*radius);
+ }
+
+ /** Replies if a circle and a segment are intersecting.
+ *
+ * @param x1 is the center of the circle
+ * @param y1 is the center of the circle
+ * @param radius is the radius of the circle
+ * @param x2 is the first point of the segment.
+ * @param y2 is the first point of the segment.
+ * @param x3 is the second point of the segment.
+ * @param y3 is the second point of the segment.
+ * @return true if the two shapes are intersecting; otherwise
+ * false
+ */
+ public static boolean intersectsCircleSegment(float x1, float y1, float radius, float x2, float y2, float x3, float y3) {
+ float d = GeometryUtil.distanceSquaredPointSegment(x1, y1, x2, y2, x3, y3, null);
+ return d<(radius*radius);
+ }
+
+ /** Replies if two ellipses are intersecting.
+ *
+ * @param x1 is the first corner of the first ellipse.
+ * @param y1 is the first corner of the first ellipse.
+ * @param x2 is the second corner of the first ellipse.
+ * @param y2 is the second corner of the first ellipse.
+ * @param x3 is the first corner of the second ellipse.
+ * @param y3 is the first corner of the second ellipse.
+ * @param x4 is the second corner of the second ellipse.
+ * @param y4 is the second corner of the second ellipse.
+ * @return true if the two shapes are intersecting; otherwise
+ * false
+ */
+ public static boolean intersectsEllipseEllipse(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4) {
+ float ell2w = Math.abs(x4 - x3);
+ float ell2h = Math.abs(y4 - y3);
+ float ellw = Math.abs(x2 - x1);
+ float ellh = Math.abs(y2 - y1);
+
+ if (ell2w <= 0f || ell2h <= 0f) return false;
+ if (ellw <= 0f || ellh <= 0f) return false;
+
+ // Normalize the second ellipse coordinates compared to the ellipse
+ // having a center at 0,0 and a radius of 0.5.
+ float normx0 = (x3 - x1) / ellw - 0.5f;
+ float normx1 = normx0 + ell2w / ellw;
+ float normy0 = (y3 - y1) / ellh - 0.5f;
+ float normy1 = normy0 + ell2h / ellh;
+
+ // find nearest x (left edge, right edge, 0.0)
+ // find nearest y (top edge, bottom edge, 0.0)
+ // if nearest x,y is inside circle of radius 0.5, then intersects
+ float nearx, neary;
+ if (normx0 > 0f) {
+ // center to left of X extents
+ nearx = normx0;
+ } else if (normx1 < 0f) {
+ // center to right of X extents
+ nearx = normx1;
+ } else {
+ nearx = 0f;
+ }
+ if (normy0 > 0f) {
+ // center above Y extents
+ neary = normy0;
+ } else if (normy1 < 0f) {
+ // center below Y extents
+ neary = normy1;
+ } else {
+ neary = 0f;
+ }
+ return (nearx * nearx + neary * neary) < 0.25f;
+ }
+
+ /** Replies if an ellipse and a line are intersecting.
+ *
+ * @param ex is the lowest corner of the ellipse.
+ * @param ey is the lowest corner of the ellipse.
+ * @param ew is the width of the ellipse.
+ * @param eh is the height of the ellipse.
+ * @param x1 is the first point of the line.
+ * @param y1 is the first point of the line.
+ * @param x2 is the second point of the line.
+ * @param y2 is the second point of the line.
+ * @return true if the two shapes are intersecting; otherwise
+ * false
+ * @see "http://blog.csharphelper.com/2012/09/24/calculate-where-a-line-segment-and-an-ellipse-intersect-in-c.aspx"
+ */
+ public static boolean intersectsEllipseLine(float ex, float ey, float ew, float eh, float x1, float y1, float x2, float y2) {
+ // If the ellipse or line segment are empty, return no intersections.
+ if (eh<=0f || ew<=0f) {
+ return false;
+ }
+
+ // Get the semimajor and semiminor axes.
+ float a = ew / 2f;
+ float b = eh / 2f;
+
+ // Translate so the ellipse is centered at the origin.
+ float ecx = ex + a;
+ float ecy = ey + b;
+ float px1 = x1 - ecx;
+ float py1 = y1 - ecy;
+ float px2 = x2 - ecx;
+ float py2 = y2 - ecy;
+
+ float sq_a = a*a;
+ float sq_b = b*b;
+ float vx = px2 - px1;
+ float vy = py2 - py1;
+
+ assert(sq_a!=0f && sq_b!=0f);
+
+ // Calculate the quadratic parameters.
+ float A = vx * vx / sq_a +
+ vy * vy / sq_b;
+ float B = 2f * px1 * vx / sq_a +
+ 2f * py1 * vy / sq_b;
+ float C = px1 * px1 / sq_a + py1 * py1 / sq_b - 1;
+
+ // Calculate the discriminant.
+ float discriminant = B * B - 4f * A * C;
+ return (discriminant>=0f);
+ }
+
+ /** Replies if an ellipse and a segment are intersecting.
+ *
+ * @param ex is the lowest corner of the ellipse.
+ * @param ey is the lowest corner of the ellipse.
+ * @param ew is the width of the ellipse.
+ * @param eh is the height of the ellipse.
+ * @param x1 is the first point of the segment.
+ * @param y1 is the first point of the segment.
+ * @param x2 is the second point of the segment.
+ * @param y2 is the second point of the segment.
+ * @return true if the two shapes are intersecting; otherwise
+ * false
+ * @see "http://blog.csharphelper.com/2012/09/24/calculate-where-a-line-segment-and-an-ellipse-intersect-in-c.aspx"
+ */
+ public static boolean intersectsEllipseSegment(float ex, float ey, float ew, float eh, float x1, float y1, float x2, float y2) {
+ // If the ellipse or line segment are empty, return no intersections.
+ if (eh<=0f || ew<=0f) {
+ return false;
+ }
+
+ // Get the semimajor and semiminor axes.
+ float a = ew / 2f;
+ float b = eh / 2f;
+
+ // Translate so the ellipse is centered at the origin.
+ float ecx = ex + a;
+ float ecy = ey + b;
+ float px1 = x1 - ecx;
+ float py1 = y1 - ecy;
+ float px2 = x2 - ecx;
+ float py2 = y2 - ecy;
+
+ float sq_a = a*a;
+ float sq_b = b*b;
+ float vx = px2 - px1;
+ float vy = py2 - py1;
+
+ assert(sq_a!=0f && sq_b!=0f);
+
+ // Calculate the quadratic parameters.
+ float A = vx * vx / sq_a + vy * vy / sq_b;
+ float B = 2f * px1 * vx / sq_a + 2f * py1 * vy / sq_b;
+ float C = px1 * px1 / sq_a + py1 * py1 / sq_b - 1;
+
+ // Calculate the discriminant.
+ float discriminant = B * B - 4f * A * C;
+ if (discriminant<0f) {
+ // No solution
+ return false;
+ }
+
+ if (discriminant==0f) {
+ // One real solution.
+ float t = -B / 2f / A;
+ return ((t >= 0f) && (t <= 1f));
+ }
+
+ assert(discriminant>0f);
+
+ // Two real solutions.
+ float t1 = (-B + (float)Math.sqrt(discriminant)) / 2f / A;
+ float t2 = (-B - (float)Math.sqrt(discriminant)) / 2f / A;
+
+ return (t1>=0 || t2>=0) && (t1<=1 || t2<=1);
+ }
+
+ /** Replies if two ellipses are intersecting.
+ *
+ * @param x1 is the first corner of the first ellipse.
+ * @param y1 is the first corner of the first ellipse.
+ * @param x2 is the second corner of the first ellipse.
+ * @param y2 is the second corner of the first ellipse.
+ * @param x3 is the first corner of the second rectangle.
+ * @param y3 is the first corner of the second rectangle.
+ * @param x4 is the second corner of the second rectangle.
+ * @param y4 is the second corner of the second rectangle.
+ * @return true if the two shapes are intersecting; otherwise
+ * false
+ */
+ public static boolean intersectsEllipseRectangle(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4) {
+ // From AWT Ellipse2D
+
+ float rectw = Math.abs(x4 - x3);
+ float recth = Math.abs(y4 - y3);
+ float ellw = Math.abs(x2 - x1);
+ float ellh = Math.abs(y2 - y1);
+
+ if (rectw <= 0f || recth <= 0f) return false;
+ if (ellw <= 0f || ellh <= 0f) return false;
+
+ // Normalize the rectangular coordinates compared to the ellipse
+ // having a center at 0,0 and a radius of 0.5.
+ float normx0 = (x3 - x1) / ellw - 0.5f;
+ float normx1 = normx0 + rectw / ellw;
+ float normy0 = (y3 - y1) / ellh - 0.5f;
+ float normy1 = normy0 + recth / ellh;
+ // find nearest x (left edge, right edge, 0.0)
+ // find nearest y (top edge, bottom edge, 0.0)
+ // if nearest x,y is inside circle of radius 0.5, then intersects
+ float nearx, neary;
+ if (normx0 > 0f) {
+ // center to left of X extents
+ nearx = normx0;
+ } else if (normx1 < 0f) {
+ // center to right of X extents
+ nearx = normx1;
+ } else {
+ nearx = 0f;
+ }
+ if (normy0 > 0f) {
+ // center above Y extents
+ neary = normy0;
+ } else if (normy1 < 0f) {
+ // center below Y extents
+ neary = normy1;
+ } else {
+ neary = 0f;
+ }
+ return (nearx * nearx + neary * neary) < 0.25f;
+ }
+
+ /** Replies if two rectangles are intersecting.
+ *
+ * @param x1 is the first corner of the first rectangle.
+ * @param y1 is the first corner of the first rectangle.
+ * @param x2 is the second corner of the first rectangle.
+ * @param y2 is the second corner of the first rectangle.
+ * @param x3 is the first corner of the second rectangle.
+ * @param y3 is the first corner of the second rectangle.
+ * @param x4 is the second corner of the second rectangle.
+ * @param y4 is the second corner of the second rectangle.
+ * @return true if the two shapes are intersecting; otherwise
+ * false
+ */
+ public static boolean intersectsRectangleRectangle(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4) {
+ assert(x1<=x2);
+ assert(y1<=y2);
+ assert(x3<=x4);
+ assert(y3<=y4);
+ return x2 > x3
+ &&
+ x1 < x4
+ &&
+ y2 > y3
+ &&
+ y1 < y4;
+ }
+
+ /** Replies if two rectangles are intersecting.
+ *
+ * @param x1 is the first corner of the rectangle.
+ * @param y1 is the first corner of the rectangle.
+ * @param x2 is the second corner of the rectangle.
+ * @param y2 is the second corner of the rectangle.
+ * @param x3 is the first point of the line.
+ * @param y3 is the first point of the line.
+ * @param x4 is the second point of the line.
+ * @param y4 is the second point of the line.
+ * @param epsilon the accuracy parameter (distance) must be the same unit of measurement as others parameters
+ * @return true if the two shapes are intersecting; otherwise
+ * false
+ */
+ public static boolean intersectsRectangleLine(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4, float epsilon) {
+ int a, b;
+ a = GeometryUtil.ccw(x3, y3, x4, y4, x1, y1, epsilon);
+ b = GeometryUtil.ccw(x3, y3, x4, y4, x2, y1, epsilon);
+ if (a!=b && b!=0) return true;
+ b = GeometryUtil.ccw(x3, y3, x4, y4, x2, y2, epsilon);
+ if (a!=b && b!=0) return true;
+ b = GeometryUtil.ccw(x3, y3, x4, y4, x1, y2, epsilon);
+ return (a!=b && b!=0);
+ }
+
+ /** Replies if two rectangles are intersecting.
+ *
+ * @param x1 is the first corner of the rectangle.
+ * @param y1 is the first corner of the rectangle.
+ * @param x2 is the second corner of the rectangle.
+ * @param y2 is the second corner of the rectangle.
+ * @param x3 is the first point of the segment.
+ * @param y3 is the first point of the segment.
+ * @param x4 is the second point of the segment.
+ * @param y4 is the second point of the segment.
+ * @return true if the two shapes are intersecting; otherwise
+ * false
+ */
+ public static boolean intersectsRectangleSegment(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4) {
+ float px1 = x3;
+ float py1 = y3;
+ float px2 = x4;
+ float py2 = y4;
+
+ // Cohen–Sutherland algorithm
+ int r1 = GeometryUtil.getCohenSutherlandCode(x1, y1, x2, y2, px1, py1);
+ int r2 = GeometryUtil.getCohenSutherlandCode(x1, y1, x2, y2, px2, py2);
+ boolean accept = false;
+
+ while (true) {
+ if ((r1 | r2)==0) {
+ // Bitwise OR is 0. Trivially accept and get out of loop
+ accept = true;
+ break;//to speed up the algorithm
+ }
+ if ((r1 & r2)!=0) {
+ // Bitwise AND is not 0. Trivially reject and get out of loop
+ break;
+ }
+
+ // failed both tests, so calculate the line segment to clip
+ // from an outside point to an intersection with clip edge
+ float x, y;
+
+ // At least one endpoint is outside the clip rectangle; pick it.
+ int outcodeOut = r1!=0 ? r1 : r2;
+
+ // Now find the intersection point;
+ // use formulas y = y0 + slope * (x - x0), x = x0 + (1 / slope) * (y - y0)
+ if ((outcodeOut & COHEN_SUTHERLAND_TOP)!=0) { // point is above the clip rectangle
+ x = px1 + (px2 - px1) * (y2 - py1) / (py2 - py1);
+ y = y2;
+ }
+ else if ((outcodeOut & COHEN_SUTHERLAND_BOTTOM)!=0) { // point is below the clip rectangle
+ x = px1 + (px2 - px1) * (y1- py1) / (py2 - py1);
+ y = y1;
+ }
+ else if ((outcodeOut & COHEN_SUTHERLAND_RIGHT)!=0) { // point is to the right of clip rectangle
+ y = py1 + (py2 - py1) * (x2 - px1) / (px2 - px1);
+ x = x2;
+ }
+ else {
+ //else if ((outcodeOut & CS_LEFT)!=0) { // point is to the left of clip rectangle
+ y = py1 + (py2 - py1) * (x1 - px1) / (px2 - px1);
+ x = x1;
+ }
+
+ //NOTE:*****************************************************************************************
+
+ /* if you follow this algorithm exactly(at least for c#), then you will fall into an infinite loop
+ in case a line crosses more than two segments. to avoid that problem, leave out the last else
+ if(outcodeOut & LEFT) and just make it else*/
+
+ //**********************************************************************************************
+
+ // Now we move outside point to intersection point to clip
+ // and get ready for next pass.
+ if (outcodeOut == r1) {
+ px1 = x;
+ py1 = y;
+ r1 = GeometryUtil.getCohenSutherlandCode(x1, y1, x2, y2, px1, py1);
+ }
+ else {
+ px2 = x;
+ py2 = y;
+ r2 = GeometryUtil.getCohenSutherlandCode(x1, y1, x2, y2, px2, py2);
+ }
+ }
+
+ return accept;
+ }
+
+ /** Replies if two lines are intersecting.
+ *
+ * @param x1 is the first point of the first line.
+ * @param y1 is the first point of the first line.
+ * @param x2 is the second point of the first line.
+ * @param y2 is the second point of the first line.
+ * @param x3 is the first point of the second line.
+ * @param y3 is the first point of the second line.
+ * @param x4 is the second point of the second line.
+ * @param y4 is the second point of the second line.
+ * @param epsilon the accuracy parameter (distance) must be the same unit of measurement as others parameters
+ * @return true if the two shapes are intersecting; otherwise
+ * false
+ */
+ public static boolean intersectsLineLine(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4, float epsilon) {
+ if (GeometryUtil.isParallelLines(x1, y1, x2, y2, x3, y3, x4, y4, epsilon)) {
+ return GeometryUtil.isCollinearPoints(x1, y1, x2, y2, x3, y3, epsilon);
+ }
+ return true;
+ }
+
+ /** Replies if a segment and a line are intersecting.
+ *
+ * @param x1 is the first point of the segment.
+ * @param y1 is the first point of the segment.
+ * @param x2 is the second point of the segment.
+ * @param y2 is the second point of the segment.
+ * @param x3 is the first point of the line.
+ * @param y3 is the first point of the line.
+ * @param x4 is the second point of the line.
+ * @param y4 is the second point of the line.
+ * @param epsilon the accuracy parameter (distance) must be the same unit of measurement as others parameters
+ * @return true if the two shapes are intersecting; otherwise
+ * false
+ */
+ public static boolean intersectsSegmentLine(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4, float epsilon) {
+ return (GeometryUtil.getPointSideOfLine(x3, y3, x4, y4, x1, y1, epsilon) *
+ GeometryUtil.getPointSideOfLine(x3, y3, x4, y4, x2, y2, epsilon)) <= 0;
+ }
+
+ private static boolean intersectsSSWE(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4) {
+ float vx1, vy1, vx2a, vy2a, vx2b, vy2b, f1, f2, sign;
+
+ vx1 = x2 - x1;
+ vy1 = y2 - y1;
+
+ // Based on CCW. It is different than MathUtil.ccw(); because
+ // this small algorithm is computing a ccw of 0 for colinear points.
+ vx2a = x3 - x1;
+ vy2a = y3 - y1;
+ f1 = vx2a * vy1 - vy2a * vx1;
+
+ vx2b = x4 - x1;
+ vy2b = y4 - y1;
+ f2 = vx2b * vy1 - vy2b * vx1;
+
+ // s = f1 * f2
+ //
+ // f1 f2 s intersect
+ // -1 -1 1 F
+ // -1 0 0 ON SEGMENT?
+ // -1 1 -1 T
+ // 0 -1 0 ON SEGMENT?
+ // 0 0 0 SEGMENT INTERSECTION?
+ // 0 1 1 ON SEGMENT?
+ // 1 -1 -1 T
+ // 1 0 0 ON SEGMENT?
+ // 1 1 1 F
+ sign = f1 * f2;
+
+ if (sign<0f) return true;
+ if (sign>0f) return false;
+
+ float squaredLength = vx1*vx1 + vy1*vy1;
+
+ if (f1==0f && f2==0f) {
+ // Projection of the point on the segment line:
+ // <0 -> means before first point
+ // >1 -> means after second point
+ // otherwhise on the segment.
+
+ f1 = (vx2a * vx1 + vy2a * vy1) / squaredLength;
+ f2 = (vx2b * vx1 + vy2b * vy1) / squaredLength;
+
+ // No intersection when:
+ // f1<0 && f2<0 or
+ // f1>1 && f2>1
+
+ return (f1>=0f || f2>=0) && (f1<=1f || f2<=1f);
+ }
+
+ if (f1==0f) {
+ // Projection of the point on the segment line:
+ // <0 -> means before first point
+ // >1 -> means after second point
+ // otherwhise on the segment.
+
+ f1 = (vx2a * vx1 + vy2a * vy1) / squaredLength;
+
+ // No intersection when:
+ // f1<=0 && f2<=0 or
+ // f1>=1 && f2>=1
+
+ return (f1>=0f && f1<=1f);
+ }
+
+ if (f2==0f) {
+ // Projection of the point on the segment line:
+ // <0 -> means before first point
+ // >1 -> means after second point
+ // otherwhise on the segment.
+
+ f2 = (vx2b * vx1 + vy2b * vy1) / squaredLength;
+
+ // No intersection when:
+ // f1<0 && f2<0 or
+ // f1>1 && f2>1
+
+ return (f2>=0 && f2<=1f);
+ }
+
+ return false;
+ }
+
+ private static boolean intersectsSSWoE(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4) {
+ float vx1, vy1, vx2a, vy2a, vx2b, vy2b, f1, f2, sign;
+
+ vx1 = x2 - x1;
+ vy1 = y2 - y1;
+
+ vx2a = x3 - x1;
+ vy2a = y3 - y1;
+ f1 = vx2a * vy1 - vy2a * vx1;
+
+ vx2b = x4 - x1;
+ vy2b = y4 - y1;
+ f2 = vx2b * vy1 - vy2b * vx1;
+
+ // s = f1 * f2
+ //
+ // f1 f2 s intersect
+ // -1 -1 1 F
+ // -1 0 0 F
+ // -1 1 -1 T
+ // 0 -1 0 F
+ // 0 0 0 SEGMENT INTERSECTION?
+ // 0 1 1 F
+ // 1 -1 -1 T
+ // 1 0 0 F
+ // 1 1 1 F
+
+ sign = f1 * f2;
+
+ if (sign<0f) return true;
+ if (sign>0f) return false;
+
+ if (f1==0f && f2==0f) {
+ // Projection of the point on the segment line:
+ // <0 -> means before first point
+ // >1 -> means after second point
+ // otherwhise on the segment.
+
+ float squaredLength = vx1*vx1 + vy1*vy1;
+
+ f1 = (vx2a * vx1 + vy2a * vy1) / squaredLength;
+ f2 = (vx2b * vx1 + vy2b * vy1) / squaredLength;
+
+ // No intersection when:
+ // f1<=0 && f2<=0 or
+ // f1>=1 && f2>=1
+
+ return (f1>0f || f2>0) && (f1<1f || f2<1f);
+ }
+
+ return false;
+ }
+
+ /** Replies if two segments are intersecting.
+ * This function considers that the ends of
+ * the segments are not intersecting.
+ * To include the ends of the segments in the intersection ranges, see
+ * {@link #intersectsSegmentSegmentWithEnds(float, float, float, float, float, float, float, float)}.
+ *
+ * @param x1 is the first point of the first segment.
+ * @param y1 is the first point of the first segment.
+ * @param x2 is the second point of the first segment.
+ * @param y2 is the second point of the first segment.
+ * @param x3 is the first point of the second segment.
+ * @param y3 is the first point of the second segment.
+ * @param x4 is the second point of the second segment.
+ * @param y4 is the second point of the second segment.
+ * @return true if the two shapes are intersecting; otherwise
+ * false
+ * @see #intersectsSegmentSegmentWithEnds(float, float, float, float, float, float, float, float)
+ */
+ public static boolean intersectsSegmentSegmentWithoutEnds(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4) {
+ boolean r;
+ r = intersectsSSWoE(x1, y1, x2, y2, x3, y3, x4, y4);
+ if (!r) return r;
+ return intersectsSSWoE(x3, y3, x4, y4, x1, y1, x2, y2);
+ }
+
+ /** Replies if two segments are intersecting.
+ * This function considers that the ends of
+ * the segments are intersecting.
+ * To ignore the ends of the segments, see
+ * {@link #intersectsSegmentSegmentWithoutEnds(float, float, float, float, float, float, float, float)}.
+ *
+ * @param x1 is the first point of the first segment.
+ * @param y1 is the first point of the first segment.
+ * @param x2 is the second point of the first segment.
+ * @param y2 is the second point of the first segment.
+ * @param x3 is the first point of the second segment.
+ * @param y3 is the first point of the second segment.
+ * @param x4 is the second point of the second segment.
+ * @param y4 is the second point of the second segment.
+ * @return true if the two shapes are intersecting; otherwise
+ * false
+ * @see #intersectsSegmentSegmentWithoutEnds(float, float, float, float, float, float, float, float)
+ */
+ public static boolean intersectsSegmentSegmentWithEnds(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4) {
+ boolean r;
+ r = intersectsSSWE(x1, y1, x2, y2, x3, y3, x4, y4);
+ if (!r) return r;
+ return intersectsSSWE(x3, y3, x4, y4, x1, y1, x2, y2);
+ }
+
+ public static boolean intersectsEllipseOrientedRectangle(
+ float ex, float ey, float ew, float eh,
+ float centerx,float centery, float axis1x, float axis1y, float axis2x, float axis2y, float extentAxis1, float extentAxis2){
+
+ // Get the semimajor and semiminor axes.
+ float a = ew/ 2f;
+ float b = eh/ 2f;
+
+ // Translate so the ellipse is centered at the origin.
+ centerx = centerx - (ex + a);
+ centery = centery - (ey + b);
+
+ return IntersectionUtil.intersectsSolidCircleOrientedRectangle(
+ 0, 0, 1,
+ centerx, centery, axis1x, axis1y, axis2x, axis2y, extentAxis1/(a*a), extentAxis2/(b*b), 0);
+ }
+
+ public static boolean intersectsSegmentOrientedRectangle(float ax,
+ float ay, float bx, float by, float cx, float cy, float rx,
+ float ry, float sx, float sy, float extentR, float extentS) {
+
+
+ //Changing Segment coordinate basis.
+ ax = (ax-cx) * sy - (ay-cy) * sx;
+ ay = (ay-cy) * rx - (ax-cx) * ry;
+ bx = (bx-cx) * sy - (by-cy) * sx;
+ by = (by-cy) * rx - (bx-cx) * ry;
+
+ return intersectsRectangleSegment(ax, ay, bx, by, -extentR, -extentS, extentR, extentS);
+ }
+
+}
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry/PointFilterXYIterable.java b/core/math/src/main/java/org/arakhne/afc/math/geometry/PointFilterXYIterable.java
new file mode 100644
index 000000000..669133db2
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry/PointFilterXYIterable.java
@@ -0,0 +1,62 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Christophe BOHRHAUER.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry;
+
+import java.util.Iterator;
+
+import org.arakhne.afc.math.geometry2d.Point2D;
+import org.arakhne.afc.math.geometry3d.Point3D;
+
+/**
+ * This class is an iterable which takes as
+ * parameters a collection of 3D points and
+ * replies 2D points with the x and y 3D coordinate inside.
+ *
+ * {@code Point2f.x = Point3d.x}
+ * {@code Point2f.y = Point3d.y}
+ *
+ * @author $Author: cbohrhauer$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+class PointFilterXYIterable implements Iterable {
+
+ private final Iterable original;
+
+ /**
+ * @param points are the points to iterate on.
+ */
+ public PointFilterXYIterable(Iterable points) {
+ assert(points!=null);
+ this.original = points;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public Iterator iterator() {
+ return new PointFilterXYIterator(this.original.iterator());
+ }
+
+}
+
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry/PointFilterXYIterator.java b/core/math/src/main/java/org/arakhne/afc/math/geometry/PointFilterXYIterator.java
new file mode 100644
index 000000000..7d27a2f23
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry/PointFilterXYIterator.java
@@ -0,0 +1,88 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Christophe BOHRHAUER.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry;
+
+import java.util.Iterator;
+
+import org.arakhne.afc.math.geometry2d.Point2D;
+import org.arakhne.afc.math.geometry2d.continuous.Point2f;
+import org.arakhne.afc.math.geometry2d.discrete.Point2i;
+import org.arakhne.afc.math.geometry3d.Point3D;
+import org.arakhne.afc.math.geometry3d.continuous.Point3f;
+
+
+/**
+ * This class is an iterator which takes as
+ * parameters a collection of 3D points and
+ * replies 2D points with the x and y 3D coordinate inside.
+ *
+ * {@code Point2f.x = Point3d.x}
+ * {@code Point2f.y = Point3d.y}
+ *
+ * @author $Author: cbohrhauer$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+class PointFilterXYIterator
+implements Iterator {
+
+ private final Iterator original;
+
+ /**
+ * @param points are the points to iterate on.
+ */
+ public PointFilterXYIterator(Iterator points) {
+ assert(points!=null);
+ this.original = points;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean hasNext() {
+ return this.original.hasNext();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public Point2D next() {
+ Point3D p = this.original.next();
+ assert(p!=null);
+ if (p instanceof Point3f) {
+ return new Point2f(p.getX(), p.getY());
+ }
+ return new Point2i(p.x(), p.y());
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void remove() {
+ this.original.remove();
+ }
+
+}
+
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry/PointFilterXZIterable.java b/core/math/src/main/java/org/arakhne/afc/math/geometry/PointFilterXZIterable.java
new file mode 100644
index 000000000..359ea7ec4
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry/PointFilterXZIterable.java
@@ -0,0 +1,63 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Christophe BOHRHAUER.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry;
+
+import java.util.Iterator;
+
+import org.arakhne.afc.math.geometry2d.Point2D;
+import org.arakhne.afc.math.geometry3d.Point3D;
+
+/**
+ * This class is an iterable which takes as
+ * parameters a collection of 3D points and
+ * replies 2D points with the x and z 3D coordinate inside.
+ *
+ * {@code Point2f.x = Point3d.x}
+ * {@code Point2f.y = Point3d.z}
+ *
+ * @author $Author: cbohrhauer$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+class PointFilterXZIterable
+implements Iterable {
+
+ private final Iterable original;
+
+ /**
+ * @param points are the points to iterate on.
+ */
+ public PointFilterXZIterable(Iterable points) {
+ assert(points!=null);
+ this.original = points;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public Iterator iterator() {
+ return new PointFilterXZIterator(this.original.iterator());
+ }
+
+}
+
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry/PointFilterXZIterator.java b/core/math/src/main/java/org/arakhne/afc/math/geometry/PointFilterXZIterator.java
new file mode 100644
index 000000000..8a44534a2
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry/PointFilterXZIterator.java
@@ -0,0 +1,87 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Christophe BOHRHAUER.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry;
+
+import java.util.Iterator;
+
+import org.arakhne.afc.math.geometry2d.Point2D;
+import org.arakhne.afc.math.geometry2d.continuous.Point2f;
+import org.arakhne.afc.math.geometry2d.discrete.Point2i;
+import org.arakhne.afc.math.geometry3d.Point3D;
+import org.arakhne.afc.math.geometry3d.continuous.Point3f;
+
+/**
+ * This class is an iterator which takes as
+ * parameters a collection of 3D points and
+ * replies 2D points with the x and z 3D coordinate inside.
+ *
+ * {@code Point2f.x = Point3d.x}
+ * {@code Point2f.y = Point3d.z}
+ *
+ * @author $Author: cbohrhauer$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+class PointFilterXZIterator
+implements Iterator {
+
+ private final Iterator original;
+
+ /**
+ * @param points are the points to iterate on.
+ */
+ public PointFilterXZIterator(Iterator points) {
+ assert(points!=null);
+ this.original = points;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean hasNext() {
+ return this.original.hasNext();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public Point2D next() {
+ Point3D p = this.original.next();
+ assert(p!=null);
+ if (p instanceof Point3f) {
+ return new Point2f(p.getX(), p.getZ());
+ }
+ return new Point2i(p.x(), p.z());
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void remove() {
+ this.original.remove();
+ }
+
+}
+
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry/PointFilterYZIterable.java b/core/math/src/main/java/org/arakhne/afc/math/geometry/PointFilterYZIterable.java
new file mode 100644
index 000000000..dff53e74d
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry/PointFilterYZIterable.java
@@ -0,0 +1,63 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Christophe BOHRHAUER.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry;
+
+import java.util.Iterator;
+
+import org.arakhne.afc.math.geometry2d.Point2D;
+import org.arakhne.afc.math.geometry3d.Point3D;
+
+/**
+ * This class is an iterable which takes as
+ * parameters a collection of 3D points and
+ * replies 2D points with the y and z 3D coordinate inside.
+ *
+ * {@code Point2f.x = Point3d.y}
+ * {@code Point2f.y = Point3d.z}
+ *
+ * @author $Author: cbohrhauer$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+class PointFilterYZIterable
+implements Iterable {
+
+ private final Iterable original;
+
+ /**
+ * @param points are the points to iterate on.
+ */
+ public PointFilterYZIterable(Iterable points) {
+ assert(points!=null);
+ this.original = points;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public Iterator iterator() {
+ return new PointFilterYZIterator(this.original.iterator());
+ }
+
+}
+
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry/PointFilterYZIterator.java b/core/math/src/main/java/org/arakhne/afc/math/geometry/PointFilterYZIterator.java
new file mode 100644
index 000000000..8de4e79d8
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry/PointFilterYZIterator.java
@@ -0,0 +1,87 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Christophe BOHRHAUER.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry;
+
+import java.util.Iterator;
+
+import org.arakhne.afc.math.geometry2d.Point2D;
+import org.arakhne.afc.math.geometry2d.continuous.Point2f;
+import org.arakhne.afc.math.geometry2d.discrete.Point2i;
+import org.arakhne.afc.math.geometry3d.Point3D;
+import org.arakhne.afc.math.geometry3d.continuous.Point3f;
+
+/**
+ * This class is an iterator which takes as
+ * parameters a collection of 3D points and
+ * replies 2D points with the y and z 3D coordinate inside.
+ *
+ * {@code Point2f.x = Point3d.y}
+ * {@code Point2f.y = Point3d.z}
+ *
+ * @author $Author: cbohrhauer$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+class PointFilterYZIterator
+implements Iterator {
+
+ private final Iterator original;
+
+ /**
+ * @param points are the points to iterate on.
+ */
+ public PointFilterYZIterator(Iterator points) {
+ assert(points!=null);
+ this.original = points;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean hasNext() {
+ return this.original.hasNext();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public Point2D next() {
+ Point3D p = this.original.next();
+ assert(p!=null);
+ if (p instanceof Point3f) {
+ return new Point2f(p.getY(), p.getZ());
+ }
+ return new Point2i(p.y(), p.z());
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void remove() {
+ this.original.remove();
+ }
+
+}
+
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry/system/CoordinateSystem.java b/core/math/src/main/java/org/arakhne/afc/math/geometry/system/CoordinateSystem.java
new file mode 100644
index 000000000..f7f35b7ac
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry/system/CoordinateSystem.java
@@ -0,0 +1,65 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2013 Christophe BOHRHAUER.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry.system;
+
+/**
+ * Represents a space referencial and provides the convertion utilities.
+ *
+ * @author $Author: cbohrhauer$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public interface CoordinateSystem {
+
+ /** Replies the count of dimension in this space referential.
+ *
+ * @return the count of dimensions.
+ */
+ public float getDimensions();
+
+ /** Replies if this coordinate system is a right-hand coordinate system.
+ *
+ * @return true if this coordinate system is right-handed, otherwise false
+ */
+ public boolean isRightHanded();
+
+ /** Replies if this coordinate system is a left-hand coordinate system.
+ *
+ * @return true if this coordinate system is left-handed, otherwise false
+ */
+ public boolean isLeftHanded();
+
+ /** Replies the name of the coordinate system.
+ *
+ * @return the name of the coordinate system.
+ */
+ public String name();
+
+ /** Replies the index of this coordinate in the enumeration of the available
+ * coordinate systems of the same type.
+ *
+ * @return the index of this coordinate system in the enumeration of the
+ * available coordinate systems of the same type.
+ */
+ public int ordinal();
+
+}
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry/system/CoordinateSystem2D.java b/core/math/src/main/java/org/arakhne/afc/math/geometry/system/CoordinateSystem2D.java
new file mode 100644
index 000000000..7accfd11c
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry/system/CoordinateSystem2D.java
@@ -0,0 +1,401 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2013 Christophe BOHRHAUER.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry.system;
+
+import org.arakhne.afc.math.geometry2d.continuous.Point2f;
+import org.arakhne.afc.math.geometry2d.continuous.Transform2D;
+import org.arakhne.afc.math.geometry2d.continuous.Vector2f;
+
+
+/**
+ * Represents the different kind of 2D referencials
+ * and provides the convertion utilities.
+ *
+ * A referencial axis is expressed by the front and left directions.
+ * For example XY_LEFT_HAND is for the coordinate system
+ * with front direction along +X axis,
+ * and left direction along the -Y axis according to
+ * a "left-hand" heuristic.
+ *
+ * The default coordinate system is:
+ *
+ * - front:
(1,0)
+ * - left:
(0,1)
+ *
+ *
+ * @author $Author: cbohrhauer$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public enum CoordinateSystem2D implements CoordinateSystem {
+
+ /** Right handed XY coordinate system.
+ *
+ * ![[Right Handed XY Coordinate System]](doc-files/xy_right.png)
+ */
+ XY_RIGHT_HAND,
+
+ /** Left handed XY coordinate system.
+ *
+ * ![[Left Handed XY Coordinate System]](doc-files/xy_left.png)
+ */
+ XY_LEFT_HAND;
+
+ private static CoordinateSystem2D defaultCoordinateSystem;
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public final float getDimensions() {
+ return 2f;
+ }
+
+ /** Convert the specified point into from the current coordinate system
+ * to the specified coordinate system.
+ *
+ * @param point is the point to convert
+ * @param targetCoordinateSystem is the target coordinate system.
+ */
+ public void toSystem(Point2f point, CoordinateSystem2D targetCoordinateSystem) {
+ if (this!=targetCoordinateSystem) {
+ point.setY(-point.getY());
+ }
+ }
+
+ /** Convert the specified point into from the current coordinate system
+ * to the specified coordinate system.
+ *
+ * @param point is the point to convert
+ * @param targetCoordinateSystem is the target coordinate system.
+ */
+ public void toSystem(Vector2f point, CoordinateSystem2D targetCoordinateSystem) {
+ if (this!=targetCoordinateSystem) {
+ point.setY(-point.getY());
+ }
+ }
+
+ /** Convert the specified rotation into from the current coordinate system
+ * to the specified coordinate system.
+ *
+ * @param rotation is the rotation to convert
+ * @param targetCoordinateSystem is the target coordinate system.
+ * @return the converted rotation
+ */
+ public float toSystem(float rotation, CoordinateSystem2D targetCoordinateSystem) {
+ if (this!=targetCoordinateSystem) {
+ return -rotation;
+ }
+ return rotation;
+ }
+
+ /** Convert the specified transformation matrix into from the current coordinate system
+ * to the specified coordinate system.
+ *
+ * @param matrix is the matrix to convert
+ * @param targetCoordinateSystem is the target coordinate system.
+ */
+ public void toSystem(Transform2D matrix, CoordinateSystem2D targetCoordinateSystem) {
+ if (this!=targetCoordinateSystem) {
+ float r = -matrix.getRotation();
+ matrix.setRotation(r);
+ matrix.m21 = -matrix.m21;
+ }
+ }
+
+ /** Convert the specified point into the default coordinate system.
+ *
+ * @param point is the point to convert
+ */
+ public void toDefault(Point2f point) {
+ if (this!=getDefaultCoordinateSystem()) {
+ point.setY(-point.getY());
+ }
+ }
+
+ /** Convert the specified point from the default coordinate system.
+ *
+ * @param point is the point to convert
+ */
+ public void fromDefault(Point2f point) {
+ if (this!=getDefaultCoordinateSystem()) {
+ point.setY(-point.getY());
+ }
+ }
+
+ /** Convert the specified vector from the default coordinate system.
+ *
+ * @param vector is the vector to convert
+ */
+ public void toDefault(Vector2f vector) {
+ if (this!=getDefaultCoordinateSystem()) {
+ vector.setY(-vector.getY());
+ }
+ }
+
+ /** Convert the specified vector from the default coordinate system.
+ *
+ * @param vector is the vector to convert
+ */
+ public void fromDefault(Vector2f vector) {
+ if (this!=getDefaultCoordinateSystem()) {
+ vector.setY(-vector.getY());
+ }
+ }
+
+ /** Convert the specified transformation matrix from the default coordinate system.
+ *
+ * @param matrix is the matrix to convert
+ * @since 4.0
+ */
+ public void fromDefault(Transform2D matrix) {
+ if (this!=getDefaultCoordinateSystem()) {
+ float r = -matrix.getRotation();
+ matrix.setRotation(r);
+ matrix.m21 = -matrix.m21;
+ }
+ }
+
+ /** Convert the specified transformation matrix from the default coordinate system.
+ *
+ * @param matrix is the matrix to convert
+ */
+ public void toDefault(Transform2D matrix) {
+ if (this!=getDefaultCoordinateSystem()) {
+ float r = -matrix.getRotation();
+ matrix.setRotation(r);
+ matrix.m21 = -matrix.m21;
+ }
+ }
+
+ /** Convert the specified rotation into the default coordinate system.
+ *
+ * @param rotation is the rotation to convert
+ * @return the converted rotation
+ */
+ public float toDefault(float rotation) {
+ if (this!=getDefaultCoordinateSystem()) {
+ return -rotation;
+ }
+ return rotation;
+ }
+
+ /** Convert the specified rotation from the default coordinate system.
+ *
+ * @param rotation is the rotation to convert
+ * @return the converted rotation
+ */
+ public float fromDefault(float rotation) {
+ if (this!=getDefaultCoordinateSystem()) {
+ return -rotation;
+ }
+ return rotation;
+ }
+
+ /** Replies the default coordinate system.
+ *
+ * @return the default coordinate system.
+ * @see #setDefaultCoordinateSystem(CoordinateSystem2D)
+ */
+ public static CoordinateSystem2D getDefaultCoordinateSystem() {
+ if (defaultCoordinateSystem==null) return CoordinateSystemConstants.SIMULATION_2D;
+ return defaultCoordinateSystem;
+ }
+
+ /** Set the default coordinate system.
+ *
+ * @param system is the new default coordinate system.
+ * @see #getDefaultCoordinateSystem()
+ */
+ public static void setDefaultCoordinateSystem(CoordinateSystem2D system) {
+ defaultCoordinateSystem = system;
+ }
+
+ /** Replies the coordinate system which is corresponding to the specified
+ * orientation unit vectors.
+ *
+ * The front vector is assumed to be always (1,0),
+ * and the left vector is (0,ly).
+ *
+ * @param ly
+ * @return the coordinate system which is corresponding to the specified vector.
+ */
+ public static CoordinateSystem2D fromVectors(int ly) {
+ assert(ly!=0);
+ return (ly<0) ? XY_LEFT_HAND : XY_RIGHT_HAND;
+ }
+
+ /** Replies the coordinate system which is corresponding to the specified
+ * orientation unit vectors.
+ *
+ * The front vector is assumed to be always (1,0),
+ * and the left vector is (0,ly).
+ *
+ * @param ly
+ * @return the coordinate system which is corresponding to the specified vector.
+ */
+ public static CoordinateSystem2D fromVectors(float ly) {
+ return fromVectors((int)ly);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public boolean isRightHanded() {
+ return this==XY_RIGHT_HAND;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public boolean isLeftHanded() {
+ return this==XY_LEFT_HAND;
+ }
+
+ /** Replies the front vector.
+ *
+ * @return the front vector.
+ */
+ public static Vector2f getViewVector() {
+ return new Vector2f(1,0);
+ }
+
+ /** Replies the front vector.
+ *
+ * @param vectorToFill is the vector to set with the front vector values.
+ * @return the front vector.
+ * @since 4.0
+ */
+ public static Vector2f getViewVector(Vector2f vectorToFill) {
+ assert(vectorToFill!=null);
+ vectorToFill.set(1,0);
+ return vectorToFill;
+ }
+
+ /** Replies the back vector.
+ *
+ * @return the back vector.
+ */
+ public static Vector2f getBackVector() {
+ return new Vector2f(-1,0);
+ }
+
+ /** Replies the back vector.
+ *
+ * @param vectorToFill is the vector to set with the back vector values.
+ * @return the back vector.
+ * @since 4.0
+ */
+ public static Vector2f getBackVector(Vector2f vectorToFill) {
+ assert(vectorToFill!=null);
+ vectorToFill.set(-1,0);
+ return vectorToFill;
+ }
+
+ /** Replies the left vector.
+ *
+ * @return the left vector.
+ */
+ public Vector2f getLeftVector() {
+ switch(this) {
+ case XY_LEFT_HAND:
+ return new Vector2f(0,-1);
+ case XY_RIGHT_HAND:
+ return new Vector2f(0,1);
+ default:
+ throw new IllegalArgumentException("this"); //$NON-NLS-1$
+ }
+ }
+
+ /** Replies the left vector.
+ *
+ * @param vectorToFill is the vector to set with the left vector values.
+ * @return the left vector.
+ * @since 4.0
+ */
+ public Vector2f getLeftVector(Vector2f vectorToFill) {
+ assert(vectorToFill!=null);
+ switch(this) {
+ case XY_LEFT_HAND:
+ vectorToFill.set(0,-1);
+ return vectorToFill;
+ case XY_RIGHT_HAND:
+ vectorToFill.set(0,1);
+ return vectorToFill;
+ default:
+ throw new IllegalArgumentException("this"); //$NON-NLS-1$
+
+ }
+ }
+
+ /** Replies the right vector.
+ *
+ * @return the right vector.
+ */
+ public Vector2f getRightVector() {
+ switch(this) {
+ case XY_LEFT_HAND:
+ return new Vector2f(0,1);
+ case XY_RIGHT_HAND:
+ return new Vector2f(0,-1);
+ default:
+ throw new IllegalArgumentException("this"); //$NON-NLS-1$
+ }
+ }
+
+ /** Replies the right vector.
+ *
+ * @param vectorToFill is the vector to set with the right vector values.
+ * @return the right vector.
+ * @since 4.0
+ */
+ public Vector2f getRightVector(Vector2f vectorToFill) {
+ assert(vectorToFill!=null);
+ switch(this) {
+ case XY_LEFT_HAND:
+ vectorToFill.set(0,1);
+ return vectorToFill;
+ case XY_RIGHT_HAND:
+ vectorToFill.set(0,-1);
+ return vectorToFill;
+ default:
+ throw new IllegalArgumentException("this"); //$NON-NLS-1$
+ }
+ }
+
+ /** Replies the 3D coordinate system which is corresponding to
+ * this 2D coordinate system.
+ *
+ * @return the 3D coordinate system
+ * @since 4.0
+ */
+ public CoordinateSystem3D toCoordinateSystem3D() {
+ switch(this) {
+ case XY_LEFT_HAND:
+ return CoordinateSystem3D.XYZ_LEFT_HAND;
+ case XY_RIGHT_HAND:
+ return CoordinateSystem3D.XYZ_RIGHT_HAND;
+ default:
+ throw new CoordinateSystemNotFoundException();
+ }
+ }
+
+}
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry/system/CoordinateSystem3D.java b/core/math/src/main/java/org/arakhne/afc/math/geometry/system/CoordinateSystem3D.java
new file mode 100644
index 000000000..509fb01e4
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry/system/CoordinateSystem3D.java
@@ -0,0 +1,1205 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2013 Christophe BOHRHAUER.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry.system;
+
+import org.arakhne.afc.math.MathConstants;
+import org.arakhne.afc.math.MathUtil;
+import org.arakhne.afc.math.geometry.GeometryUtil;
+import org.arakhne.afc.math.geometry2d.Point2D;
+import org.arakhne.afc.math.geometry2d.continuous.Point2f;
+import org.arakhne.afc.math.geometry2d.continuous.Transform2D;
+import org.arakhne.afc.math.geometry2d.continuous.Vector2f;
+import org.arakhne.afc.math.geometry3d.Point3D;
+import org.arakhne.afc.math.geometry3d.continuous.Point3f;
+import org.arakhne.afc.math.geometry3d.continuous.Quaternion;
+import org.arakhne.afc.math.geometry3d.continuous.Transform3D;
+import org.arakhne.afc.math.geometry3d.continuous.Tuple3f;
+import org.arakhne.afc.math.geometry3d.continuous.Vector3f;
+
+/**
+ * Represents the different kind of 3D referencials
+ * and provides the convertion utilities.
+ *
+ * A referencial axis is expressed by the front, left and top directions.
+ * For example XYZ_LEFT_HAND is for the coordinate system
+ * with front direction along +/-X axis,
+ * left direction along the +/-Y axis
+ * and top direction along the +/-Z axis according to
+ * a "left-hand" heuristic.
+ *
+ * The default coordinate system is:
+ *
+ * - front:
(1,0,0)
+ * - left:
(0,1,0)
+ * - top:
(0,0,1)
+ *
+ *
+ * Rotations
+ *
+ * Rotations in a 3D coordinate system follow the right/left hand rules
+ * (assuming that OX, OY and OZ are the three axis of the coordinate system):
+ *
+ *
+ * | Right-handed rule: |
+ *
+ * - axis cycle is:
OX > OY > OZ > OX > OY;
+ * - when rotating around
OX, positive rotation angle is going from OY to OZ
+ * - when rotating around
OY, positive rotation angle is going from OZ to OX
+ * - when rotating around
OZ, positive rotation angle is going from OX to OY
+ *
+ * ![[Right-handed Rotation Rule]](doc-files/rotation_right.png)
+ * |
+ *
+ * | Left-handed rule: |
+ *
+ * - axis cycle is:
OX > OY > OZ > OX > OY;
+ * - when rotating around
OX, positive rotation angle is going from OY to OZ
+ * - when rotating around
OY, positive rotation angle is going from OZ to OX
+ * - when rotating around
OZ, positive rotation angle is going from OX to OY
+ *
+ * ![[Left-handed Rotation Rule]](doc-files/rotation_left.png)
+ * |
+ *
+ *
+ * @author $Author: sgalland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public enum CoordinateSystem3D implements CoordinateSystem {
+
+ /** Left handed XZY coordinate system.
+ *
+ * ![[Left Handed XZY Coordinate System]](doc-files/xzy_left.png)
+ */
+ XZY_LEFT_HAND(0,1,/* */1,0),
+
+ /** Left handed XYZ coordinate system.
+ *
+ * ![[Left Handed XYZ Coordinate System]](doc-files/xyz_left.png)
+ */
+ XYZ_LEFT_HAND(-1,0,/* */0,1),
+
+ /** Right handed XZY coordinate system.
+ *
+ * ![[Right Handed XZY Coordinate System]](doc-files/xzy_right.png)
+ */
+ XZY_RIGHT_HAND(0,-1,/* */1,0),
+
+ /** Right handed XYZ coordinate system.
+ *
+ * ![[Right Handed XYZ Coordinate System]](doc-files/xyz_right.png)
+ */
+ XYZ_RIGHT_HAND(1,0,/* */0,1);
+
+ private static final byte PIVOT_SYSTEM = 0;
+
+ private static CoordinateSystem3D userDefault;
+
+ private final byte system;
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public final float getDimensions() {
+ return 3f;
+ }
+
+ private static byte toSystemIndex(int lefty, int leftz, int topy, int topz) {
+ if (lefty<0) {
+ if (leftz == 0 && topy == 0 && topz!=0) {
+ if (topz<0) return 1;
+ return 2;
+ }
+ }
+ else if (lefty>0) {
+ if (leftz == 0 && topy == 0 && topz!=0) {
+ if (topz<0) return 3;
+ return 0;
+ }
+ }
+ else {
+ if (lefty==0 && leftz!=0) {
+ if (leftz<0) {
+ if (topz==0 && topy!=0) {
+ if (topy<0) return 4;
+ return 5;
+ }
+ }
+ else {
+ if (topz==0 && topy!=0) {
+ if (topy<0) return 6;
+ return 7;
+ }
+ }
+ }
+ }
+ throw new CoordinateSystemNotFoundException();
+ }
+
+ private static float[] fromSystemIndex(int index) {
+ // Compute the lower right sub-matrix
+ float c1, c2, c3, c4;
+ switch(index) {
+ case 1:
+ c1 = -1; c2 = 0; c3 = 0; c4 = -1;
+ break;
+ case 2:
+ c1 = -1; c2 = 0; c3 = 0; c4 = 1;
+ break;
+ case 3:
+ c1 = 1; c2 = 0; c3 = 0; c4 = -1;
+ break;
+ case 4:
+ c1 = 0; c2 = -1; c3 = -1; c4 = 0;
+ break;
+ case 5:
+ c1 = 0; c2 = -1; c3 = 1; c4 = 0;
+ break;
+ case 6:
+ c1 = 0; c2 = 1; c3 = -1; c4 = 0;
+ break;
+ case 7:
+ c1 = 0; c2 = 1; c3 = 1; c4 = 0;
+ break;
+ default: //0
+ c1 = 1; c2 = 0; c3 = 0; c4 = 1;
+ break;
+ }
+
+ return new float[] {c1,c2,c3,c4};
+ }
+
+ /**
+ */
+ private CoordinateSystem3D(int lefty, int leftz, int topy, int topz) {
+ this.system = toSystemIndex(lefty,leftz,topy,topz);
+ }
+
+ /** Convert the specified point into from the current coordinate system
+ * to the specified coordinate system.
+ *
+ * @param point is the point to convert
+ * @param targetCoordinateSystem is the target coordinate system.
+ */
+ public void toSystem(Point3f point, CoordinateSystem3D targetCoordinateSystem) {
+ if (targetCoordinateSystem!=this) {
+ toPivot(point);
+ targetCoordinateSystem.fromPivot(point);
+ }
+ }
+
+ /** Convert the specified point into from the current coordinate system
+ * to the specified coordinate system.
+ *
+ * @param point is the point to convert
+ * @param targetCoordinateSystem is the target coordinate system.
+ */
+ public void toSystem(Vector3f point, CoordinateSystem3D targetCoordinateSystem) {
+ if (targetCoordinateSystem!=this) {
+ toPivot(point);
+ targetCoordinateSystem.fromPivot(point);
+ }
+ }
+
+ /** Convert the specified rotation axis into from the current coordinate system
+ * to the specified coordinate system.
+ *
+ * @param rotation is the rotation to convert
+ * @param targetCoordinateSystem is the target coordinate system.
+ */
+ public void toSystem(Quaternion rotation, CoordinateSystem3D targetCoordinateSystem) {
+ if (targetCoordinateSystem!=this) {
+ toPivot(rotation);
+ targetCoordinateSystem.fromPivot(rotation);
+ }
+ }
+
+ /** Convert the specified transformation matrix into from the current coordinate system
+ * to the specified coordinate system.
+ *
+ * @param trans is the matrix to convert
+ * @param targetCoordinateSystem is the target coordinate system.
+ */
+ public void toSystem(Transform3D trans, CoordinateSystem3D targetCoordinateSystem) {
+ if (targetCoordinateSystem!=this) {
+ toPivot(trans);
+ targetCoordinateSystem.fromPivot(trans);
+ }
+ }
+
+ private void toPivot(Point3f point) {
+ if (this.system!=PIVOT_SYSTEM) {
+ float[] factors = fromSystemIndex(this.system);
+ float y = point.getY() * factors[0] + point.getZ() * factors[1];
+ float z = point.getY() * factors[2] + point.getZ() * factors[3];
+ point.setY(y);
+ point.setZ(z);
+ }
+ }
+
+ /** Convert the specified point into the default coordinate system.
+ *
+ * @param point is the point to convert
+ */
+ public void toDefault(Point3f point) {
+ toSystem(point, getDefaultCoordinateSystem());
+ }
+
+ private void fromPivot(Point3f point) {
+ if (this.system!=PIVOT_SYSTEM) {
+ float[] factors = fromSystemIndex(this.system);
+ float y = point.getY() * factors[0] + point.getZ() * factors[2];
+ float z = point.getY() * factors[1] + point.getZ() * factors[3];
+ point.setY(y);
+ point.setZ(z);
+ }
+ }
+
+ /** Convert the specified point from the default coordinate system.
+ *
+ * @param point is the point to convert
+ */
+ public void fromDefault(Point3f point) {
+ getDefaultCoordinateSystem().toSystem(point, this);
+ }
+
+ private void toPivot(Vector3f vector) {
+ if (this.system!=PIVOT_SYSTEM) {
+ float[] factors = fromSystemIndex(this.system);
+ float y = vector.getY() * factors[0] + vector.getZ() * factors[1];
+ float z = vector.getY() * factors[2] + vector.getZ() * factors[3];
+ vector.setY(y);
+ vector.setZ(z);
+ }
+ }
+
+ /** Convert the specified vector from the default coordinate system.
+ *
+ * @param vector is the vector to convert
+ */
+ public void toDefault(Vector3f vector) {
+ toSystem(vector, getDefaultCoordinateSystem());
+ }
+
+ private void fromPivot(Vector3f vector) {
+ if (this.system!=PIVOT_SYSTEM) {
+ float[] factors = fromSystemIndex(this.system);
+ float y = vector.getY() * factors[0] + vector.getZ() * factors[2];
+ float z = vector.getY() * factors[1] + vector.getZ() * factors[3];
+ vector.setY(y);
+ vector.setZ(z);
+ }
+ }
+
+ /** Convert the specified vector from the default coordinate system.
+ *
+ * @param vector is the vector to convert
+ */
+ public void fromDefault(Vector3f vector) {
+ getDefaultCoordinateSystem().toSystem(vector, this);
+ }
+
+ private void fromPivot(Transform3D transformation) {
+ if (this.system!=PIVOT_SYSTEM) {
+ // Translation
+ Vector3f tr = transformation.getTranslation();
+ fromPivot(tr);
+ transformation.setTranslation(tr);
+ // Rotation
+ Quaternion rotation = transformation.getRotation();
+ fromPivot(rotation);
+ transformation.setRotation(rotation);
+ }
+ }
+
+ private void toPivot(Transform3D trans) {
+ if (this.system!=PIVOT_SYSTEM) {
+ float[] factors = fromSystemIndex(this.system);
+ float ty = trans.m13 * factors[0] + trans.m23* factors[1];
+ float tz = trans.m13 * factors[2] + trans.m23* factors[3];
+ trans.setTranslation(trans.getTranslationX(), ty, tz);
+ Quaternion r = trans.getRotation();
+ Vector3f vector = r.getAxis();
+ float ry = vector.getY() * factors[0] + vector.getZ() * factors[1];
+ float rz = vector.getY() * factors[2] + vector.getZ() * factors[3];
+
+ float angle = r.getAngle();
+
+ if (isLeftHanded()) angle = -angle;
+
+ r.setAxisAngle(vector.getX(), ry, rz, angle);
+ trans.setRotation(r);
+ }
+
+ }
+
+ /** Convert the specified rotation into the default coordinate system.
+ *
+ * @param rotation is the rotation to convert
+ */
+ public void toDefault(Transform3D rotation) {
+ toSystem(rotation, getDefaultCoordinateSystem());
+ }
+
+ /** Convert the specified rotation from the default coordinate system.
+ *
+ * @param rotation is the rotation to convert
+ */
+ public void fromDefault(Transform3D rotation) {
+ getDefaultCoordinateSystem().toSystem(rotation, this);
+ }
+
+ private void toPivot(Quaternion quaternion) {
+ if (this.system!=PIVOT_SYSTEM) {
+ float[] factors = fromSystemIndex(this.system);
+ Vector3f vector = quaternion.getAxis();
+ float y = vector.getY() * factors[0] + vector.getZ() * factors[1];
+ float z = vector.getY() * factors[2] + vector.getZ() * factors[3];
+ vector.setY(y);
+ vector.setZ(z);
+
+ float angle = quaternion.getAngle();
+
+ if (isLeftHanded()) angle = -angle;
+
+ quaternion.setAxisAngle(vector, angle);
+ }
+ }
+
+ /** Convert the specified rotation into the default coordinate system.
+ *
+ * @param rotation is the rotation to convert
+ */
+ public void toDefault(Quaternion rotation) {
+ toSystem(rotation, getDefaultCoordinateSystem());
+ }
+
+ private void fromPivot(Quaternion rotation) {
+ if (this.system!=PIVOT_SYSTEM) {
+ Vector3f vector = rotation.getAxis();
+ float[] factors = fromSystemIndex(this.system);
+ float y = vector.getY() * factors[0] + vector.getZ() * factors[2];
+ float z = vector.getY() * factors[1] + vector.getZ() * factors[3];
+ vector.setY(y);
+ vector.setZ(z);
+
+ float angle = rotation.getAngle();
+
+ if (isLeftHanded()) angle = -angle;
+
+ rotation.setAxisAngle(vector, angle);
+ }
+ }
+
+ /** Convert the specified rotation from the default coordinate system.
+ *
+ * @param rotation is the rotation to convert
+ */
+ public void fromDefault(Quaternion rotation) {
+ getDefaultCoordinateSystem().toSystem(rotation, this);
+ }
+
+ /** Replies the preferred coordinate system.
+ *
+ * @return the preferred coordinate system.
+ */
+ public static CoordinateSystem3D getDefaultCoordinateSystem() {
+ if (userDefault!=null) return userDefault;
+ return CoordinateSystemConstants.SIMULATION_3D;
+ }
+
+ /** Set the preferred coordinate system.
+ *
+ * @param newDefault is the new default coordinate system. If null the default
+ * coordinate system will be set back to the value replied by
+ * {@link CoordinateSystemConstants#SIMULATION_3D}.
+ */
+ public static void setDefaultCoordinateSystem(CoordinateSystem3D newDefault) {
+ userDefault = newDefault;
+ }
+
+ /** Replies the coordinate system which is corresponding to the specified
+ * orientation unit vectors.
+ *
+ * The front vector is assumed to be always (1,0,0),
+ * the left vector is (0,ly,lz), and the top
+ * vector is (0,ty,tz).
+ *
+ * @param ly
+ * @param lz
+ * @param ty
+ * @param tz
+ * @return the coordinate system which is defined by the specified vectors.
+ */
+ public static CoordinateSystem3D fromVectors(int ly, int lz, int ty, int tz) {
+ byte system = toSystemIndex(ly, lz, ty, tz);
+ for(CoordinateSystem3D cs : CoordinateSystem3D.values()) {
+ if (cs.system==system) return cs;
+ }
+ throw new CoordinateSystemNotFoundException();
+ }
+
+ /** Replies the coordinate system which is corresponding to the specified
+ * orientation unit vectors.
+ *
+ * The front vector is assumed to be always (1,0,0),
+ * the left vector is (0,ly,lz), and the top
+ * vector is (0,ty,tz).
+ *
+ * @param ly
+ * @param lz
+ * @param ty
+ * @param tz
+ * @return the coordinate system which is defined by the specified vectors.
+ */
+ public static CoordinateSystem3D fromVectors(float ly, float lz, float ty, float tz) {
+ return fromVectors((int)ly,(int)lz,(int)ty,(int)tz);
+ }
+
+ /** Replies the coordinate system which is corresponding to the specified
+ * orientation unit vectors.
+ *
+ * @param vx
+ * @param vy
+ * @param vz
+ * @param lx
+ * @param ly
+ * @param lz
+ * @param ux
+ * @param uy
+ * @param uz
+ * @return the coordinate system which is defined by the specified vectors.
+ */
+ public static CoordinateSystem3D fromVectors(float vx, float vy, float vz, float lx, float ly, float lz, float ux, float uy, float uz) {
+ if (vx==1. && vy==0. && vz==0.) {
+ assert(lx==0. && ux==0.);
+ return fromVectors(ly, lz, uy, uz);
+ }
+
+ // Transform the given vectors to align V along (1,0,0)
+ Transform3D mat = new Transform3D();
+ mat.m00 = vx;
+ mat.m10 = lx;
+ mat.m20 = ux;
+ mat.m01 = vy;
+ mat.m11 = ly;
+ mat.m21 = uy;
+ mat.m02 = vz;
+ mat.m12 = lz;
+ mat.m22 = uz;
+
+ Vector3f v1 = new Vector3f(vx, vy, vz);
+ mat.transform(v1);
+ normalizeVector(v1);
+
+ Vector3f v2 = new Vector3f(lx, ly, lz);
+ mat.transform(v2);
+ normalizeVector(v2);
+
+ Vector3f v3 = new Vector3f(ux, uy, uz);
+ mat.transform(v3);
+ normalizeVector(v3);
+
+ assert(v1.getX()==1. && v1.getY()==0. && v1.getZ()==0.);
+ assert(v2.getX()==0. && v3.getX()==0.);
+ return fromVectors(v2.getY(), v2.getZ(), v3.getY(), v3.getZ());
+ }
+
+ private static void normalizeVector(Vector3f v) {
+ v.normalize();
+ if (Math.abs(v.getX()-1.) <= MathConstants.JVM_MIN_FLOAT_EPSILON) {
+ v.setX(Math.signum(v.getX()));
+ v.setY(0.f);
+ v.setZ(0.f);
+ }
+ else if (Math.abs(v.getY()-1.) <= MathConstants.JVM_MIN_FLOAT_EPSILON) {
+ v.setY(Math.signum(v.getY()));
+ v.setX(0.f);
+ v.setZ(0.f);
+ }
+ else if (Math.abs(v.getZ()-1.) <= MathConstants.JVM_MIN_FLOAT_EPSILON) {
+ v.setZ(Math.signum(v.getZ()));
+ v.setX(0.f);
+ v.setY(0.f);
+ }
+ }
+
+ /** Replies the coordinate system which is corresponding to the specified
+ * orientation unit vectors.
+ *
+ * @param vx
+ * @param vy
+ * @param vz
+ * @param lx
+ * @param ly
+ * @param lz
+ * @param ux
+ * @param uy
+ * @param uz
+ * @return the coordinate system which is defined by the specified vectors.
+ */
+ public static CoordinateSystem3D fromVectors(int vx, int vy, int vz, int lx, int ly, int lz, int ux, int uy, int uz) {
+ if (vx==1 && vy==0 && vz==0) {
+ assert(lx==0 && ux==0);
+ return fromVectors(ly, lz, uy, uz);
+ }
+
+ // Transform the given vectors to align V along (1,0,0)
+ Transform3D mat = new Transform3D();
+ mat.m00 = vx;
+ mat.m10 = lx;
+ mat.m20 = ux;
+ mat.m01 = vy;
+ mat.m11 = ly;
+ mat.m21 = uy;
+ mat.m02 = vz;
+ mat.m12 = lz;
+ mat.m22 = uz;
+
+ Vector3f v1 = new Vector3f(vx, vy, vz);
+ mat.transform(v1);
+ normalizeVector(v1);
+
+ Vector3f v2 = new Vector3f(lx, ly, lz);
+ mat.transform(v2);
+ normalizeVector(v2);
+
+ Vector3f v3 = new Vector3f(ux, uy, uz);
+ mat.transform(v3);
+ normalizeVector(v3);
+
+ assert(v1.getX()==1. && v1.getY()==0. && v1.getZ()==0.);
+ assert(v2.getX()==0. && v3.getX()==0.);
+ return fromVectors((int)v2.getY(), (int)v2.getZ(), (int)v3.getY(), (int)v3.getZ());
+ }
+
+ /** Replies the vertical position from the given 3D point for this coordinate system.
+ *
+ * @param tuple
+ * @return the vertical position in x/, y or z
+ */
+ public final float height(Tuple3f tuple) {
+ return height(tuple.getX(), tuple.getY(), tuple.getZ());
+ }
+
+ /** Replies the vertical position from the given 3D point for this coordinate system.
+ *
+ * @param x
+ * @param y
+ * @param z
+ * @return the vertical position in x/, y or z
+ */
+ public float height(float x, float y, float z) {
+ float[] factors = fromSystemIndex(this.system);
+ return (factors[2]!=0.) ? y : z;
+ }
+
+ /** Replies the horizontal left-right position from the given 3D point for this coordinate system.
+ *
+ * @param tuple
+ * @return the horizontal and left-right position in x/, y or z
+ * @since 4.0
+ */
+ public final float side(Tuple3f tuple) {
+ return side(tuple.getX(), tuple.getY(), tuple.getZ());
+ }
+
+ /** Replies the horizontal left-right position from the given 3D point for this coordinate system.
+ *
+ * @param x
+ * @param y
+ * @param z
+ * @return the horizontal and left-right position in x/, y or z
+ * @since 4.0
+ */
+ public float side(float x, float y, float z) {
+ float[] factors = fromSystemIndex(this.system);
+ return (factors[2]!=0.) ? z : y;
+ }
+
+ /** Replies the horizontal front-back position from the given 3D point for this coordinate system.
+ *
+ * @param tuple
+ * @return the horizontal and front-back position in x/, y or z
+ * @since 4.0
+ */
+ public final static float view(Tuple3f tuple) {
+ return view(tuple.getX(), tuple.getY(), tuple.getZ());
+ }
+
+ /** Replies the horizontal front-back position from the given 3D point for this coordinate system.
+ *
+ * @param x
+ * @param y
+ * @param z
+ * @return the horizontal and front-back position in x/, y or z
+ * @since 4.0
+ */
+ public static float view(float x, float y, float z) {
+ return x;
+ }
+
+ /** Replies index of the coordinate which is corresponding to
+ * the height for the coordinate system.
+ *
+ * @return the index of the coordinate of the height.
+ */
+ public int getHeightCoordinateIndex() {
+ float[] factors = fromSystemIndex(this.system);
+ return (factors[2]!=0.) ? 1 : 2;
+ }
+
+ /** Replies index of the coordinate which is corresponding to
+ * the side for the coordinate system.
+ *
+ * @return the index of the coordinate of the side.
+ * @since 4.0
+ */
+ public int getSideCoordinateIndex() {
+ float[] factors = fromSystemIndex(this.system);
+ return (factors[2]!=0.) ? 2 : 1;
+ }
+
+ /** Replies index of the coordinate which is corresponding to
+ * the view for the coordinate system.
+ *
+ * @return the index of the coordinate of the view.
+ * @since 4.0
+ */
+ public static int getViewCoordinateIndex() {
+ return 0;
+ }
+
+ /** Set the vertical position in the given 3D point for this coordinate system.
+ *
+ * @param tuple
+ * @param height is the height to put in the tuple.
+ */
+ public final void setHeight(Tuple3f tuple, float height) {
+ float[] factors = fromSystemIndex(this.system);
+ if (factors[2]!=0.) tuple.setY(height);
+ else tuple.setZ(height);
+ }
+
+ /** Add the vertical amount to the height field of the given 3D point
+ * for this coordinate system.
+ *
+ * @param tuple
+ * @param additionalHeight is the height amount to add to the tuple.
+ */
+ public final void addHeight(Tuple3f tuple, float additionalHeight) {
+ float[] factors = fromSystemIndex(this.system);
+ if (factors[2]!=0.) tuple.setY(tuple.getY() + additionalHeight);
+ else tuple.setZ(tuple.getZ() + additionalHeight);
+ }
+
+ /** Set the left-right position in the given 3D point for this coordinate system.
+ *
+ * @param tuple
+ * @param side is the side amount to put in the tuple.
+ * @since 4.0
+ */
+ public final void setSide(Tuple3f tuple, float side) {
+ float[] factors = fromSystemIndex(this.system);
+ if (factors[2]!=0.) tuple.setZ(side);
+ else tuple.setY(side);
+ }
+
+ /** Add the left-right amount to the field of the given 3D point
+ * for this coordinate system.
+ *
+ * @param tuple
+ * @param additionalAmount is the amount to add to the tuple.
+ * @since 4.0
+ */
+ public final void addSide(Tuple3f tuple, float additionalAmount) {
+ float[] factors = fromSystemIndex(this.system);
+ if (factors[2]!=0.) tuple.setZ(tuple.getZ() + additionalAmount);
+ else tuple.setY(tuple.getY() + additionalAmount);
+ }
+
+ /** Set the front-back position in the given 3D point for this coordinate system.
+ *
+ * @param tuple
+ * @param amount is the amount to put in the tuple.
+ * @since 4.0
+ */
+ public final static void setView(Tuple3f tuple, float amount) {
+ tuple.setX(amount);
+ }
+
+ /** Add the front-back amount to the field of the given 3D point
+ * for this coordinate system.
+ *
+ * @param tuple
+ * @param additionalViewAmount is the amount to add to the tuple.
+ */
+ public final static void addView(Tuple3f tuple, float additionalViewAmount) {
+ tuple.setX(tuple.getX() + additionalViewAmount);
+ }
+
+ /** Replies the 2D coordinate system which is corresponding to
+ * this 3D coordinate system.
+ *
+ * Be carreful because the y semantic could differ from
+ * the 3D primitive to the 2D primitive.
+ *
+ * @return the 2D coordinate system.
+ */
+ public CoordinateSystem2D toCoordinateSystem2D() {
+ switch(this) {
+ case XYZ_LEFT_HAND:
+ return CoordinateSystem2D.XY_LEFT_HAND;
+ case XYZ_RIGHT_HAND:
+ return CoordinateSystem2D.XY_RIGHT_HAND;
+ case XZY_LEFT_HAND:
+ return CoordinateSystem2D.XY_RIGHT_HAND;
+ case XZY_RIGHT_HAND:
+ return CoordinateSystem2D.XY_LEFT_HAND;
+ default:
+ throw new CoordinateSystemNotFoundException();
+ }
+ }
+
+ /** Convert the specified point into from the current coordinate system
+ * to the specified coordinate system.
+ *
+ * @param point is the point to convert
+ * @return the 2D point
+ */
+ public Point2f toCoordinateSystem2D(Point3f point) {
+ switch(this) {
+ case XYZ_RIGHT_HAND:
+ case XYZ_LEFT_HAND:
+ return new Point2f(point.getX(), point.getY());
+ case XZY_LEFT_HAND:
+ case XZY_RIGHT_HAND:
+ return new Point2f(point.getX(), point.getZ());
+ default:
+ throw new CoordinateSystemNotFoundException();
+ }
+ }
+
+ /** Convert the specified point into from the current coordinate system
+ * to the specified coordinate system.
+ *
+ * @param point is the point to convert
+ * @return the 2D point
+ */
+ public Point2D toCoordinateSystem2D(Point3D point) {
+ switch(this) {
+ case XYZ_RIGHT_HAND:
+ case XYZ_LEFT_HAND:
+ return new Point2f(point.getX(), point.getY());
+ case XZY_LEFT_HAND:
+ case XZY_RIGHT_HAND:
+ return new Point2f(point.getX(), point.getZ());
+ default:
+ throw new CoordinateSystemNotFoundException();
+ }
+ }
+
+ /** Convert the specified point into from the current coordinate system
+ * to the specified coordinate system.
+ *
+ * @param vector is the vector to convert
+ * @return the 2D vector
+ */
+ public Vector2f toCoordinateSystem2D(Vector3f vector) {
+ switch(this) {
+ case XYZ_RIGHT_HAND:
+ case XYZ_LEFT_HAND:
+ return new Vector2f(vector.getX(), vector.getY());
+ case XZY_LEFT_HAND:
+ case XZY_RIGHT_HAND:
+ return new Vector2f(vector.getX(), vector.getZ());
+ default:
+ throw new CoordinateSystemNotFoundException();
+ }
+ }
+
+ /** Convert the specified rotation axis into from the current coordinate system
+ * to the specified coordinate system.
+ *
+ * @param rotation is the rotation to convert
+ * @return the 2D rotation
+ */
+ public Transform2D toCoordinateSystem2D(Transform3D rotation) {
+ float angle = toCoordinateSystem2DAngleFromTransformation(rotation);
+ Transform2D out = new Transform2D();
+
+ out.setRotation(angle);
+ out.setTranslation(,);
+ return new Transform2D(cos(angle), m01, m02, m10, m11, m12)
+ }
+
+ /** Convert the specified rotation axis into from the current coordinate system
+ * to the specified coordinate system.
+ *
+ * @param rotation is the rotation to convert
+ * @return the 2D rotation
+ */
+ public float toCoordinateSystem2D(Quaternion rotation) {
+ Transform3D trans = new Transform3D();
+ trans.setRotation(rotation);
+ return toCoordinateSystem2DAngleFromTransformation(trans);
+ }
+
+ private float toCoordinateSystem2DAngleFromTransformation(Transform3D mat) {
+ Vector3f ptR = new Vector3f(1,0,0);
+ mat.transform(ptR);
+ switch(this) {
+ case XYZ_LEFT_HAND:
+ case XYZ_RIGHT_HAND:
+ return (float)GeometryUtil.signedAngle(1, 0,ptR.getX(), ptR.getY());
+ case XZY_LEFT_HAND:
+ case XZY_RIGHT_HAND:
+ return (float)GeometryUtil.signedAngle(ptR.getX(), ptR.getZ(), 1, 0);
+ default:
+ }
+ throw new CoordinateSystemNotFoundException();
+ }
+
+ /** Convert the specified point from the 2D coordinate system
+ * to this specified coordinate system.
+ *
+ * Thr third coordinate is set to 0.
+ *
+ * @param point is the point to convert
+ * @return the 3D point
+ */
+ public final Point3f fromCoordinateSystem2D(Point2f point) {
+ return fromCoordinateSystem2D(point, 0);
+ }
+
+ /** Convert the specified point from the 2D coordinate system
+ * to this specified coordinate system.
+ *
+ * Thr third coordinate is set to 0.
+ *
+ * @param point is the point to convert
+ * @return the 3D point
+ */
+ public final Point3D fromCoordinateSystem2D(Point2D point) {
+ return fromCoordinateSystem2D(point, 0);
+ }
+
+ /** Convert the specified point from the 2D coordinate system
+ * to this specified coordinate system.
+ *
+ * @param point is the point to convert
+ * @param thirdCoordinate is the value of the third coordinate to put in the replied point.
+ * @return the 3D point
+ */
+ public Point3f fromCoordinateSystem2D(Point2f point, float thirdCoordinate) {
+ switch(this) {
+ case XYZ_LEFT_HAND:
+ case XYZ_RIGHT_HAND:
+ return new Point3f(point.getX(), point.getY(), thirdCoordinate);
+ case XZY_LEFT_HAND:
+ case XZY_RIGHT_HAND:
+ return new Point3f(point.getX(), thirdCoordinate, point.getY());
+ default:
+ throw new CoordinateSystemNotFoundException();
+ }
+ }
+
+ /** Convert the specified point from the 2D coordinate system
+ * to this specified coordinate system.
+ *
+ * @param point is the point to convert
+ * @param thirdCoordinate is the value of the third coordinate to put in the replied point.
+ * @return the 3D point
+ */
+ public Point3D fromCoordinateSystem2D(Point2D point, float thirdCoordinate) {
+ switch(this) {
+ case XYZ_LEFT_HAND:
+ case XYZ_RIGHT_HAND:
+ return new Point3f(point.getX(), point.getY(), thirdCoordinate);
+ case XZY_LEFT_HAND:
+ case XZY_RIGHT_HAND:
+ return new Point3f(point.getX(), thirdCoordinate, point.getY());
+ default:
+ throw new CoordinateSystemNotFoundException();
+ }
+ }
+
+ /** Convert the specified vector from the 2D coordinate system
+ * to this specified coordinate system.
+ *
+ * Thr third coordinate is set to 0.
+ *
+ * @param vector is the vector to convert
+ * @return the 3D vector
+ */
+ public final Vector3f fromCoordinateSystem2D(Vector2f vector) {
+ return fromCoordinateSystem2D(vector, 0);
+ }
+
+ /** Convert the specified vector from the 2D coordinate system
+ * to this specified coordinate system.
+ *
+ * @param point is the vector to convert
+ * @param thirdCoordinate is the value of the third coordinate to put in the replied vector.
+ * @return the 3D vector
+ */
+ public Vector3f fromCoordinateSystem2D(Vector2f point, float thirdCoordinate) {
+ switch(this) {
+ case XYZ_LEFT_HAND:
+ case XYZ_RIGHT_HAND:
+ return new Vector3f(point.getX(), point.getY(), thirdCoordinate);
+ case XZY_LEFT_HAND:
+ case XZY_RIGHT_HAND:
+ return new Vector3f(point.getX(), thirdCoordinate, point.getY());
+ default:
+ throw new CoordinateSystemNotFoundException();
+ }
+ }
+
+ /** Convert the specified rotation from the 2D coordinate system
+ * to this specified coordinate system.
+ *
+ * @param rotation is the angle of rotation to convert.
+ * @return the 3D axis angle
+ */
+ public Quaternion fromCoordinateSystem2D(float rotation) {
+ switch(this) {
+ case XYZ_LEFT_HAND:
+ case XYZ_RIGHT_HAND:
+ return Quaternion.newAxisAngle(0,0,1,rotation);
+ case XZY_LEFT_HAND:
+ case XZY_RIGHT_HAND:
+ return Quaternion.newAxisAngle(0,1,0,rotation);
+ default:
+ throw new CoordinateSystemNotFoundException();
+ }
+ }
+
+ /** Replies the view vector of this coordinate space.
+ *
+ * When objects have not been rotated, they are supposed to
+ * have a front direction colinear to this view vector.
+ *
+ * @return the view vector (always normalized).
+ */
+ public final Vector3f getViewVector() {
+ return getViewVector(new Vector3f());
+ }
+
+ /** Replies the view vector of this coordinate space.
+ *
+ * When objects have not been rotated, they are supposed to
+ * have a front direction colinear to this view vector.
+ *
+ * @param vectorToFill is the vector to fill with the view vector coordinates.
+ * @return vectorToFill.
+ */
+ public final Vector3f getViewVector(Vector3f vectorToFill) {
+ if (vectorToFill!=null) {
+ vectorToFill.set(1.f,0.f,0.f);
+ fromPivot(vectorToFill);
+ }
+ return vectorToFill;
+ }
+
+ /** Replies the back vector of this coordinate space.
+ *
+ * When objects have not been rotated, they are supposed to
+ * have a back direction colinear to this back vector.
+ *
+ * @return the back vector (always normalized).
+ */
+ public final Vector3f getBackVector() {
+ return getBackVector(new Vector3f());
+ }
+
+ /** Replies the back vector of this coordinate space.
+ *
+ * When objects have not been rotated, they are supposed to
+ * have a back direction colinear to this back vector.
+ *
+ * @param vectorToFill is the vector to fill with the back vector coordinates.
+ * @return vectorToFill.
+ */
+ public final Vector3f getBackVector(Vector3f vectorToFill) {
+ if (vectorToFill!=null) {
+ vectorToFill.set(-1.f,0.f,0.f);
+ fromPivot(vectorToFill);
+ }
+ return vectorToFill;
+ }
+
+ /** Replies the left vector of this coordinate space.
+ *
+ * When objects have not been rotated, they are supposed to
+ * have a left direction colinear to this left vector.
+ *
+ * @return the left vector (always normalized).
+ */
+ public final Vector3f getLeftVector() {
+ return getLeftVector(new Vector3f());
+ }
+
+ /** Replies the left vector of this coordinate space.
+ *
+ * When objects have not been rotated, they are supposed to
+ * have a left direction colinear to this left vector.
+ *
+ * @param vectorToFill is the vector to fill with the left vector coordinates.
+ * @return vectorToFill.
+ */
+ public final Vector3f getLeftVector(Vector3f vectorToFill) {
+ if (vectorToFill!=null) {
+ vectorToFill.set(0.f,1.f,0.f);
+ fromPivot(vectorToFill);
+ }
+ return vectorToFill;
+ }
+
+ /** Replies the right vector of this coordinate space.
+ *
+ * When objects have not been rotated, they are supposed to
+ * have a right direction colinear to this right vector.
+ *
+ * @return the right vector (always normalized).
+ */
+ public final Vector3f getRightVector() {
+ return getRightVector(new Vector3f());
+ }
+
+ /** Replies the right vector of this coordinate space.
+ *
+ * When objects have not been rotated, they are supposed to
+ * have a right direction colinear to this right vector.
+ *
+ * @param vectorToFill is the vector to fill with the right vector coordinates.
+ * @return vectorToFill.
+ */
+ public final Vector3f getRightVector(Vector3f vectorToFill) {
+ if (vectorToFill!=null) {
+ vectorToFill.set(0.f,-1.f,0.f);
+ fromPivot(vectorToFill);
+ }
+ return vectorToFill;
+ }
+
+ /** Replies the up vector of this coordinate space.
+ *
+ * When objects have not been rotated, they are supposed to
+ * have a up direction colinear to this up vector.
+ *
+ * @return the up vector (always normalized).
+ */
+ public final Vector3f getUpVector() {
+ return getUpVector(new Vector3f());
+ }
+
+ /** Replies the up vector of this coordinate space.
+ *
+ * When objects have not been rotated, they are supposed to
+ * have a up direction colinear to this up vector.
+ *
+ * @param vectorToFill is the vector to fill with the up vector coordinates.
+ * @return vectorToFill.
+ */
+ public final Vector3f getUpVector(Vector3f vectorToFill) {
+ if (vectorToFill!=null) {
+ vectorToFill.set(0.f,0.f,1.f);
+ fromPivot(vectorToFill);
+ }
+ return vectorToFill;
+ }
+
+ /** Replies the down vector of this coordinate space.
+ *
+ * When objects have not been rotated, they are supposed to
+ * have a down direction colinear to this down vector.
+ *
+ * @return the down vector (always normalized).
+ */
+ public final Vector3f getDownVector() {
+ return getDownVector(new Vector3f());
+ }
+
+ /** Replies the down vector of this coordinate space.
+ *
+ * When objects have not been rotated, they are supposed to
+ * have a down direction colinear to this down vector.
+ *
+ * @param vectorToFill is the vector to fill with the down vector coordinates.
+ * @return vectorToFill.
+ */
+ public final Vector3f getDownVector(Vector3f vectorToFill) {
+ if (vectorToFill!=null) {
+ vectorToFill.set(0.f,0.f,-1.f);
+ fromPivot(vectorToFill);
+ }
+ return vectorToFill;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public boolean isRightHanded() {
+ return this==XYZ_RIGHT_HAND || this==XZY_RIGHT_HAND;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public boolean isLeftHanded() {
+ return this==XYZ_LEFT_HAND || this==XZY_LEFT_HAND;
+ }
+
+ /** Replies if the z coordinate is the up direction.
+ *
+ * @return true if z coordiante is up.
+ */
+ public boolean isZOnUp() {
+ return this==XYZ_LEFT_HAND || this==XYZ_RIGHT_HAND;
+ }
+
+ /** Replies if the y coordinate is the up direction.
+ *
+ * @return true if y coordiante is up.
+ */
+ public boolean isYOnUp() {
+ return this==XZY_LEFT_HAND || this==XZY_RIGHT_HAND;
+ }
+
+ /** Replies if the z coordinate is the side direction (left or right).
+ *
+ * @return true if z coordiante is side.
+ */
+ public boolean isZOnSide() {
+ return this==XZY_LEFT_HAND || this==XZY_RIGHT_HAND;
+ }
+
+ /** Replies if the y coordinate is the side direction (left or right).
+ *
+ * @return true if y coordiante is side.
+ */
+ public boolean isYOnSide() {
+ return this==XYZ_LEFT_HAND || this==XYZ_RIGHT_HAND;
+ }
+
+}
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry/system/CoordinateSystemConstants.java b/core/math/src/main/java/org/arakhne/afc/math/geometry/system/CoordinateSystemConstants.java
new file mode 100644
index 000000000..fc3494423
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry/system/CoordinateSystemConstants.java
@@ -0,0 +1,228 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2013 Christophe BOHRHAUER.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry.system;
+
+/**
+ * Represents the different 2D/3D referencials
+ * used in different domains.
+ *
+ * @author $Author: cbohrhauer$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class CoordinateSystemConstants {
+
+ private CoordinateSystemConstants() {
+ //
+ }
+
+ /** Replies the preferred coordinate system for
+ * Geographical
+ * Information System (GIS).
+ *
+ * GIS uses {@link CoordinateSystem3D#XYZ_RIGHT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem3D GIS_3D = CoordinateSystem3D.XYZ_RIGHT_HAND;
+
+ /** Replies the preferred coordinate system for
+ * Geographical
+ * Information System (GIS) with a
+ * conical map projection.
+ * @deprecated see {@link #GIS_3D}
+ */
+ @Deprecated
+ public static final CoordinateSystem3D CONICAL_GIS_3D = GIS_3D;
+
+ /** Replies the preferred coordinate system for simulation spaces.
+ *
+ * Simulation use {@link CoordinateSystem2D#XY_RIGHT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem2D SIMULATION_2D = CoordinateSystem2D.XY_RIGHT_HAND;
+
+ /** Replies the preferred coordinate system for simulation spaces.
+ *
+ * Simulation use {@link CoordinateSystem3D#XYZ_RIGHT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem3D SIMULATION_3D = CoordinateSystem3D.XYZ_RIGHT_HAND;
+
+ /** Replies the preferred coordinate system for default 3DSMAX modelers.
+ *
+ * 3DSMAX uses the {@link CoordinateSystem2D#XY_RIGHT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem2D MODEL_3DMAX_2D = CoordinateSystem2D.XY_RIGHT_HAND;
+
+ /** Replies the preferred coordinate system for default 3DSMAX modelers.
+ *
+ * 3DSMAX uses the {@link CoordinateSystem3D#XYZ_RIGHT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem3D MODEL_3DMAX_3D = CoordinateSystem3D.XYZ_RIGHT_HAND;
+
+ /** Replies the preferred coordinate system for default DirectX viewers.
+ *
+ * DirectX uses the {@link CoordinateSystem2D#XY_RIGHT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem2D DIRECTX_2D = CoordinateSystem2D.XY_RIGHT_HAND;
+
+ /** Replies the preferred coordinate system for default DirectX viewers.
+ *
+ * DirectX uses the {@link CoordinateSystem3D#XZY_LEFT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem3D DIRECTX_3D = CoordinateSystem3D.XZY_LEFT_HAND;
+
+ /** Replies the preferred coordinate system for default Java3D viewers.
+ *
+ * Java3D uses the {@link CoordinateSystem2D#XY_LEFT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem2D JAVA3D_2D = CoordinateSystem2D.XY_LEFT_HAND;
+
+ /** Replies the preferred coordinate system for default Java3D viewers.
+ *
+ * Java3D uses the {@link CoordinateSystem3D#XZY_RIGHT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem3D JAVA3D_3D = CoordinateSystem3D.XZY_RIGHT_HAND;
+
+ /** Replies the preferred coordinate system for default OpenGL viewers.
+ *
+ * OpenGL uses the {@link CoordinateSystem2D#XY_LEFT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem2D OPENGL_2D = CoordinateSystem2D.XY_LEFT_HAND;
+
+ /** Replies the preferred coordinate system for default OpenGL viewers.
+ *
+ * OpenGL uses the {@link CoordinateSystem3D#XZY_RIGHT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem3D OPENGL_3D = CoordinateSystem3D.XZY_RIGHT_HAND;
+
+ /** Replies the preferred coordinate system for default X3D viewers.
+ *
+ * X3D uses the {@link CoordinateSystem2D#XY_LEFT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem2D X3D_2D = CoordinateSystem2D.XY_LEFT_HAND;
+
+ /** Replies the preferred coordinate system for default X3D viewers.
+ *
+ * X3D uses the {@link CoordinateSystem3D#XZY_RIGHT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem3D X3D_3D = CoordinateSystem3D.XZY_RIGHT_HAND;
+
+ /** Replies the preferred coordinate system for default NASA airplane standards.
+ *
+ * NASA airplane standards use the {@link CoordinateSystem2D#XY_LEFT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem2D NASA_2D = CoordinateSystem2D.XY_LEFT_HAND;
+
+ /** Replies the preferred coordinate system for default NASA airplane standards.
+ *
+ * NASA airplane standards use the {@link CoordinateSystem3D#XYZ_LEFT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem3D NASA_3D = CoordinateSystem3D.XYZ_LEFT_HAND;
+
+ /** Replies the preferred coordinate system for default Collada viewers.
+ *
+ * Collada uses the {@link CoordinateSystem2D#XY_LEFT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem2D COLLADA_2D = CoordinateSystem2D.XY_LEFT_HAND;
+
+ /** Replies the preferred coordinate system for default Collada viewers.
+ *
+ * Collada uses the {@link CoordinateSystem3D#XZY_LEFT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem3D COLLADA_3D = CoordinateSystem3D.XZY_LEFT_HAND;
+
+ /** Replies the preferred coordinate system for 3DVIA Virtools.
+ *
+ * 3DVIA Virtools uses the {@link CoordinateSystem2D#XY_RIGHT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem2D VIRTOOLS_2D = CoordinateSystem2D.XY_RIGHT_HAND;
+
+ /** Replies the preferred coordinate system for 3DVIA Virtools.
+ *
+ * 3DVIA Virtools uses the {@link CoordinateSystem3D#XZY_LEFT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem3D VIRTOOLS_3D = CoordinateSystem3D.XZY_LEFT_HAND;
+
+ /** Replies the preferred coordinate system for Maya modeller.
+ *
+ * Maya uses the {@link CoordinateSystem2D#XY_LEFT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem2D MAYA_2D = CoordinateSystem2D.XY_LEFT_HAND;
+
+ /** Replies the preferred coordinate system for Maya modeler.
+ *
+ * Maya uses the {@link CoordinateSystem3D#XZY_RIGHT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem3D MAYA_3D = CoordinateSystem3D.XZY_RIGHT_HAND;
+
+ /** Replies the preferred coordinate system for Unity 3D modeller.
+ *
+ * Unity 3D uses the {@link CoordinateSystem2D#XY_LEFT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem2D UNITY3D_2D = CoordinateSystem2D.XY_LEFT_HAND;
+
+ /** Replies the preferred coordinate system for Unity 3D modeler.
+ *
+ * Unity 3D uses the {@link CoordinateSystem3D#XZY_LEFT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem3D UNITY3D_3D = CoordinateSystem3D.XZY_LEFT_HAND;
+
+ /** Replies the preferred coordinate system for Catia V5 modeller.
+ *
+ * Catia V5 uses the {@link CoordinateSystem2D#XY_LEFT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem2D CATIAV5_2D = CoordinateSystem2D.XY_LEFT_HAND;
+
+ /** Replies the preferred coordinate system for Catia V5 modeler.
+ *
+ * Catia V5 uses the {@link CoordinateSystem3D#XZY_RIGHT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem3D CATIAV5_3D = CoordinateSystem3D.XZY_RIGHT_HAND;
+
+ /** Replies the preferred coordinate system for Blender modeller.
+ *
+ * Blender uses the {@link CoordinateSystem2D#XY_LEFT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem2D BLENDER_2D = CoordinateSystem2D.XY_RIGHT_HAND;
+
+ /** Replies the preferred coordinate system for Blender modeler.
+ *
+ * Blender uses the {@link CoordinateSystem3D#XZY_RIGHT_HAND} coordinate system.
+ */
+ public static final CoordinateSystem3D BLENDER_3D = CoordinateSystem3D.XYZ_RIGHT_HAND;
+
+}
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry/system/CoordinateSystemNotFoundException.java b/core/math/src/main/java/org/arakhne/afc/math/geometry/system/CoordinateSystemNotFoundException.java
new file mode 100644
index 000000000..606f09bfb
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry/system/CoordinateSystemNotFoundException.java
@@ -0,0 +1,42 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2013 Christophe BOHRHAUER.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry.system;
+
+/**
+ * Exception thrown when no coordinate system was found.
+ *
+ * @author $Author: cbohrhauer$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class CoordinateSystemNotFoundException
+extends AssertionError {
+
+ private static final long serialVersionUID = -7403966914116118753L;
+
+ /**
+ */
+ public CoordinateSystemNotFoundException() {
+ //
+ }
+
+}
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/Path2D.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/Path2D.java
new file mode 100644
index 000000000..8f3aff006
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/Path2D.java
@@ -0,0 +1,93 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2013 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d;
+
+import java.util.Iterator;
+
+import org.arakhne.afc.math.geometry.PathElementType;
+import org.arakhne.afc.math.geometry.PathWindingRule;
+
+/** 2D Path.
+ *
+ * @param is the type of the bounding box.
+ * @param is the type of the elements of the path.
+ * @param is the type of the iterator used to obtain the elements of the path.
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public interface Path2D> extends Shape2D {
+
+ /**
+ * Replies the bounds of this path.
+ *
+ * @return the bounds
+ */
+ public B toBoundingBox();
+
+ /** Replies the winding rule for the path.
+ *
+ * @return the winding rule for the path.
+ */
+ public PathWindingRule getWindingRule();
+
+ /** Replies the path is composed only by
+ * MOVE_TO, LINE_TO
+ * or CLOSE primitives (no curve).
+ *
+ * @return true if the path does not
+ * contain curve primitives, false
+ * otherwise.
+ */
+ public boolean isPolyline();
+
+ /** Replies an iterator on the path elements.
+ *
+ * Only {@link PathElementType#MOVE_TO},
+ * {@link PathElementType#LINE_TO}, and
+ * {@link PathElementType#CLOSE} types are returned by the iterator.
+ *
+ * The amount of subdivision of the curved segments is controlled by the
+ * flatness parameter, which specifies the maximum distance that any point
+ * on the unflattened transformed curve can deviate from the returned
+ * flattened path segments. Note that a limit on the accuracy of the
+ * flattened path might be silently imposed, causing very small flattening
+ * parameters to be treated as larger values. This limit, if there is one,
+ * is defined by the particular implementation that is used.
+ *
+ * The iterator for this class is not multi-threaded safe.
+ *
+ * @param flatness is the maximum distance that the line segments used to approximate
+ * the curved segments are allowed to deviate from any point on the original curve.
+ * @return an iterator on the path elements.
+ */
+ public I getPathIterator(float flatness);
+
+ /** Replies an iterator on the path elements.
+ *
+ * The iterator for this class is not multi-threaded safe.
+ *
+ * @return an iterator on the path elements.
+ */
+ public I getPathIterator();
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/PathElement2D.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/PathElement2D.java
new file mode 100644
index 000000000..077596bbd
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/PathElement2D.java
@@ -0,0 +1,60 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2005-09 Stephane GALLAND.
+ * Copyright (C) 2012 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d;
+
+import java.io.Serializable;
+
+import org.arakhne.afc.math.geometry.PathElementType;
+
+/** An element of the path.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public interface PathElement2D extends Serializable {
+
+ /** Replies the type of the element.
+ *
+ * @return true if the points are
+ * the same; otherwise false.
+ */
+ public PathElementType getType();
+
+ /** Replies if the element is empty, ie. the points are the same.
+ *
+ * @return true if the points are
+ * the same; otherwise false.
+ */
+ public boolean isEmpty();
+
+ /** Replies if the element is not empty and its drawable.
+ * Only the path elements that may produce pixels on the screen
+ * must reply true in this function.
+ *
+ * @return true if the path element
+ * is drawable; otherwise false.
+ */
+ public boolean isDrawable();
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/Point2D.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/Point2D.java
new file mode 100644
index 000000000..169f8db6c
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/Point2D.java
@@ -0,0 +1,151 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2012 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d;
+
+/** 2D Point.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public interface Point2D extends Tuple2D {
+
+ /**
+ * Computes the square of the distance between this point and point p1.
+ * @param p1 the other point
+ * @return the distance.
+ */
+ public float distanceSquared(Point2D p1);
+
+ /**
+ * Computes the distance between this point and point p1.
+ * @param p1 the other point
+ * @return the distance.
+ */
+ public float distance(Point2D p1);
+
+ /**
+ * Computes the L-1 (Manhattan) distance between this point and
+ * point p1. The L-1 distance is equal to abs(x1-x2) + abs(y1-y2).
+ * @param p1 the other point
+ * @return the distance.
+ */
+ public float distanceL1(Point2D p1);
+
+ /**
+ * Computes the L-infinite distance between this point and
+ * point p1. The L-infinite distance is equal to
+ * MAX[abs(x1-x2), abs(y1-y2)].
+ * @param p1 the other point
+ * @return the distance.
+ */
+ public float distanceLinf(Point2D p1);
+
+ /**
+ * Sets the value of this tuple to the sum of tuples t1 and t2.
+ * @param t1 the first tuple
+ * @param t2 the second tuple
+ */
+ public void add(Point2D t1, Vector2D t2);
+
+ /**
+ * Sets the value of this tuple to the sum of tuples t1 and t2.
+ * @param t1 the first tuple
+ * @param t2 the second tuple
+ */
+ public void add(Vector2D t1, Point2D t2);
+
+ /**
+ * Sets the value of this tuple to the sum of itself and t1.
+ * @param t1 the other tuple
+ */
+ public void add(Vector2D t1);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of tuple t1 plus tuple t2 (this = s*t1 + t2).
+ * @param s the scalar value
+ * @param t1 the tuple to be multipled
+ * @param t2 the tuple to be added
+ */
+ public void scaleAdd(int s, Vector2D t1, Point2D t2);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of tuple t1 plus tuple t2 (this = s*t1 + t2).
+ * @param s the scalar value
+ * @param t1 the tuple to be multipled
+ * @param t2 the tuple to be added
+ */
+ public void scaleAdd(float s, Vector2D t1, Point2D t2);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of tuple t1 plus tuple t2 (this = s*t1 + t2).
+ * @param s the scalar value
+ * @param t1 the tuple to be multipled
+ * @param t2 the tuple to be added
+ */
+ public void scaleAdd(int s, Point2D t1, Vector2D t2);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of tuple t1 plus tuple t2 (this = s*t1 + t2).
+ * @param s the scalar value
+ * @param t1 the tuple to be multipled
+ * @param t2 the tuple to be added
+ */
+ public void scaleAdd(float s, Point2D t1, Vector2D t2);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of itself and then adds tuple t1 (this = s*this + t1).
+ * @param s the scalar value
+ * @param t1 the tuple to be added
+ */
+ public void scaleAdd(int s, Vector2D t1);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of itself and then adds tuple t1 (this = s*this + t1).
+ * @param s the scalar value
+ * @param t1 the tuple to be added
+ */
+ public void scaleAdd(float s, Vector2D t1);
+
+
+ /**
+ * Sets the value of this tuple to the difference
+ * of tuples t1 and t2 (this = t1 - t2).
+ * @param t1 the first tuple
+ * @param t2 the second tuple
+ */
+ public void sub(Point2D t1, Vector2D t2);
+
+ /**
+ * Sets the value of this tuple to the difference
+ * of itself and t1 (this = this - t1).
+ * @param t1 the other tuple
+ */
+ public void sub(Vector2D t1);
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/Shape2D.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/Shape2D.java
new file mode 100644
index 000000000..e8cd98b52
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/Shape2D.java
@@ -0,0 +1,103 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d;
+
+import java.io.Serializable;
+
+/** 2D shape.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public interface Shape2D
+extends Cloneable, Serializable {
+
+ /** Replies if this shape is empty.
+ * The semantic associated to the state "empty"
+ * depends on the implemented shape. See the
+ * subclasses for details.
+ *
+ * @return true if the shape is empty;
+ * false otherwise.
+ */
+ public boolean isEmpty();
+
+ /** Clone this shape.
+ *
+ * @return the clone.
+ */
+ public Shape2D clone();
+
+ /** Reset this shape to be equivalent to
+ * an just-created instance of this shape type.
+ */
+ public void clear();
+
+ /** Replies if the given point is inside this shape.
+ *
+ * @param p
+ * @return true if the given shape is intersecting this
+ * shape, otherwise false.
+ */
+ public boolean contains(Point2D p);
+
+ /** Replies the point on the shape that is closest to the given point.
+ *
+ * @param p
+ * @return the closest point on the shape; or the point itself
+ * if it is inside the shape.
+ */
+ public Point2D getClosestPointTo(Point2D p);
+
+ /**
+ * Replies the area covered by the shape.
+ * @return the area.
+ */
+ //TODO public float getArea();
+
+ /**
+ * Replies the area covered by the shape.
+ * @return the area.
+ */
+ //TODO public IntersectionType classifies(Shape2D);
+
+ /**
+ * Replies the area covered by the shape.
+ * @return the area.
+ */
+ //TODO public Shape2D createUnion(Shape2D); HP
+
+ /**
+ * Replies the area covered by the shape.
+ * @return the area.
+ */
+ //TODO public Shape2D createIntersection(Shape2D); HP
+
+ /** Replies the point on the shape that is closest to the given point.
+ *
+ * @param p
+ * @return the closest point on the shape; or the point itself
+ * if it is inside the shape.
+ */
+ //TODO public Point2D getFarthestPointTo(Point2D p); HHP
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/Tuple2D.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/Tuple2D.java
new file mode 100644
index 000000000..dbcea81f8
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/Tuple2D.java
@@ -0,0 +1,426 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2012 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d;
+
+import java.io.Serializable;
+
+/** 2D tuple.
+ *
+ * @param is the type of data that can be added or substracted to this tuple.
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public interface Tuple2D>
+extends Cloneable, Serializable {
+
+ /** Clone this point.
+ *
+ * @return the clone.
+ */
+ public TT clone();
+
+ /**
+ * Sets each component of this tuple to its absolute value.
+ */
+ public void absolute();
+
+ /**
+ * Sets each component of the tuple parameter to its absolute
+ * value and places the modified values into this tuple.
+ * @param t the source tuple, which will not be modified
+ */
+ public void absolute(TT t);
+
+ /**
+ * Sets the value of this tuple to the sum of itself and x and y.
+ * @param x
+ * @param y
+ */
+ public void add(int x, int y);
+
+ /**
+ * Sets the value of this tuple to the sum of itself and x and y.
+ * @param x
+ * @param y
+ */
+ public void add(float x, float y);
+
+ /**
+ * Sets the x value of this tuple to the sum of itself and x.
+ * @param x
+ */
+ public void addX(int x);
+
+ /**
+ * Sets the x value of this tuple to the sum of itself and x.
+ * @param x
+ */
+ public void addX(float x);
+
+ /**
+ * Sets the y value of this tuple to the sum of itself and y.
+ * @param y
+ */
+ public void addY(int y);
+
+ /**
+ * Sets the y value of this tuple to the sum of itself and y.
+ * @param y
+ */
+ public void addY(float y);
+
+ /**
+ * Clamps this tuple to the range [low, high].
+ * @param min the lowest value in this tuple after clamping
+ * @param max the highest value in this tuple after clamping
+ */
+ public void clamp(int min, int max);
+
+ /**
+ * Clamps this tuple to the range [low, high].
+ * @param min the lowest value in this tuple after clamping
+ * @param max the highest value in this tuple after clamping
+ */
+ public void clamp(float min, float max);
+
+ /**
+ * Clamps the minimum value of this tuple to the min parameter.
+ * @param min the lowest value in this tuple after clamping
+ */
+ public void clampMin(int min);
+
+ /**
+ * Clamps the minimum value of this tuple to the min parameter.
+ * @param min the lowest value in this tuple after clamping
+ */
+ public void clampMin(float min);
+
+ /**
+ * Clamps the maximum value of this tuple to the max parameter.
+ * @param max the highest value in the tuple after clamping
+ */
+ public void clampMax(int max);
+
+ /**
+ * Clamps the maximum value of this tuple to the max parameter.
+ * @param max the highest value in the tuple after clamping
+ */
+ public void clampMax(float max);
+
+ /**
+ * Clamps the tuple parameter to the range [low, high] and
+ * places the values into this tuple.
+ * @param min the lowest value in the tuple after clamping
+ * @param max the highest value in the tuple after clamping
+ * @param t the source tuple, which will not be modified
+ */
+ public void clamp(int min, int max, TT t);
+
+ /**
+ * Clamps the tuple parameter to the range [low, high] and
+ * places the values into this tuple.
+ * @param min the lowest value in the tuple after clamping
+ * @param max the highest value in the tuple after clamping
+ * @param t the source tuple, which will not be modified
+ */
+ public void clamp(float min, float max, TT t);
+
+ /**
+ * Clamps the minimum value of the tuple parameter to the min
+ * parameter and places the values into this tuple.
+ * @param min the lowest value in the tuple after clamping
+ * @param t the source tuple, which will not be modified
+ */
+ public void clampMin(int min, TT t);
+
+ /**
+ * Clamps the minimum value of the tuple parameter to the min
+ * parameter and places the values into this tuple.
+ * @param min the lowest value in the tuple after clamping
+ * @param t the source tuple, which will not be modified
+ */
+ public void clampMin(float min, TT t);
+
+ /**
+ * Clamps the maximum value of the tuple parameter to the max
+ * parameter and places the values into this tuple.
+ * @param max the highest value in the tuple after clamping
+ * @param t the source tuple, which will not be modified
+ */
+ public void clampMax(int max, TT t);
+
+ /**
+ * Clamps the maximum value of the tuple parameter to the max
+ * parameter and places the values into this tuple.
+ * @param max the highest value in the tuple after clamping
+ * @param t the source tuple, which will not be modified
+ */
+ public void clampMax(float max, TT t);
+
+ /**
+ * Copies the values of this tuple into the tuple t.
+ * @param t is the target tuple
+ */
+ public void get(TT t);
+
+ /**
+ * Copies the value of the elements of this tuple into the array t.
+ * @param t the array that will contain the values of the vector
+ */
+ public void get(int[] t);
+
+ /**
+ * Copies the value of the elements of this tuple into the array t.
+ * @param t the array that will contain the values of the vector
+ */
+ public void get(float[] t);
+
+ /**
+ * Sets the value of this tuple to the negation of tuple t1.
+ * @param t1 the source tuple
+ */
+ public void negate(TT t1);
+
+ /**
+ * Negates the value of this tuple in place.
+ */
+ public void negate();
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of tuple t1.
+ * @param s the scalar value
+ * @param t1 the source tuple
+ */
+ public void scale(int s, TT t1);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of tuple t1.
+ * @param s the scalar value
+ * @param t1 the source tuple
+ */
+ public void scale(float s, TT t1);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of the scale factor with this.
+ * @param s the scalar value
+ */
+ public void scale(int s);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of the scale factor with this.
+ * @param s the scalar value
+ */
+ public void scale(float s);
+
+ /**
+ * Sets the value of this tuple to the value of tuple t1.
+ * @param t1 the tuple to be copied
+ */
+ public void set(Tuple2D t1);
+
+ /**
+ * Sets the value of this tuple to the specified x and y
+ * coordinates.
+ * @param x the x coordinate
+ * @param y the y coordinate
+ */
+ public void set(int x, int y);
+
+ /**
+ * Sets the value of this tuple to the specified x and y
+ * coordinates.
+ * @param x the x coordinate
+ * @param y the y coordinate
+ */
+ public void set(float x, float y);
+
+ /**
+ * Sets the value of this tuple from the 2 values specified in
+ * the array.
+ * @param t the array of length 2 containing xy in order
+ */
+ public void set(int[] t);
+
+ /**
+ * Sets the value of this tuple from the 2 values specified in
+ * the array.
+ * @param t the array of length 2 containing xy in order
+ */
+ public void set(float[] t);
+
+ /**
+ * Get the x coordinate.
+ *
+ * @return the x coordinate.
+ */
+ public float getX();
+
+ /**
+ * Get the x coordinate.
+ *
+ * @return the x coordinate.
+ */
+ public int x();
+
+ /**
+ * Set the x coordinate.
+ *
+ * @param x value to x coordinate.
+ */
+ public void setX(int x);
+
+ /**
+ * Set the x coordinate.
+ *
+ * @param x value to x coordinate.
+ */
+ public void setX(float x);
+
+ /**
+ * Get the y coordinate.
+ *
+ * @return the y coordinate.
+ */
+ public float getY();
+
+ /**
+ * Get the y coordinate.
+ *
+ * @return the y coordinate.
+ */
+ public int y();
+
+ /**
+ * Set the y coordinate.
+ *
+ * @param y value to y coordinate.
+ */
+ public void setY(int y);
+
+ /**
+ * Set the y coordinate.
+ *
+ * @param y value to y coordinate.
+ */
+ public void setY(float y);
+
+ /**
+ * Sets the value of this tuple to the difference of itself and x and y.
+ * @param x
+ * @param y
+ */
+ public void sub(int x, int y);
+
+ /**
+ * Sets the value of this tuple to the difference of itself and x and y.
+ * @param x
+ * @param y
+ */
+ public void sub(float x, float y);
+
+ /**
+ * Sets the x value of this tuple to the difference of itself and x.
+ * @param x
+ */
+ public void subX(int x);
+
+ /**
+ * Sets the x value of this tuple to the difference of itself and x.
+ * @param x
+ */
+ public void subX(float x);
+
+ /**
+ * Sets the y value of this tuple to the difference of itself and y.
+ * @param y
+ */
+ public void subY(int y);
+
+ /**
+ * Sets the y value of this tuple to the difference of itself and y.
+ * @param y
+ */
+ public void subY(float y);
+
+ /**
+ * Linearly interpolates between tuples t1 and t2 and places the
+ * result into this tuple: this = (1-alpha)*t1 + alpha*t2.
+ * @param t1 the first tuple
+ * @param t2 the second tuple
+ * @param alpha the alpha interpolation parameter
+ */
+ public void interpolate(TT t1, TT t2, float alpha);
+
+ /**
+ * Linearly interpolates between this tuple and tuple t1 and
+ * places the result into this tuple: this = (1-alpha)*this + alpha*t1.
+ * @param t1 the first tuple
+ * @param alpha the alpha interpolation parameter
+ */
+ public void interpolate(TT t1, float alpha);
+
+ /**
+ * Returns true if all of the data members of Tuple2f t1 are
+ * equal to the corresponding data members in this Tuple2f.
+ * @param t1 the vector with which the comparison is made
+ * @return true or false
+ */
+ public boolean equals(Tuple2D t1);
+
+ /**
+ * Returns true if the Object t1 is of type Tuple2f and all of the
+ * data members of t1 are equal to the corresponding data members in
+ * this Tuple2f.
+ * @param t1 the object with which the comparison is made
+ * @return true or false
+ */
+ @Override
+ public boolean equals(Object t1);
+
+ /**
+ * Returns true if the L-infinite distance between this tuple
+ * and tuple t1 is less than or equal to the epsilon parameter,
+ * otherwise returns false. The L-infinite
+ * distance is equal to MAX[abs(x1-x2), abs(y1-y2)].
+ * @param t1 the tuple to be compared to this tuple
+ * @param epsilon the threshold value
+ * @return true or false
+ */
+ public boolean epsilonEquals(TT t1, float epsilon);
+
+ /**
+ * Returns a hash code value based on the data values in this
+ * object. Two different Tuple2f objects with identical data values
+ * (i.e., Tuple2f.equals returns true) will return the same hash
+ * code value. Two objects with different data members may return the
+ * same hash value, although this is not likely.
+ * @return the integer hash code value
+ */
+ @Override
+ public int hashCode();
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/Vector2D.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/Vector2D.java
new file mode 100644
index 000000000..59294bee6
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/Vector2D.java
@@ -0,0 +1,183 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2012 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d;
+
+
+/** 2D Vector.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public interface Vector2D extends Tuple2D {
+
+ /**
+ * Sets the value of this tuple to the sum of tuples t1 and t2.
+ * @param t1 the first tuple
+ * @param t2 the second tuple
+ */
+ public void add(Vector2D t1, Vector2D t2);
+
+
+ /**
+ * Sets the value of this tuple to the sum of itself and t1.
+ * @param t1 the other tuple
+ */
+ public void add(Vector2D t1);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of tuple t1 plus tuple t2 (this = s*t1 + t2).
+ * @param s the scalar value
+ * @param t1 the tuple to be multipled
+ * @param t2 the tuple to be added
+ */
+ public void scaleAdd(int s, Vector2D t1, Vector2D t2);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of tuple t1 plus tuple t2 (this = s*t1 + t2).
+ * @param s the scalar value
+ * @param t1 the tuple to be multipled
+ * @param t2 the tuple to be added
+ */
+ public void scaleAdd(float s, Vector2D t1, Vector2D t2);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of itself and then adds tuple t1 (this = s*this + t1).
+ * @param s the scalar value
+ * @param t1 the tuple to be added
+ */
+ public void scaleAdd(int s, Vector2D t1);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of itself and then adds tuple t1 (this = s*this + t1).
+ * @param s the scalar value
+ * @param t1 the tuple to be added
+ */
+ public void scaleAdd(float s, Vector2D t1);
+
+
+ /**
+ * Sets the value of this tuple to the difference
+ * of tuples t1 and t2 (this = t1 - t2).
+ * @param t1 the first tuple
+ * @param t2 the second tuple
+ */
+ public void sub(Vector2D t1, Vector2D t2);
+
+ /**
+ * Sets the value of this tuple to the difference
+ * of tuples t1 and t2 (this = t1 - t2).
+ * @param t1 the first tuple
+ * @param t2 the second tuple
+ */
+ public void sub(Point2D t1, Point2D t2);
+
+ /**
+ * Sets the value of this tuple to the difference
+ * of itself and t1 (this = this - t1).
+ * @param t1 the other tuple
+ */
+ public void sub(Vector2D t1);
+
+ /**
+ * Computes the dot product of the this vector and vector v1.
+ * @param v1 the other vector
+ * @return the dot product.
+ */
+ public float dot(Vector2D v1);
+
+ /** Change the coordinates of this vector to make it a perpendicular
+ * vector to the original coordinates.
+ */
+ public void perpendicularize();
+
+ /**
+ * Returns the length of this vector.
+ * @return the length of this vector
+ */
+ public float length();
+
+ /**
+ * Returns the squared length of this vector.
+ * @return the squared length of this vector
+ */
+ public float lengthSquared();
+
+ /**
+ * Sets the value of this vector to the normalization of vector v1.
+ * @param v1 the un-normalized vector
+ */
+ public void normalize(Vector2D v1);
+
+ /**
+ * Normalizes this vector in place.
+ */
+ public void normalize();
+
+
+ /**
+ * Returns the angle in radians between this vector and the vector
+ * parameter; the return value is constrained to the range [0,PI].
+ * @param v1 the other vector
+ * @return the angle in radians in the range [0,PI]
+ */
+ public float angle(Vector2D v1);
+
+ /** Compute a signed angle between this vector and the given vector.
+ *
+ * The signed angle between this vector and {@code v}
+ * is the rotation angle to apply to this vector
+ * to be colinear to {@code v} and pointing the
+ * same demi-plane. It means that the angle replied
+ * by this function is be negative if the rotation
+ * to apply is clockwise, and positive if
+ * the rotation is counterclockwise.
+ *
+ * The value replied by {@link #angle(Vector2D)}
+ * is the absolute value of the vlaue replied by this
+ * function.
+ *
+ * @param v is the vector to reach.
+ * @return the rotation angle to turn this vector to reach
+ * {@code v}.
+ */
+ public float signedAngle(Vector2D v);
+
+ /** Turn this vector about the given rotation angle.
+ *
+ * @param angle is the rotation angle in radians.
+ */
+ public void turnVector(float angle);
+
+ /** Replies the orientation angle on a trigonometric circle
+ * that is corresponding to the given direction of this vector.
+ *
+ * @return the angle on a trigonometric circle that is corresponding
+ * to the given orientation vector.
+ */
+ public float getOrientationAngle();
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/AbstractRectangularShape2f.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/AbstractRectangularShape2f.java
new file mode 100644
index 000000000..1a3c17e0d
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/AbstractRectangularShape2f.java
@@ -0,0 +1,392 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.continuous;
+
+import org.arakhne.afc.math.geometry2d.Point2D;
+
+
+
+/** Abstract implementation of 2D rectangular shapes.
+ *
+ * @param is the type of the shape implemented by the instance of this class.
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public abstract class AbstractRectangularShape2f>
+extends AbstractShape2f {
+
+ private static final long serialVersionUID = -2330319571109966087L;
+
+ /** Lowest x-coordinate covered by this rectangular shape. */
+ protected float minx = 0f;
+ /** Lowest y-coordinate covered by this rectangular shape. */
+ protected float miny = 0f;
+ /** Highest x-coordinate covered by this rectangular shape. */
+ protected float maxx = 0f;
+ /** Highest y-coordinate covered by this rectangular shape. */
+ protected float maxy = 0f;
+
+ /**
+ */
+ public AbstractRectangularShape2f() {
+ //
+ }
+
+ /**
+ * @param min is the min corner of the rectangle.
+ * @param max is the max corner of the rectangle.
+ */
+ public AbstractRectangularShape2f(Point2f min, Point2f max) {
+ setFromCorners(min.getX(), min.getY(), max.getX(), max.getY());
+ }
+
+ /**
+ * @param x
+ * @param y
+ * @param width
+ * @param height
+ */
+ public AbstractRectangularShape2f(float x, float y, float width, float height) {
+ setFromCorners(x, y, x+width, y+height);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public void toBoundingBox(Rectangle2f box) {
+ box.setFromCorners(this.minx, this.minx, this.maxx, this.maxy);
+ }
+
+
+ @Override
+ public void clear() {
+ this.minx = this.miny = this.maxx = this.maxy = 0f;
+ }
+
+ /** Change the frame of the rectangle.
+ *
+ * @param x
+ * @param y
+ * @param width
+ * @param height
+ */
+ public void set(float x, float y, float width, float height) {
+ setFromCorners(x, y, x+width, y+height);
+ }
+
+ /** Change the frame of te rectangle.
+ *
+ * @param min is the min corner of the rectangle.
+ * @param max is the max corner of the rectangle.
+ */
+ public void set(Point2f min, Point2f max) {
+ setFromCorners(min.getX(), min.getY(), max.getX(), max.getY());
+ }
+
+ /** Change the frame of te rectangle.
+ *
+ * @param r
+ */
+ public void set(AbstractRectangularShape2f r) {
+ setFromCorners(r.getMinX(), r.getMinY(), r.getMaxX(), r.getMaxY());
+ }
+
+ /** Change the width of the rectangle, not the min corner.
+ *
+ * @param width
+ */
+ public void setWidth(float width) {
+ this.maxx = this.minx + Math.max(0f, width);
+ }
+
+ /** Change the height of the rectangle, not the min corner.
+ *
+ * @param height
+ */
+ public void setHeight(float height) {
+ this.maxy = this.miny + Math.max(0f, height);
+ }
+
+ /** Change the frame of the rectangle.
+ *
+ * @param p1 is the coordinate of the first corner.
+ * @param p2 is the coordinate of the second corner.
+ */
+ public void setFromCorners(Point2D p1, Point2D p2) {
+ setFromCorners(p1.getX(), p1.getY(), p2.getX(), p2.getY());
+ }
+
+ /** Change the frame of the rectangle.
+ *
+ * @param x1 is the coordinate of the first corner.
+ * @param y1 is the coordinate of the first corner.
+ * @param x2 is the coordinate of the second corner.
+ * @param y2 is the coordinate of the second corner.
+ */
+ public void setFromCorners(float x1, float y1, float x2, float y2) {
+ if (x1Shape
+ * based on the specified center point coordinates and corner point
+ * coordinates. The framing rectangle is used by the subclasses of
+ * RectangularShape to define their geometry.
+ *
+ * @param centerX the X coordinate of the specified center point
+ * @param centerY the Y coordinate of the specified center point
+ * @param cornerX the X coordinate of the specified corner point
+ * @param cornerY the Y coordinate of the specified corner point
+ */
+ public void setFromCenter(float centerX, float centerY, float cornerX, float cornerY) {
+ float dx = centerX - cornerX;
+ float dy = centerY - cornerY;
+ setFromCorners(cornerX, cornerY, centerX + dx, centerY + dy);
+ }
+
+ /** Replies the min X.
+ *
+ * @return the min x.
+ */
+ public float getMinX() {
+ return this.minx;
+ }
+
+ /** Set the min X.
+ *
+ * @param x the min x.
+ */
+ public void setMinX(float x) {
+ float o = this.maxx;
+ if (ox) {
+ this.maxx = o;
+ this.minx = x;
+ }
+ else {
+ this.maxx = x;
+ }
+ }
+
+ /** Replies the min y.
+ *
+ * @return the min y.
+ */
+ public float getMinY() {
+ return this.miny;
+ }
+
+ /** Set the min Y.
+ *
+ * @param y the min y.
+ */
+ public void setMinY(float y) {
+ float o = this.maxy;
+ if (oy) {
+ this.maxy = o;
+ this.miny = y;
+ }
+ else {
+ this.maxy = y;
+ }
+ }
+
+ /** Replies the width.
+ *
+ * @return the width.
+ */
+ public float getWidth() {
+ return this.maxx - this.minx;
+ }
+
+ /** Replies the height.
+ *
+ * @return the height.
+ */
+ public float getHeight() {
+ return this.maxy - this.miny;
+ }
+
+ @Override
+ public void translate(float dx, float dy) {
+ this.minx += dx;
+ this.miny += dy;
+ this.maxx += dx;
+ this.maxy += dy;
+ }
+
+ /** Replies if this rectangular shape is empty.
+ * The rectangular shape is empty when the
+ * two corners are at the same location.
+ *
+ * @return true if the rectangular shape is empty;
+ * otherwise false.
+ */
+ @Override
+ public boolean isEmpty() {
+ return this.minx==this.maxx && this.miny==this.maxy;
+ }
+
+ /** Inflate this rectangle with the given amounts.
+ *
+ * @param left
+ * @param top
+ * @param right
+ * @param bottom
+ */
+ public void inflate(float left, float top, float right, float bottom) {
+ this.minx -= left;
+ this.miny -= top;
+ this.maxx += right;
+ this.maxy += bottom;
+ }
+
+ /** Replies the rectangle that corresponds to the
+ * upper (max y), right (max x) corner.
+ *
+ * @return the upper-right quarter of this rectangle.
+ */
+ public Rectangle2f getNorthEastRectangle() {
+ float midX = (this.minx+this.maxx)/2.f;
+ float midY = (this.miny+this.maxy)/2.f;
+ return new Rectangle2f(
+ midX,midY,
+ this.maxx,this.maxy);
+ }
+
+ /** Replies the rectangle that corresponds to the
+ * max (max y), left (min x) corner.
+ *
+ * @return the max-left quarter of this rectangle.
+ */
+ public Rectangle2f getNorthWestRectangle() {
+ float midX = (this.minx+this.maxx)/2.f;
+ float midY = (this.miny+this.maxy)/2.f;
+ return new Rectangle2f(
+ this.minx,midY,
+ midX,this.maxy);
+ }
+
+ /** Replies the rectangle that corresponds to the
+ * min (min y), right (max x) corner.
+ *
+ * @return the min-right quarter of this rectangle.
+ */
+ public Rectangle2f getSouthEastRectangle() {
+ float midX = (this.minx+this.maxx)/2.f;
+ float midY = (this.miny+this.maxy)/2.f;
+ return new Rectangle2f(
+ midX,this.miny,
+ this.maxx,midY);
+ }
+
+ /** Replies the rectangle that corresponds to the
+ * min (min y), left (min x) corner.
+ *
+ * @return the min-left quarter of this rectangle.
+ */
+ public Rectangle2f getSouthWestRectangle() {
+ float midX = (this.minx+this.maxx)/2.f;
+ float midY = (this.miny+this.maxy)/2.f;
+ return new Rectangle2f(
+ this.minx,this.miny,
+ midX,midY);
+ }
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/AbstractShape2f.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/AbstractShape2f.java
new file mode 100644
index 000000000..b90ba4ca2
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/AbstractShape2f.java
@@ -0,0 +1,110 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.continuous;
+
+import org.arakhne.afc.math.geometry2d.Point2D;
+
+
+
+/** Abstract implementation of shapes.
+ *
+ * @param is the type of the shape implemented by the instance of this class.
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public abstract class AbstractShape2f implements Shape2f {
+
+ private static final long serialVersionUID = -2724377801599470453L;
+
+ /**
+ */
+ public AbstractShape2f() {
+ //
+ }
+
+ /** {@inheritDoc}
+ */
+ @SuppressWarnings("unchecked")
+ @Override
+ public T clone() {
+ try {
+ return (T)super.clone();
+ }
+ catch (CloneNotSupportedException e) {
+ throw new Error(e);
+ }
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public Shape2f createTransformedShape(Transform2D transform) {
+ return new Path2f(getPathIterator(transform));
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public final PathIterator2f getPathIterator() {
+ return getPathIterator(null);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public float distance(Point2D p) {
+ return (float)Math.sqrt(distanceSquared(p));
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public final boolean contains(Point2D p) {
+ return contains(p.getX(), p.getY());
+ }
+
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public abstract boolean equals(Object obj);
+
+ /** Compute the bit representation of the floating-point value.
+ *
+ * @param d
+ * @return the bit representation.
+ */
+ protected static int floatToIntBits(float d) {
+ // Check for +0 or -0
+ if (d == 0f) {
+ return 0;
+ }
+ return Float.floatToIntBits(d);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public abstract int hashCode();
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Circle2f.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Circle2f.java
new file mode 100644
index 000000000..d051799fe
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Circle2f.java
@@ -0,0 +1,579 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.continuous;
+
+import java.util.NoSuchElementException;
+
+import org.arakhne.afc.math.MathConstants;
+import org.arakhne.afc.math.geometry.GeometryUtil;
+import org.arakhne.afc.math.geometry.IntersectionUtil;
+import org.arakhne.afc.math.geometry.PathWindingRule;
+import org.arakhne.afc.math.geometry2d.Point2D;
+
+
+
+/** 2D circle with floating-point points.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class Circle2f extends AbstractShape2f {
+
+ private static final long serialVersionUID = -5535463117356287850L;
+
+ /**
+ * ArcIterator.btan(Math.PI/2)
+ */
+ static final float CTRL_VAL = 0.5522847498307933f;
+
+ /**
+ * ctrlpts contains the control points for a set of 4 cubic
+ * bezier curves that approximate a circle of radius 0.5
+ * centered at 0.5, 0.5
+ */
+ static final float PCV = 0.5f + CTRL_VAL * 0.5f;
+ /**
+ * ctrlpts contains the control points for a set of 4 cubic
+ * bezier curves that approximate a circle of radius 0.5
+ * centered at 0.5, 0.5
+ */
+ static final float NCV = 0.5f - CTRL_VAL * 0.5f;
+ /**
+ * ctrlpts contains the control points for a set of 4 cubic
+ * bezier curves that approximate a circle of radius 0.5
+ * centered at 0.5, 0.5
+ */
+ static float CTRL_PTS[][] = {
+ { 1.0f, PCV, PCV, 1.0f, 0.5f, 1.0f },
+ { NCV, 1.0f, 0.0f, PCV, 0.0f, 0.5f },
+ { 0.0f, NCV, NCV, 0.0f, 0.5f, 0.0f },
+ { PCV, 0.0f, 1.0f, NCV, 1.0f, 0.5f }
+ };
+
+ /** X-coordinate of the circle center. */
+ protected float cx = 0f;
+ /** Y-coordinate of the circle center. */
+ protected float cy = 0f;
+ /** Radius of the circle center (must be always positive). */
+ protected float radius = 0f;
+
+ /**
+ */
+ public Circle2f() {
+ //
+ }
+
+ /**
+ * @param center
+ * @param radius
+ */
+ public Circle2f(Point2D center, float radius) {
+ set(center, radius);
+ }
+
+ /**
+ * @param x
+ * @param y
+ * @param radius
+ */
+ public Circle2f(float x, float y, float radius) {
+ set(x, y, radius);
+ }
+
+ @Override
+ public void clear() {
+ this.cx = this.cy = 0f;
+ this.radius = 0f;
+ }
+
+ /** Replies if the circle is empty.
+ * The circle is empty when the radius is nul.
+ *
+ * @return true if the radius is nul;
+ * otherwise false.
+ */
+ @Override
+ public boolean isEmpty() {
+ return this.radius<=+0f;
+ }
+
+ /** Change the frame of the circle.
+ *
+ * @param x
+ * @param y
+ * @param radius
+ */
+ public void set(float x, float y, float radius) {
+ this.cx = x;
+ this.cy = y;
+ this.radius = Math.abs(radius);
+ }
+
+ /** Change the frame of te circle.
+ *
+ * @param center
+ * @param radius
+ */
+ public void set(Point2D center, float radius) {
+ this.cx = center.getX();
+ this.cy = center.getY();
+ this.radius = Math.abs(radius);
+ }
+
+ /** Replies the center X.
+ *
+ * @return the center x.
+ */
+ public float getX() {
+ return this.cx;
+ }
+
+ /** Replies the center y.
+ *
+ * @return the center y.
+ */
+ public float getY() {
+ return this.cy;
+ }
+
+ /** Replies the center.
+ *
+ * @return a copy of the center.
+ */
+ public Point2f getCenter() {
+ return new Point2f(this.cx, this.cy);
+ }
+
+ /** Replies the center.
+ *
+ * @param center
+ */
+ public void setCenter(Point2D center) {
+ this.cx = center.getX();
+ this.cy = center.getY();
+ }
+
+ /** Replies the center.
+ *
+ * @param x
+ * @param y
+ */
+ public void setCenter(float x, float y) {
+ this.cx = x;
+ this.cy = y;
+ }
+
+ /** Replies the radius.
+ *
+ * @return the radius.
+ */
+ public float getRadius() {
+ return this.radius;
+ }
+
+ /** Set the radius.
+ *
+ * @param radius is the radius.
+ */
+ public void setRadius(float radius) {
+ this.radius = Math.abs(radius);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public Rectangle2f toBoundingBox() {
+ Rectangle2f r = new Rectangle2f();
+ r.setFromCorners(
+ this.cx-this.radius,
+ this.cy-this.radius,
+ this.cx+this.radius,
+ this.cy+this.radius);
+ return r;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public void toBoundingBox(Rectangle2f box) {
+ box.setFromCorners(this.cx-this.radius,
+ this.cy-this.radius,
+ this.cx+this.radius,
+ this.cy+this.radius);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public float distance(Point2D p) {
+ float d = GeometryUtil.distancePointPoint(getX(), getY(), p.getX(), p.getY()) - getRadius();
+ return Math.max(0f, d);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public float distanceSquared(Point2D p) {
+ float d = distance(p);
+ return d * d;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public float distanceL1(Point2D p) {
+ Point2D r = getClosestPointTo(p);
+ return r.distanceL1(p);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public float distanceLinf(Point2D p) {
+ Point2D r = getClosestPointTo(p);
+ return r.distanceLinf(p);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public boolean contains(float x, float y) {
+ return GeometryUtil.isInsidePointCircle(x, y, getX(), getY(), getRadius());
+ }
+
+ @Override
+ public boolean contains(Rectangle2f r) {
+ return GeometryUtil.isInsideRectangleCircle(r.getMinX(), r.getMinY(), r.getWidth(), r.getHeight(),
+ getX(), getY(), getRadius());
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public Point2f getClosestPointTo(Point2D p) {
+
+ Point2f m = new Point2f();
+ GeometryUtil.closestPointPointCircle(p.getX(), p.getY(), this.cx, this.cy, this.radius, m);
+ return m;
+ }
+
+ @Override
+ public void translate(float dx, float dy) {
+ this.cx += dx;
+ this.cy += dy;
+ }
+
+ @Override
+ public PathIterator2f getPathIterator(Transform2D transform) {
+ if (transform==null)
+ return new CopyPathIterator(getX(), getY(), getRadius());
+ return new TransformPathIterator(getX(), getY(), getRadius(), transform);
+ }
+
+ @Override
+ public boolean equals(Object obj) {
+ if (obj == this) {
+ return true;
+ }
+ if (obj instanceof Circle2f) {
+ Circle2f rr2d = (Circle2f) obj;
+ return ((getX() == rr2d.getX()) &&
+ (getY() == rr2d.getY()) &&
+ (getRadius() == rr2d.getRadius()));
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ long bits = 1L;
+ bits = 31L * bits + floatToIntBits(getX());
+ bits = 31L * bits + floatToIntBits(getY());
+ bits = 31L * bits + floatToIntBits(getRadius());
+ return (int) (bits ^ (bits >> 32));
+ }
+
+ @Override
+ public boolean intersects(Rectangle2f s) {
+ return IntersectionUtil.intersectsCircleRectangle(
+ getX(), getY(), getRadius(),
+ s.getMinX(), s.getMinY(),
+ s.getMaxX(), s.getMaxY());
+ }
+
+ @Override
+ public boolean intersects(Ellipse2f s) {
+ return IntersectionUtil.intersectsEllipseEllipse(
+ getX()-getRadius(), getY()-getRadius(),
+ getX()+getRadius(), getY()+getRadius(),
+ s.getMinX(), s.getMinY(),
+ s.getMaxX(), s.getMaxY());
+ }
+
+ @Override
+ public boolean intersects(Circle2f s) {
+ return IntersectionUtil.intersectsCircleCircle(
+ getX(), getY(), getRadius(),
+ s.getX(), s.getY(), s.getRadius());
+ }
+
+ @Override
+ public boolean intersects(Segment2f s) {
+ return IntersectionUtil.intersectsCircleSegment(
+ getX(), getY(), getRadius(),
+ s.getX1(), s.getY1(),
+ s.getX2(), s.getY2());
+ }
+
+ @Override
+ public boolean intersects(OrientedRectangle2f s) {
+ return IntersectionUtil.intersectsSolidCircleOrientedRectangle(
+ this.cx,this.cy, this.radius,
+ s.getCx(), s.getCy(), s.getRx(), s.getRy(), s.getSx(), s.getSy(), s.getExtentR(), s.getExtentS(), 0);
+ }
+
+ @Override
+ public boolean intersects(Path2f s) {
+ return intersects(s.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO));
+ }
+
+ @Override
+ public boolean intersects(PathIterator2f s) {
+ int mask = (s.getWindingRule() == PathWindingRule.NON_ZERO ? -1 : 2);
+ int crossings = Path2f.computeCrossingsFromCircle(
+ 0,
+ s,
+ getX(), getY(), getRadius(),
+ false, true);
+ return (crossings == MathConstants.SHAPE_INTERSECTS ||
+ (crossings & mask) != 0);
+ }
+
+ @Override
+ public String toString() {
+ StringBuilder b = new StringBuilder();
+ b.append("["); //$NON-NLS-1$
+ b.append(getX());
+ b.append(";"); //$NON-NLS-1$
+ b.append(getY());
+ b.append(";"); //$NON-NLS-1$
+ b.append(getRadius());
+ b.append("]"); //$NON-NLS-1$
+ return b.toString();
+ }
+
+ /** Iterator on the path elements of the circle.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private static class CopyPathIterator implements PathIterator2f {
+
+ private final float x;
+ private final float y;
+ private final float r;
+ private int index = 0;
+ private float movex, movey;
+ private float lastx, lasty;
+
+ /**
+ * @param x
+ * @param y
+ * @param r
+ */
+ public CopyPathIterator(float x, float y, float r) {
+ this.r = Math.max(0f, r);
+ this.x = x - this.r;
+ this.y = y - this.r;
+ if (this.r<=0f) {
+ this.index = 6;
+ }
+ }
+
+ @Override
+ public boolean hasNext() {
+ return this.index<=5;
+ }
+
+ @Override
+ public PathElement2f next() {
+ if (this.index>5) throw new NoSuchElementException();
+ int idx = this.index;
+ ++this.index;
+ if (idx==0) {
+ float dr = 2f * this.r;
+ float ctrls[] = CTRL_PTS[3];
+ this.movex = (this.x + ctrls[4] * dr);
+ this.movey = (this.y + ctrls[5] * dr);
+ this.lastx = this.movex;
+ this.lasty = this.movey;
+ return new PathElement2f.MovePathElement2f(
+ this.lastx, this.lasty);
+ }
+ else if (idx<5) {
+ float dr = 2f * this.r;
+ float ctrls[] = CTRL_PTS[idx - 1];
+ float ppx = this.lastx;
+ float ppy = this.lasty;
+ this.lastx = (this.x + ctrls[4] * dr);
+ this.lasty = (this.y + ctrls[5] * dr);
+ return new PathElement2f.CurvePathElement2f(
+ ppx, ppy,
+ (this.x + ctrls[0] * dr),
+ (this.y + ctrls[1] * dr),
+ (this.x + ctrls[2] * dr),
+ (this.y + ctrls[3] * dr),
+ this.lastx, this.lasty);
+ }
+ float ppx = this.lastx;
+ float ppy = this.lasty;
+ this.lastx = this.movex;
+ this.lasty = this.movey;
+ return new PathElement2f.ClosePathElement2f(
+ ppx, ppy,
+ this.lastx, this.lasty);
+ }
+
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public PathWindingRule getWindingRule() {
+ return PathWindingRule.NON_ZERO;
+ }
+
+ @Override
+ public boolean isPolyline() {
+ return false;
+ }
+
+ }
+
+ /** Iterator on the path elements of the circle.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private static class TransformPathIterator implements PathIterator2f {
+
+ private final Point2D p1 = new Point2f();
+ private final Point2D p2 = new Point2f();
+ private final Point2D ptmp1 = new Point2f();
+ private final Point2D ptmp2 = new Point2f();
+ private final Transform2D transform;
+ private final float x;
+ private final float y;
+ private final float r;
+ private int index = 0;
+ private float movex, movey;
+
+ /**
+ * @param x
+ * @param y
+ * @param r
+ * @param transform
+ */
+ public TransformPathIterator(float x, float y, float r, Transform2D transform) {
+ assert(transform!=null);
+ this.transform = transform;
+ this.r = Math.max(0f, r);
+ this.x = x - this.r;
+ this.y = y - this.r;
+ if (this.r<=0f) {
+ this.index = 6;
+ }
+ }
+
+ @Override
+ public boolean hasNext() {
+ return this.index<=5;
+ }
+
+ @Override
+ public PathElement2f next() {
+ if (this.index>5) throw new NoSuchElementException();
+ int idx = this.index;
+ ++this.index;
+ if (idx==0) {
+ float dr = 2f * this.r;
+ float ctrls[] = CTRL_PTS[3];
+ this.movex = (this.x + ctrls[4] * dr);
+ this.movey = (this.y + ctrls[5] * dr);
+ this.p2.set(this.movex, this.movey);
+ this.transform.transform(this.p2);
+ return new PathElement2f.MovePathElement2f(
+ this.p2.getX(), this.p2.getY());
+ }
+ else if (idx<5) {
+ float dr = 2f * this.r;
+ float ctrls[] = CTRL_PTS[idx - 1];
+ this.p1.set(this.p2);
+ this.p2.set(
+ (this.x + ctrls[4] * dr),
+ (this.y + ctrls[5] * dr));
+ this.transform.transform(this.p2);
+ this.ptmp1.set(
+ (this.x + ctrls[0] * dr),
+ (this.y + ctrls[1] * dr));
+ this.transform.transform(this.ptmp1);
+ this.ptmp2.set(
+ (this.x + ctrls[2] * dr),
+ (this.y + ctrls[3] * dr));
+ this.transform.transform(this.ptmp2);
+ return new PathElement2f.CurvePathElement2f(
+ this.p1.getX(), this.p1.getY(),
+ this.ptmp1.getX(), this.ptmp1.getY(),
+ this.ptmp2.getX(), this.ptmp2.getY(),
+ this.p2.getX(), this.p2.getY());
+ }
+ this.p1.set(this.p2);
+ this.p2.set(this.movex, this.movey);
+ this.transform.transform(this.p2);
+ return new PathElement2f.ClosePathElement2f(
+ this.p1.getX(), this.p1.getY(),
+ this.p2.getX(), this.p2.getY());
+ }
+
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public PathWindingRule getWindingRule() {
+ return PathWindingRule.NON_ZERO;
+ }
+
+ @Override
+ public boolean isPolyline() {
+ return false;
+ }
+
+ }
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Ellipse2f.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Ellipse2f.java
new file mode 100644
index 000000000..1ccb23293
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Ellipse2f.java
@@ -0,0 +1,457 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.continuous;
+
+import java.util.NoSuchElementException;
+
+import org.arakhne.afc.math.MathConstants;
+import org.arakhne.afc.math.geometry.GeometryUtil;
+import org.arakhne.afc.math.geometry.IntersectionUtil;
+import org.arakhne.afc.math.geometry.PathWindingRule;
+import org.arakhne.afc.math.geometry2d.Point2D;
+
+
+/** 2D ellipse with floating-point points.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class Ellipse2f extends AbstractRectangularShape2f {
+
+ private static final long serialVersionUID = -2745313055404516167L;
+
+ // ArcIterator.btan(Math.PI/2)
+ private static final float CTRL_VAL = 0.5522847498307933f;
+
+ /**
+ * ctrlpts contains the control points for a set of 4 cubic
+ * bezier curves that approximate a circle of radius 0.5
+ * centered at 0.5, 0.5
+ */
+ static final float PCV = 0.5f + CTRL_VAL * 0.5f;
+ /**
+ * ctrlpts contains the control points for a set of 4 cubic
+ * bezier curves that approximate a circle of radius 0.5
+ * centered at 0.5, 0.5
+ */
+ static final float NCV = 0.5f - CTRL_VAL * 0.5f;
+ /**
+ * ctrlpts contains the control points for a set of 4 cubic
+ * bezier curves that approximate a circle of radius 0.5
+ * centered at 0.5, 0.5
+ */
+ static final float CTRL_PTS[][] = {
+ { 1.0f, PCV, PCV, 1.0f, 0.5f, 1.0f },
+ { NCV, 1.0f, 0.0f, PCV, 0.0f, 0.5f },
+ { 0.0f, NCV, NCV, 0.0f, 0.5f, 0.0f },
+ { PCV, 0.0f, 1.0f, NCV, 1.0f, 0.5f }
+ };
+
+ /**
+ */
+ public Ellipse2f() {
+ //
+ }
+
+ /**
+ * @param min is the min corner of the ellipse.
+ * @param max is the max corner of the ellipse.
+ */
+ public Ellipse2f(Point2f min, Point2f max) {
+ super(min, max);
+ }
+
+ /**
+ * @param x
+ * @param y
+ * @param width
+ * @param height
+ */
+ public Ellipse2f(float x, float y, float width, float height) {
+ super(x, y, width, height);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public Rectangle2f toBoundingBox() {
+ return new Rectangle2f(getMinX(), getMinY(), getMaxX(), getMaxY());
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public float distanceSquared(Point2D p) {
+ Point2D r = getClosestPointTo(p);
+ return r.distanceSquared(p);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public float distanceL1(Point2D p) {
+ Point2D r = getClosestPointTo(p);
+ return r.distanceL1(p);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public float distanceLinf(Point2D p) {
+ Point2D r = getClosestPointTo(p);
+ return r.distanceLinf(p);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean contains(float x, float y) {
+ return GeometryUtil.isInsidePointEllipse(
+ x, y,
+ getMinX(), getMinY(), getWidth(), getHeight());
+ }
+
+ @Override
+ public boolean contains(Rectangle2f r) {
+ return GeometryUtil.isInsideRectangleEllipse(
+ getMinX(), getMinY(), getWidth(), getHeight(),
+ r.getMinX(), r.getMinY(), r.getWidth(), r.getHeight());
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public Point2D getClosestPointTo(Point2D p) {
+
+ Point2f closest = new Point2f();
+ GeometryUtil.closestPointPointSolidEllipse(
+ p.getX(), p.getY(),
+ getMinX(), getMinY(),
+ getWidth(), getHeight(),false, closest);
+ return closest;
+ }
+
+ @Override
+ public PathIterator2f getPathIterator(Transform2D transform) {
+ if (transform==null) {
+ return new CopyPathIterator(
+ getMinX(), getMinY(),
+ getMaxX(), getMaxY());
+ }
+ return new TransformPathIterator(
+ getMinX(), getMinY(),
+ getMaxX(), getMaxY(),
+ transform);
+ }
+
+ @Override
+ public boolean equals(Object obj) {
+ if (obj == this) {
+ return true;
+ }
+ if (obj instanceof Ellipse2f) {
+ Ellipse2f rr2d = (Ellipse2f) obj;
+ return ((getMinX() == rr2d.getMinX()) &&
+ (getMinY() == rr2d.getMinY()) &&
+ (getWidth() == rr2d.getWidth()) &&
+ (getHeight() == rr2d.getHeight()));
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ long bits = 1L;
+ bits = 31L * bits + floatToIntBits(getMinX());
+ bits = 31L * bits + floatToIntBits(getMinY());
+ bits = 31L * bits + floatToIntBits(getMaxX());
+ bits = 31L * bits + floatToIntBits(getMaxY());
+ return (int) (bits ^ (bits >> 32));
+ }
+
+ @Override
+ public boolean intersects(Rectangle2f s) {
+ return IntersectionUtil.intersectsEllipseRectangle(
+ getMinX(), getMinY(),
+ getMaxX(), getMaxY(),
+ s.getMinX(), s.getMinY(),
+ s.getMaxX(), s.getMaxY());
+ }
+
+ @Override
+ public boolean intersects(Ellipse2f s) {
+ return IntersectionUtil.intersectsEllipseRectangle(
+ getMinX(), getMinY(),
+ getMaxX(), getMaxY(),
+ s.getMinX(), s.getMinY(),
+ s.getMaxX(), s.getMaxY());
+ }
+
+ @Override
+ public boolean intersects(Circle2f s) {
+ return IntersectionUtil.intersectsEllipseEllipse(
+ getMinX(), getMinY(),
+ getMaxX(), getMaxY(),
+ s.getX()-s.getRadius(), s.getY()-s.getRadius(),
+ s.getX()+s.getRadius(), s.getY()+s.getRadius());
+ }
+
+ @Override
+ public boolean intersects(Segment2f s) {
+ return IntersectionUtil.intersectsEllipseSegment(
+ getMinX(), getMinY(),
+ getWidth(), getHeight(),
+ s.getX1(), s.getY1(),
+ s.getX2(), s.getY2());
+ }
+
+ @Override
+ public boolean intersects(OrientedRectangle2f s) {
+ return IntersectionUtil.intersectsEllipseOrientedRectangle(
+ minx, miny, maxy - minx, maxy - maxy,
+ s.getCx(), s.getCy(), s.getRx(), s.getRy(), s.getSx(), s.getSy(), s.getExtentR(), s.getExtentS());
+ }
+
+ @Override
+ public boolean intersects(Path2f s) {
+ return intersects(s.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO));
+ }
+
+ @Override
+ public boolean intersects(PathIterator2f s) {
+ int mask = (s.getWindingRule() == PathWindingRule.NON_ZERO ? -1 : 2);
+ int crossings = Path2f.computeCrossingsFromEllipse(
+ 0,
+ s,
+ getMinX(), getMinY(), getWidth(), getHeight(),
+ false, true);
+ return (crossings == MathConstants.SHAPE_INTERSECTS ||
+ (crossings & mask) != 0);
+ }
+
+ @Override
+ public String toString() {
+ StringBuilder b = new StringBuilder();
+ b.append("["); //$NON-NLS-1$
+ b.append(getMinX());
+ b.append(";"); //$NON-NLS-1$
+ b.append(getMinY());
+ b.append(";"); //$NON-NLS-1$
+ b.append(getMaxX());
+ b.append(";"); //$NON-NLS-1$
+ b.append(getMaxY());
+ b.append("]"); //$NON-NLS-1$
+ return b.toString();
+ }
+
+ /**
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ public static class CopyPathIterator implements PathIterator2f {
+
+ private final float x1;
+ private final float y1;
+ private final float w;
+ private final float h;
+ private int index;
+ private float lastX, lastY;
+
+ /**
+ * @param x1
+ * @param y1
+ * @param x2
+ * @param y2
+ */
+ public CopyPathIterator(float x1, float y1, float x2, float y2) {
+ this.x1 = x1;
+ this.y1 = y1;
+ this.w = x2 - x1;
+ this.h = y2 - y1;
+ if (this.w==0f && this.h==0f) {
+ this.index = 6;
+ }
+ }
+
+ @Override
+ public boolean hasNext() {
+ return this.index<=5;
+ }
+
+ @Override
+ public PathElement2f next() {
+ if (this.index>5) throw new NoSuchElementException();
+ int idx = this.index;
+ ++this.index;
+
+ if (idx==0) {
+ float ctrls[] = CTRL_PTS[3];
+ this.lastX = this.x1 + ctrls[4] * this.w;
+ this.lastY = this.y1 + ctrls[5] * this.h;
+ return new PathElement2f.MovePathElement2f(
+ this.lastX, this.lastY);
+ }
+ else if (idx<5) {
+ float ctrls[] = CTRL_PTS[idx - 1];
+ float ix = this.lastX;
+ float iy = this.lastY;
+ this.lastX = (this.x1 + ctrls[4] * this.w);
+ this.lastY = (this.y1 + ctrls[5] * this.h);
+ return new PathElement2f.CurvePathElement2f(
+ ix, iy,
+ (this.x1 + ctrls[0] * this.w),
+ (this.y1 + ctrls[1] * this.h),
+ (this.x1 + ctrls[2] * this.w),
+ (this.y1 + ctrls[3] * this.h),
+ this.lastX,
+ this.lastY);
+ }
+
+ return new PathElement2f.ClosePathElement2f(
+ this.lastX, this.lastY,
+ this.lastX, this.lastY);
+ }
+
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public PathWindingRule getWindingRule() {
+ return PathWindingRule.NON_ZERO;
+ }
+
+ @Override
+ public boolean isPolyline() {
+ return false;
+ }
+
+ }
+
+ /**
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ public static class TransformPathIterator implements PathIterator2f {
+
+ private final Point2D lastPoint = new Point2f();
+ private final Point2D ptmp1 = new Point2f();
+ private final Point2D ptmp2 = new Point2f();
+ private final Transform2D transform;
+ private final float x1;
+ private final float y1;
+ private final float w;
+ private final float h;
+ private int index;
+
+ /**
+ * @param x1
+ * @param y1
+ * @param x2
+ * @param y2
+ * @param transform
+ */
+ public TransformPathIterator(float x1, float y1, float x2, float y2, Transform2D transform) {
+ this.transform = transform;
+ this.x1 = x1;
+ this.y1 = y1;
+ this.w = x2 - x1;
+ this.h = y2 - y1;
+ if (this.w==0f && this.h==0f) {
+ this.index = 6;
+ }
+ }
+
+ @Override
+ public boolean hasNext() {
+ return this.index<=5;
+ }
+
+ @Override
+ public PathElement2f next() {
+ if (this.index>5) throw new NoSuchElementException();
+ int idx = this.index;
+ ++this.index;
+
+ if (idx==0) {
+ float ctrls[] = CTRL_PTS[3];
+ this.lastPoint.set(
+ this.x1 + ctrls[4] * this.w,
+ this.y1 + ctrls[5] * this.h);
+ this.transform.transform(this.lastPoint);
+ return new PathElement2f.MovePathElement2f(
+ this.lastPoint.getX(), this.lastPoint.getY());
+ }
+ else if (idx<5) {
+ float ctrls[] = CTRL_PTS[idx - 1];
+ float ix = this.lastPoint.getX();
+ float iy = this.lastPoint.getY();
+ this.lastPoint.set(
+ (this.x1 + ctrls[4] * this.w),
+ (this.y1 + ctrls[5] * this.h));
+ this.transform.transform(this.lastPoint);
+ this.ptmp1.set(
+ (this.x1 + ctrls[0] * this.w),
+ (this.y1 + ctrls[1] * this.h));
+ this.transform.transform(this.ptmp1);
+ this.ptmp2.set(
+ (this.x1 + ctrls[2] * this.w),
+ (this.y1 + ctrls[3] * this.h));
+ this.transform.transform(this.ptmp2);
+ return new PathElement2f.CurvePathElement2f(
+ ix, iy,
+ this.ptmp1.getX(), this.ptmp1.getY(),
+ this.ptmp2.getX(), this.ptmp2.getY(),
+ this.lastPoint.getX(), this.lastPoint.getY());
+ }
+
+ float ix = this.lastPoint.getX();
+ float iy = this.lastPoint.getY();
+ return new PathElement2f.ClosePathElement2f(
+ ix, iy,
+ ix, iy);
+ }
+
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public PathWindingRule getWindingRule() {
+ return PathWindingRule.NON_ZERO;
+ }
+
+ @Override
+ public boolean isPolyline() {
+ return false;
+ }
+
+ }
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/OrientedRectangle2f.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/OrientedRectangle2f.java
new file mode 100644
index 000000000..7ac4a04c0
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/OrientedRectangle2f.java
@@ -0,0 +1,899 @@
+/*
+ * $Id$
+ *
+ * Copyright (c) 2013 Christophe BOHRHAUER
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+
+package org.arakhne.afc.math.geometry2d.continuous;
+
+import java.util.Arrays;
+import java.util.NoSuchElementException;
+
+import org.arakhne.afc.math.MathConstants;
+import org.arakhne.afc.math.MathUtil;
+import org.arakhne.afc.math.Matrix2f;
+import org.arakhne.afc.math.geometry.GeometryUtil;
+import org.arakhne.afc.math.geometry.IntersectionUtil;
+import org.arakhne.afc.math.geometry.PathWindingRule;
+import org.arakhne.afc.math.geometry2d.Point2D;
+import org.arakhne.afc.math.geometry2d.Vector2D;
+
+
+
+/**
+ * Definition of a fixed Oriented Bounding Rectangle (OBR),
+ * at least a 2D oriented bounding box.
+ *
+ * Algo inspired from Mathematics for 3D Game Programming and Computer Graphics (MGPCG)
+ * and from 3D Game Engine Design (GED)
+ * and from Real Time Collision Detection (RTCD).
+ *
+ * Rotations are not managed yet.
+ *
+ * @author $Author: cbohrhauer$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ * @see Mathematics for 3D Game Programming & Computer Graphics
+ */
+public class OrientedRectangle2f extends AbstractShape2f{
+
+ /**
+ *
+ */
+ private static final long serialVersionUID = -38383714233472426L;
+
+
+ /**
+ * Compute the oriented bounding box axis.
+ *
+ * @param is the type of the points.
+ * @param points is the list of the points enclosed by the OBR
+ * @param R is the vector where the R axis of the OBR is put
+ * @param S is the vector where the S axis of the OBR is put
+ * @see "MGPCG pages 219-221"
+ */
+ public static
void computeOBRAxis(P[] points, Vector2f R, Vector2f S) {
+ computeOBRAxis(Arrays.asList(points), R, S);
+ }
+
+ /**
+ * Compute the oriented bounding box axis.
+ *
+ * @param points is the list of the points enclosed by the OBR
+ * @param R is the vector where the R axis of the OBR is put
+ * @param S is the vector where the S axis of the OBR is put
+ * @see "MGPCG pages 219-221"
+ */
+ public static void computeOBRAxis(Iterable points, Vector2f R, Vector2f S) {
+ // Determining the covariance matrix of the points
+ // and set the center of the box
+ Matrix2f cov = new Matrix2f();
+ GeometryUtil.cov(cov, points);
+
+ //Determining eigenvectors of covariance matrix and defines RS axis
+ Matrix2f rs = new Matrix2f();//eigenvectors
+
+ MathUtil.eigenVectorsOfSymmetricMatrix(cov, rs);
+
+ rs.getColumn(0,R);
+ rs.getColumn(1,S);
+ }
+
+ /**
+ * Compute the OBR center and extents.
+ *
+ * @param
is the type of the points.
+ * @param points is the list of the points enclosed by the OBR
+ * @param R is the R axis of the OBR
+ * @param S is the S axis of the OBR
+ * @param center is the point which is set with the OBR's center coordinates.
+ * @param extents are the extents of the OBR for the R and S axis.
+ * @see "MGPCG pages 222-223"
+ */
+ public static
void computeOBRCenterExtents(
+ P[] points,
+ Vector2f R, Vector2f S,
+ Point2f center, float[] extents) {
+ computeOBRCenterExtents(Arrays.asList(points), R, S, center, extents);
+ }
+
+ /**
+ * Compute the OBR center and extents.
+ *
+ * @param points is the list of the points enclosed by the OBR
+ * @param R is the R axis of the OBR
+ * @param S is the S axis of the OBR
+ * @param center is the point which is set with the OBR's center coordinates.
+ * @param extents are the extents of the OBR for the R and S axis.
+ * @see "MGPCG pages 222-223"
+ */
+ public static void computeOBRCenterExtents(
+ Iterable points,
+ Vector2f R, Vector2f S,
+ Point2f center, float[] extents) {
+ assert(points!=null);
+ assert(center!=null);
+ assert(extents!=null && extents.length>=2);
+
+ float minR = Float.POSITIVE_INFINITY;
+ float maxR = Float.NEGATIVE_INFINITY;
+ float minS = Float.POSITIVE_INFINITY;
+ float maxS = Float.NEGATIVE_INFINITY;
+
+ float PdotR;
+ float PdotS;
+ Vector2f v = new Vector2f();
+
+ for(Point2f tuple : points) {
+ v.set(tuple);
+
+ PdotR = v.dot(R);
+ PdotS = v.dot(S);
+
+ if (PdotR < minR) minR = PdotR;
+ if (PdotR > maxR) maxR = PdotR;
+ if (PdotS < minS) minS = PdotS;
+ if (PdotS > maxS) maxS = PdotS;
+ }
+
+ float a = (maxR + minR) / 2.f;
+ float b = (maxS + minS) / 2.f;
+
+ // Set the center of the OBR
+ center.set(
+ a*R.getX()
+ +b*S.getX(),
+
+ a*R.getY()
+ +b*S.getY());
+
+ // Compute extents
+ extents[0] = (maxR - minR) / 2.f;
+ extents[1] = (maxS - minS) / 2.f;
+ }
+
+
+ /**
+ * Center of the OBR
+ */
+ private float cx;
+
+ /**
+ * Center of the OBR
+ */
+ private float cy;
+
+ /**
+ * X coordinate of the first axis of the OBR
+ */
+ private float rx;
+
+ /**
+ * Y coordinate of the first axis of the OBR
+ */
+ private float ry;
+
+ /**
+ * X coordinate of the second axis of the OBR
+ */
+ private float sx;
+
+ /**
+ * Y coordinate of the second axis of the OBR
+ */
+ private float sy;
+
+ /**
+ * Half-size of the first axis of the OBR
+ */
+ private float extentR;
+
+ /**
+ * Half-size of the second axis of the OBR
+ */
+ private float extentS;
+
+ /**
+ */
+ public OrientedRectangle2f() {
+ //
+ }
+
+ /**
+ */
+ public OrientedRectangle2f(OrientedRectangle2f r) {
+ this.cx = r.getCx();
+ this.cy = r.getCy();
+ this.rx = r.getRx();
+ this.ry = r.getRy();
+ this.sx = r.getSx();
+ this.sy = r.getSy();
+ this.extentR = r.getExtentR();
+ this.extentS = r.getExtentS();
+ }
+
+ public OrientedRectangle2f(Point2f center, Vector2f axis1, float axis1Extent, float axis2Extent){
+
+ assert(axis1.lengthSquared() == 1);
+ assert(axis1Extent > 0 && axis2Extent > 0);
+
+ if(axis1 != null && center != null){
+ this.cx = center.getX();
+ this.cy = center.getY();
+ this.rx = axis1.getX();
+ this.ry = axis1.getY();
+ Vector2f axis2 = new Vector2f(axis1);
+
+ axis2.perpendicularize();
+
+ this.sx = axis2.getX();
+ this.sy= axis2.getY();
+
+ this.extentR = axis1Extent;
+ this.extentS = axis2Extent;
+ }
+
+ }
+
+ /**the vector (rx,ry) must be normalized
+ *
+ * @param centerx
+ * @param centery
+ * @param rx
+ * @param ry
+ * @param axis1Extent
+ * @param axis2Extent
+ */
+ public OrientedRectangle2f(float centerx, float centery, float rx, float ry, float axis1Extent, float axis2Extent){
+
+ assert(axis1Extent > 0 && axis2Extent > 0);
+
+ this.cx = centerx;
+ this.cy = centery;
+ this.rx = rx;
+ this.ry = ry;
+
+ Vector2f axis2 = new Vector2f(rx, ry);
+
+ assert(axis2.lengthSquared() == 1);
+
+ axis2.perpendicularize();
+
+ this.sx = axis2.getX();
+ this.sy= axis2.getY();
+
+ this.extentR = axis1Extent;
+ this.extentS = axis2Extent;
+ }
+
+ public OrientedRectangle2f(Point2f... p){
+ Vector2f R,S;
+
+ R = new Vector2f();
+ S = new Vector2f();
+
+ OrientedRectangle2f.computeOBRAxis(p,R , S);
+
+ Point2f center = new Point2f();
+ float extents[] = new float[2];
+
+ OrientedRectangle2f.computeOBRCenterExtents(p, R, S, center, extents);
+
+ this.cx = center.getX();
+ this.cy = center.getY();
+ this.rx = R.getX();
+ this.ry = R.getY();
+ this.sx = S.getX();
+ this.sy = S.getY();
+ this.extentR = extents[0];
+ this.extentS = extents[1];
+ }
+
+ /**
+ * @return the cx
+ */
+ public float getCx() {
+ return cx;
+ }
+
+ /**
+ * @param cx the cx to set
+ */
+ public void setCx(float cx) {
+ this.cx = cx;
+ }
+
+ /**
+ * @return the cy
+ */
+ public float getCy() {
+ return cy;
+ }
+
+ /**
+ * @param cy the cy to set
+ */
+ public void setCy(float cy) {
+ this.cy = cy;
+ }
+
+ /**
+ * @return the rx
+ */
+ public float getRx() {
+ return rx;
+ }
+
+ /**
+ * @return the ry
+ */
+ public float getRy() {
+ return ry;
+ }
+
+ /**
+ * @return the sx
+ */
+ public float getSx() {
+ return sx;
+ }
+
+ /**
+ * @return the sy
+ */
+ public float getSy() {
+ return sy;
+ }
+
+ /**
+ * @return the extentR
+ */
+ public float getExtentR() {
+ return extentR;
+ }
+
+ /**
+ * @param extentR the extentR to set
+ */
+ public void setExtentR(float extentR) {
+ if(extentR > 0)
+ this.extentR = extentR;
+ }
+
+ /**
+ * @return the extentS
+ */
+ public float getExtentS() {
+ return extentS;
+ }
+
+ /**
+ * @param extentS the extentS to set
+ */
+ public void setExtentS(float extentS) {
+ if(extentS > 0)
+ this.extentS = extentS;
+ }
+
+ public Vector2f getR(){
+ return new Vector2f(this.rx, this.ry);
+ }
+
+ public Vector2f getS(){
+ return new Vector2f(this.sx, this.sy);
+ }
+
+
+ /**Set the R(rx,ry) vector with the vector newR this function also recompute Sx
+ *
+ * @param newR a non-zero normalized vector
+ */
+ public void setR(Vector2f newR){
+ assert(newR.lengthSquared() == 1);
+ if(newR != null){
+ this.rx = newR.getX();
+ this.ry = newR.getY();
+ Vector2f axis2 = new Vector2f(newR);
+
+ axis2.perpendicularize();
+
+ this.sx = axis2.getX();
+ this.sy= axis2.getY();
+ }
+ }
+
+ @Override
+ public Rectangle2f toBoundingBox() {
+ Rectangle2f rect = new Rectangle2f();
+ toBoundingBox(rect);
+ return rect;
+ }
+
+ @Override
+ public void toBoundingBox(Rectangle2f box) {
+ Point2f minCorner, maxCorner;
+
+ minCorner = new Point2f(cx,cy);
+ maxCorner = new Point2f(cx,cy);
+
+ float srx, sry, ssx, ssy;
+
+ srx = this.rx* this.extentR;
+ sry = this.ry* this.extentR;
+ ssx = this.sx* this.extentS;
+ ssy = this.sy* this.extentS;
+
+ if(rx >= 0)
+ if(ry >= 0){
+ minCorner.add(-srx + ssx, -sry - ssy);
+ maxCorner.sub(-srx + ssx, -sry - ssy);
+ }
+ else{
+ minCorner.add(-srx - ssx, sry - ssy);
+ maxCorner.sub(-srx - ssx, sry - ssy);
+ }
+ else{
+ if(ry >= 0){
+ minCorner.add( srx + ssx, -sry + ssy);
+ maxCorner.sub( srx + ssx, -sry + ssy);
+ }else{
+ minCorner.add( srx - ssx, sry + ssy);
+ maxCorner.sub( srx - ssx, sry + ssy);
+ }
+ }
+ box.setFromCorners(minCorner, maxCorner);
+ }
+
+ @Override
+ public float distanceSquared(Point2D p) {
+ Point2f closest = new Point2f();
+ GeometryUtil.closestFarthestPointsOBRPoint(p.getX(), p.getY(), cx, cx, rx, ry, sx, sx, extentR, extentS, closest, null);
+ return GeometryUtil.distanceSquaredPointPoint(p.getX(), p.getY(), closest.getX(), closest.getY());
+ }
+
+ @Override
+ public float distanceL1(Point2D p) {
+ Point2f closest = new Point2f();
+ GeometryUtil.closestFarthestPointsOBRPoint(p.getX(), p.getY(), cx, cx, rx, ry, sx, sx, extentR, extentS, closest, null);
+
+ return GeometryUtil.distanceL1PointPoint(p.getX(), p.getY(), closest.getX(), closest.getY());
+ }
+
+ @Override
+ public float distanceLinf(Point2D p) {
+ Point2f closest = new Point2f();
+ GeometryUtil.closestFarthestPointsOBRPoint(p.getX(), p.getY(), cx, cx, rx, ry, sx, sx, extentR, extentS, closest, null);
+
+ return GeometryUtil.distanceLInfPointPoint(p.getX(), p.getY(), closest.getX(), closest.getY());
+ }
+
+ @Override
+ public void translate(float dx, float dy) {
+ cx += dx;
+ cy += dy;
+ }
+
+ @Override
+ public boolean contains(float x, float y) {
+
+ return GeometryUtil.isInsidePointPointOrientedRectangle(x, y, this.cx, this.cy, this.rx, this.ry, this.sx, this.sy, extentR, extentS);
+ }
+
+ @Override
+ public boolean contains(Rectangle2f r) {
+
+ GeometryUtil.isInsideRectangleRectangleOrientedRectangle(
+ r.minx, r.miny, r.maxy, r.maxy,
+ this.cx, this.cy, this.rx, this.ry, this.sx, this.sy, extentR, extentS);
+ return false;
+ }
+
+ @Override
+ public PathIterator2f getPathIterator(Transform2D transform) {
+ if (transform==null) {
+ return new CopyPathIterator(
+ this.cx, this.cy, this.rx, this.ry, extentR, extentS);
+ }
+ return new TransformPathIterator(
+ this.cx, this.cy, this.rx, this.ry, extentR, extentS,
+ transform);
+ }
+
+ @Override
+ public boolean intersects(Rectangle2f s) {
+ return IntersectionUtil.intersectsAlignedRectangleOrientedRectangle(
+ s.minx, s.miny, s.maxy, s.maxy,
+ this.cx, this.cy, this.rx, this.ry, this.sx, this.sy, extentR, extentS);
+ }
+
+ @Override
+ public boolean intersects(Ellipse2f s) {
+ return IntersectionUtil.intersectsEllipseOrientedRectangle(
+ s.minx, s.miny, s.maxy - s.minx, s.maxy - s.maxy,
+ this.cx, this.cy, this.rx, this.ry, this.sx, this.sy, extentR, extentS);
+ }
+
+ @Override
+ public boolean intersects(Circle2f s) {
+ return IntersectionUtil.intersectsSolidCircleOrientedRectangle(
+ s.cx,s.cy, s.radius,
+ this.cx, this.cy, this.rx, this.ry, this.sx, this.sy, extentR, extentS, 0);
+ }
+
+ @Override
+ public boolean intersects(Segment2f s) {
+ return IntersectionUtil.intersectsSegmentOrientedRectangle(
+ s.ax, s.ay, s.bx, s.by,
+ this.cx, this.cy, this.rx, this.ry, this.sx, this.sy, extentR, extentS);
+ }
+
+ @Override
+ public boolean intersects(OrientedRectangle2f s) {
+ return IntersectionUtil.intersectsOrientedRectangleOrientedRectangle(
+ cx, cy, rx, ry, sx, sy, extentR, extentS,
+ s.getCx(), s.getCy(), s.getRx(), s.getRy(), s.getSx(), s.getSy(), s.getExtentR(), s.getExtentS());
+ }
+
+ @Override
+ public boolean intersects(Path2f s) {
+
+ return intersects(s.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO));
+ }
+
+
+ @Override
+ public boolean intersects(PathIterator2f s) {
+ if (isEmpty()) return false;
+ int mask = (s.getWindingRule() == PathWindingRule.NON_ZERO ? -1 : 2);
+
+
+ int crossings = Path2f.computeCrossingsFromRect(
+ new TranformPathIteratorWrapper(s), extentR/-2, extentS/-2, extentR/2, extentS/2,
+ false, true);
+ return (crossings == MathConstants.SHAPE_INTERSECTS ||
+ (crossings & mask) != 0);
+ }
+
+ private class TranformPathIteratorWrapper implements PathIterator2f{
+
+ private final PathIterator2f p;
+
+ public TranformPathIteratorWrapper(PathIterator2f p){
+ this.p = p;
+ }
+
+ @Override
+ public boolean hasNext() {
+ return this.p.hasNext();
+ }
+
+ @Override
+ public PathElement2f next() {
+ PathElement2f elem = this.p.next();
+ switch(elem.type){
+ case CURVE_TO:
+ return new PathElement2f.CurvePathElement2f(
+ (elem.fromX-cx) * sy - (elem.fromY-cy) * sx, (elem.fromY-cy) * rx - (elem.fromX-cx) * ry,
+ (elem.ctrlX1-cx) * sy - (elem.ctrlY1-cy) * sx, (elem.ctrlY1-cy) * rx - (elem.ctrlX1-cx) * ry,
+ (elem.ctrlX2-cx) * sy - (elem.ctrlY2-cy) * sx, (elem.ctrlY2-cy) * rx - (elem.ctrlX2-cx) * ry,
+ (elem.toX-cx) * sy - (elem.toY-cy) * sx, (elem.toY-cy) * rx - (elem.toX-cx) * ry);
+ case LINE_TO:
+ return new PathElement2f.LinePathElement2f(
+ (elem.fromX-cx) * sy - (elem.fromY-cy) * sx, (elem.fromY-cy) * rx - (elem.fromX-cx) * ry,
+ (elem.toX-cx) * sy - (elem.toY-cy) * sx, (elem.toY-cy) * rx - (elem.toX-cx) * ry);
+ case MOVE_TO:
+ return new PathElement2f.MovePathElement2f((elem.toX-cx) * sy - (elem.toY-cy) * sx, (elem.toY-cy) * rx - (elem.toX-cx) * ry);
+ case QUAD_TO:
+ return new PathElement2f.QuadPathElement2f(
+ (elem.fromX-cx) * sy - (elem.fromY-cy) * sx, (elem.fromY-cy) * rx - (elem.fromX-cx) * ry,
+ (elem.ctrlX1-cx) * sy - (elem.ctrlY1-cy) * sx, (elem.ctrlY1-cy) * rx - (elem.ctrlX1-cx) * ry,
+ (elem.toX-cx) * sy - (elem.toY-cy) * sx, (elem.toY-cy) * rx - (elem.toX-cx) * ry);
+ case CLOSE:
+ default:
+ break;
+
+ }
+ return null;
+ }
+
+ @Override
+ public void remove() {
+ this.p.remove();
+ }
+
+ @Override
+ public PathWindingRule getWindingRule() {
+ return this.p.getWindingRule();
+ }
+
+ @Override
+ public boolean isPolyline() {
+ return this.p.isPolyline();
+ }
+
+
+
+ }
+ @Override
+ public boolean isEmpty() {
+ return(this.extentR == 0 || this.extentS == 0 || (this.rx ==0 && this.ry == 0) || (this.sx == 0 && this.sy ==0));
+ }
+
+ @Override
+ public void clear() {
+
+ this.extentR = this.extentS = this.rx = this.ry = this.sx = this.sy = this.cx = this.cy = 0;
+ }
+
+ @Override
+ public Point2D getClosestPointTo(Point2D p) {
+ Point2f closest = new Point2f();
+ GeometryUtil.closestFarthestPointsOBRPoint(
+ p.getX(), p.getY(),this.cx, this.cy, this.rx, this.ry, this.sx, this.sy, extentR, extentS, closest, null);
+ return closest;
+ }
+
+ @Override
+ public boolean equals(Object obj) {
+
+ if (this==obj) return true;
+ if (obj instanceof OrientedRectangle2f) {
+ OrientedRectangle2f s = (OrientedRectangle2f)obj;
+ if(s.cx == this.cx &&
+ s.cy == this.cy &&
+ s.rx == this.rx &&
+ s.ry == this.ry &&
+ s.sx == this.sx &&
+ s.sy == this.sy &&
+ s.extentR == this.extentR &&
+ s.extentS == this.extentS)
+ return true;
+ }
+ return false;
+
+ }
+
+ @Override
+ public int hashCode() {
+ long bits = 1L;
+ bits = 31L * bits + floatToIntBits(getCx());
+ bits = 31L * bits + floatToIntBits(getCy());
+ bits = 31L * bits + floatToIntBits(getRx());
+ bits = 31L * bits + floatToIntBits(getRy());
+ bits = 31L * bits + floatToIntBits(getSx());
+ bits = 31L * bits + floatToIntBits(getSy());
+ bits = 31L * bits + floatToIntBits(getExtentR());
+ bits = 31L * bits + floatToIntBits(getExtentS());
+ return (int) (bits ^ (bits >> 32));
+ }
+
+ /** Iterator on the path elements of the rectangle.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private static class CopyPathIterator implements PathIterator2f {
+
+ private float x1;
+ private float y1;
+ private Vector2D r;
+ private Vector2D s;
+ private int index = 0;
+
+
+ public CopyPathIterator(float centerx, float centery, float rx, float ry, float axis1Extent, float axis2Extent) {
+ assert(axis1Extent > 0 && axis2Extent > 0);
+
+ this.r = new Vector2f(rx, ry);
+
+ assert(this.r.lengthSquared() == 1);
+
+ this.s = new Vector2f(rx, ry);
+ s.perpendicularize();
+
+ r.scale(axis1Extent);
+ s.scale(axis2Extent);
+
+ this.x1 = centerx - (r.getX() + s.getX());
+ this.y1 = centerx - (r.getY() + s.getY());
+
+ this.index = 6;
+
+ r.scale(2);
+ s.scale(2);
+ }
+
+ @Override
+ public boolean hasNext() {
+ return this.index<=5;
+ }
+
+ @Override
+ public PathElement2f next() {
+ int idx = this.index;
+ ++this.index;
+ switch(idx) {
+ case 0:
+ return new PathElement2f.MovePathElement2f(
+ this.x1, this.y1);
+ case 1:
+ return new PathElement2f.LinePathElement2f(
+ this.x1, this.y1,
+ this.x1 + r.getX(), this.y1 + r.getY());
+ case 2:
+ return new PathElement2f.LinePathElement2f(
+ this.x1 + r.getX(), this.y1 + r.getY(),
+ this.x1 + r.getX() + s.getX(), this.y1 + r.getY() + s.getY());
+ case 3:
+ return new PathElement2f.LinePathElement2f(
+ this.x1 + r.getX() + s.getX(), this.y1 + r.getY() + s.getY(),
+ this.x1 + s.getX(), this.y1 + s.getY());
+ case 4:
+ return new PathElement2f.LinePathElement2f(
+ this.x1 + s.getX(), this.y1 + s.getY(),
+ this.x1, this.y1);
+ case 5:
+ return new PathElement2f.ClosePathElement2f(
+ this.x1, this.y1,
+ this.x1, this.y1);
+ default:
+ throw new NoSuchElementException();
+ }
+ }
+
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public PathWindingRule getWindingRule() {
+ return PathWindingRule.NON_ZERO;
+ }
+
+ @Override
+ public boolean isPolyline() {
+ return true;
+ }
+ }
+
+ /** Iterator on the path elements of the rectangle.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private static class TransformPathIterator implements PathIterator2f {
+
+ private final Transform2D transform;
+ private float x1;
+ private float y1;
+ private Vector2D r;
+ private Vector2D s;
+
+ private int index = 0;
+
+ private final Point2D p1 = new Point2f();
+ private final Point2D p2 = new Point2f();
+
+ public TransformPathIterator(float centerx, float centery, float rx, float ry, float axis1Extent, float axis2Extent, Transform2D transform) {
+
+ assert(axis1Extent > 0 && axis2Extent > 0);
+
+ this.r = new Vector2f(rx, ry);
+
+ assert(this.r.lengthSquared() == 1);
+
+ this.s = new Vector2f(rx, ry);
+ s.perpendicularize();
+
+ r.scale(axis1Extent);
+ s.scale(axis2Extent);
+
+ this.transform = transform;
+
+ this.x1 = centerx - 1/2*(r.getX() + s.getX());
+ this.y1 = centerx - 1/2*(r.getY() + s.getY());
+
+ this.index = 6;
+
+ }
+
+ @Override
+ public boolean hasNext() {
+ return this.index<=5;
+ }
+
+ @Override
+ public PathElement2f next() {
+ int idx = this.index;
+ ++this.index;
+ switch(idx) {
+ case 0:
+ this.p2.set(this.x1, this.y1);
+ if (this.transform!=null) {
+ this.transform.transform(this.p2);
+ }
+ return new PathElement2f.MovePathElement2f(
+ this.p2.getX(), this.p2.getY());
+ case 1:
+ this.p1.set(this.p2);
+ this.p2.add(this.r);
+ if (this.transform!=null) {
+ this.transform.transform(this.p2);
+ }
+ return new PathElement2f.LinePathElement2f(
+ this.p1.getX(), this.p1.getY(),
+ this.p2.getX(), this.p2.getY());
+ case 2:
+ this.p1.set(this.p2);
+ this.p2.add(this.s);
+ if (this.transform!=null) {
+ this.transform.transform(this.p2);
+ }
+ return new PathElement2f.LinePathElement2f(
+ this.p1.getX(), this.p1.getY(),
+ this.p2.getX(), this.p2.getY());
+ case 3:
+ this.p1.set(this.p2);
+ this.p2.sub(this.r);
+ if (this.transform!=null) {
+ this.transform.transform(this.p2);
+ }
+ return new PathElement2f.LinePathElement2f(
+ this.p1.getX(), this.p1.getY(),
+ this.p2.getX(), this.p2.getY());
+ case 4:
+ this.p1.set(this.p2);
+ this.p2.sub(this.s);
+ if (this.transform!=null) {
+ this.transform.transform(this.p2);
+ }
+ return new PathElement2f.LinePathElement2f(
+ this.p1.getX(), this.p1.getY(),
+ this.p2.getX(), this.p2.getY());
+ case 5:
+ return new PathElement2f.ClosePathElement2f(
+ this.p2.getX(), this.p2.getY(),
+ this.p2.getX(), this.p2.getY());
+ default:
+ throw new NoSuchElementException();
+ }
+ }
+
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public PathWindingRule getWindingRule() {
+ return PathWindingRule.NON_ZERO;
+ }
+
+ @Override
+ public boolean isPolyline() {
+ return true;
+ }
+
+ }
+}
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Path2f.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Path2f.java
new file mode 100644
index 000000000..feb72e075
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Path2f.java
@@ -0,0 +1,3121 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2005-09 Stephane GALLAND.
+ * Copyright (C) 2012-13 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.continuous;
+
+import java.lang.ref.SoftReference;
+import java.util.Arrays;
+import java.util.Collection;
+import java.util.Iterator;
+import java.util.NoSuchElementException;
+
+import org.arakhne.afc.math.MathConstants;
+import org.arakhne.afc.math.MathUtil;
+import org.arakhne.afc.math.geometry.GeometryUtil;
+import org.arakhne.afc.math.geometry.PathElementType;
+import org.arakhne.afc.math.geometry.PathWindingRule;
+import org.arakhne.afc.math.geometry2d.Path2D;
+import org.arakhne.afc.math.geometry2d.Point2D;
+
+
+/** A generic path.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class Path2f extends AbstractShape2f implements Path2D {
+
+ private static final long serialVersionUID = -873231223923726975L;
+
+ /** Multiple of cubic & quad curve size.
+ */
+ static final int GROW_SIZE = 24;
+
+ /** Replies the point on the path that is closest to the given point.
+ *
+ * CAUTION: This function works only on path iterators
+ * that are replying polyline primitives, ie. if the
+ * {@link PathIterator2f#isPolyline()} of pi is replying
+ * true.
+ * {@link #getClosestPointTo(Point2D)} avoids this restriction.
+ *
+ * @param pi is the iterator on the elements of the path.
+ * @param x
+ * @param y
+ * @return the closest point on the shape; or the point itself
+ * if it is inside the shape.
+ */
+ public static Point2D getClosestPointTo(PathIterator2f pi, float x, float y) {
+ Point2D closest = null;
+ float bestDist = Float.POSITIVE_INFINITY;
+ Point2D candidate;
+ PathElement2f pe;
+
+ int mask = (pi.getWindingRule() == PathWindingRule.NON_ZERO ? -1 : 1);
+ int crossings = 0;
+
+ while (pi.hasNext()) {
+ pe = pi.next();
+
+ candidate = null;
+
+ switch(pe.type) {
+ case MOVE_TO:
+ candidate = new Point2f(pe.toX, pe.toY);
+ break;
+ case LINE_TO:
+ {
+ float factor = GeometryUtil.getPointProjectionFactorOnLine(
+ x, y,
+ pe.fromX, pe.fromY, pe.toX, pe.toY);
+ factor = MathUtil.clamp(factor, 0f, 1f);
+ Vector2f v = new Vector2f(pe.toX, pe.toY);
+ v.sub(pe.fromX, pe.fromY);
+ v.scale(factor);
+ candidate = new Point2f(
+ pe.fromX + v.getX(),
+ pe.fromY + v.getY());
+ crossings += GeometryUtil.computeCrossingsFromPoint(
+ x, y,
+ pe.fromX, pe.fromY, pe.toX, pe.toY);
+ break;
+ }
+ case CLOSE:
+ crossings += GeometryUtil.computeCrossingsFromPoint(
+ x, y,
+ pe.fromX, pe.fromY, pe.toX, pe.toY);
+ if ((crossings & mask) != 0) return new Point2f(x, y);
+
+ if (!pe.isEmpty()) {
+ float factor = GeometryUtil.getPointProjectionFactorOnLine(
+ x, y,
+ pe.fromX, pe.fromY, pe.toX, pe.toY);
+ factor = MathUtil.clamp(factor, 0f, 1f);
+ Vector2f v = new Vector2f(pe.toX, pe.toY);
+ v.sub(pe.fromX, pe.fromY);
+ v.scale(factor);
+ candidate = new Point2f(
+ pe.fromX + v.getX(),
+ pe.fromY + v.getY());
+ }
+ crossings = 0;
+ break;
+ case QUAD_TO:
+ case CURVE_TO:
+ default:
+ throw new IllegalStateException(
+ pe.type==null ? null : pe.type.toString());
+ }
+
+ if (candidate!=null) {
+ float d = candidate.distanceSquared(new Point2f(x,y));
+ if (d
+ * This method provides a basic facility for implementors of
+ * the {@link Shape2f} interface to implement support for the
+ * {@link Shape2f#contains(float, float)} method.
+ *
+ * @param pi the specified {@code PathIterator2f}
+ * @param x the specified X coordinate
+ * @param y the specified Y coordinate
+ * @return {@code true} if the specified coordinates are inside the
+ * specified {@code PathIterator2f}; {@code false} otherwise
+ */
+ public static boolean contains(PathIterator2f pi, float x, float y) {
+ // Copied from the AWT API
+ int mask = (pi.getWindingRule() == PathWindingRule.NON_ZERO ? -1 : 1);
+ int cross = computeCrossingsFromPoint(pi, x, y, false, true);
+ return ((cross & mask) != 0);
+ }
+
+ /**
+ * Tests if the specified rectangle is inside the closed
+ * boundary of the specified {@link PathIterator2f}.
+ *
+ * This method provides a basic facility for implementors of
+ * the {@link Shape2f} interface to implement support for the
+ * {@link Shape2f#contains(Rectangle2f)} method.
+ *
+ * @param pi the specified {@code PathIterator2f}
+ * @param rx the lowest corner of the rectangle.
+ * @param ry the lowest corner of the rectangle.
+ * @param rwidth is the width of the rectangle.
+ * @param rheight is the width of the rectangle.
+ * @return {@code true} if the specified rectangle is inside the
+ * specified {@code PathIterator2f}; {@code false} otherwise.
+ */
+ public static boolean contains(PathIterator2f pi, float rx, float ry, float rwidth, float rheight) {
+ // Copied from AWT API
+ if (rwidth <= 0 || rheight <= 0) {
+ return false;
+ }
+ int mask = (pi.getWindingRule() == PathWindingRule.NON_ZERO ? -1 : 2);
+ int crossings = computeCrossingsFromRect(
+ pi,
+ rx, ry, rx+rwidth, ry+rheight,
+ false,
+ true);
+ return (crossings != MathConstants.SHAPE_INTERSECTS &&
+ (crossings & mask) != 0);
+ }
+
+ /**
+ * Calculates the number of times the given path
+ * crosses the ray extending to the right from (px,py).
+ * If the point lies on a part of the path,
+ * then no crossings are counted for that intersection.
+ * +1 is added for each crossing where the Y coordinate is increasing
+ * -1 is added for each crossing where the Y coordinate is decreasing
+ * The return value is the sum of all crossings for every segment in
+ * the path.
+ * The path must start with a MOVE_TO, otherwise an exception is
+ * thrown.
+ *
+ * @param pi is the description of the path.
+ * @param px is the reference point to test.
+ * @param py is the reference point to test.
+ * @return the crossing
+ */
+ public static int computeCrossingsFromPoint(PathIterator2f pi, float px, float py) {
+ return computeCrossingsFromPoint(pi, px, py, true, true);
+ }
+
+ /**
+ * Calculates the number of times the given path
+ * crosses the ray extending to the right from (px,py).
+ * If the point lies on a part of the path,
+ * then no crossings are counted for that intersection.
+ * +1 is added for each crossing where the Y coordinate is increasing
+ * -1 is added for each crossing where the Y coordinate is decreasing
+ * The return value is the sum of all crossings for every segment in
+ * the path.
+ * The path must start with a MOVE_TO, otherwise an exception is
+ * thrown.
+ *
+ * @param pi is the description of the path.
+ * @param px is the reference point to test.
+ * @param py is the reference point to test.
+ * @param closeable indicates if the shape is automatically closed or not.
+ * @param onlyIntersectWhenOpen indicates if the crossings is set to 0 when
+ * the path is open and there is not SHAPE_INTERSECT.
+ * @return the crossing
+ */
+ public static int computeCrossingsFromPoint(
+ PathIterator2f pi,
+ float px, float py,
+ boolean closeable,
+ boolean onlyIntersectWhenOpen) {
+ // Copied from the AWT API
+ if (!pi.hasNext()) return 0;
+ PathElement2f element;
+
+ element = pi.next();
+ if (element.type != PathElementType.MOVE_TO) {
+ throw new IllegalArgumentException("missing initial moveto in path definition"); //$NON-NLS-1$
+ }
+
+ Path2f subPath;
+ float movx = element.toX;
+ float movy = element.toY;
+ float curx = movx;
+ float cury = movy;
+ float endx, endy;
+ int r, crossings = 0;
+ while (pi.hasNext()) {
+ element = pi.next();
+ switch (element.type) {
+ case MOVE_TO:
+ movx = curx = element.toX;
+ movy = cury = element.toY;
+ break;
+ case LINE_TO:
+ endx = element.toX;
+ endy = element.toY;
+ if (endx==px && endy==py)
+ return MathConstants.SHAPE_INTERSECTS;
+ crossings += GeometryUtil.computeCrossingsFromPoint(
+ px, py,
+ curx, cury,
+ endx, endy);
+ curx = endx;
+ cury = endy;
+ break;
+ case QUAD_TO:
+ endx = element.toX;
+ endy = element.toY;
+ if (endx==px && endy==py)
+ return MathConstants.SHAPE_INTERSECTS;
+ subPath = new Path2f();
+ subPath.moveTo(curx, cury);
+ subPath.quadTo(
+ element.ctrlX1, element.ctrlY1,
+ endx, endy);
+ r = computeCrossingsFromPoint(
+ subPath.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ px, py,
+ false,
+ false);
+ if (r==MathConstants.SHAPE_INTERSECTS)
+ return r;
+ crossings += r;
+ curx = endx;
+ cury = endy;
+ break;
+ case CURVE_TO:
+ endx = element.toX;
+ endy = element.toY;
+ if (endx==px || endy==py)
+ return MathConstants.SHAPE_INTERSECTS;
+ subPath = new Path2f();
+ subPath.moveTo(curx, cury);
+ subPath.curveTo(
+ element.ctrlX1, element.ctrlY1,
+ element.ctrlX2, element.ctrlY2,
+ endx, endy);
+ r = computeCrossingsFromPoint(
+ subPath.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ px, py,
+ false,
+ false);
+ if (r==MathConstants.SHAPE_INTERSECTS) {
+ return r;
+ }
+ crossings += r;
+ curx = endx;
+ cury = endy;
+ break;
+ case CLOSE:
+ if (cury != movy || curx != movx) {
+ if (movx==px && movy==py)
+ return MathConstants.SHAPE_INTERSECTS;
+ crossings += GeometryUtil.computeCrossingsFromPoint(
+ px, py,
+ curx, cury,
+ movx, movy);
+ }
+ curx = movx;
+ cury = movy;
+ break;
+ default:
+ }
+ }
+
+ assert(crossings!=MathConstants.SHAPE_INTERSECTS);
+
+ boolean isOpen = (curx != movx) || (cury != movy);
+
+ if (isOpen) {
+ if (closeable) {
+ // Not closed
+ if (movx==px && movy==py)
+ return MathConstants.SHAPE_INTERSECTS;
+ crossings += GeometryUtil.computeCrossingsFromPoint(
+ px, py,
+ curx, cury,
+ movx, movy);
+ }
+ else if (onlyIntersectWhenOpen) {
+ // Assume that when is the path is open, only
+ // SHAPE_INTERSECTS may be return
+ crossings = 0;
+ }
+ }
+
+ return crossings;
+ }
+
+ /**
+ * Calculates the number of times the given path
+ * crosses the given ellipse extending to the right.
+ *
+ * @param pi is the description of the path.
+ * @param ex is the first point of the ellipse.
+ * @param ey is the first point of the ellipse.
+ * @param ew is the width of the ellipse.
+ * @param eh is the height of the ellipse.
+ * @return the crossing or {@link MathConstants#SHAPE_INTERSECTS}
+ */
+ public static int computeCrossingsFromEllipse(PathIterator2f pi, float ex, float ey, float ew, float eh) {
+ return computeCrossingsFromEllipse(0, pi, ex, ey, ew, eh, true, true);
+ }
+
+ /**
+ * Calculates the number of times the given path
+ * crosses the given ellipse extending to the right.
+ *
+ * @param crossings is the initial value for crossing.
+ * @param pi is the description of the path.
+ * @param ex is the first point of the ellipse.
+ * @param ey is the first point of the ellipse.
+ * @param ew is the width of the ellipse.
+ * @param eh is the height of the ellipse.
+ * @param closeable indicates if the shape is automatically closed or not.
+ * @param onlyIntersectWhenOpen indicates if the crossings is set to 0 when
+ * the path is open and there is not SHAPE_INTERSECT.
+ * @return the crossing or {@link MathConstants#SHAPE_INTERSECTS}
+ */
+ public static int computeCrossingsFromEllipse(
+ int crossings,
+ PathIterator2f pi,
+ float ex, float ey, float ew, float eh,
+ boolean closeable,
+ boolean onlyIntersectWhenOpen) {
+ // Copied from the AWT API
+ if (!pi.hasNext()) return 0;
+ PathElement2f element;
+
+ element = pi.next();
+ if (element.type != PathElementType.MOVE_TO) {
+ throw new IllegalArgumentException("missing initial moveto in path definition"); //$NON-NLS-1$
+ }
+
+ float movx = element.toX;
+ float movy = element.toY;
+ float curx = movx;
+ float cury = movy;
+ float endx, endy;
+ int numCrosses = crossings;
+ while (numCrosses!=MathConstants.SHAPE_INTERSECTS && pi.hasNext()) {
+ element = pi.next();
+ switch (element.type) {
+ case MOVE_TO:
+ movx = curx = element.toX;
+ movy = cury = element.toY;
+ break;
+ case LINE_TO:
+ endx = element.toX;
+ endy = element.toY;
+ numCrosses = GeometryUtil.computeCrossingsFromEllipse(
+ numCrosses,
+ ex, ey, ew, eh,
+ curx, cury,
+ endx, endy);
+ if (numCrosses==MathConstants.SHAPE_INTERSECTS) {
+ return numCrosses;
+ }
+ curx = endx;
+ cury = endy;
+ break;
+ case QUAD_TO:
+ {
+ endx = element.toX;
+ endy = element.toY;
+ Path2f localPath = new Path2f();
+ localPath.moveTo(curx, cury);
+ localPath.quadTo(
+ element.ctrlX1, element.ctrlY1,
+ endx, endy);
+ numCrosses = computeCrossingsFromEllipse(
+ numCrosses,
+ localPath.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ ex, ey, ew, eh,
+ false,
+ false);
+ if (numCrosses==MathConstants.SHAPE_INTERSECTS) {
+ return numCrosses;
+ }
+ curx = endx;
+ cury = endy;
+ break;
+ }
+ case CURVE_TO:
+ endx = element.toX;
+ endy = element.toY;
+ Path2f localPath = new Path2f();
+ localPath.moveTo(curx, cury);
+ localPath.curveTo(
+ element.ctrlX1, element.ctrlY1,
+ element.ctrlX2, element.ctrlY2,
+ endx, endy);
+ numCrosses = computeCrossingsFromEllipse(
+ numCrosses,
+ localPath.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ ex, ey, ew, eh,
+ false,
+ false);
+ if (numCrosses==MathConstants.SHAPE_INTERSECTS) {
+ return numCrosses;
+ }
+ curx = endx;
+ cury = endy;
+ break;
+ case CLOSE:
+ if (cury != movy || curx != movx) {
+ numCrosses = GeometryUtil.computeCrossingsFromEllipse(
+ numCrosses,
+ ex, ey, ew, eh,
+ curx, cury,
+ movx, movy);
+ if (numCrosses==MathConstants.SHAPE_INTERSECTS) {
+ return numCrosses;
+ }
+ }
+ curx = movx;
+ cury = movy;
+ break;
+ default:
+ }
+ }
+
+ assert(numCrosses!=MathConstants.SHAPE_INTERSECTS);
+
+ boolean isOpen = (curx != movx) || (cury != movy);
+
+ if (isOpen) {
+ if (closeable) {
+ // Not closed
+ numCrosses = GeometryUtil.computeCrossingsFromEllipse(
+ numCrosses,
+ ex, ey, ew, eh,
+ curx, cury,
+ movx, movy);
+ }
+ else if (onlyIntersectWhenOpen) {
+ // Assume that when is the path is open, only
+ // SHAPE_INTERSECTS may be return
+ numCrosses = 0;
+ }
+ }
+
+ return numCrosses;
+ }
+
+ /**
+ * Calculates the number of times the given path
+ * crosses the given ellipse extending to the right.
+ *
+ * @param pi is the description of the path.
+ * @param cx is the center of the circle.
+ * @param cy is the center of the circle.
+ * @param radius is the radius of the circle.
+ * @return the crossing or {@link MathConstants#SHAPE_INTERSECTS}.
+ */
+ public static int computeCrossingsFromCircle(PathIterator2f pi, float cx, float cy, float radius) {
+ return computeCrossingsFromCircle(0, pi, cx, cy, radius, true, true);
+ }
+
+ /**
+ * Calculates the number of times the given path
+ * crosses the given circle extending to the right.
+ *
+ * @param crossings is the initial value for crossing.
+ * @param pi is the description of the path.
+ * @param cx is the center of the circle.
+ * @param cy is the center of the circle.
+ * @param radius is the radius of the circle.
+ * @param closeable indicates if the shape is automatically closed or not.
+ * @param onlyIntersectWhenOpen indicates if the crossings is set to 0 when
+ * the path is open and there is not SHAPE_INTERSECT.
+ * @return the crossing
+ */
+ public static int computeCrossingsFromCircle(
+ int crossings,
+ PathIterator2f pi,
+ float cx, float cy, float radius,
+ boolean closeable,
+ boolean onlyIntersectWhenOpen) {
+ // Copied from the AWT API
+ if (!pi.hasNext()) return 0;
+ PathElement2f element;
+
+ element = pi.next();
+ if (element.type != PathElementType.MOVE_TO) {
+ throw new IllegalArgumentException("missing initial moveto in path definition"); //$NON-NLS-1$
+ }
+
+ float movx = element.toX;
+ float movy = element.toY;
+ float curx = movx;
+ float cury = movy;
+ float endx, endy;
+ int numCrosses = crossings;
+ while (numCrosses!=MathConstants.SHAPE_INTERSECTS && pi.hasNext()) {
+ element = pi.next();
+ switch (element.type) {
+ case MOVE_TO:
+ movx = curx = element.toX;
+ movy = cury = element.toY;
+ break;
+ case LINE_TO:
+ endx = element.toX;
+ endy = element.toY;
+ numCrosses = GeometryUtil.computeCrossingsFromCircle(
+ numCrosses,
+ cx, cy, radius,
+ curx, cury,
+ endx, endy);
+ if (numCrosses==MathConstants.SHAPE_INTERSECTS) {
+ return numCrosses;
+ }
+ curx = endx;
+ cury = endy;
+ break;
+ case QUAD_TO:
+ {
+ endx = element.toX;
+ endy = element.toY;
+ Path2f localPath = new Path2f();
+ localPath.moveTo(element.fromX, element.fromY);
+ localPath.quadTo(
+ element.ctrlX1, element.ctrlY1,
+ endx, endy);
+ numCrosses = computeCrossingsFromCircle(
+ numCrosses,
+ localPath.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ cx, cy, radius,
+ false,
+ false);
+ if (numCrosses==MathConstants.SHAPE_INTERSECTS) {
+ return numCrosses;
+ }
+ curx = endx;
+ cury = endy;
+ break;
+ }
+ case CURVE_TO:
+ endx = element.toX;
+ endy = element.toY;
+ Path2f localPath = new Path2f();
+ localPath.moveTo(element.fromX, element.fromY);
+ localPath.curveTo(
+ element.ctrlX1, element.ctrlY1,
+ element.ctrlX2, element.ctrlY2,
+ endx, endy);
+ numCrosses = computeCrossingsFromCircle(
+ numCrosses,
+ localPath.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ cx, cy, radius,
+ false,
+ false);
+ if (numCrosses==MathConstants.SHAPE_INTERSECTS) {
+ return numCrosses;
+ }
+ curx = endx;
+ cury = endy;
+ break;
+ case CLOSE:
+ if (cury != movy || curx != movx) {
+ numCrosses = GeometryUtil.computeCrossingsFromCircle(
+ numCrosses,
+ cx, cy, radius,
+ curx, cury,
+ movx, movy);
+ if (numCrosses==MathConstants.SHAPE_INTERSECTS) {
+ return numCrosses;
+ }
+ }
+ curx = movx;
+ cury = movy;
+ break;
+ default:
+ }
+ }
+
+ assert(numCrosses!=MathConstants.SHAPE_INTERSECTS);
+
+ boolean isOpen = (curx != movx) || (cury != movy);
+
+ if (isOpen) {
+ if (closeable) {
+ // Not closed
+ numCrosses = GeometryUtil.computeCrossingsFromCircle(
+ numCrosses,
+ cx, cy, radius,
+ curx, cury,
+ movx, movy);
+ }
+ else if (onlyIntersectWhenOpen) {
+ // Assume that when is the path is open, only
+ // SHAPE_INTERSECTS may be return
+ numCrosses = 0;
+ }
+ }
+
+ return numCrosses;
+ }
+
+ /**
+ * Calculates the number of times the given path
+ * crosses the given segment extending to the right.
+ *
+ * @param pi is the description of the path.
+ * @param x1 is the first point of the segment.
+ * @param y1 is the first point of the segment.
+ * @param x2 is the first point of the segment.
+ * @param y2 is the first point of the segment.
+ * @return the crossing or {@link MathConstants#SHAPE_INTERSECTS}.
+ */
+ public static int computeCrossingsFromSegment(PathIterator2f pi, float x1, float y1, float x2, float y2) {
+ return computeCrossingsFromSegment(0, pi, x1, y1, x2, y2, true);
+ }
+
+ /**
+ * Calculates the number of times the given path
+ * crosses the given segment extending to the right.
+ *
+ * @param crossings is the initial value for crossing.
+ * @param pi is the description of the path.
+ * @param x1 is the first point of the segment.
+ * @param y1 is the first point of the segment.
+ * @param x2 is the first point of the segment.
+ * @param y2 is the first point of the segment.
+ * @param closeable indicates if the shape is automatically closed or not.
+ * @return the crossing
+ */
+ public static int computeCrossingsFromSegment(int crossings, PathIterator2f pi, float x1, float y1, float x2, float y2, boolean closeable) {
+ // Copied from the AWT API
+ if (!pi.hasNext() || crossings==MathConstants.SHAPE_INTERSECTS) return crossings;
+ PathElement2f element;
+
+ element = pi.next();
+ if (element.type != PathElementType.MOVE_TO) {
+ throw new IllegalArgumentException("missing initial moveto in path definition"); //$NON-NLS-1$
+ }
+
+ float movx = element.toX;
+ float movy = element.toY;
+ float curx = movx;
+ float cury = movy;
+ float endx, endy;
+ int numCrosses = crossings;
+ while (numCrosses!=MathConstants.SHAPE_INTERSECTS && pi.hasNext()) {
+ element = pi.next();
+ switch (element.type) {
+ case MOVE_TO:
+ movx = curx = element.toX;
+ movy = cury = element.toY;
+ break;
+ case LINE_TO:
+ endx = element.toX;
+ endy = element.toY;
+ numCrosses = GeometryUtil.computeCrossingsFromSegment(
+ numCrosses,
+ x1, y1, x2, y2,
+ curx, cury,
+ endx, endy);
+ if (numCrosses==MathConstants.SHAPE_INTERSECTS)
+ return numCrosses;
+ curx = endx;
+ cury = endy;
+ break;
+ case QUAD_TO:
+ {
+ endx = element.toX;
+ endy = element.toY;
+ Path2f localPath = new Path2f();
+ localPath.moveTo(curx, cury);
+ localPath.quadTo(
+ element.ctrlX1, element.ctrlY1,
+ endx, endy);
+ numCrosses = computeCrossingsFromSegment(
+ numCrosses,
+ localPath.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ x1, y1, x2, y2,
+ false);
+ if (numCrosses==MathConstants.SHAPE_INTERSECTS)
+ return numCrosses;
+ curx = endx;
+ cury = endy;
+ break;
+ }
+ case CURVE_TO:
+ endx = element.toX;
+ endy = element.toY;
+ Path2f localPath = new Path2f();
+ localPath.moveTo(curx, cury);
+ localPath.curveTo(
+ element.ctrlX1, element.ctrlY1,
+ element.ctrlX2, element.ctrlY2,
+ endx, endy);
+ numCrosses = computeCrossingsFromSegment(
+ numCrosses,
+ localPath.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ x1, y1, x2, y2,
+ false);
+ if (numCrosses==MathConstants.SHAPE_INTERSECTS)
+ return numCrosses;
+ curx = endx;
+ cury = endy;
+ break;
+ case CLOSE:
+ if (cury != movy || curx != movx) {
+ numCrosses = GeometryUtil.computeCrossingsFromSegment(
+ numCrosses,
+ x1, y1, x2, y2,
+ curx, cury,
+ movx, movy);
+ }
+ if (numCrosses!=0) return numCrosses;
+ curx = movx;
+ cury = movy;
+ break;
+ default:
+ }
+ }
+
+ assert(numCrosses!=MathConstants.SHAPE_INTERSECTS);
+
+ boolean isOpen = (curx != movx) || (cury != movy);
+
+ if (isOpen) {
+ if (closeable) {
+ numCrosses = GeometryUtil.computeCrossingsFromSegment(
+ numCrosses,
+ x1, y1, x2, y2,
+ curx, cury,
+ movx, movy);
+ }
+ else {
+ // Assume that when is the path is open, only
+ // SHAPE_INTERSECTS may be return
+ numCrosses = 0;
+ }
+ }
+
+ return numCrosses;
+ }
+
+ /**
+ * Accumulate the number of times the path crosses the shadow
+ * extending to the right of the rectangle. See the comment
+ * for the SHAPE_INTERSECTS constant for more complete details.
+ * The return value is the sum of all crossings for both the
+ * top and bottom of the shadow for every segment in the path,
+ * or the special value SHAPE_INTERSECTS if the path ever enters
+ * the interior of the rectangle.
+ * The path must start with a SEG_MOVETO, otherwise an exception is
+ * thrown.
+ * The caller must check r[xy]{min,max} for NaN values.
+ *
+ * @param pi is the iterator on the path elements.
+ * @param rxmin is the first corner of the rectangle.
+ * @param rymin is the first corner of the rectangle.
+ * @param rxmax is the second corner of the rectangle.
+ * @param rymax is the second corner of the rectangle.
+ * @return the crossings.
+ */
+ public static int computeCrossingsFromRect(PathIterator2f pi,
+ float rxmin, float rymin,
+ float rxmax, float rymax) {
+ return computeCrossingsFromRect(pi, rxmin, rymin, rxmax, rymax, true, true);
+ }
+
+ /**
+ * Accumulate the number of times the path crosses the shadow
+ * extending to the right of the rectangle. See the comment
+ * for the SHAPE_INTERSECTS constant for more complete details.
+ * The return value is the sum of all crossings for both the
+ * top and bottom of the shadow for every segment in the path,
+ * or the special value SHAPE_INTERSECTS if the path ever enters
+ * the interior of the rectangle.
+ * The path must start with a SEG_MOVETO, otherwise an exception is
+ * thrown.
+ * The caller must check r[xy]{min,max} for NaN values.
+ *
+ * @param pi is the iterator on the path elements.
+ * @param rxmin is the first corner of the rectangle.
+ * @param rymin is the first corner of the rectangle.
+ * @param rxmax is the second corner of the rectangle.
+ * @param rymax is the second corner of the rectangle.
+ * @param closeable indicates if the shape is automatically closed or not.
+ * @param onlyIntersectWhenOpen indicates if the crossings is set to 0 when
+ * the path is open and there is not SHAPE_INTERSECT.
+ * @return the crossings.
+ */
+ public static int computeCrossingsFromRect(PathIterator2f pi,
+ float rxmin, float rymin,
+ float rxmax, float rymax,
+ boolean closeable,
+ boolean onlyIntersectWhenOpen) {
+ // Copied from AWT API
+ if (rxmax <= rxmin || rymax <= rymin) return 0;
+ if (!pi.hasNext()) return 0;
+
+ PathElement2f pathElement = pi.next();
+
+ if (pathElement.type != PathElementType.MOVE_TO) {
+ throw new IllegalArgumentException("missing initial moveto in path definition"); //$NON-NLS-1$
+ }
+
+ Path2f subPath;
+ float curx, cury, movx, movy, endx, endy;
+ curx = movx = pathElement.toX;
+ cury = movy = pathElement.toY;
+ int crossings = 0;
+ int n;
+
+ while (crossings != MathConstants.SHAPE_INTERSECTS
+ && pi.hasNext()) {
+ pathElement = pi.next();
+ switch (pathElement.type) {
+ case MOVE_TO:
+ // Count should always be a multiple of 2 here.
+ // assert((crossings & 1) != 0);
+ movx = curx = pathElement.toX;
+ movy = cury = pathElement.toY;
+ break;
+ case LINE_TO:
+ endx = pathElement.toX;
+ endy = pathElement.toY;
+ crossings = GeometryUtil.computeCrossingsFromRect(crossings,
+ rxmin, rymin,
+ rxmax, rymax,
+ curx, cury,
+ endx, endy);
+ if (crossings==MathConstants.SHAPE_INTERSECTS)
+ return crossings;
+ curx = endx;
+ cury = endy;
+ break;
+ case QUAD_TO:
+ endx = pathElement.toX;
+ endy = pathElement.toY;
+ subPath = new Path2f();
+ subPath.moveTo(curx, cury);
+ subPath.quadTo(
+ pathElement.ctrlX1, pathElement.ctrlY1,
+ endx, endy);
+ n = computeCrossingsFromRect(
+ subPath.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ rxmin, rymin,
+ rxmax, rymax,
+ false,
+ false);
+ if (n==MathConstants.SHAPE_INTERSECTS)
+ return n;
+ crossings += n;
+ curx = endx;
+ cury = endy;
+ break;
+ case CURVE_TO:
+ endx = pathElement.toX;
+ endy = pathElement.toY;
+ subPath = new Path2f();
+ subPath.moveTo(curx, cury);
+ subPath.curveTo(
+ pathElement.ctrlX1, pathElement.ctrlY1,
+ pathElement.ctrlX2, pathElement.ctrlY2,
+ endx, endy);
+ n = computeCrossingsFromRect(
+ subPath.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ rxmin, rymin,
+ rxmax, rymax,
+ false,
+ false);
+ if (n==MathConstants.SHAPE_INTERSECTS)
+ return n;
+ crossings += n;
+ curx = endx;
+ cury = endy;
+ break;
+ case CLOSE:
+ if (curx != movx || cury != movy) {
+ crossings = GeometryUtil.computeCrossingsFromRect(crossings,
+ rxmin, rymin,
+ rxmax, rymax,
+ curx, cury,
+ movx, movy);
+ }
+ // Stop as soon as possible
+ if (crossings!=0) return crossings;
+ curx = movx;
+ cury = movy;
+ break;
+ default:
+ }
+ }
+
+ assert(crossings != MathConstants.SHAPE_INTERSECTS);
+
+ boolean isOpen = (curx != movx) || (cury != movy);
+
+ if (isOpen) {
+ if (closeable) {
+ // Not closed
+ crossings = GeometryUtil.computeCrossingsFromRect(crossings,
+ rxmin, rymin,
+ rxmax, rymax,
+ curx, cury,
+ movx, movy);
+ }
+ else if (onlyIntersectWhenOpen) {
+ // Assume that when is the path is open, only
+ // SHAPE_INTERSECTS may be return
+ crossings = 0;
+ }
+ }
+
+ return crossings;
+ }
+
+ /** Array of types.
+ */
+ PathElementType[] types;
+
+ /** Array of coords.
+ */
+ float[] coords;
+
+ /** Number of types in the array.
+ */
+ int numTypes = 0;
+
+ /** Number of coords in the array.
+ */
+ int numCoords = 0;
+
+ /** Winding rule for the path.
+ */
+ PathWindingRule windingRule;
+
+ /** Indicates if the path is empty.
+ * The path is empty when there is no point inside, or
+ * all the points are at the same coordinate, or
+ * when the path does not represents a drawable path
+ * (a path with a line or a curve).
+ */
+ private Boolean isEmpty = Boolean.TRUE;
+
+ /** Indicates if the path contains base primitives
+ * (no curve).
+ */
+ private Boolean isPolyline = Boolean.TRUE;
+
+ /** Buffer for the bounds of the path that corresponds
+ * to the points really on the path (eg, the pixels
+ * drawn). The control points of the curves are
+ * not considered in this bounds.
+ */
+ private SoftReference graphicalBounds = null;
+
+ /** Buffer for the bounds of the path that corresponds
+ * to all the points added in the path.
+ */
+ private SoftReference logicalBounds = null;
+
+ /**
+ */
+ public Path2f() {
+ this(PathWindingRule.NON_ZERO);
+ }
+
+ /**
+ * @param iterator
+ */
+ public Path2f(Iterator iterator) {
+ this(PathWindingRule.NON_ZERO, iterator);
+ }
+
+ /**
+ * @param windingRule
+ */
+ public Path2f(PathWindingRule windingRule) {
+ assert(windingRule!=null);
+ this.types = new PathElementType[GROW_SIZE];
+ this.coords = new float[GROW_SIZE];
+ this.windingRule = windingRule;
+ }
+
+ /**
+ * @param windingRule
+ * @param iterator
+ */
+ public Path2f(PathWindingRule windingRule, Iterator iterator) {
+ assert(windingRule!=null);
+ this.types = new PathElementType[GROW_SIZE];
+ this.coords = new float[GROW_SIZE];
+ this.windingRule = windingRule;
+ add(iterator);
+ }
+
+ @Override
+ public void clear() {
+ this.types = new PathElementType[GROW_SIZE];
+ this.coords = new float[GROW_SIZE];
+ this.windingRule = PathWindingRule.NON_ZERO;
+ this.numCoords = 0;
+ this.numTypes = 0;
+ this.isEmpty = true;
+ this.isPolyline = true;
+ this.graphicalBounds = null;
+ this.logicalBounds = null;
+ }
+
+ @Override
+ public String toString() {
+ StringBuilder b = new StringBuilder();
+ b.append("["); //$NON-NLS-1$
+ if (this.numCoords>0) {
+ b.append(this.coords[0]);
+ for(int i=1; i iterator) {
+ PathElement2f element;
+ while (iterator.hasNext()) {
+ element = iterator.next();
+ switch(element.type) {
+ case MOVE_TO:
+ moveTo(element.toX, element.toY);
+ break;
+ case LINE_TO:
+ lineTo(element.toX, element.toY);
+ break;
+ case QUAD_TO:
+ quadTo(element.ctrlX1, element.ctrlY1, element.toX, element.toY);
+ break;
+ case CURVE_TO:
+ curveTo(element.ctrlX1, element.ctrlY1, element.ctrlX2, element.ctrlY2, element.toX, element.toY);
+ break;
+ case CLOSE:
+ closePath();
+ break;
+ default:
+ }
+ }
+ }
+
+ private void ensureSlots(boolean needMove, int n) {
+ if (needMove && this.numTypes==0) {
+ throw new IllegalStateException("missing initial moveto in path definition"); //$NON-NLS-1$
+ }
+ if (this.types.length==this.numTypes) {
+ this.types = Arrays.copyOf(this.types, this.types.length+GROW_SIZE);
+ }
+ while ((this.numCoords+n)>=this.coords.length) {
+ this.coords = Arrays.copyOf(this.coords, this.coords.length+GROW_SIZE);
+ }
+ }
+
+ /**
+ * Adds a point to the path by moving to the specified
+ * coordinates specified in float precision.
+ *
+ * @param x the specified X coordinate
+ * @param y the specified Y coordinate
+ */
+ public void moveTo(float x, float y) {
+ if (this.numTypes>0 && this.types[this.numTypes-1]==PathElementType.MOVE_TO) {
+ this.coords[this.numCoords-2] = x;
+ this.coords[this.numCoords-1] = y;
+ }
+ else {
+ ensureSlots(false, 2);
+ this.types[this.numTypes++] = PathElementType.MOVE_TO;
+ this.coords[this.numCoords++] = x;
+ this.coords[this.numCoords++] = y;
+ }
+ this.graphicalBounds = null;
+ this.logicalBounds = null;
+ }
+
+ /**
+ * Adds a point to the path by drawing a straight line from the
+ * current coordinates to the new specified coordinates
+ * specified in float precision.
+ *
+ * @param x the specified X coordinate
+ * @param y the specified Y coordinate
+ */
+ public void lineTo(float x, float y) {
+ ensureSlots(true, 2);
+ this.types[this.numTypes++] = PathElementType.LINE_TO;
+ this.coords[this.numCoords++] = x;
+ this.coords[this.numCoords++] = y;
+ this.isEmpty = null;
+ this.graphicalBounds = null;
+ this.logicalBounds = null;
+ }
+
+ /**
+ * Adds a curved segment, defined by two new points, to the path by
+ * drawing a Quadratic curve that intersects both the current
+ * coordinates and the specified coordinates {@code (x2,y2)},
+ * using the specified point {@code (x1,y1)} as a quadratic
+ * parametric control point.
+ * All coordinates are specified in float precision.
+ *
+ * @param x1 the X coordinate of the quadratic control point
+ * @param y1 the Y coordinate of the quadratic control point
+ * @param x2 the X coordinate of the final end point
+ * @param y2 the Y coordinate of the final end point
+ */
+ public void quadTo(float x1, float y1, float x2, float y2) {
+ ensureSlots(true, 4);
+ this.types[this.numTypes++] = PathElementType.QUAD_TO;
+ this.coords[this.numCoords++] = x1;
+ this.coords[this.numCoords++] = y1;
+ this.coords[this.numCoords++] = x2;
+ this.coords[this.numCoords++] = y2;
+ this.isEmpty = null;
+ this.isPolyline = false;
+ this.graphicalBounds = null;
+ this.logicalBounds = null;
+ }
+
+ /**
+ * Adds a curved segment, defined by three new points, to the path by
+ * drawing a Bézier curve that intersects both the current
+ * coordinates and the specified coordinates {@code (x3,y3)},
+ * using the specified points {@code (x1,y1)} and {@code (x2,y2)} as
+ * Bézier control points.
+ * All coordinates are specified in float precision.
+ *
+ * @param x1 the X coordinate of the first Bézier control point
+ * @param y1 the Y coordinate of the first Bézier control point
+ * @param x2 the X coordinate of the second Bézier control point
+ * @param y2 the Y coordinate of the second Bézier control point
+ * @param x3 the X coordinate of the final end point
+ * @param y3 the Y coordinate of the final end point
+ */
+ public void curveTo(float x1, float y1,
+ float x2, float y2,
+ float x3, float y3) {
+ ensureSlots(true, 6);
+ this.types[this.numTypes++] = PathElementType.CURVE_TO;
+ this.coords[this.numCoords++] = x1;
+ this.coords[this.numCoords++] = y1;
+ this.coords[this.numCoords++] = x2;
+ this.coords[this.numCoords++] = y2;
+ this.coords[this.numCoords++] = x3;
+ this.coords[this.numCoords++] = y3;
+ this.isEmpty = null;
+ this.isPolyline = false;
+ this.graphicalBounds = null;
+ this.logicalBounds = null;
+ }
+
+ /**
+ * Closes the current subpath by drawing a straight line back to
+ * the coordinates of the last {@code moveTo}. If the path is already
+ * closed or if the previous coordinates are for a {@code moveTo}
+ * then this method has no effect.
+ */
+ public void closePath() {
+ if (this.numTypes<=0 ||
+ (this.types[this.numTypes-1]!=PathElementType.CLOSE
+ &&this.types[this.numTypes-1]!=PathElementType.MOVE_TO)) {
+ ensureSlots(true, 0);
+ this.types[this.numTypes++] = PathElementType.CLOSE;
+ }
+ }
+
+ @Override
+ public PathIterator2f getPathIterator(float flatness) {
+ return new FlatteningPathIterator(getWindingRule(), getPathIterator(null), flatness, 10);
+ }
+
+ /** Replies an iterator on the path elements.
+ *
+ * Only {@link PathElementType#MOVE_TO},
+ * {@link PathElementType#LINE_TO}, and
+ * {@link PathElementType#CLOSE} types are returned by the iterator.
+ *
+ * The amount of subdivision of the curved segments is controlled by the
+ * flatness parameter, which specifies the maximum distance that any point
+ * on the unflattened transformed curve can deviate from the returned
+ * flattened path segments. Note that a limit on the accuracy of the
+ * flattened path might be silently imposed, causing very small flattening
+ * parameters to be treated as larger values. This limit, if there is one,
+ * is defined by the particular implementation that is used.
+ *
+ * The iterator for this class is not multi-threaded safe.
+ *
+ * @param transform is an optional affine Transform2D to be applied to the
+ * coordinates as they are returned in the iteration, or null if
+ * untransformed coordinates are desired.
+ * @param flatness is the maximum distance that the line segments used to approximate
+ * the curved segments are allowed to deviate from any point on the original curve.
+ * @return an iterator on the path elements.
+ */
+ public PathIterator2f getPathIterator(Transform2D transform, float flatness) {
+ return new FlatteningPathIterator(getWindingRule(), getPathIterator(transform), flatness, 10);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public PathIterator2f getPathIterator(Transform2D transform) {
+ if (transform == null) {
+ return new CopyPathIterator();
+ }
+ return new TransformPathIterator(transform);
+ }
+
+ /** Transform the current path.
+ * This function changes the current path.
+ *
+ * @param transform is the affine transformation to apply.
+ * @see #createTransformedShape(Transform2D)
+ */
+ public void transform(Transform2D transform) {
+ if (transform!=null) {
+ Point2D p = new Point2f();
+ for(int i=0; ixmax) xmax = element.fromX;
+ if (element.fromY>ymax) ymax = element.fromY;
+ if (element.toXxmax) xmax = element.toX;
+ if (element.toY>ymax) ymax = element.toY;
+ foundOneLine = true;
+ break;
+ case CURVE_TO:
+ subPath = new Path2f();
+ subPath.moveTo(element.fromX, element.fromY);
+ subPath.curveTo(
+ element.ctrlX1, element.ctrlY1,
+ element.ctrlX2, element.ctrlY2,
+ element.toX, element.toY);
+ if (buildGraphicalBoundingBox(
+ subPath.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ box)) {
+ if (box.getMinX()xmax) xmax = box.getMaxX();
+ if (box.getMinX()>ymax) ymax = box.getMinX();
+ foundOneLine = true;
+ }
+ break;
+ case QUAD_TO:
+ subPath = new Path2f();
+ subPath.moveTo(element.fromX, element.fromY);
+ subPath.quadTo(
+ element.ctrlX1, element.ctrlY1,
+ element.toX, element.toY);
+ if (buildGraphicalBoundingBox(
+ subPath.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ box)) {
+ if (box.getMinX()xmax) xmax = box.getMaxX();
+ if (box.getMinX()>ymax) ymax = box.getMinX();
+ foundOneLine = true;
+ }
+ break;
+ case MOVE_TO:
+ case CLOSE:
+ default:
+ }
+ }
+ if (foundOneLine) {
+ box.setFromCorners(xmin, ymin, xmax, ymax);
+ }
+ else {
+ box.clear();
+ }
+ return foundOneLine;
+ }
+
+ private boolean buildLogicalBoundingBox(Rectangle2f box) {
+ if (this.numCoords>0) {
+ float xmin = Float.POSITIVE_INFINITY;
+ float ymin = Float.POSITIVE_INFINITY;
+ float xmax = Float.NEGATIVE_INFINITY;
+ float ymax = Float.NEGATIVE_INFINITY;
+ for(int i=0; ixmax) xmax = this.coords[i];
+ if (this.coords[i+1]>ymax) ymax = this.coords[i+1];
+ }
+ box.setFromCorners(xmin, ymin, xmax, ymax);
+ return true;
+ }
+ return false;
+ }
+
+ /**
+ * {@inheritDoc}
+ *
+ * The replied bounding box does not consider the control points
+ * of the path. Only the "visible" points are considered.
+ *
+ * @see #toBoundingBoxWithCtrlPoints()
+ */
+ @Override
+ public Rectangle2f toBoundingBox() {
+ Rectangle2f bb = this.graphicalBounds==null ? null : this.graphicalBounds.get();
+ if (bb==null) {
+ bb = new Rectangle2f();
+ buildGraphicalBoundingBox(
+ getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ bb);
+ this.graphicalBounds = new SoftReference(bb);
+ }
+ return bb.clone();
+ }
+
+ /** Replies the bounding box of all the points added in this path.
+ *
+ * The replied bounding box includes the (invisible) control points.
+ *
+ * @return the bounding box with the control points.
+ * @see #toBoundingBox()
+ */
+ public Rectangle2f toBoundingBoxWithCtrlPoints() {
+ Rectangle2f bb = this.logicalBounds==null ? null : this.logicalBounds.get();
+ if (bb==null) {
+ bb = new Rectangle2f();
+ buildLogicalBoundingBox(bb);
+ this.logicalBounds = new SoftReference(bb);
+ }
+ return bb.clone();
+ }
+
+ /**
+ * {@inheritDoc}
+ *
+ * The replied bounding box does not consider the control points
+ * of the path. Only the "visible" points are considered.
+ *
+ * @see #toBoundingBoxWithCtrlPoints(Rectangle2f)
+ */
+ @Override
+ public void toBoundingBox(Rectangle2f box) {
+ Rectangle2f bb = this.graphicalBounds==null ? null : this.graphicalBounds.get();
+ if (bb==null) {
+ bb = new Rectangle2f();
+ buildGraphicalBoundingBox(
+ getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ bb);
+ this.graphicalBounds = new SoftReference(bb);
+ }
+ box.set(bb);
+ }
+
+ /** Compute the bounding box of all the points added in this path.
+ *
+ * The replied bounding box includes the (invisible) control points.
+ *
+ * @param box is the rectangle to set with the bounds.
+ * @see #toBoundingBox()
+ */
+ public void toBoundingBoxWithCtrlPoints(Rectangle2f box) {
+ Rectangle2f bb = this.logicalBounds==null ? null : this.logicalBounds.get();
+ if (bb==null) {
+ bb = new Rectangle2f();
+ buildLogicalBoundingBox(bb);
+ this.logicalBounds = new SoftReference(bb);
+ }
+ box.set(bb);
+ }
+
+ @Override
+ public Point2D getClosestPointTo(Point2D p) {
+ return getClosestPointTo(
+ getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ p.getX(), p.getY());
+ }
+
+ @Override
+ public boolean equals(Object obj) {
+ if (obj instanceof Path2f) {
+ Path2f p = (Path2f)obj;
+ return (this.numCoords==p.numCoords
+ &&this.numTypes==p.numTypes
+ &&Arrays.equals(this.coords, p.coords)
+ &&Arrays.equals(this.types, p.types)
+ &&this.windingRule==p.windingRule);
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ long bits = 1L;
+ bits = 31L * bits + this.numCoords;
+ bits = 31L * bits + this.numTypes;
+ bits = 31L * bits + Arrays.hashCode(this.coords);
+ bits = 31L * bits + Arrays.hashCode(this.types);
+ bits = 31L * bits + this.windingRule.ordinal();
+ return (int) (bits ^ (bits >> 32));
+ }
+
+ /** Replies the coordinates of this path in an array of
+ * single precision floating-point numbers.
+ *
+ * @return the coordinates.
+ */
+ public final float[] toFloatArray() {
+ return toFloatArray(null);
+ }
+
+ /** Replies the coordinates of this path in an array of
+ * single precision floating-point numbers.
+ *
+ * @param transform is the transformation to apply to all the coordinates.
+ * @return the coordinates.
+ */
+ public float[] toFloatArray(Transform2D transform) {
+ if (transform==null) {
+ return Arrays.copyOf(this.coords, this.numCoords);
+ }
+ Point2f p = new Point2f();
+ float[] clone = new float[this.numCoords];
+ for(int i=0; i toCollection() {
+ return new PointCollection();
+ }
+
+ /** Replies the coordinate at the given index.
+ * The index is in [0;{@link #size()}*2).
+ *
+ * @param index
+ * @return the coordinate at the given index.
+ */
+ public float getCoordAt(int index) {
+ return this.coords[index];
+ }
+
+ /** Replies the point at the given index.
+ * The index is in [0;{@link #size()}).
+ *
+ * @param index
+ * @return the point at the given index.
+ */
+ public Point2f getPointAt(int index) {
+ return new Point2f(
+ this.coords[index*2],
+ this.coords[index*2+1]);
+ }
+
+ /** Replies the last point in the path.
+ *
+ * @return the last point.
+ */
+ public Point2f getCurrentPoint() {
+ return new Point2f(
+ this.coords[this.coords.length-1],
+ this.coords[this.coords.length-2]);
+ }
+
+ /** Replies the number of points in the path.
+ *
+ * @return the number of points in the path.
+ */
+ public int size() {
+ return this.numCoords/2;
+ }
+
+ /** Replies if this path is empty.
+ * The path is empty when there is no point inside, or
+ * all the points are at the same coordinate, or
+ * when the path does not represents a drawable path
+ * (a path with a line or a curve).
+ *
+ * @return true if the path does not contain
+ * a coordinate; otherwise false.
+ */
+ @Override
+ public boolean isEmpty() {
+ if (this.isEmpty==null) {
+ this.isEmpty = Boolean.TRUE;
+ PathIterator2f pi = getPathIterator();
+ PathElement2f pe;
+ while (this.isEmpty==Boolean.TRUE && pi.hasNext()) {
+ pe = pi.next();
+ if (pe.isDrawable()) {
+ this.isEmpty = Boolean.FALSE;
+ }
+ }
+ }
+ return this.isEmpty.booleanValue();
+ }
+
+ @Override
+ public boolean isPolyline() {
+ if (this.isPolyline==null) {
+ this.isPolyline = Boolean.TRUE;
+ PathIterator2f pi = getPathIterator();
+ PathElement2f pe;
+ PathElementType t;
+ while (this.isPolyline==Boolean.TRUE && pi.hasNext()) {
+ pe = pi.next();
+ t = pe.getType();
+ if (t==PathElementType.CURVE_TO || t==PathElementType.QUAD_TO) {
+ this.isPolyline = Boolean.FALSE;
+ }
+ }
+ }
+ return this.isPolyline.booleanValue();
+ }
+ /** Replies if the given points exists in the coordinates of this path.
+ *
+ * @param p
+ * @return true if the point is a control point of the path.
+ */
+ boolean containsPoint(Point2D p) {
+ float x, y;
+ for(int i=0; itrue if the point was removed; false otherwise.
+ */
+ boolean remove(float x, float y) {
+ for(int i=0, j=0; i0) {
+ switch(this.types[this.numTypes-1]) {
+ case CLOSE:
+ // no coord to remove
+ break;
+ case MOVE_TO:
+ case LINE_TO:
+ this.numCoords -= 2;
+ break;
+ case CURVE_TO:
+ this.numCoords -= 6;
+ this.isPolyline = null;
+ break;
+ case QUAD_TO:
+ this.numCoords -= 4;
+ this.isPolyline = null;
+ break;
+ default:
+ throw new IllegalStateException();
+ }
+ --this.numTypes;
+ this.isEmpty = null;
+ this.graphicalBounds = null;
+ this.logicalBounds = null;
+ }
+ }
+
+ /** Change the coordinates of the last inserted point.
+ *
+ * @param x
+ * @param y
+ */
+ public void setLastPoint(float x, float y) {
+ if (this.numCoords>=2) {
+ this.coords[this.numCoords-2] = x;
+ this.coords[this.numCoords-1] = y;
+ this.graphicalBounds = null;
+ this.logicalBounds = null;
+ }
+ }
+
+ /**
+ * Tests if the interior of the specified {@link PathIterator2f}
+ * intersects the interior of a specified set of rectangular
+ * coordinates.
+ *
+ * This method provides a basic facility for implementors of
+ * the {@link Shape2f} interface to implement support for the
+ * {@code intersects()} method.
+ *
+ * This method object may conservatively return true in
+ * cases where the specified rectangular area intersects a
+ * segment of the path, but that segment does not represent a
+ * boundary between the interior and exterior of the path.
+ * Such a case may occur if some set of segments of the
+ * path are retraced in the reverse direction such that the
+ * two sets of segments cancel each other out without any
+ * interior area between them.
+ * To determine whether segments represent true boundaries of
+ * the interior of the path would require extensive calculations
+ * involving all of the segments of the path and the winding
+ * rule and are thus beyond the scope of this implementation.
+ *
+ * @param pi the specified {@code PathIterator}
+ * @param x the specified X coordinate
+ * @param y the specified Y coordinate
+ * @param w the width of the specified rectangular coordinates
+ * @param h the height of the specified rectangular coordinates
+ * @return {@code true} if the specified {@code PathIterator} and
+ * the interior of the specified set of rectangular
+ * coordinates intersect each other; {@code false} otherwise.
+ */
+ public static boolean intersects(PathIterator2f pi, float x, float y, float w, float h) {
+ if (w <= 0f || h <= 0f) {
+ return false;
+ }
+ int mask = (pi.getWindingRule() == PathWindingRule.NON_ZERO ? -1 : 2);
+ int crossings = computeCrossingsFromRect(pi, x, y, x+w, y+h, false, true);
+ return (crossings == MathConstants.SHAPE_INTERSECTS ||
+ (crossings & mask) != 0);
+ }
+
+ /** A path iterator that does not transform the coordinates.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private class CopyPathIterator implements PathIterator2f {
+
+ private final Point2D p1 = new Point2f();
+ private final Point2D p2 = new Point2f();
+ private int iType = 0;
+ private int iCoord = 0;
+ private float movex, movey;
+
+ /**
+ */
+ public CopyPathIterator() {
+ //
+ }
+
+ @Override
+ public boolean hasNext() {
+ return this.iType=Path2f.this.numTypes) {
+ throw new NoSuchElementException();
+ }
+ PathElement2f element = null;
+ switch(Path2f.this.types[type]) {
+ case MOVE_TO:
+ if (this.iCoord+2>Path2f.this.numCoords) {
+ throw new NoSuchElementException();
+ }
+ this.movex = Path2f.this.coords[this.iCoord++];
+ this.movey = Path2f.this.coords[this.iCoord++];
+ this.p2.set(this.movex, this.movey);
+ element = new PathElement2f.MovePathElement2f(
+ this.p2.getX(), this.p2.getY());
+ break;
+ case LINE_TO:
+ if (this.iCoord+2>Path2f.this.numCoords) {
+ throw new NoSuchElementException();
+ }
+ this.p1.set(this.p2);
+ this.p2.set(
+ Path2f.this.coords[this.iCoord++],
+ Path2f.this.coords[this.iCoord++]);
+ element = new PathElement2f.LinePathElement2f(
+ this.p1.getX(), this.p1.getY(),
+ this.p2.getX(), this.p2.getY());
+ break;
+ case QUAD_TO:
+ {
+ if (this.iCoord+4>Path2f.this.numCoords) {
+ throw new NoSuchElementException();
+ }
+ this.p1.set(this.p2);
+ float ctrlx = Path2f.this.coords[this.iCoord++];
+ float ctrly = Path2f.this.coords[this.iCoord++];
+ this.p2.set(
+ Path2f.this.coords[this.iCoord++],
+ Path2f.this.coords[this.iCoord++]);
+ element = new PathElement2f.QuadPathElement2f(
+ this.p1.getX(), this.p1.getY(),
+ ctrlx, ctrly,
+ this.p2.getX(), this.p2.getY());
+ }
+ break;
+ case CURVE_TO:
+ {
+ if (this.iCoord+6>Path2f.this.numCoords) {
+ throw new NoSuchElementException();
+ }
+ this.p1.set(this.p2);
+ float ctrlx1 = Path2f.this.coords[this.iCoord++];
+ float ctrly1 = Path2f.this.coords[this.iCoord++];
+ float ctrlx2 = Path2f.this.coords[this.iCoord++];
+ float ctrly2 = Path2f.this.coords[this.iCoord++];
+ this.p2.set(
+ Path2f.this.coords[this.iCoord++],
+ Path2f.this.coords[this.iCoord++]);
+ element = new PathElement2f.CurvePathElement2f(
+ this.p1.getX(), this.p1.getY(),
+ ctrlx1, ctrly1,
+ ctrlx2, ctrly2,
+ this.p2.getX(), this.p2.getY());
+ }
+ break;
+ case CLOSE:
+ this.p1.set(this.p2);
+ this.p2.set(this.movex, this.movey);
+ element = new PathElement2f.ClosePathElement2f(
+ this.p1.getX(), this.p1.getY(),
+ this.p2.getX(), this.p2.getY());
+ break;
+ default:
+ }
+ if (element==null)
+ throw new NoSuchElementException();
+
+ ++this.iType;
+
+ return element;
+ }
+
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public PathWindingRule getWindingRule() {
+ return Path2f.this.getWindingRule();
+ }
+
+ @Override
+ public boolean isPolyline() {
+ return Path2f.this.isPolyline();
+ }
+
+ } // class CopyPathIterator
+
+ /** A path iterator that transforms the coordinates.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private class TransformPathIterator implements PathIterator2f {
+
+ private final Transform2D transform;
+ private final Point2D p1 = new Point2f();
+ private final Point2D p2 = new Point2f();
+ private final Point2D ptmp1 = new Point2f();
+ private final Point2D ptmp2 = new Point2f();
+ private int iType = 0;
+ private int iCoord = 0;
+ private float movex, movey;
+
+ /**
+ * @param transform
+ */
+ public TransformPathIterator(Transform2D transform) {
+ assert(transform!=null);
+ this.transform = transform;
+ }
+
+ @Override
+ public boolean hasNext() {
+ return this.iType=Path2f.this.numTypes) {
+ throw new NoSuchElementException();
+ }
+ PathElement2f element = null;
+ switch(Path2f.this.types[this.iType++]) {
+ case MOVE_TO:
+ this.movex = Path2f.this.coords[this.iCoord++];
+ this.movey = Path2f.this.coords[this.iCoord++];
+ this.p2.set(this.movex, this.movey);
+ this.transform.transform(this.p2);
+ element = new PathElement2f.MovePathElement2f(
+ this.p2.getX(), this.p2.getY());
+ break;
+ case LINE_TO:
+ this.p1.set(this.p2);
+ this.p2.set(
+ Path2f.this.coords[this.iCoord++],
+ Path2f.this.coords[this.iCoord++]);
+ this.transform.transform(this.p2);
+ element = new PathElement2f.LinePathElement2f(
+ this.p1.getX(), this.p1.getY(),
+ this.p2.getX(), this.p2.getY());
+ break;
+ case QUAD_TO:
+ {
+ this.p1.set(this.p2);
+ this.ptmp1.set(
+ Path2f.this.coords[this.iCoord++],
+ Path2f.this.coords[this.iCoord++]);
+ this.transform.transform(this.ptmp1);
+ this.p2.set(
+ Path2f.this.coords[this.iCoord++],
+ Path2f.this.coords[this.iCoord++]);
+ this.transform.transform(this.p2);
+ element = new PathElement2f.QuadPathElement2f(
+ this.p1.getX(), this.p1.getY(),
+ this.ptmp1.getX(), this.ptmp1.getY(),
+ this.p2.getX(), this.p2.getY());
+ }
+ break;
+ case CURVE_TO:
+ {
+ this.p1.set(this.p2);
+ this.ptmp1.set(
+ Path2f.this.coords[this.iCoord++],
+ Path2f.this.coords[this.iCoord++]);
+ this.transform.transform(this.ptmp1);
+ this.ptmp2.set(
+ Path2f.this.coords[this.iCoord++],
+ Path2f.this.coords[this.iCoord++]);
+ this.transform.transform(this.ptmp2);
+ this.p2.set(
+ Path2f.this.coords[this.iCoord++],
+ Path2f.this.coords[this.iCoord++]);
+ this.transform.transform(this.p2);
+ element = new PathElement2f.CurvePathElement2f(
+ this.p1.getX(), this.p1.getY(),
+ this.ptmp1.getX(), this.ptmp1.getY(),
+ this.ptmp2.getX(), this.ptmp2.getY(),
+ this.p2.getX(), this.p2.getY());
+ }
+ break;
+ case CLOSE:
+ this.p1.set(this.p2);
+ this.p2.set(this.movex, this.movey);
+ this.transform.transform(this.p2);
+ element = new PathElement2f.ClosePathElement2f(
+ this.p1.getX(), this.p1.getY(),
+ this.p2.getX(), this.p2.getY());
+ break;
+ default:
+ }
+ if (element==null)
+ throw new NoSuchElementException();
+ return element;
+ }
+
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public PathWindingRule getWindingRule() {
+ return Path2f.this.getWindingRule();
+ }
+
+ @Override
+ public boolean isPolyline() {
+ return Path2f.this.isPolyline();
+ }
+
+ } // class TransformPathIterator
+
+ /** A path iterator that is flattening the path.
+ * This iterator was copied from AWT FlatteningPathIterator.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private static class FlatteningPathIterator implements PathIterator2f {
+
+ /** Winding rule of the path.
+ */
+ private final PathWindingRule windingRule;
+
+ /** The source iterator.
+ */
+ private final PathIterator2f pathIterator;
+
+ /**
+ * Square of the flatness parameter for testing against squared lengths.
+ */
+ private final float squaredFlatness;
+
+ /**
+ * Maximum number of recursion levels.
+ */
+ private final int limit;
+
+ /** The recursion level at which each curve being held in storage was generated.
+ */
+ private int levels[];
+
+ /** The cache of interpolated coords.
+ * Note that this must be long enough
+ * to store a full cubic segment and
+ * a relative cubic segment to avoid
+ * aliasing when copying the coords
+ * of a curve to the end of the array.
+ * This is also serendipitously equal
+ * to the size of a full quad segment
+ * and 2 relative quad segments.
+ */
+ private float hold[] = new float[14];
+
+ /** The index of the last curve segment being held for interpolation.
+ */
+ private int holdEnd;
+
+ /**
+ * The index of the curve segment that was last interpolated. This
+ * is the curve segment ready to be returned in the next call to
+ * next().
+ */
+ private int holdIndex;
+
+ /** The ending x of the last segment.
+ */
+ private float currentX;
+
+ /** The ending y of the last segment.
+ */
+ private float currentY;
+
+ /** The x of the last move segment.
+ */
+ private float moveX;
+
+ /** The y of the last move segment.
+ */
+ private float moveY;
+
+ /** The index of the entry in the
+ * levels array of the curve segment
+ * at the holdIndex
+ */
+ private int levelIndex;
+
+ /** True when iteration is done.
+ */
+ private boolean done;
+
+ /** The type of the path element.
+ */
+ private PathElementType holdType;
+
+ /** The x of the last move segment replied by next.
+ */
+ private float lastNextX;
+
+ /** The y of the last move segment replied by next.
+ */
+ private float lastNextY;
+
+ /**
+ * @param windingRule is the winding rule of the path.
+ * @param pathIterator is the path iterator that may be used to initialize the path.
+ * @param flatness the maximum allowable distance between the
+ * control points and the flattened curve
+ * @param limit the maximum number of recursive subdivisions
+ * allowed for any curved segment
+ */
+ public FlatteningPathIterator(PathWindingRule windingRule, PathIterator2f pathIterator, float flatness, int limit) {
+ assert(windingRule!=null);
+ assert(flatness>=0f);
+ assert(limit>=0);
+ this.windingRule = windingRule;
+ this.pathIterator = pathIterator;
+ this.squaredFlatness = flatness * flatness;
+ this.limit = limit;
+ this.levels = new int[limit + 1];
+ searchNext();
+ }
+
+ /**
+ * Ensures that the hold array can hold up to (want) more values.
+ * It is currently holding (hold.length - holdIndex) values.
+ */
+ private void ensureHoldCapacity(int want) {
+ if (this.holdIndex - want < 0) {
+ int have = this.hold.length - this.holdIndex;
+ int newsize = this.hold.length + GROW_SIZE;
+ float newhold[] = new float[newsize];
+ System.arraycopy(this.hold, this.holdIndex,
+ newhold, this.holdIndex + GROW_SIZE,
+ have);
+ this.hold = newhold;
+ this.holdIndex += GROW_SIZE;
+ this.holdEnd += GROW_SIZE;
+ }
+ }
+
+ /**
+ * Returns the square of the flatness, or maximum distance of a
+ * control point from the line connecting the end points, of the
+ * quadratic curve specified by the control points stored in the
+ * indicated array at the indicated index.
+ * @param coords an array containing coordinate values
+ * @param offset the index into coords from which to
+ * to start getting the values from the array
+ * @return the flatness of the quadratic curve that is defined by the
+ * values in the specified array at the specified index.
+ */
+ private static float getQuadSquaredFlatness(float coords[], int offset) {
+ return GeometryUtil.distanceSquaredPointLine(
+ coords[offset + 2], coords[offset + 3],
+ coords[offset + 0], coords[offset + 1],
+ coords[offset + 4], coords[offset + 5]);
+ }
+
+ /**
+ * Subdivides the quadratic curve specified by the coordinates
+ * stored in the src array at indices
+ * srcoff through srcoff + 5
+ * and stores the resulting two subdivided curves into the two
+ * result arrays at the corresponding indices.
+ * Either or both of the left and right
+ * arrays can be null or a reference to the same array
+ * and offset as the src array.
+ * Note that the last point in the first subdivided curve is the
+ * same as the first point in the second subdivided curve. Thus,
+ * it is possible to pass the same array for left and
+ * right and to use offsets such that
+ * rightoff equals leftoff + 4 in order
+ * to avoid allocating extra storage for this common point.
+ * @param src the array holding the coordinates for the source curve
+ * @param srcoff the offset into the array of the beginning of the
+ * the 6 source coordinates
+ * @param left the array for storing the coordinates for the first
+ * half of the subdivided curve
+ * @param leftoff the offset into the array of the beginning of the
+ * the 6 left coordinates
+ * @param right the array for storing the coordinates for the second
+ * half of the subdivided curve
+ * @param rightoff the offset into the array of the beginning of the
+ * the 6 right coordinates
+ */
+ private static void subdivideQuad(float src[], int srcoff,
+ float left[], int leftoff,
+ float right[], int rightoff) {
+ float x1 = src[srcoff + 0];
+ float y1 = src[srcoff + 1];
+ float ctrlx = src[srcoff + 2];
+ float ctrly = src[srcoff + 3];
+ float x2 = src[srcoff + 4];
+ float y2 = src[srcoff + 5];
+ if (left != null) {
+ left[leftoff + 0] = x1;
+ left[leftoff + 1] = y1;
+ }
+ if (right != null) {
+ right[rightoff + 4] = x2;
+ right[rightoff + 5] = y2;
+ }
+ x1 = (x1 + ctrlx) / 2f;
+ y1 = (y1 + ctrly) / 2f;
+ x2 = (x2 + ctrlx) / 2f;
+ y2 = (y2 + ctrly) / 2f;
+ ctrlx = (x1 + x2) / 2f;
+ ctrly = (y1 + y2) / 2f;
+ if (left != null) {
+ left[leftoff + 2] = x1;
+ left[leftoff + 3] = y1;
+ left[leftoff + 4] = ctrlx;
+ left[leftoff + 5] = ctrly;
+ }
+ if (right != null) {
+ right[rightoff + 0] = ctrlx;
+ right[rightoff + 1] = ctrly;
+ right[rightoff + 2] = x2;
+ right[rightoff + 3] = y2;
+ }
+ }
+
+ /**
+ * Returns the square of the flatness of the cubic curve specified
+ * by the control points stored in the indicated array at the
+ * indicated index. The flatness is the maximum distance
+ * of a control point from the line connecting the end points.
+ * @param coords an array containing coordinates
+ * @param offset the index of coords from which to begin
+ * getting the end points and control points of the curve
+ * @return the square of the flatness of the CubicCurve2D
+ * specified by the coordinates in coords at
+ * the specified offset.
+ */
+ private static float getCurveSquaredFlatness(float coords[], int offset) {
+ return Math.max(
+ GeometryUtil.distanceSquaredPointSegment(
+ coords[offset + 0],
+ coords[offset + 1],
+ coords[offset + 6],
+ coords[offset + 7],
+ coords[offset + 2],
+ coords[offset + 3],null),
+ GeometryUtil.distanceSquaredPointSegment(
+ coords[offset + 0],
+ coords[offset + 1],
+ coords[offset + 6],
+ coords[offset + 7],
+ coords[offset + 4], coords[offset + 5],null));
+ }
+
+ /**
+ * Subdivides the cubic curve specified by the coordinates
+ * stored in the src array at indices srcoff
+ * through (srcoff + 7) and stores the
+ * resulting two subdivided curves into the two result arrays at the
+ * corresponding indices.
+ * Either or both of the left and right
+ * arrays may be null or a reference to the same array
+ * as the src array.
+ * Note that the last point in the first subdivided curve is the
+ * same as the first point in the second subdivided curve. Thus,
+ * it is possible to pass the same array for left
+ * and right and to use offsets, such as rightoff
+ * equals (leftoff + 6), in order
+ * to avoid allocating extra storage for this common point.
+ * @param src the array holding the coordinates for the source curve
+ * @param srcoff the offset into the array of the beginning of the
+ * the 6 source coordinates
+ * @param left the array for storing the coordinates for the first
+ * half of the subdivided curve
+ * @param leftoff the offset into the array of the beginning of the
+ * the 6 left coordinates
+ * @param right the array for storing the coordinates for the second
+ * half of the subdivided curve
+ * @param rightoff the offset into the array of the beginning of the
+ * the 6 right coordinates
+ */
+ private static void subdivideCurve(
+ float src[], int srcoff,
+ float left[], int leftoff,
+ float right[], int rightoff) {
+ float x1 = src[srcoff + 0];
+ float y1 = src[srcoff + 1];
+ float ctrlx1 = src[srcoff + 2];
+ float ctrly1 = src[srcoff + 3];
+ float ctrlx2 = src[srcoff + 4];
+ float ctrly2 = src[srcoff + 5];
+ float x2 = src[srcoff + 6];
+ float y2 = src[srcoff + 7];
+ if (left != null) {
+ left[leftoff + 0] = x1;
+ left[leftoff + 1] = y1;
+ }
+ if (right != null) {
+ right[rightoff + 6] = x2;
+ right[rightoff + 7] = y2;
+ }
+ x1 = (x1 + ctrlx1) / 2f;
+ y1 = (y1 + ctrly1) / 2f;
+ x2 = (x2 + ctrlx2) / 2f;
+ y2 = (y2 + ctrly2) / 2f;
+ float centerx = (ctrlx1 + ctrlx2) / 2f;
+ float centery = (ctrly1 + ctrly2) / 2f;
+ ctrlx1 = (x1 + centerx) / 2f;
+ ctrly1 = (y1 + centery) / 2f;
+ ctrlx2 = (x2 + centerx) / 2f;
+ ctrly2 = (y2 + centery) / 2f;
+ centerx = (ctrlx1 + ctrlx2) / 2f;
+ centery = (ctrly1 + ctrly2) / 2f;
+ if (left != null) {
+ left[leftoff + 2] = x1;
+ left[leftoff + 3] = y1;
+ left[leftoff + 4] = ctrlx1;
+ left[leftoff + 5] = ctrly1;
+ left[leftoff + 6] = centerx;
+ left[leftoff + 7] = centery;
+ }
+ if (right != null) {
+ right[rightoff + 0] = centerx;
+ right[rightoff + 1] = centery;
+ right[rightoff + 2] = ctrlx2;
+ right[rightoff + 3] = ctrly2;
+ right[rightoff + 4] = x2;
+ right[rightoff + 5] = y2;
+ }
+ }
+
+ private void searchNext() {
+ int level;
+
+ if (this.holdIndex >= this.holdEnd) {
+ if (!this.pathIterator.hasNext()) {
+ this.done = true;
+ return;
+ }
+ PathElement2f pathElement = this.pathIterator.next();
+ this.holdType = pathElement.type;
+ pathElement.toArray(this.hold);
+ this.levelIndex = 0;
+ this.levels[0] = 0;
+ }
+
+ switch (this.holdType) {
+ case MOVE_TO:
+ case LINE_TO:
+ this.currentX = this.hold[0];
+ this.currentY = this.hold[1];
+ if (this.holdType == PathElementType.MOVE_TO) {
+ this.moveX = this.currentX;
+ this.moveY = this.currentY;
+ }
+ this.holdIndex = 0;
+ this.holdEnd = 0;
+ break;
+ case CLOSE:
+ this.currentX = this.moveX;
+ this.currentY = this.moveY;
+ this.holdIndex = 0;
+ this.holdEnd = 0;
+ break;
+ case QUAD_TO:
+ if (this.holdIndex >= this.holdEnd) {
+ // Move the coordinates to the end of the array.
+ this.holdIndex = this.hold.length - 6;
+ this.holdEnd = this.hold.length - 2;
+ this.hold[this.holdIndex + 0] = this.currentX;
+ this.hold[this.holdIndex + 1] = this.currentY;
+ this.hold[this.holdIndex + 2] = this.hold[0];
+ this.hold[this.holdIndex + 3] = this.hold[1];
+ this.hold[this.holdIndex + 4] = this.currentX = this.hold[2];
+ this.hold[this.holdIndex + 5] = this.currentY = this.hold[3];
+ }
+
+ level = this.levels[this.levelIndex];
+ while (level < this.limit) {
+ if (getQuadSquaredFlatness(this.hold, this.holdIndex) < this.squaredFlatness) {
+ break;
+ }
+
+ ensureHoldCapacity(4);
+ subdivideQuad(
+ this.hold, this.holdIndex,
+ this.hold, this.holdIndex - 4,
+ this.hold, this.holdIndex);
+ this.holdIndex -= 4;
+
+ // Now that we have subdivided, we have constructed
+ // two curves of one depth lower than the original
+ // curve. One of those curves is in the place of
+ // the former curve and one of them is in the next
+ // set of held coordinate slots. We now set both
+ // curves level values to the next higher level.
+ level++;
+ this.levels[this.levelIndex] = level;
+ this.levelIndex++;
+ this.levels[this.levelIndex] = level;
+ }
+
+ // This curve segment is flat enough, or it is too deep
+ // in recursion levels to try to flatten any more. The
+ // two coordinates at holdIndex+4 and holdIndex+5 now
+ // contain the endpoint of the curve which can be the
+ // endpoint of an approximating line segment.
+ this.holdIndex += 4;
+ this.levelIndex--;
+ break;
+ case CURVE_TO:
+ if (this.holdIndex >= this.holdEnd) {
+ // Move the coordinates to the end of the array.
+ this.holdIndex = this.hold.length - 8;
+ this.holdEnd = this.hold.length - 2;
+ this.hold[this.holdIndex + 0] = this.currentX;
+ this.hold[this.holdIndex + 1] = this.currentY;
+ this.hold[this.holdIndex + 2] = this.hold[0];
+ this.hold[this.holdIndex + 3] = this.hold[1];
+ this.hold[this.holdIndex + 4] = this.hold[2];
+ this.hold[this.holdIndex + 5] = this.hold[3];
+ this.hold[this.holdIndex + 6] = this.currentX = this.hold[4];
+ this.hold[this.holdIndex + 7] = this.currentY = this.hold[5];
+ }
+
+ level = this.levels[this.levelIndex];
+ while (level < this.limit) {
+ if (getCurveSquaredFlatness(this.hold,this. holdIndex) < this.squaredFlatness) {
+ break;
+ }
+
+ ensureHoldCapacity(6);
+ subdivideCurve(
+ this.hold, this.holdIndex,
+ this.hold, this.holdIndex - 6,
+ this.hold, this.holdIndex);
+ this.holdIndex -= 6;
+
+ // Now that we have subdivided, we have constructed
+ // two curves of one depth lower than the original
+ // curve. One of those curves is in the place of
+ // the former curve and one of them is in the next
+ // set of held coordinate slots. We now set both
+ // curves level values to the next higher level.
+ level++;
+ this.levels[this.levelIndex] = level;
+ this.levelIndex++;
+ this.levels[this.levelIndex] = level;
+ }
+
+ // This curve segment is flat enough, or it is too deep
+ // in recursion levels to try to flatten any more. The
+ // two coordinates at holdIndex+6 and holdIndex+7 now
+ // contain the endpoint of the curve which can be the
+ // endpoint of an approximating line segment.
+ this.holdIndex += 6;
+ this.levelIndex--;
+ break;
+ default:
+ }
+ }
+
+ @Override
+ public boolean hasNext() {
+ return !this.done;
+ }
+
+ @Override
+ public PathElement2f next() {
+ if (this.done) {
+ throw new NoSuchElementException("flattening iterator out of bounds"); //$NON-NLS-1$
+ }
+
+ PathElement2f element;
+ PathElementType type = this.holdType;
+ if (type!=PathElementType.CLOSE) {
+ float x = this.hold[this.holdIndex + 0];
+ float y = this.hold[this.holdIndex + 1];
+ if (type == PathElementType.MOVE_TO) {
+ element = new PathElement2f.MovePathElement2f(x, y);
+ }
+ else {
+ element = new PathElement2f.LinePathElement2f(
+ this.lastNextX, this.lastNextY,
+ x, y);
+ }
+ this.lastNextX = x;
+ this.lastNextY = y;
+ }
+ else {
+ element = new PathElement2f.ClosePathElement2f(
+ this.lastNextX, this.lastNextY,
+ this.moveX, this.moveY);
+ this.lastNextX = this.moveX;
+ this.lastNextY = this.moveY;
+ }
+
+ searchNext();
+
+ return element;
+ }
+
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public PathWindingRule getWindingRule() {
+ return this.windingRule;
+ }
+
+ @Override
+ public boolean isPolyline() {
+ return false; // Because the iterator flats the path, this is no curve inside.
+ }
+
+ } // class FlatteningPathIterator
+
+ /** An collection of the points of the path.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private class PointCollection implements Collection {
+
+ /**
+ */
+ public PointCollection() {
+ //
+ }
+
+ @Override
+ public int size() {
+ return Path2f.this.size();
+ }
+
+ @Override
+ public boolean isEmpty() {
+ return Path2f.this.size()<=0;
+ }
+
+ @Override
+ public boolean contains(Object o) {
+ if (o instanceof Point2D) {
+ return Path2f.this.containsPoint((Point2D)o);
+ }
+ return false;
+ }
+
+ @Override
+ public Iterator iterator() {
+ return new PointIterator();
+ }
+
+ @Override
+ public Object[] toArray() {
+ return Path2f.this.toPointArray();
+ }
+
+ @SuppressWarnings("unchecked")
+ @Override
+ public T[] toArray(T[] a) {
+ Iterator iterator = new PointIterator();
+ for(int i=0; i c) {
+ for(Object obj : c) {
+ if ((!(obj instanceof Point2D))
+ ||(!Path2f.this.containsPoint((Point2D)obj))) {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ @Override
+ public boolean addAll(Collection c) {
+ boolean changed = false;
+ for(Point2D pts : c) {
+ if (add(pts)) {
+ changed = true;
+ }
+ }
+ return changed;
+ }
+
+ @Override
+ public boolean removeAll(Collection c) {
+ boolean changed = false;
+ for(Object obj : c) {
+ if (obj instanceof Point2D) {
+ Point2D pts = (Point2D)obj;
+ if (Path2f.this.remove(pts.getX(), pts.getY())) {
+ changed = true;
+ }
+ }
+ }
+ return changed;
+ }
+
+ @Override
+ public boolean retainAll(Collection c) {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public void clear() {
+ Path2f.this.clear();
+ }
+
+ } // class PointCollection
+
+ /** Iterator on the points of the path.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private class PointIterator implements Iterator {
+
+ private int index = 0;
+ private Point2D lastReplied = null;
+
+ /**
+ */
+ public PointIterator() {
+ //
+ }
+
+ @Override
+ public boolean hasNext() {
+ return this.indexMOVE_TO.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ public static class MovePathElement2f extends PathElement2f {
+
+ private static final long serialVersionUID = -5596181248741970433L;
+
+ /**
+ * @param x
+ * @param y
+ */
+ public MovePathElement2f(float x, float y) {
+ super(PathElementType.MOVE_TO,
+ Float.NaN, Float.NaN,
+ Float.NaN, Float.NaN,
+ Float.NaN, Float.NaN,
+ x, y);
+ }
+
+ @Override
+ public boolean isEmpty() {
+ return (this.fromX==this.toX) && (this.fromY==this.toY);
+ }
+
+ @Override
+ public boolean isDrawable() {
+ return false;
+ }
+
+ @Override
+ public void toArray(float[] array) {
+ array[0] = this.toX;
+ array[1] = this.toY;
+ }
+
+ @Override
+ public float[] toArray() {
+ return new float[] {this.toX, this.toY};
+ }
+
+ @Override
+ public String toString() {
+ return "MOVE("+ //$NON-NLS-1$
+ this.toX+"x"+ //$NON-NLS-1$
+ this.toY+")"; //$NON-NLS-1$
+ }
+
+ @Override
+ public final PathElementType getType() {
+ return PathElementType.MOVE_TO;
+ }
+
+ }
+
+ /** An element of the path that represents a LINE_TO.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ public static class LinePathElement2f extends PathElement2f {
+
+ private static final long serialVersionUID = -5878571187312098882L;
+
+ /**
+ * @param fromx
+ * @param fromy
+ * @param tox
+ * @param toy
+ */
+ public LinePathElement2f(float fromx, float fromy, float tox, float toy) {
+ super(PathElementType.LINE_TO,
+ fromx, fromy,
+ Float.NaN, Float.NaN,
+ Float.NaN, Float.NaN,
+ tox, toy);
+ }
+
+ @Override
+ public final PathElementType getType() {
+ return PathElementType.LINE_TO;
+ }
+
+ @Override
+ public boolean isEmpty() {
+ return (this.fromX==this.toX) && (this.fromY==this.toY);
+ }
+
+ @Override
+ public boolean isDrawable() {
+ return !isEmpty();
+ }
+
+ @Override
+ public void toArray(float[] array) {
+ array[0] = this.toX;
+ array[1] = this.toY;
+ }
+
+ @Override
+ public float[] toArray() {
+ return new float[] {this.toX, this.toY};
+ }
+
+ @Override
+ public String toString() {
+ return "LINE("+ //$NON-NLS-1$
+ this.toX+"x"+ //$NON-NLS-1$
+ this.toY+")"; //$NON-NLS-1$
+ }
+
+ }
+
+ /** An element of the path that represents a QUAD_TO.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ public static class QuadPathElement2f extends PathElement2f {
+
+ private static final long serialVersionUID = 5641358330446739160L;
+
+ /**
+ * @param fromx
+ * @param fromy
+ * @param ctrlx
+ * @param ctrly
+ * @param tox
+ * @param toy
+ */
+ public QuadPathElement2f(float fromx, float fromy, float ctrlx, float ctrly, float tox, float toy) {
+ super(PathElementType.QUAD_TO,
+ fromx, fromy,
+ ctrlx, ctrly,
+ Float.NaN, Float.NaN,
+ tox, toy);
+ }
+
+ @Override
+ public final PathElementType getType() {
+ return PathElementType.QUAD_TO;
+ }
+
+ @Override
+ public boolean isEmpty() {
+ return (this.fromX==this.toX) && (this.fromY==this.toY) &&
+ (this.ctrlX1==this.toX) && (this.ctrlY1==this.toY);
+ }
+
+ @Override
+ public boolean isDrawable() {
+ return !isEmpty();
+ }
+
+ @Override
+ public void toArray(float[] array) {
+ array[0] = this.ctrlX1;
+ array[1] = this.ctrlY1;
+ array[2] = this.toX;
+ array[3] = this.toY;
+ }
+
+ @Override
+ public float[] toArray() {
+ return new float[] {this.ctrlX1, this.ctrlY1, this.toX, this.toY};
+ }
+
+ @Override
+ public String toString() {
+ return "QUAD("+ //$NON-NLS-1$
+ this.ctrlX1+"x"+ //$NON-NLS-1$
+ this.ctrlY1+"|"+ //$NON-NLS-1$
+ this.toX+"x"+ //$NON-NLS-1$
+ this.toY+")"; //$NON-NLS-1$
+ }
+
+ }
+
+ /** An element of the path that represents a CURVE_TO.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ public static class CurvePathElement2f extends PathElement2f {
+
+ private static final long serialVersionUID = -1449309552719221756L;
+
+ /**
+ * @param fromx
+ * @param fromy
+ * @param ctrlx1
+ * @param ctrly1
+ * @param ctrlx2
+ * @param ctrly2
+ * @param tox
+ * @param toy
+ */
+ public CurvePathElement2f(float fromx, float fromy, float ctrlx1, float ctrly1, float ctrlx2, float ctrly2, float tox, float toy) {
+ super(PathElementType.CURVE_TO,
+ fromx, fromy,
+ ctrlx1, ctrly1,
+ ctrlx2, ctrly2,
+ tox, toy);
+ }
+
+ @Override
+ public final PathElementType getType() {
+ return PathElementType.CURVE_TO;
+ }
+
+ @Override
+ public boolean isEmpty() {
+ return (this.fromX==this.toX) && (this.fromY==this.toY) &&
+ (this.ctrlX1==this.toX) && (this.ctrlY1==this.toY) &&
+ (this.ctrlX2==this.toX) && (this.ctrlY2==this.toY);
+ }
+
+ @Override
+ public boolean isDrawable() {
+ return !isEmpty();
+ }
+
+ @Override
+ public void toArray(float[] array) {
+ array[0] = this.ctrlX1;
+ array[1] = this.ctrlY1;
+ array[2] = this.ctrlX2;
+ array[3] = this.ctrlY2;
+ array[4] = this.toX;
+ array[5] = this.toY;
+ }
+
+ @Override
+ public float[] toArray() {
+ return new float[] {this.ctrlX1, this.ctrlY1, this.ctrlX2, this.ctrlY2, this.toX, this.toY};
+ }
+
+ @Override
+ public String toString() {
+ return "CURVE("+ //$NON-NLS-1$
+ this.ctrlX1+"x"+ //$NON-NLS-1$
+ this.ctrlY1+"|"+ //$NON-NLS-1$
+ this.ctrlX2+"x"+ //$NON-NLS-1$
+ this.ctrlY2+"|"+ //$NON-NLS-1$
+ this.toX+"x"+ //$NON-NLS-1$
+ this.toY+")"; //$NON-NLS-1$
+ }
+
+ }
+
+ /** An element of the path that represents a CLOSE.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ public static class ClosePathElement2f extends PathElement2f {
+
+ private static final long serialVersionUID = 4643537091880303796L;
+
+ /**
+ * @param fromx
+ * @param fromy
+ * @param tox
+ * @param toy
+ */
+ public ClosePathElement2f(float fromx, float fromy, float tox, float toy) {
+ super(PathElementType.CLOSE,
+ fromx, fromy,
+ Float.NaN, Float.NaN,
+ Float.NaN, Float.NaN,
+ tox, toy);
+ }
+
+ @Override
+ public final PathElementType getType() {
+ return PathElementType.CLOSE;
+ }
+
+ @Override
+ public boolean isEmpty() {
+ return (this.fromX==this.toX) && (this.fromY==this.toY);
+ }
+
+ @Override
+ public boolean isDrawable() {
+ return false;
+ }
+
+ @Override
+ public void toArray(float[] array) {
+ //
+ }
+
+ @Override
+ public float[] toArray() {
+ return new float[0];
+ }
+
+ @Override
+ public String toString() {
+ return "CLOSE"; //$NON-NLS-1$
+ }
+
+ }
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/PathIterator2f.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/PathIterator2f.java
new file mode 100644
index 000000000..fbd0ff0ad
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/PathIterator2f.java
@@ -0,0 +1,54 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2005-09 Stephane GALLAND.
+ * Copyright (C) 2012 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.continuous;
+
+import java.util.Iterator;
+
+import org.arakhne.afc.math.geometry.PathWindingRule;
+
+
+/** This interface describes an interator on path elements.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public interface PathIterator2f extends Iterator {
+
+ /** Replies the winding rule for the path.
+ *
+ * @return the winding rule for the path.
+ */
+ public PathWindingRule getWindingRule();
+
+ /** Replies the iterator may reply only elements of type
+ * MOVE_TO, LINE_TO, or
+ * CLOSE (no curve).
+ *
+ * @return true if the iterator does not
+ * contain curve primitives, false
+ * otherwise.
+ */
+ public boolean isPolyline();
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/PathShadow2f.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/PathShadow2f.java
new file mode 100644
index 000000000..77ca36897
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/PathShadow2f.java
@@ -0,0 +1,459 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2005-09 Stephane GALLAND.
+ * Copyright (C) 2012-13 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.continuous;
+
+import java.util.Iterator;
+
+import org.arakhne.afc.math.MathConstants;
+import org.arakhne.afc.math.geometry.GeometryUtil;
+import org.arakhne.afc.math.geometry.IntersectionUtil;
+import org.arakhne.afc.math.geometry.PathElementType;
+import org.arakhne.afc.math.geometry.PathWindingRule;
+import org.arakhne.afc.math.geometry2d.Path2D;
+
+/** Shadow of a path.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+class PathShadow2f {//TODO : clarify documentation
+
+ private final Path2D path;
+ private final Rectangle2f bounds;
+
+ /**
+ * @param path
+ */
+ public PathShadow2f(Path2D path) {
+ this.path = path;
+ this.bounds = this.path.toBoundingBox();
+ }
+
+ /** Compute the crossings between this shadow and
+ * the given segment.
+ *
+ * @param crossings is the initial value of the crossings.
+ * @param x0 is the first point of the segment.
+ * @param y0 is the first point of the segment.
+ * @param x1 is the second point of the segment.
+ * @param y1 is the second point of the segment.
+ * @return the crossings or {@link MathConstants#SHAPE_INTERSECTS}.
+ */
+ public int computeCrossings(
+ int crossings,
+ float x0, float y0,
+ float x1, float y1) {
+ if (this.bounds==null) return crossings;
+
+ int numCrosses =
+ GeometryUtil.computeCrossingsFromRect(crossings,
+ this.bounds.getMinX(),
+ this.bounds.getMinY(),
+ this.bounds.getMaxX(),
+ this.bounds.getMaxY(),
+ x0, y0,
+ x1, y1);
+
+ if (numCrosses==MathConstants.SHAPE_INTERSECTS) {
+ // The segment is intersecting the bounds of the shadow path.
+ // We must consider the shape of shadow path now.
+ PathShadowData data = new PathShadowData(
+ this.bounds.getMaxX(),
+ this.bounds.getMinY(),
+ this.bounds.getMaxY());
+
+ computeCrossings1(
+ this.path.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ x0, y0, x1, y1,
+ false,
+ data);
+ numCrosses = data.crossings;
+
+ int mask = (this.path.getWindingRule() == PathWindingRule.NON_ZERO ? -1 : 2);
+ if (numCrosses == MathConstants.SHAPE_INTERSECTS ||
+ (numCrosses & mask) != 0) {
+ // The given line is intersecting the path shape
+ return MathConstants.SHAPE_INTERSECTS;
+ }
+
+ // There is no intersection with the shadow path's shape.
+ int inc = 0;
+ if (data.hasX4ymin &&
+ data.x4ymin>=data.xmin4ymin) {
+ ++inc;
+ }
+ if (data.hasX4ymax &&
+ data.x4ymax>=data.xmin4ymax) {
+ ++inc;
+ }
+
+ if (y0 pi,
+ float x1, float y1, float x2, float y2,
+ boolean closeable, PathShadowData data) {
+ if (!pi.hasNext() || data.crossings==MathConstants.SHAPE_INTERSECTS) return;
+ PathElement2f element;
+
+ element = pi.next();
+ if (element.type != PathElementType.MOVE_TO) {
+ throw new IllegalArgumentException("missing initial moveto in path definition"); //$NON-NLS-1$
+ }
+
+ float movx = element.toX;
+ float movy = element.toY;
+ float curx = movx;
+ float cury = movy;
+ float endx, endy;
+ while (data.crossings!=MathConstants.SHAPE_INTERSECTS && pi.hasNext()) {
+ element = pi.next();
+ switch (element.type) {
+ case MOVE_TO:
+ movx = curx = element.toX;
+ movy = cury = element.toY;
+ break;
+ case LINE_TO:
+ endx = element.toX;
+ endy = element.toY;
+ computeCrossings2(
+ curx, cury,
+ endx, endy,
+ x1, y1, x2, y2,
+ data);
+ if (data.crossings==MathConstants.SHAPE_INTERSECTS) {
+ return;
+ }
+ curx = endx;
+ cury = endy;
+ break;
+ case QUAD_TO:
+ {
+ endx = element.toX;
+ endy = element.toY;
+ Path2f localPath = new Path2f();
+ localPath.moveTo(curx, cury);
+ localPath.quadTo(
+ element.ctrlX1, element.ctrlY1,
+ endx, endy);
+ computeCrossings1(
+ localPath.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ x1, y1, x2, y2,
+ false,
+ data);
+ if (data.crossings==MathConstants.SHAPE_INTERSECTS) {
+ return;
+ }
+ curx = endx;
+ cury = endy;
+ break;
+ }
+ case CURVE_TO:
+ endx = element.toX;
+ endy = element.toY;
+ Path2f localPath = new Path2f();
+ localPath.moveTo(curx, cury);
+ localPath.curveTo(
+ element.ctrlX1, element.ctrlY1,
+ element.ctrlX2, element.ctrlY2,
+ endx, endy);
+ computeCrossings1(
+ localPath.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ x1, y1, x2, y2,
+ false,
+ data);
+ if (data.crossings==MathConstants.SHAPE_INTERSECTS) {
+ return;
+ }
+ curx = endx;
+ cury = endy;
+ break;
+ case CLOSE:
+ if (cury != movy || curx != movx) {
+ computeCrossings2(
+ curx, cury,
+ movx, movy,
+ x1, y1, x2, y2,
+ data);
+ }
+ if (data.crossings!=0) return;
+ curx = movx;
+ cury = movy;
+ break;
+ default:
+ }
+ }
+
+ assert(data.crossings!=MathConstants.SHAPE_INTERSECTS);
+
+ boolean isOpen = (curx != movx) || (cury != movy);
+
+ if (isOpen) {
+ if (closeable) {
+ computeCrossings2(
+ curx, cury,
+ movx, movy,
+ x1, y1, x2, y2,
+ data);
+ }
+ else {
+ // Assume that when is the path is open, only
+ // SHAPE_INTERSECTS may be return
+ data.crossings = 0;
+ }
+ }
+ }
+
+ private static void computeCrossings2(
+ float shadow_x0, float shadow_y0,
+ float shadow_x1, float shadow_y1,
+ float sx0, float sy0,
+ float sx1, float sy1,
+ PathShadowData data) {
+ float shadow_xmin = Math.min(shadow_x0, shadow_x1);
+ float shadow_xmax = Math.max(shadow_x0, shadow_x1);
+ float shadow_ymin = Math.min(shadow_y0, shadow_y1);
+ float shadow_ymax = Math.max(shadow_y0, shadow_y1);
+
+ data.updateShadowLimits(shadow_x0, shadow_y0, shadow_x1, shadow_y1);
+
+ if (sy0<=shadow_ymin && sy1<=shadow_ymin) return;
+ if (sy0>=shadow_ymax && sy1>=shadow_ymax) return;
+ if (sx0<=shadow_xmin && sx1<=shadow_xmin) return;
+ if (sx0>=shadow_xmax && sx1>=shadow_xmax) {
+ float xintercept;
+ // The line is entirely at the right of the shadow
+ float alpha = (sx1 - sx0) / (sy1 - sy0);
+ if (sy0=shadow_ymax) {
+ xintercept = sx0 + (shadow_ymax - sy0) * alpha;
+ data.setCrossingForYMax(xintercept, shadow_ymax);
+ ++data.crossings;
+ }
+ }
+ else {
+ if (sy1<=shadow_ymin) {
+ xintercept = sx0 + (shadow_ymin - sy0) * alpha;
+ data.setCrossingForYMin(xintercept, shadow_ymin);
+ --data.crossings;
+ }
+ if (sy0>=shadow_ymax) {
+ xintercept = sx0 + (shadow_ymax - sy0) * alpha;
+ data.setCrossingForYMax(xintercept, shadow_ymax);
+ --data.crossings;
+ }
+ }
+ }
+ else if (IntersectionUtil.intersectsSegmentSegmentWithoutEnds(
+ shadow_x0, shadow_y0, shadow_x1, shadow_y1,
+ sx0, sy0, sx1, sy1)) {
+ data.crossings = MathConstants.SHAPE_INTERSECTS;
+ }
+ else {
+ int side1, side2;
+ boolean isUp = (shadow_y0<=shadow_y1);
+ if (isUp) {
+ side1 = GeometryUtil.getPointSideOfLine(
+ shadow_x0, shadow_y0,
+ shadow_x1, shadow_y1,
+ sx0, sy0, 0f);
+ side2 = GeometryUtil.getPointSideOfLine(
+ shadow_x0, shadow_y0,
+ shadow_x1, shadow_y1,
+ sx1, sy1, 0f);
+ }
+ else {
+ side1 = GeometryUtil.getPointSideOfLine(
+ shadow_x1, shadow_y1,
+ shadow_x0, shadow_y0,
+ sx0, sy0, 0f);
+ side2 = GeometryUtil.getPointSideOfLine(
+ shadow_x1, shadow_y1,
+ shadow_x0, shadow_y0,
+ sx1, sy1, 0f);
+ }
+ if (side1>0 || side2>0) {
+ computeCrossings3(
+ shadow_x0, shadow_y0,
+ sx0, sy0, sx1, sy1,
+ data, isUp);
+ computeCrossings3(
+ shadow_x1, shadow_y1,
+ sx0, sy0, sx1, sy1,
+ data, !isUp);
+ }
+ }
+ }
+
+ private static void computeCrossings3(
+ float shadowx, float shadowy,
+ float sx0, float sy0,
+ float sx1, float sy1,
+ PathShadowData data,
+ boolean isUp) {
+ if (shadowy < sy0 && shadowy < sy1) return;
+ if (shadowy > sy0 && shadowy > sy1) return;
+ if (shadowx > sx0 && shadowx > sx1) return;
+ float xintercept = sx0 + (shadowy - sy0) * (sx1 - sx0) / (sy1 - sy0);
+ if (shadowx > xintercept) return;
+ if (isUp) {
+ data.setCrossingForYMax(xintercept, shadowy);
+ }
+ else {
+ data.setCrossingForYMin(xintercept, shadowy);
+ }
+ data.crossings += (sy0 < sy1) ? 1 : -1;
+ }
+
+ /**
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private static class PathShadowData {
+
+ public int crossings = 0;
+ public boolean hasX4ymin = false;
+ public boolean hasX4ymax = false;
+ public float x4ymin;
+ public float x4ymax;
+ public float xmin4ymin;
+ public float xmin4ymax;
+ public float ymin;
+ public float ymax;
+
+ @Override
+ public String toString() {
+ StringBuilder b = new StringBuilder();
+ b.append("SHADOW {\n\tlow: ( "); //$NON-NLS-1$
+ b.append(this.xmin4ymin);
+ b.append(" | "); //$NON-NLS-1$
+ b.append(this.ymin);
+ b.append(" )\n\thigh: ( "); //$NON-NLS-1$
+ b.append(this.xmin4ymax);
+ b.append(" | "); //$NON-NLS-1$
+ b.append(this.ymax);
+ b.append(")\n}\nCROSSINGS {\n\tcrossings="); //$NON-NLS-1$
+ b.append(this.crossings);
+ b.append("\n\tlow: "); //$NON-NLS-1$
+ if (this.hasX4ymin) {
+ b.append("( "); //$NON-NLS-1$
+ b.append(this.x4ymin);
+ b.append(" | "); //$NON-NLS-1$
+ b.append(this.ymin);
+ b.append(" )\n"); //$NON-NLS-1$
+ }
+ else {
+ b.append("none\n"); //$NON-NLS-1$
+ }
+ b.append("\thigh: "); //$NON-NLS-1$
+ if (this.hasX4ymax) {
+ b.append("( "); //$NON-NLS-1$
+ b.append(this.x4ymax);
+ b.append(" | "); //$NON-NLS-1$
+ b.append(this.ymax);
+ b.append(" )\n"); //$NON-NLS-1$
+ }
+ else {
+ b.append("none\n"); //$NON-NLS-1$
+ }
+ b.append("}\n"); //$NON-NLS-1$
+ return b.toString();
+ }
+
+ public PathShadowData(float xmax, float miny, float maxy) {
+ this.x4ymin = this.x4ymax = xmax;
+ this.xmin4ymax = this.xmin4ymin = xmax;
+ this.ymin = miny;
+ this.ymax = maxy;
+ }
+
+ public void setCrossingForYMax(float x, float y) {
+ if (y>=this.ymax) {
+ if (x=this.ymax && xh implements Point2D {
+
+ private static final long serialVersionUID = 8963319137253544821L;
+
+ /**
+ */
+ public Point2f() {
+ //
+ }
+
+ /**
+ * @param tuple is the tuple to copy.
+ */
+ public Point2f(Tuple2D tuple) {
+ super(tuple);
+ }
+
+ /**
+ * @param tuple is the tuple to copy.
+ */
+ public Point2f(int[] tuple) {
+ super(tuple);
+ }
+
+ /**
+ * @param tuple is the tuple to copy.
+ */
+ public Point2f(float[] tuple) {
+ super(tuple);
+ }
+
+ /**
+ * @param x
+ * @param y
+ */
+ public Point2f(int x, int y) {
+ super(x,y);
+ }
+
+ /**
+ * @param x
+ * @param y
+ */
+ public Point2f(float x, float y) {
+ super(x,y);
+ }
+
+ /**
+ * @param x
+ * @param y
+ */
+ public Point2f(double x, double y) {
+ super((float)x,(float)y);
+ }
+
+ /**
+ * @param x
+ * @param y
+ */
+ public Point2f(long x, long y) {
+ super(x,y);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public Point2f clone() {
+ return (Point2f)super.clone();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float distanceSquared(Point2D p1) {
+ float dx, dy;
+ dx = this.x-p1.getX();
+ dy = this.y-p1.getY();
+ return (dx*dx+dy*dy);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float distance(Point2D p1) {
+ float dx, dy;
+ dx = this.x-p1.getX();
+ dy = this.y-p1.getY();
+ return (float)Math.sqrt(dx*dx+dy*dy);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float distanceL1(Point2D p1) {
+ return (Math.abs(this.x-p1.getX()) + Math.abs(this.y-p1.getY()));
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float distanceLinf(Point2D p1) {
+ return Math.max( Math.abs(this.x-p1.getX()), Math.abs(this.y-p1.getY()));
+ }
+
+ @Override
+ public void add(Point2D t1, Vector2D t2) {
+ this.x = t1.getX() + t2.getX();
+ this.y = t1.getY() + t2.getY();
+ }
+
+ @Override
+ public void add(Vector2D t1, Point2D t2) {
+ this.x = t1.getX() + t2.getX();
+ this.y = t1.getY() + t2.getY();
+ }
+
+ @Override
+ public void add(Vector2D t1) {
+ this.x += t1.getX();
+ this.y += t1.getY();
+ }
+
+ @Override
+ public void scaleAdd(int s, Vector2D t1, Point2D t2) {
+ this.x = s * t1.getX() + t2.getX();
+ this.y = s * t1.getY() + t2.getY();
+ }
+
+ @Override
+ public void scaleAdd(float s, Vector2D t1, Point2D t2) {
+ this.x = s * t1.getX() + t2.getX();
+ this.y = s * t1.getY() + t2.getY();
+ }
+
+ @Override
+ public void scaleAdd(int s, Point2D t1, Vector2D t2) {
+ this.x = s * t1.getX() + t2.getX();
+ this.y = s * t1.getY() + t2.getY();
+ }
+
+ @Override
+ public void scaleAdd(float s, Point2D t1, Vector2D t2) {
+ this.x = s * t1.getX() + t2.getX();
+ this.y = s * t1.getY() + t2.getY();
+ }
+
+ @Override
+ public void scaleAdd(int s, Vector2D t1) {
+ this.x = s * this.x + t1.getX();
+ this.y = s * this.y + t1.getY();
+ }
+
+ @Override
+ public void scaleAdd(float s, Vector2D t1) {
+ this.x = s * this.x + t1.getX();
+ this.y = s * this.y + t1.getY();
+ }
+
+ @Override
+ public void sub(Point2D t1, Vector2D t2) {
+ this.x = t1.getX() - t2.getX();
+ this.y = t1.getY() - t2.getY();
+ }
+
+ @Override
+ public void sub(Vector2D t1) {
+ this.x -= t1.getX();
+ this.y -= t1.getY();
+ }
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Rectangle2f.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Rectangle2f.java
new file mode 100644
index 000000000..1effff553
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Rectangle2f.java
@@ -0,0 +1,708 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.continuous;
+
+import java.util.NoSuchElementException;
+
+import org.arakhne.afc.math.MathConstants;
+import org.arakhne.afc.math.geometry.GeometryUtil;
+import org.arakhne.afc.math.geometry.IntersectionUtil;
+import org.arakhne.afc.math.geometry.PathWindingRule;
+import org.arakhne.afc.math.geometry2d.Point2D;
+import org.arakhne.afc.math.geometry2d.Vector2D;
+
+
+
+/** 2D rectangle with floating-point points.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class Rectangle2f extends AbstractRectangularShape2f {
+
+ private static final long serialVersionUID = 8716296371653330467L;
+
+ /**
+ */
+ public Rectangle2f() {
+ //
+ }
+
+ /**
+ * @param min is the min corner of the rectangle.
+ * @param max is the max corner of the rectangle.
+ */
+ public Rectangle2f(Point2f min, Point2f max) {
+ super(min, max);
+ }
+
+ /**
+ * @param x
+ * @param y
+ * @param width
+ * @param height
+ */
+ public Rectangle2f(float x, float y, float width, float height) {
+ super(x, y, width, height);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public Rectangle2f toBoundingBox() {
+ return clone();
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public float distanceSquared(Point2D p) {
+ return GeometryUtil.distanceSquaredPointRectangle(p.getX(), p.getY(), this.getMinX(), this.getMinY(), this.getMaxX(),this.getMaxY());
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public float distanceL1(Point2D p) {
+ float dx;
+ if (p.getX()getMaxX()) {
+ dx = p.getX() - getMaxX();
+ }
+ else {
+ dx = 0f;
+ }
+ float dy;
+ if (p.getY()getMaxY()) {
+ dy = p.getY() - getMaxY();
+ }
+ else {
+ dy = 0f;
+ }
+ return dx + dy;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public float distanceLinf(Point2D p) {
+ float dx;
+ if (p.getX()getMaxX()) {
+ dx = p.getX() - getMaxX();
+ }
+ else {
+ dx = 0f;
+ }
+ float dy;
+ if (p.getY()getMaxY()) {
+ dy = p.getY() - getMaxY();
+ }
+ else {
+ dy = 0f;
+ }
+ return Math.max(dx, dy);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public boolean contains(float x, float y) {
+ return GeometryUtil.isInsidePointRectangle(x, y, getMinX(), getMinY(), getMaxX(), getMaxY());
+ }
+
+ @Override
+ public boolean contains(Rectangle2f r) {
+ return GeometryUtil.isInsideRectangleRectangle(
+ getMinX(), getMinY(), getMaxX(), getMaxY(),
+ r.getMinX(), r.getMinY(), r.getMaxX(), r.getMaxY());
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public Point2D getClosestPointTo(Point2D p) {
+
+ Point2f closest = new Point2f();
+ GeometryUtil.closestPointPointRectangle(p.getX(), p.getY(), getMinX(), getMinY(), getMaxX(), getMaxY(), closest);
+ return closest;
+ }
+
+ /** Add the given coordinate in the rectangle.
+ *
+ * The corners of the rectangles are moved to
+ * enclosed the given coordinate.
+ *
+ * @param p
+ */
+ public void add(Point2D p) {
+ add(p.getX(), p.getY());
+ }
+
+ /** Add the given coordinate in the rectangle.
+ *
+ * The corners of the rectangles are moved to
+ * enclosed the given coordinate.
+ *
+ * @param x
+ * @param y
+ */
+ public void add(float x, float y) {
+ if (xgetMaxX()) {
+ setMaxX(x);
+ }
+ if (ygetMaxY()) {
+ setMaxY(y);
+ }
+ }
+
+ /** Compute and replies the union of this rectangle and the given rectangle.
+ * This function does not change this rectangle.
+ *
+ * It is equivalent to (where ur is the union):
+ *
+ * Rectangle2f ur = new Rectangle2f();
+ * Rectangle2f.union(ur, this, r);
+ *
+ *
+ * @param r
+ * @return the union of this rectangle and the given rectangle.
+ * @see #union(Rectangle2f, Rectangle2f, Rectangle2f)
+ * @see #setUnion(Rectangle2f)
+ */
+ public Rectangle2f createUnion(Rectangle2f r) {
+ Rectangle2f rr = this.clone();
+ rr.union(r);
+ return rr;
+ }
+
+ /** Compute and replies the intersection of this rectangle and the given rectangle.
+ * This function does not change this rectangle.
+ *
+ * It is equivalent to (where ir is the intersection):
+ *
+ * Rectangle2f ir = new Rectangle2f();
+ * Rectangle2f.intersection(ir, this, r);
+ *
+ *
+ * @param r
+ * @return the union of this rectangle and the given rectangle.
+ * @see #intersection(Rectangle2f, Rectangle2f, Rectangle2f)
+ * @see #createIntersection(Rectangle2f)
+ */
+ public Rectangle2f createIntersection(Rectangle2f r) {
+ Rectangle2f rr = this.clone();
+ rr.intersection(r);
+ return rr;
+ }
+
+
+ @Override
+ public PathIterator2f getPathIterator(Transform2D transform) {
+ if (transform==null) {
+ return new CopyPathIterator(
+ getMinX(), getMinY(),
+ getMaxX(), getMaxY());
+ }
+ return new TransformPathIterator(
+ getMinX(), getMinY(),
+ getMaxX(), getMaxY(),
+ transform);
+ }
+
+ @Override
+ public boolean equals(Object obj) {
+ if (obj == this) {
+ return true;
+ }
+ if (obj instanceof Rectangle2f) {
+ Rectangle2f rr2d = (Rectangle2f) obj;
+ return ((getMinX() == rr2d.getMinX()) &&
+ (getMinY() == rr2d.getMinY()) &&
+ (getWidth() == rr2d.getWidth()) &&
+ (getHeight() == rr2d.getHeight()));
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ long bits = 1L;
+ bits = 31L * bits + floatToIntBits(getMinX());
+ bits = 31L * bits + floatToIntBits(getMinY());
+ bits = 31L * bits + floatToIntBits(getMaxX());
+ bits = 31L * bits + floatToIntBits(getMaxY());
+ return (int) (bits ^ (bits >> 32));
+ }
+
+ @Override
+ public boolean intersects(Rectangle2f s) {
+ return IntersectionUtil.intersectsRectangleRectangle(
+ getMinX(), getMinY(),
+ getMaxX(), getMaxY(),
+ s.getMinX(), s.getMinY(),
+ s.getMaxX(), s.getMaxY());
+ }
+
+ @Override
+ public boolean intersects(Ellipse2f s) {
+ return IntersectionUtil.intersectsEllipseRectangle(
+ s.getMinX(), s.getMinY(),
+ s.getMaxX(), s.getMaxY(),
+ getMinX(), getMinY(),
+ getMaxX(), getMaxY());
+ }
+
+ @Override
+ public boolean intersects(Circle2f s) {
+ return IntersectionUtil.intersectsCircleRectangle(
+ s.getX(), s.getY(),
+ s.getRadius(),
+ getMinX(), getMinY(),
+ getMaxX(), getMaxY());
+ }
+
+ @Override
+ public boolean intersects(Segment2f s) {
+ return IntersectionUtil.intersectsRectangleSegment(
+ getMinX(), getMinY(),
+ getMaxX(), getMaxY(),
+ s.getX1(), s.getY1(),
+ s.getX2(), s.getY2());
+ }
+
+ @Override
+ public boolean intersects(OrientedRectangle2f s) {
+ return IntersectionUtil.intersectsAlignedRectangleOrientedRectangle(
+ this.minx, this.miny, this.maxy, this.maxy,
+ s.getCx(), s.getCy(), s.getRx(), s.getRy(), s.getSx(), s.getSy(), s.getExtentR(), s.getExtentS());
+ }
+
+ @Override
+ public boolean intersects(Path2f s) {
+ return intersects(s.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO));
+ }
+
+ @Override
+ public boolean intersects(PathIterator2f s) {
+ // Copied from AWT API
+ if (isEmpty()) return false;
+ int mask = (s.getWindingRule() == PathWindingRule.NON_ZERO ? -1 : 2);
+ int crossings = Path2f.computeCrossingsFromRect(
+ s,
+ getMinX(), getMinY(), getMaxX(), getMaxY(),
+ false, true);
+ return (crossings == MathConstants.SHAPE_INTERSECTS ||
+ (crossings & mask) != 0);
+ }
+
+ @Override
+ public String toString() {
+ StringBuilder b = new StringBuilder();
+ b.append("["); //$NON-NLS-1$
+ b.append(getMinX());
+ b.append(";"); //$NON-NLS-1$
+ b.append(getMinY());
+ b.append(";"); //$NON-NLS-1$
+ b.append(getMaxX());
+ b.append(";"); //$NON-NLS-1$
+ b.append(getMaxY());
+ b.append("]"); //$NON-NLS-1$
+ return b.toString();
+ }
+
+ /** Move this rectangle to avoid collision
+ * with the reference rectangle.
+ *
+ * @param reference is the rectangle to avoid collision with.
+ * @return the displacement vector.
+ */
+ public Vector2D avoidCollisionWith(Rectangle2f reference) {
+ float dx1 = reference.getMaxX() - getMinX();
+ float dx2 = getMaxX() - reference.getMinX();
+ float dy1 = reference.getMaxY() - getMinY();
+ float dy2 = getMaxY() - reference.getMinY();
+
+ float absdx1 = Math.abs(dx1);
+ float absdx2 = Math.abs(dx2);
+ float absdy1 = Math.abs(dy1);
+ float absdy2 = Math.abs(dy2);
+
+ float dx = 0;
+ float dy = 0;
+
+ if (dx1>=0 && absdx1<=absdx2 && absdx1<=absdy1 && absdx1<=absdy2) {
+ // Move according to dx1
+ dx = dx1;
+ }
+ else if (dx2>=0 && absdx2<=absdx1 && absdx2<=absdy1 && absdx2<=absdy2) {
+ // Move according to dx2
+ dx = - dx2;
+ }
+ else if (dy1>=0 && absdy1<=absdx1 && absdy1<=absdx2 && absdy1<=absdy2) {
+ // Move according to dy1
+ dy = dy1;
+ }
+ else {
+ // Move according to dy2
+ dy = - dy2;
+ }
+
+ set(
+ getMinX()+dx,
+ getMinY()+dy,
+ getWidth(),
+ getHeight());
+
+ return new Vector2f(dx, dy);
+ }
+
+ /** Move this rectangle to avoid collision
+ * with the reference rectangle.
+ *
+ * @param reference is the rectangle to avoid collision with.
+ * @param displacementDirection is the direction of the allowed displacement.
+ * @return the displacement vector.
+ */
+ public Vector2D avoidCollisionWith(Rectangle2f reference, Vector2D displacementDirection) {
+ if (displacementDirection==null || displacementDirection.lengthSquared()==0f)
+ return avoidCollisionWith(reference);
+
+ float dx1 = reference.getMaxX() - getMinX();
+ float dx2 = reference.getMinX() - getMaxX();
+ float dy1 = reference.getMaxY() - getMinY();
+ float dy2 = reference.getMinY() - getMaxY();
+
+ float absdx1 = Math.abs(dx1);
+ float absdx2 = Math.abs(dx2);
+ float absdy1 = Math.abs(dy1);
+ float absdy2 = Math.abs(dy2);
+
+ float dx, dy;
+
+ if (displacementDirection.getX()<0) {
+ dx = -Math.min(absdx1, absdx2);
+ }
+ else {
+ dx = Math.min(absdx1, absdx2);
+ }
+
+ if (displacementDirection.getY()<0) {
+ dy = -Math.min(absdy1, absdy2);
+ }
+ else {
+ dy = Math.min(absdy1, absdy2);
+ }
+
+ set(
+ getMinX()+dx,
+ getMinY()+dy,
+ getWidth(),
+ getHeight());
+
+ displacementDirection.set(dx, dy);
+ return displacementDirection;
+ }
+
+ /** Compute the union of r1 and r2. Set this with the result.
+ *
+ * @param dest is the union.
+ * @param r1
+ * @param r2
+ */
+ public void union(Rectangle2f r1, Rectangle2f r2) {
+ this.setFromCorners(
+ Math.min(r1.getMinX(), r2.getMinX()),
+ Math.min(r1.getMinY(), r2.getMinY()),
+ Math.max(r1.getMaxX(), r2.getMaxX()),
+ Math.max(r1.getMaxY(), r2.getMaxY()));
+ }
+
+ /** Compute the union of this and r1. Set this with the result.
+ *
+ * @param r1
+ */
+ public void union(Rectangle2f r1) {
+ this.setFromCorners(
+ Math.min(this.getMinX(), r1.getMinX()),
+ Math.min(this.getMinY(), r1.getMinY()),
+ Math.max(this.getMaxX(), r1.getMaxX()),
+ Math.max(this.getMaxY(), r1.getMaxY()));
+ }
+
+ /** Compute the intersection of r1 and this. Set this with the result.
+ *
+ * @param r1
+ *
+ */
+ public void intersection(Rectangle2f r1) {
+ float x1 = Math.max(r1.getMinX(), this.getMinX());
+ float y1 = Math.max(r1.getMinY(), this.getMinY());
+ float x2 = Math.min(r1.getMaxX(), this.getMaxX());
+ float y2 = Math.min(r1.getMaxY(), this.getMaxY());
+ if (x1<=x2 && y1<=y2) {
+ this.setFromCorners(x1, y1, x2, y2);
+ }
+ else {
+ this.set(0, 0, 0, 0);
+ }
+ }
+
+ /** Compute the intersection of r1 and r2. Set this with the result.
+ *
+ * @param r1
+ * @param r2
+ */
+ public void intersection(Rectangle2f r1, Rectangle2f r2) {
+ float x1 = Math.max(r1.getMinX(), r2.getMinX());
+ float y1 = Math.max(r1.getMinY(), r2.getMinY());
+ float x2 = Math.min(r1.getMaxX(), r2.getMaxX());
+ float y2 = Math.min(r1.getMaxY(), r2.getMaxY());
+ if (x1<=x2 && y1<=y2) {
+ this.setFromCorners(x1, y1, x2, y2);
+ }
+ else {
+ this.set(0, 0, 0, 0);
+ }
+ }
+
+
+
+
+ /** Iterator on the path elements of the rectangle.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private static class CopyPathIterator implements PathIterator2f {
+
+ private final float x1;
+ private final float y1;
+ private final float x2;
+ private final float y2;
+ private int index = 0;
+
+ /**
+ * @param x1
+ * @param y1
+ * @param x2
+ * @param y2
+ */
+ public CopyPathIterator(float x1, float y1, float x2, float y2) {
+ this.x1 = Math.min(x1, x2);
+ this.y1 = Math.min(y1, y2);
+ this.x2 = Math.max(x1, x2);
+ this.y2 = Math.max(y1, y2);
+ if (Math.abs(this.x1-this.x2)<=0f || Math.abs(this.y1-this.y2)<=0f) {
+ this.index = 6;
+ }
+ }
+
+ @Override
+ public boolean hasNext() {
+ return this.index<=5;
+ }
+
+ @Override
+ public PathElement2f next() {
+ int idx = this.index;
+ ++this.index;
+ switch(idx) {
+ case 0:
+ return new PathElement2f.MovePathElement2f(
+ this.x1, this.y1);
+ case 1:
+ return new PathElement2f.LinePathElement2f(
+ this.x1, this.y1,
+ this.x2, this.y1);
+ case 2:
+ return new PathElement2f.LinePathElement2f(
+ this.x2, this.y1,
+ this.x2, this.y2);
+ case 3:
+ return new PathElement2f.LinePathElement2f(
+ this.x2, this.y2,
+ this.x1, this.y2);
+ case 4:
+ return new PathElement2f.LinePathElement2f(
+ this.x1, this.y2,
+ this.x1, this.y1);
+ case 5:
+ return new PathElement2f.ClosePathElement2f(
+ this.x1, this.y1,
+ this.x1, this.y1);
+ default:
+ throw new NoSuchElementException();
+ }
+ }
+
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public PathWindingRule getWindingRule() {
+ return PathWindingRule.NON_ZERO;
+ }
+
+ @Override
+ public boolean isPolyline() {
+ return true;
+ }
+ }
+
+ /** Iterator on the path elements of the rectangle.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private static class TransformPathIterator implements PathIterator2f {
+
+ private final Transform2D transform;
+ private final float x1;
+ private final float y1;
+ private final float x2;
+ private final float y2;
+ private int index = 0;
+
+ private final Point2D p1 = new Point2f();
+ private final Point2D p2 = new Point2f();
+
+ /**
+ * @param x1
+ * @param y1
+ * @param x2
+ * @param y2
+ * @param transform
+ */
+ public TransformPathIterator(float x1, float y1, float x2, float y2, Transform2D transform) {
+ this.transform = transform;
+ this.x1 = Math.min(x1, x2);
+ this.y1 = Math.min(y1, y2);
+ this.x2 = Math.max(x1, x2);
+ this.y2 = Math.max(y1, y2);
+ if (Math.abs(this.x1-this.x2)<=0f || Math.abs(this.y1-this.y2)<=0f) {
+ this.index = 6;
+ }
+ }
+
+ @Override
+ public boolean hasNext() {
+ return this.index<=5;
+ }
+
+ @Override
+ public PathElement2f next() {
+ int idx = this.index;
+ ++this.index;
+ switch(idx) {
+ case 0:
+ this.p2.set(this.x1, this.y1);
+ if (this.transform!=null) {
+ this.transform.transform(this.p2);
+ }
+ return new PathElement2f.MovePathElement2f(
+ this.p2.getX(), this.p2.getY());
+ case 1:
+ this.p1.set(this.p2);
+ this.p2.set(this.x2, this.y1);
+ if (this.transform!=null) {
+ this.transform.transform(this.p2);
+ }
+ return new PathElement2f.LinePathElement2f(
+ this.p1.getX(), this.p1.getY(),
+ this.p2.getX(), this.p2.getY());
+ case 2:
+ this.p1.set(this.p2);
+ this.p2.set(this.x2, this.y2);
+ if (this.transform!=null) {
+ this.transform.transform(this.p2);
+ }
+ return new PathElement2f.LinePathElement2f(
+ this.p1.getX(), this.p1.getY(),
+ this.p2.getX(), this.p2.getY());
+ case 3:
+ this.p1.set(this.p2);
+ this.p2.set(this.x1, this.y2);
+ if (this.transform!=null) {
+ this.transform.transform(this.p2);
+ }
+ return new PathElement2f.LinePathElement2f(
+ this.p1.getX(), this.p1.getY(),
+ this.p2.getX(), this.p2.getY());
+ case 4:
+ this.p1.set(this.p2);
+ this.p2.set(this.x1, this.y1);
+ if (this.transform!=null) {
+ this.transform.transform(this.p2);
+ }
+ return new PathElement2f.LinePathElement2f(
+ this.p1.getX(), this.p1.getY(),
+ this.p2.getX(), this.p2.getY());
+ case 5:
+ return new PathElement2f.ClosePathElement2f(
+ this.p2.getX(), this.p2.getY(),
+ this.p2.getX(), this.p2.getY());
+ default:
+ throw new NoSuchElementException();
+ }
+ }
+
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public PathWindingRule getWindingRule() {
+ return PathWindingRule.NON_ZERO;
+ }
+
+ @Override
+ public boolean isPolyline() {
+ return true;
+ }
+
+ }
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/RoundRectangle2f.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/RoundRectangle2f.java
new file mode 100644
index 000000000..06e597d95
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/RoundRectangle2f.java
@@ -0,0 +1,618 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.continuous;
+
+import java.util.NoSuchElementException;
+
+import org.arakhne.afc.math.MathConstants;
+import org.arakhne.afc.math.geometry.GeometryUtil;
+import org.arakhne.afc.math.geometry.IntersectionUtil;
+import org.arakhne.afc.math.geometry.PathElementType;
+import org.arakhne.afc.math.geometry.PathWindingRule;
+import org.arakhne.afc.math.geometry2d.Point2D;
+
+
+/** 2D round rectangle with floating-point points.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class RoundRectangle2f extends AbstractRectangularShape2f {
+
+ private static final long serialVersionUID = 4681356809053380781L;
+
+ private static final float ANGLE = MathConstants.PI / 4f;
+ private static final float A = 1f - (float)Math.cos(ANGLE);
+ private static final float B = (float)Math.tan(ANGLE);
+ private static final float C = (float)Math.sqrt(1f + B * B) - 1f + A;
+ private static final float CV = 4f / 3f * A * B / C;
+ private static final float ACV = (1f - CV) / 2f;
+
+ /** For each array:
+ * 4 values for each point {v0, v1, v2, v3}:
+ * point = (x + v0 * w + v1 * arcWidth,
+ * y + v2 * h + v3 * arcHeight)
+ */
+ static float CTRL_PTS[][] = {
+ { 0f, 0f, 0f, .5f },
+ { 0f, 0f, 1f, -.5f },
+ { 0f, 0f, 1f, -ACV,
+ 0f, ACV, 1f, 0f,
+ 0f, .5f, 1f, 0f },
+ { 1f, -.5f, 1f, 0f },
+ { 1f, -ACV, 1f, 0f,
+ 1f, 0f, 1f, -ACV,
+ 1f, 0f, 1f, -.5f },
+ { 1f, 0f, 0f, .5f },
+ { 1f, 0f, 0f, ACV,
+ 1f, -ACV, 0f, 0f,
+ 1f, -.5f, 0f, 0f },
+ { 0f, .5f, 0f, 0f },
+ { 0f, ACV, 0f, 0f,
+ 0f, 0f, 0f, ACV,
+ 0f, 0f, 0f, .5f },
+ {},
+ };
+
+ /** Types of path elements for the round rectangle.
+ */
+ static PathElementType TYPES[] = {
+ PathElementType.MOVE_TO,
+ PathElementType.LINE_TO, PathElementType.CURVE_TO,
+ PathElementType.LINE_TO, PathElementType.CURVE_TO,
+ PathElementType.LINE_TO, PathElementType.CURVE_TO,
+ PathElementType.LINE_TO, PathElementType.CURVE_TO,
+ PathElementType.CLOSE,
+ };
+
+
+ /** Width of the arcs at the corner of the box. */
+ protected float arcWidth;
+ /** Height of the arcs at the corner of the box. */
+ protected float arcHeight;
+
+ /**
+ */
+ public RoundRectangle2f() {
+ this.arcHeight = this.arcWidth = 0f;
+ }
+
+ /**
+ * @param min is the min corner of the rectangle.
+ * @param max is the max corner of the rectangle.
+ * @param arcWidth
+ * @param arcHeight
+ */
+ public RoundRectangle2f(Point2f min, Point2f max, float arcWidth, float arcHeight) {
+ super(min, max);
+ this.arcWidth = arcWidth;
+ this.arcHeight = arcHeight;
+ }
+
+ /**
+ * @param x
+ * @param y
+ * @param width
+ * @param height
+ * @param arcWidth
+ * @param arcHeight
+ */
+ public RoundRectangle2f(float x, float y, float width, float height, float arcWidth, float arcHeight) {
+ super(x, y, width, height);
+ this.arcWidth = arcWidth;
+ this.arcHeight = arcHeight;
+ }
+
+ @Override
+ public void clear() {
+ this.arcHeight = this.arcWidth = 0f;
+ super.clear();
+ }
+
+ /**
+ * Gets the width of the arc that rounds off the corners.
+ * @return the width of the arc that rounds off the corners
+ * of this RoundRectangle2f.
+ */
+ public float getArcWidth() {
+ return this.arcWidth;
+ }
+
+ /**
+ * Gets the height of the arc that rounds off the corners.
+ * @return the height of the arc that rounds off the corners
+ * of this RoundRectangle2f.
+ */
+ public float getArcHeight() {
+ return this.arcHeight;
+ }
+
+ /**
+ * Set the width of the arc that rounds off the corners.
+ * @param a is the width of the arc that rounds off the corners
+ * of this RoundRectangle2f.
+ */
+ public void setArcWidth(float a) {
+ this.arcWidth = a;
+ }
+
+ /**
+ * Set the height of the arc that rounds off the corners.
+ * @param a is the height of the arc that rounds off the corners
+ * of this RoundRectangle2f.
+ */
+ public void setArcHeight(float a) {
+ this.arcHeight = a;
+ }
+
+ /** Change the frame of the rectangle.
+ *
+ * @param x
+ * @param y
+ * @param width
+ * @param height
+ * @param arcWidth is the width of the arc that rounds off the corners
+ * of this RoundRectangle2f.
+ * @param arcHeight is the height of the arc that rounds off the corners
+ * of this RoundRectangle2f.
+ */
+ public void set(float x, float y, float width, float height, float arcWidth, float arcHeight) {
+ setFromCorners(x, y, x+width, y+height);
+ this.arcWidth = arcWidth;
+ this.arcHeight = arcHeight;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public boolean contains(float x, float y) {
+ return GeometryUtil.isInsidePointRoundRectangle(
+ x, y,
+ getMinX(), getMinY(), getWidth(), getHeight(), getArcWidth(), getArcHeight());
+ }
+
+ @Override
+ public boolean contains(Rectangle2f r) {
+ return GeometryUtil.isInsideRectangleRoundRectangle(
+ r.getMinX(), r.getMinY(), r.getMaxX(), r.getMaxY(),
+ getMinX(), getMinY(), getMaxX(), getMaxY(), getArcWidth(), getArcHeight());
+ }
+
+ @Override
+ public float distanceSquared(Point2D p) {
+ Point2D n = getClosestPointTo(p);
+ return n.distanceSquared(p);
+ }
+
+ @Override
+ public float distanceL1(Point2D p) {
+ Point2D n = getClosestPointTo(p);
+ return n.distanceL1(p);
+ }
+
+ @Override
+ public float distanceLinf(Point2D p) {
+ Point2D n = getClosestPointTo(p);
+ return n.distanceLinf(p);
+ }
+
+ @Override
+ public Point2D getClosestPointTo(Point2D p) {
+
+ Point2f closest = new Point2f();
+ GeometryUtil.closestPointPointRoundRectangle(p.getX(), p.getY(), getMinX(), getMinY(), getMaxX(), getMaxY(), getArcWidth(), getArcHeight(), closest);
+ return closest;
+ }
+
+ @Override
+ public PathIterator2f getPathIterator(Transform2D transform) {
+ if (transform==null) {
+ return new CopyPathIterator(
+ getMinX(), getMinY(),
+ getWidth(), getHeight(),
+ getArcWidth(), getArcHeight());
+ }
+ return new TransformPathIterator(
+ getMinX(), getMinY(),
+ getWidth(), getHeight(),
+ getArcWidth(), getArcHeight(),
+ transform);
+ }
+
+ @Override
+ public boolean equals(Object obj) {
+ if (obj == this) {
+ return true;
+ }
+ if (obj instanceof RoundRectangle2f) {
+ RoundRectangle2f rr2d = (RoundRectangle2f) obj;
+ return ((getMinX() == rr2d.getMinX()) &&
+ (getMinY() == rr2d.getMinY()) &&
+ (getWidth() == rr2d.getWidth()) &&
+ (getHeight() == rr2d.getHeight()) &&
+ (getArcWidth() == rr2d.getArcWidth()) &&
+ (getArcHeight() == rr2d.getArcHeight()));
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ long bits = 1L;
+ bits = 31L * bits + floatToIntBits(getMinX());
+ bits = 31L * bits + floatToIntBits(getMinY());
+ bits = 31L * bits + floatToIntBits(getMaxX());
+ bits = 31L * bits + floatToIntBits(getMaxY());
+ bits = 31L * bits + floatToIntBits(getArcWidth());
+ bits = 31L * bits + floatToIntBits(getArcHeight());
+ return (int) (bits ^ (bits >> 32));
+ }
+
+ @Override
+ public Rectangle2f toBoundingBox() {
+ return new Rectangle2f(getMinX(), getMinY(), getWidth(), getHeight());
+ }
+
+ @Override
+ public boolean intersects(Rectangle2f s) {
+ return IntersectionUtil.intersectsRectangleRectangle(
+ getMinX(), getMinY(),
+ getMaxX(), getMaxY(),
+ s.getMinX(), s.getMinY(),
+ s.getMaxX(), s.getMaxY());
+ }
+
+ @Override
+ public boolean intersects(Ellipse2f s) {
+ return IntersectionUtil.intersectsEllipseRectangle(
+ s.getMinX(), s.getMinY(),
+ s.getMaxX(), s.getMaxY(),
+ getMinX(), getMinY(),
+ getMaxX(), getMaxY());
+ }
+
+ @Override
+ public boolean intersects(Circle2f s) {
+ return IntersectionUtil.intersectsCircleRectangle(
+ s.getX(), s.getY(),
+ s.getRadius(),
+ getMinX(), getMinY(),
+ getMaxX(), getMaxY());
+ }
+
+ @Override
+ public boolean intersects(Segment2f s) {
+ return IntersectionUtil.intersectsRectangleSegment(
+ getMinX(), getMinY(),
+ getMaxX(), getMaxY(),
+ s.getX1(), s.getY1(),
+ s.getX2(), s.getY2());
+ }
+
+ @Override
+ public boolean intersects(OrientedRectangle2f s) {
+ return IntersectionUtil.intersectsAlignedRectangleOrientedRectangle(
+ this.minx, this.miny, this.maxy, this.maxy,
+ s.getCx(), s.getCy(), s.getRx(), s.getRy(), s.getSx(), s.getSy(), s.getExtentR(), s.getExtentS());
+ }
+
+ @Override
+ public boolean intersects(Path2f s) {
+ return intersects(s.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO));
+ }
+
+ @Override
+ public boolean intersects(PathIterator2f s) {
+ // Copied from AWT API
+ if (isEmpty()) return false;
+ int mask = (s.getWindingRule() == PathWindingRule.NON_ZERO ? -1 : 2);
+ int crossings = Path2f.computeCrossingsFromRect(
+ s,
+ getMinX(), getMinY(), getMaxX(), getMaxY(),
+ false, true);
+ return (crossings == MathConstants.SHAPE_INTERSECTS ||
+ (crossings & mask) != 0);
+ }
+
+ @Override
+ public String toString() {
+ StringBuilder b = new StringBuilder();
+ b.append("["); //$NON-NLS-1$
+ b.append(getMinX());
+ b.append(";"); //$NON-NLS-1$
+ b.append(getMinY());
+ b.append(";"); //$NON-NLS-1$
+ b.append(getMaxX());
+ b.append(";"); //$NON-NLS-1$
+ b.append(getMaxY());
+ b.append("|"); //$NON-NLS-1$
+ b.append(getArcWidth());
+ b.append("x"); //$NON-NLS-1$
+ b.append(getArcHeight());
+ b.append("]"); //$NON-NLS-1$
+ return b.toString();
+ }
+
+ /** Iterator on the path elements of the rectangle.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private static class CopyPathIterator implements PathIterator2f {
+
+ private final float x;
+ private final float y;
+ private final float w;
+ private final float h;
+ private final float aw;
+ private final float ah;
+ private int index = 0;
+
+ private float moveX, moveY, lastX, lastY;
+
+ /**
+ * @param x
+ * @param y
+ * @param w
+ * @param h
+ * @param aw
+ * @param ah
+ */
+ public CopyPathIterator(float x, float y, float w, float h, float aw, float ah) {
+ this.x = x;
+ this.y = y;
+ this.w = Math.max(0f, w);
+ this.h = Math.max(0f, h);
+ this.aw = Math.min(Math.abs(aw), w);
+ this.ah = Math.min(Math.abs(ah), h);
+ if (this.w<=0f || this.h<=0f) {
+ this.index = TYPES.length;
+ }
+ }
+
+ @Override
+ public boolean hasNext() {
+ return this.index=TYPES.length) throw new NoSuchElementException();
+ int idx = this.index;
+
+ PathElement2f element = null;
+ PathElementType type = TYPES[idx];
+ float ctrls[] = CTRL_PTS[idx];
+ float ix, iy;
+ float ctrlx1, ctrly1, ctrlx2, ctrly2;
+
+ switch(type) {
+ case MOVE_TO:
+ this.moveX = this.lastX = this.x + ctrls[0] * this.w + ctrls[1] * this.aw;
+ this.moveY = this.lastY = this.y + ctrls[2] * this.h + ctrls[3] * this.ah;
+ element = new PathElement2f.MovePathElement2f(
+ this.lastX, this.lastY);
+ break;
+ case LINE_TO:
+ ix = this.lastX;
+ iy = this.lastY;
+ this.lastX = this.x + ctrls[0] * this.w + ctrls[1] * this.aw;
+ this.lastY = this.y + ctrls[2] * this.h + ctrls[3] * this.ah;
+ element = new PathElement2f.LinePathElement2f(
+ ix, iy,
+ this.lastX, this.lastY);
+ break;
+ case CURVE_TO:
+ ix = this.lastX;
+ iy = this.lastY;
+ ctrlx1 = this.x + ctrls[0] * this.w + ctrls[1] * this.aw;
+ ctrly1 = this.y + ctrls[2] * this.h + ctrls[3] * this.ah;
+ ctrlx2 = this.x + ctrls[4] * this.w + ctrls[5] * this.aw;
+ ctrly2 = this.y + ctrls[6] * this.h + ctrls[7] * this.ah;
+ this.lastX = this.x + ctrls[8] * this.w + ctrls[9] * this.aw;
+ this.lastY = this.y + ctrls[10] * this.h + ctrls[11] * this.ah;
+ element = new PathElement2f.CurvePathElement2f(
+ ix, iy,
+ ctrlx1, ctrly1,
+ ctrlx2, ctrly2,
+ this.lastX, this.lastY);
+ break;
+ case CLOSE:
+ ix = this.lastX;
+ iy = this.lastY;
+ this.lastX = this.moveX;
+ this.lastY = this.moveY;
+ element = new PathElement2f.ClosePathElement2f(
+ ix, iy,
+ this.lastX, this.lastY);
+ break;
+ case QUAD_TO:
+ default:
+ throw new NoSuchElementException();
+ }
+
+ assert(element!=null);
+
+ ++this.index;
+
+ return element;
+ }
+
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public PathWindingRule getWindingRule() {
+ return PathWindingRule.NON_ZERO;
+ }
+
+ @Override
+ public boolean isPolyline() {
+ return false;
+ }
+
+ }
+
+ /** Iterator on the path elements of the rectangle.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private static class TransformPathIterator implements PathIterator2f {
+
+ private final Transform2D transform;
+ private final float x;
+ private final float y;
+ private final float w;
+ private final float h;
+ private final float aw;
+ private final float ah;
+ private int index = 0;
+
+ private float moveX, moveY;
+ private final Point2D last = new Point2f();
+ private final Point2D ctrl1 = new Point2f();
+ private final Point2D ctrl2 = new Point2f();
+
+ /**
+ * @param x
+ * @param y
+ * @param w
+ * @param h
+ * @param aw
+ * @param ah
+ * @param transform
+ */
+ public TransformPathIterator(float x, float y, float w, float h, float aw, float ah, Transform2D transform) {
+ this.transform = transform;
+ this.x = x;
+ this.y = y;
+ this.w = Math.max(0f, w);
+ this.h = Math.max(0f, h);
+ this.aw = Math.min(Math.abs(aw), w);
+ this.ah = Math.min(Math.abs(ah), h);
+ if (this.w<=0f || this.h<=0f) {
+ this.index = TYPES.length;
+ }
+ }
+
+ @Override
+ public boolean hasNext() {
+ return this.index=TYPES.length) throw new NoSuchElementException();
+ int idx = this.index;
+
+ PathElement2f element = null;
+ PathElementType type = TYPES[idx];
+ float ctrls[] = CTRL_PTS[idx];
+ float ix, iy;
+
+ switch(type) {
+ case MOVE_TO:
+ this.moveX = this.x + ctrls[0] * this.w + ctrls[1] * this.aw;
+ this.moveY = this.y + ctrls[2] * this.h + ctrls[3] * this.ah;
+ this.last.set(this.moveX, this.moveY);
+ this.transform.transform(this.last);
+ element = new PathElement2f.MovePathElement2f(
+ this.last.getX(), this.last.getY());
+ break;
+ case LINE_TO:
+ ix = this.last.getX();
+ iy = this.last.getY();
+ this.last.set(
+ this.x + ctrls[0] * this.w + ctrls[1] * this.aw,
+ this.y + ctrls[2] * this.h + ctrls[3] * this.ah);
+ this.transform.transform(this.last);
+ element = new PathElement2f.LinePathElement2f(
+ ix, iy,
+ this.last.getX(), this.last.getY());
+ break;
+ case CURVE_TO:
+ ix = this.last.getX();
+ iy = this.last.getY();
+ this.ctrl1.set(
+ this.x + ctrls[0] * this.w + ctrls[1] * this.aw,
+ this.y + ctrls[2] * this.h + ctrls[3] * this.ah);
+ this.transform.transform(this.ctrl1);
+ this.ctrl2.set(
+ this.x + ctrls[4] * this.w + ctrls[5] * this.aw,
+ this.y + ctrls[6] * this.h + ctrls[7] * this.ah);
+ this.transform.transform(this.ctrl2);
+ this.last.set(
+ this.x + ctrls[8] * this.w + ctrls[9] * this.aw,
+ this.y + ctrls[10] * this.h + ctrls[11] * this.ah);
+ this.transform.transform(this.last);
+ element = new PathElement2f.CurvePathElement2f(
+ ix, iy,
+ this.ctrl1.getX(), this.ctrl1.getY(),
+ this.ctrl2.getX(), this.ctrl2.getY(),
+ this.last.getX(), this.last.getY());
+ break;
+ case CLOSE:
+ ix = this.last.getX();
+ iy = this.last.getY();
+ this.last.set(this.moveX, this.moveY);
+ this.transform.transform(this.last);
+ element = new PathElement2f.ClosePathElement2f(
+ ix, iy,
+ this.last.getX(), this.last.getY());
+ break;
+ case QUAD_TO:
+ default:
+ throw new NoSuchElementException();
+ }
+
+ assert(element!=null);
+
+ ++this.index;
+
+ return element;
+ }
+
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public PathWindingRule getWindingRule() {
+ return PathWindingRule.NON_ZERO;
+ }
+
+ @Override
+ public boolean isPolyline() {
+ return false;
+ }
+
+ }
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Segment2f.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Segment2f.java
new file mode 100644
index 000000000..18a6b59c2
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Segment2f.java
@@ -0,0 +1,485 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.continuous;
+
+import java.util.NoSuchElementException;
+
+import org.arakhne.afc.math.MathConstants;
+import org.arakhne.afc.math.MathUtil;
+import org.arakhne.afc.math.geometry.GeometryUtil;
+import org.arakhne.afc.math.geometry.IntersectionUtil;
+import org.arakhne.afc.math.geometry.PathWindingRule;
+import org.arakhne.afc.math.geometry2d.Point2D;
+
+
+
+/** 2D line segment with floating-point coordinates.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class Segment2f extends AbstractShape2f {
+
+ private static final long serialVersionUID = -82425036308183925L;
+
+ /** X-coordinate of the first point. */
+ protected float ax = 0f;
+ /** Y-coordinate of the first point. */
+ protected float ay = 0f;
+ /** X-coordinate of the second point. */
+ protected float bx = 0f;
+ /** Y-coordinate of the second point. */
+ protected float by = 0f;
+
+ /**
+ */
+ public Segment2f() {
+ //
+ }
+
+ /**
+ * @param a
+ * @param b
+ */
+ public Segment2f(Point2D a, Point2D b) {
+ set(a, b);
+ }
+
+ /**
+ * @param x1
+ * @param y1
+ * @param x2
+ * @param y2
+ */
+ public Segment2f(float x1, float y1, float x2, float y2) {
+ set(x1, y1, x2, y2);
+ }
+
+ @Override
+ public void clear() {
+ this.ax = this.ay = this.bx = this.by = 0f;
+ }
+
+ /**
+ * Replies if this segment is empty.
+ * The segment is empty when the two
+ * points are equal.
+ *
+ * @return true if the two points are
+ * equal.
+ */
+ @Override
+ public boolean isEmpty() {
+ return this.ax==this.bx && this.ay==this.by;
+ }
+
+ /** Change the line.
+ *
+ * @param x1
+ * @param y1
+ * @param x2
+ * @param y2
+ */
+ public void set(float x1, float y1, float x2, float y2) {
+ this.ax = x1;
+ this.ay = y1;
+ this.bx = x2;
+ this.by = y2;
+ }
+
+ /** Change the line.
+ *
+ * @param a
+ * @param b
+ */
+ public void set(Point2D a, Point2D b) {
+ this.ax = a.getX();
+ this.ay = a.getY();
+ this.bx = b.getX();
+ this.by = b.getY();
+ }
+
+ /** Replies the X of the first point.
+ *
+ * @return the x of the first point.
+ */
+ public float getX1() {
+ return this.ax;
+ }
+
+ /** Replies the Y of the first point.
+ *
+ * @return the y of the first point.
+ */
+ public float getY1() {
+ return this.ay;
+ }
+
+ /** Replies the X of the second point.
+ *
+ * @return the x of the second point.
+ */
+ public float getX2() {
+ return this.bx;
+ }
+
+ /** Replies the Y of the second point.
+ *
+ * @return the y of the second point.
+ */
+ public float getY2() {
+ return this.by;
+ }
+
+ /** Replies the first point.
+ *
+ * @return the first point.
+ */
+ public Point2D getP1() {
+ return new Point2f(this.ax, this.ay);
+ }
+
+ /** Replies the second point.
+ *
+ * @return the second point.
+ */
+ public Point2D getP2() {
+ return new Point2f(this.bx, this.by);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public Rectangle2f toBoundingBox() {
+ Rectangle2f r = new Rectangle2f();
+ r.setFromCorners(
+ this.ax,
+ this.ay,
+ this.bx,
+ this.by);
+ return r;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public void toBoundingBox(Rectangle2f box) {
+ box.setFromCorners(
+ this.ax,
+ this.ay,
+ this.bx,
+ this.by);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public float distanceSquared(Point2D p) {
+ return GeometryUtil.distanceSquaredPointSegment(p.getX(), p.getY(),
+ this.ax, this.ay,
+ this.bx, this.by, null);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public float distanceL1(Point2D p) {
+ float ratio = GeometryUtil.getPointProjectionFactorOnLine(p.getX(), p.getY(), this.ax, this.ay, this.bx, this.by);
+ ratio = MathUtil.clamp(ratio, 0f, 1f);
+ Vector2f v = new Vector2f(this.bx, this.by);
+ v.sub(this.ax, this.ay);
+ v.scale(ratio);
+ return Math.abs(this.ax + v.getX() - p.getX())
+ + Math.abs(this.ay + v.getY() - p.getY());
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public float distanceLinf(Point2D p) {
+ float ratio = GeometryUtil.getPointProjectionFactorOnLine(p.getX(), p.getY(), this.ax, this.ay, this.bx, this.by);
+ ratio = MathUtil.clamp(ratio, 0f, 1f);
+ Vector2f v = new Vector2f(this.bx, this.by);
+ v.sub(this.ax, this.ay);
+ v.scale(ratio);
+ return Math.max(
+ Math.abs(this.ax + v.getX() - p.getX()),
+ Math.abs(this.ay + v.getY() - p.getY()));
+ }
+
+ /** {@inheritDoc}
+ *
+ * This function uses the equal-to-zero test with the error {@link MathConstants#EPSILON}.
+ *
+ * @see MathUtil#isEpsilonZero(float)
+ */
+ @Override
+ public boolean contains(float x, float y) {
+ return GeometryUtil.isInsidePointSegment(x, y,
+ this.ax, this.ay,
+ this.bx, this.by, 0);
+ }
+
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public boolean contains(Rectangle2f r) {
+ return contains(r.getMinX(), r.getMinY())
+ && contains(r.getMaxX(), r.getMaxY());
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public Point2f getClosestPointTo(Point2D p) {
+ Point2f closest = new Point2f();
+ GeometryUtil.closestPointPointSegment(p.getX(), p.getY(), this.ax, this.ay, this.bx, this.by, closest);
+ return closest;
+ }
+
+ @Override
+ public void translate(float dx, float dy) {
+ this.ax += dx;
+ this.ay += dy;
+ this.bx += dx;
+ this.by += dy;
+ }
+
+ @Override
+ public PathIterator2f getPathIterator(Transform2D transform) {
+ return new SegmentPathIterator(
+ getX1(), getY1(), getX2(), getY2(),
+ transform);
+ }
+
+ @Override
+ public boolean equals(Object obj) {
+ if (obj == this) {
+ return true;
+ }
+ if (obj instanceof Segment2f) {
+ Segment2f rr2d = (Segment2f) obj;
+ return ((getX1() == rr2d.getX1()) &&
+ (getY1() == rr2d.getY1()) &&
+ (getX2() == rr2d.getX2()) &&
+ (getY2() == rr2d.getY2()));
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ long bits = 1L;
+ bits = 31L * bits + floatToIntBits(getX1());
+ bits = 31L * bits + floatToIntBits(getY1());
+ bits = 31L * bits + floatToIntBits(getX2());
+ bits = 31L * bits + floatToIntBits(getY2());
+ return (int) (bits ^ (bits >> 32));
+ }
+
+ /** Transform the current segment.
+ * This function changes the current segment.
+ *
+ * @param transform is the affine transformation to apply.
+ * @see #createTransformedShape(Transform2D)
+ */
+ public void transform(Transform2D transform) {
+ Point2f p = new Point2f(this.ax, this.ay);
+ transform.transform(p);
+ this.ax = p.getX();
+ this.ay = p.getY();
+ p.set(this.bx, this.by);
+ transform.transform(p);
+ this.bx = p.getX();
+ this.by = p.getY();
+ }
+
+ @Override
+ public Shape2f createTransformedShape(Transform2D transform) {
+ Point2D p1 = transform.transform(this.ax, this.ay);
+ Point2D p2 = transform.transform(this.bx, this.by);
+ return new Segment2f(p1, p2);
+ }
+
+ @Override
+ public boolean intersects(Rectangle2f s) {
+ return IntersectionUtil.intersectsRectangleSegment(
+ s.getMinX(), s.getMinY(),
+ s.getMaxX(), s.getMaxY(),
+ getX1(), getY1(),
+ getX2(), getY2());
+ }
+
+ @Override
+ public boolean intersects(Ellipse2f s) {
+ return IntersectionUtil.intersectsEllipseSegment(
+ s.getMinX(), s.getMinY(),
+ s.getWidth(), s.getHeight(),
+ getX1(), getY1(),
+ getX2(), getY2());
+ }
+
+ @Override
+ public boolean intersects(Circle2f s) {
+ return IntersectionUtil.intersectsCircleSegment(
+ s.getX(), s.getY(),
+ s.getRadius(),
+ getX1(), getY1(),
+ getX2(), getY2());
+ }
+
+ @Override
+ public boolean intersects(Segment2f s) {
+ return IntersectionUtil.intersectsSegmentSegmentWithoutEnds(
+ getX1(), getY1(),
+ getX2(), getY2(),
+ s.getX1(), s.getY1(),
+ s.getX2(), s.getY2());
+ }
+
+ @Override
+ public boolean intersects(OrientedRectangle2f s) {
+ return IntersectionUtil.intersectsSegmentOrientedRectangle(
+ this.ax, this.ay, this.bx, this.bx,
+ s.getCx(), s.getCy(), s.getRx(), s.getRy(), s.getSx(), s.getSy(), s.getExtentR(), s.getExtentS());
+ }
+
+ @Override
+ public boolean intersects(Path2f s) {
+ return intersects(s.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO));
+ }
+
+ @Override
+ public boolean intersects(PathIterator2f s) {
+ int mask = (s.getWindingRule() == PathWindingRule.NON_ZERO ? -1 : 2);
+ int crossings = Path2f.computeCrossingsFromSegment(
+ 0,
+ s,
+ getX1(), getY1(), getX2(), getY2(),
+ false);
+ return (crossings == MathConstants.SHAPE_INTERSECTS ||
+ (crossings & mask) != 0);
+ }
+
+ @Override
+ public String toString() {
+ StringBuilder b = new StringBuilder();
+ b.append("["); //$NON-NLS-1$
+ b.append(getX1());
+ b.append(";"); //$NON-NLS-1$
+ b.append(getY1());
+ b.append("|"); //$NON-NLS-1$
+ b.append(getX2());
+ b.append(";"); //$NON-NLS-1$
+ b.append(getY2());
+ b.append("]"); //$NON-NLS-1$
+ return b.toString();
+ }
+
+
+ /** Iterator on the path elements of the segment.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private static class SegmentPathIterator implements PathIterator2f {
+
+ private final Point2D p1 = new Point2f();
+ private final Point2D p2 = new Point2f();
+ private final Transform2D transform;
+ private final float x1;
+ private final float y1;
+ private final float x2;
+ private final float y2;
+ private int index = 0;
+
+ /**
+ * @param x1
+ * @param y1
+ * @param x2
+ * @param y2
+ * @param transform
+ */
+ public SegmentPathIterator(float x1, float y1, float x2, float y2, Transform2D transform) {
+ this.transform = transform;
+ this.x1 = x1;
+ this.y1 = y1;
+ this.x2 = x2;
+ this.y2 = y2;
+ if (this.x1==this.x2 && this.y1==this.y2) {
+ this.index = 2;
+ }
+ }
+
+ @Override
+ public boolean hasNext() {
+ return this.index<=1;
+ }
+
+ @Override
+ public PathElement2f next() {
+ if (this.index>1) throw new NoSuchElementException();
+ int idx = this.index;
+ ++this.index;
+ switch(idx) {
+ case 0:
+ this.p2.set(this.x1, this.y1);
+ if (this.transform!=null) {
+ this.transform.transform(this.p2);
+ }
+ return new PathElement2f.MovePathElement2f(
+ this.p2.getX(), this.p2.getY());
+ case 1:
+ this.p1.set(this.p2);
+ this.p2.set(this.x2, this.y2);
+ if (this.transform!=null) {
+ this.transform.transform(this.p2);
+ }
+ return new PathElement2f.LinePathElement2f(
+ this.p1.getX(), this.p1.getY(),
+ this.p2.getX(), this.p2.getY());
+ default:
+ throw new NoSuchElementException();
+ }
+ }
+
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public PathWindingRule getWindingRule() {
+ return PathWindingRule.NON_ZERO;
+ }
+
+ @Override
+ public boolean isPolyline() {
+ return true;
+ }
+
+ }
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Shape2f.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Shape2f.java
new file mode 100644
index 000000000..b739f468c
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Shape2f.java
@@ -0,0 +1,228 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.continuous;
+
+import org.arakhne.afc.math.geometry2d.Point2D;
+import org.arakhne.afc.math.geometry2d.Shape2D;
+
+/** 2D shape with floating-point points.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public interface Shape2f extends Shape2D {
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public Shape2f clone();
+
+ /** Replies the bounds of the shape.
+ *
+ * @return the bounds of the shape.
+ */
+ public Rectangle2f toBoundingBox();
+
+ /** Replies the bounds of the shape.
+ *
+ * @param box is set with the bounds of the shape.
+ */
+ public void toBoundingBox(Rectangle2f box);
+
+ /** Replies the minimal distance from this shape to the given point.
+ *
+ * @param p
+ * @return the minimal distance between this shape and the point.
+ */
+ public float distance(Point2D p);
+
+ /** Replies the squared value of the minimal distance from this shape to the given point.
+ *
+ * @param p
+ * @return squared value of the minimal distance between this shape and the point.
+ */
+ public float distanceSquared(Point2D p);
+
+ /**
+ * Computes the L-1 (Manhattan) distance between this shape and
+ * point p1. The L-1 distance is equal to abs(x1-x2) + abs(y1-y2).
+ * @param p the point
+ * @return the distance.
+ */
+ public float distanceL1(Point2D p);
+
+ /**
+ * Computes the L-infinite distance between this shape and
+ * point p1. The L-infinite distance is equal to
+ * MAX[abs(x1-x2), abs(y1-y2)].
+ * @param p the point
+ * @return the distance.
+ */
+ public float distanceLinf(Point2D p);
+
+ /** Translate the shape.
+ *
+ * @param dx
+ * @param dy
+ */
+ public void translate(float dx, float dy);
+
+ /** Replies if the given point is inside this shape.
+ *
+ * @param x
+ * @param y
+ * @return true if the given point is inside this
+ * shape, otherwise false.
+ */
+ public boolean contains(float x, float y);
+
+ /** Replies if the given rectangle is inside this shape.
+ *
+ * @param r
+ * @return true if the given rectangle is inside this
+ * shape, otherwise false.
+ */
+ public boolean contains(Rectangle2f r);
+
+ /** Replies the elements of the paths.
+ *
+ * @param transform is the transformation to apply to the path.
+ * @return the elements of the path.
+ */
+ public PathIterator2f getPathIterator(Transform2D transform);
+
+ /** Replies the elements of the paths.
+ *
+ * @return the elements of the path.
+ */
+ public PathIterator2f getPathIterator();
+
+ /** Apply the transformation to the shape and reply the result.
+ * This function does not change the current shape.
+ *
+ * @param transform is the transformation to apply to the shape.
+ * @return the result of the transformation.
+ */
+ public Shape2f createTransformedShape(Transform2D transform);
+
+ /** Replies if this shape is intersecting the given rectangle.
+ *
+ * @param s
+ * @return true if this shape is intersecting the given shape;
+ * false if there is no intersection.
+ */
+ public boolean intersects(Rectangle2f s);
+
+ /** Replies if this shape is intersecting the given ellipse.
+ *
+ * @param s
+ * @return true if this shape is intersecting the given shape;
+ * false if there is no intersection.
+ */
+ public boolean intersects(Ellipse2f s);
+
+ /** Replies if this shape is intersecting the given circle.
+ *
+ * @param s
+ * @return true if this shape is intersecting the given shape;
+ * false if there is no intersection.
+ */
+ public boolean intersects(Circle2f s);
+
+ /** Replies if this shape is intersecting the given line.
+ *
+ * @param s
+ * @return true if this shape is intersecting the given shape;
+ * false if there is no intersection.
+ */
+ public boolean intersects(Segment2f s);
+
+ /** Replies if this shape is intersecting the given AlignedRectangle.
+ *
+ * @param s
+ * @return true if this shape is intersecting the given shape;
+ * false if there is no intersection.
+ */
+ public boolean intersects(OrientedRectangle2f s);
+
+
+ /** Replies if this shape is intersecting the given path.
+ *
+ * @param s
+ * @return true if this shape is intersecting the given path;
+ * false if there is no intersection.
+ */
+ public boolean intersects(Path2f s);
+
+ /** Replies if this shape is intersecting the given path.
+ *
+ * @param s
+ * @return true if this shape is intersecting the given path;
+ * false if there is no intersection.
+ */
+ public boolean intersects(PathIterator2f s);
+
+ /**
+ * @param p
+ * @return
+ */
+ //TODO public float distanceToFarthestPoint(Point2D p);
+ /**
+ * @param p
+ * @return
+ */
+ //TODO public float distanceSquaredToFarthestPoint(Point2D p);
+
+ /**
+ * @param p
+ * @return
+ */
+ //TODO public float distanceL1ToFarthestPoint(Point2D p);
+
+ /**
+ * @param p
+ * @return
+ */
+ //TODO public float distanceLinfToFarthestPoint(Point2D p);
+
+
+ /**
+ * @param p
+ * @return
+ */
+ //TODO public void scale(float ratio);
+
+
+ /**
+ * @param p
+ * @return
+ */
+ //TODO public void rotate(float angle);
+
+ /**
+ * @param p
+ * @return
+ */
+ //TODO public void shear(Vector2D ratios);
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Transform2D.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Transform2D.java
new file mode 100644
index 000000000..b63342237
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Transform2D.java
@@ -0,0 +1,875 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2012 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.continuous;
+
+import org.arakhne.afc.math.Matrix3f;
+import org.arakhne.afc.math.SingularMatrixException;
+import org.arakhne.afc.math.geometry2d.Point2D;
+import org.arakhne.afc.math.geometry2d.Tuple2D;
+
+
+/** A 2D transformation.
+ * Is represented internally as a 3x3 floating point matrix. The
+ * mathematical representation is row major, as in traditional
+ * matrix mathematics.
+ *
+ * The transformation matrix is:
+ *
+ * | cos(theta) | -sin(theta) | Tx |
+ * | sin(theta) | cos(theta) | Ty |
+ * | 0 | 0 | 1 |
+ *
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class Transform2D extends Matrix3f {
+
+ private static final long serialVersionUID = -3437760883865605968L;
+
+ /** This is the identifity transformation.
+ */
+ public static final Transform2D IDENTITY = new Transform2D();
+
+ /**
+ * Constructs a new Transform2D object and sets it to the identity transformation.
+ */
+ public Transform2D() {
+ super(1f, 0f, 0f, 0f, 1f, 0f, 0f, 0f, 1f);
+ }
+
+ /**
+ * Constructs a new Transform2D object and initializes it from the
+ * specified transform.
+ *
+ * @param t
+ */
+ public Transform2D(Transform2D t) {
+ super(t);
+ }
+
+ /**
+ * @param m
+ */
+ public Transform2D(Matrix3f m) {
+ super(m);
+ }
+
+ /**
+ * Constructs and initializes a Matrix3f from the specified nine values.
+ *
+ * @param m00
+ * the [0][0] element
+ * @param m01
+ * the [0][1] element
+ * @param m02
+ * the [0][2] element
+ * @param m10
+ * the [1][0] element
+ * @param m11
+ * the [1][1] element
+ * @param m12
+ * the [1][2] element
+ */
+ public Transform2D(float m00, float m01, float m02, float m10, float m11, float m12) {
+ super(m00, m01, m02, m10, m11, m12, 0f, 0f, 1f);
+ }
+
+ @Override
+ public Transform2D clone() {
+ return (Transform2D)super.clone();
+ }
+
+ /** Set the position.
+ *
+ * This function changes only the elements of
+ * the matrix related to the translation (m02,
+ * m12). The scaling and the shearing are not changed.
+ *
+ * After a call to this function, the matrix will
+ * contains (? means any value):
+ *
+ * [ ? ? x ]
+ * [ ? ? y ]
+ * [ ? ? ? ]
+ *
+ *
+ * @param x
+ * @param y
+ * @see #makeTranslationMatrix(float, float)
+ */
+ public void setTranslation(float x, float y) {
+ this.m02 = x;
+ this.m12 = y;
+ }
+
+ /** Set the position.
+ *
+ * This function changes only the elements of
+ * the matrix related to the translation (m02,
+ * m12). The scaling and the shearing are not changed.
+ *
+ * After a call to this function, the matrix will
+ * contains (? means any value):
+ *
+ * [ ? ? t.x ]
+ * [ ? ? t.y ]
+ * [ ? ? ? ]
+ *
+ *
+ * @param t
+ * @see #makeTranslationMatrix(float, float)
+ */
+ public void setTranslation(Tuple2D t) {
+ this.m02 = t.getX();
+ this.m12 = t.getY();
+ }
+
+ /** Translate the position.
+ *
+ * This function is equivalent to:
+ *
+ * this = this * [ 0 0 dx ]
+ * [ 0 0 dy ]
+ * [ 0 0 1 ]
+ *
+ *
+ * @param dx
+ * @param dy
+ */
+ public void translate(float dx, float dy) {
+ this.m02 = this.m00 * dx + this.m01 * dy + this.m02;
+ this.m12 = this.m10 * dx + this.m11 * dy + this.m12;
+ }
+
+ /** Translate the position.
+ *
+ * This function is equivalent to:
+ *
+ * this = this * [ 0 0 t.x ]
+ * [ 0 0 t.y ]
+ * [ 0 0 1 ]
+ *
+ *
+ * @param t
+ */
+ public void translate(Vector2f t) {
+ translate(t.getX(), t.getY());
+ }
+
+ /** Replies the X translation.
+ *
+ * @return the amount
+ */
+ public float getTranslationX() {
+ return this.m02;
+ }
+
+ /** Replies the Y translation.
+ *
+ * @return the amount
+ */
+ public float getTranslationY() {
+ return this.m12;
+ }
+
+ /** Replies the translation.
+ *
+ * @return the amount
+ */
+ public Vector2f getTranslationVector() {
+ return new Vector2f(this.m02, this.m12);
+ }
+
+ /**
+ * Replies the rotation for the object (theta).
+ *
+ * @return the amount
+ */
+ public float getRotation() {
+ float cosAngle = (float)Math.acos(this.m00);
+ float sinAngle = (float)Math.asin(this.m10);
+ return (sinAngle<0f) ? -cosAngle : cosAngle;
+ }
+
+ /**
+ * Set the rotation for the object (theta).
+ *
+ * This function changes only the elements of
+ * the matrix related to the rotation (m00,
+ * m01, m10, m11). The translation is not changed.
+ *
+ * After a call to this function, the matrix will
+ * contains (? means any value):
+ *
+ * [ cos(theta) -sin(theta) ? ]
+ * [ sin(theta) cos(theta) ? ]
+ * [ ? ? ? ]
+ *
+ *
+ * @param theta
+ * @see #makeRotationMatrix(float)
+ */
+ public void setRotation(float theta) {
+ float cosTheta = (float)Math.cos(theta);
+ float sinTheta = (float)Math.sin(theta);
+ this.m00 = cosTheta;
+ this.m01 = -sinTheta;
+ this.m10 = sinTheta;
+ this.m11 = cosTheta;
+ }
+
+ /**
+ * Rotate the object (theta).
+ *
+ * This function is equivalent to:
+ *
+ * this = this * [ cos(theta) -sin(theta) 0 ]
+ * [ sin(theta) cos(theta) 0 ]
+ * [ 0 0 1 ]
+ *
+ *
+ * @param theta
+ */
+ public void rotate(float theta) {
+ // Copied from AWT API
+ float sin = (float)Math.sin(theta);
+ if (sin == 1f) {
+ rotate90();
+ }
+ else if (sin == -1f) {
+ rotate270();
+ }
+ else {
+ float cos = (float)Math.cos(theta);
+ if (cos == -1f) {
+ rotate180();
+ }
+ else if (cos != 1f) {
+ float M0, M1;
+ M0 = this.m00;
+ M1 = this.m01;
+ this.m00 = cos * M0 + sin * M1;
+ this.m01 = -sin * M0 + cos * M1;
+ M0 = this.m10;
+ M1 = this.m11;
+ this.m10 = cos * M0 + sin * M1;
+ this.m11 = -sin * M0 + cos * M1;
+ }
+ }
+ }
+
+ private final void rotate90() {
+ // Copied from AWT API
+ float M0 = this.m00;
+ this.m00 = this.m01;
+ this.m01 = -M0;
+ M0 = this.m10;
+ this.m10 = this.m11;
+ this.m11 = -M0;
+ }
+
+ private final void rotate180() {
+ // Copied from AWT API
+ this.m00 = -this.m00;
+ this.m11 = -this.m11;
+ // If there was a shear, then this rotation has no
+ // effect on the state.
+ this.m01 = -this.m01;
+ this.m10 = -this.m10;
+ }
+
+ private final void rotate270() {
+ // Copied from AWT API
+ float M0 = this.m00;
+ this.m00 = -this.m01;
+ this.m01 = M0;
+ M0 = this.m10;
+ this.m10 = -this.m11;
+ this.m11 = M0;
+ }
+
+ /** Set the scale.
+ *
+ * This function changes only the elements of
+ * the matrix related to the scaling (m00,
+ * m11). The shearing and the translation are not changed.
+ *
+ * After a call to this function, the matrix will
+ * contains (? means any value):
+ *
+ * [ sx ? ? ]
+ * [ ? sy ? ]
+ * [ ? ? ? ]
+ *
+ *
+ * @param sx
+ * @param sy
+ * @see #makeScaleMatrix(float, float)
+ */
+ public void setScale(float sx, float sy) {
+ this.m00 = sx;
+ this.m11 = sy;
+ }
+
+ /** Set the scale.
+ *
+ * This function changes only the elements of
+ * the matrix related to the scaling (m00,
+ * m11). The shearing and the translation are not changed.
+ *
+ * After a call to this function, the matrix will
+ * contains (? means any value):
+ *
+ * [ t.x ? ? ]
+ * [ ? t.y ? ]
+ * [ ? ? ? ]
+ *
+ *
+ * @param t
+ * @see #makeScaleMatrix(float, float)
+ */
+ public void setScale(Tuple2D t) {
+ this.m00 = t.getX();
+ this.m11 = t.getY();
+ }
+
+ /** Concatenates this transform with a scaling transformation.
+ *
+ * This function is equivalent to:
+ *
+ * this = this * [ sx 0 0 ]
+ * [ 0 sy 0 ]
+ * [ 0 0 1 ]
+ *
+ *
+ * @param sx
+ * @param sy
+ */
+ public void scale(float sx, float sy) {
+ this.m00 *= sx;
+ this.m11 *= sy;
+ this.m01 *= sy;
+ this.m10 *= sx;
+ }
+
+ /** Concatenates this transform with a scaling transformation.
+ *
+ * This function is equivalent to:
+ *
+ * this = this * [ t.x 0 0 ]
+ * [ 0 t.y 0 ]
+ * [ 0 0 1 ]
+ *
+ *
+ * @param t
+ */
+ public void scale(Tuple2D t) {
+ scale(t.getX(), t.getY());
+ }
+
+ /** Replies the X scaling.
+ *
+ *
+ * [ sx 0 0 ]
+ * [ 0 sy 0 ]
+ * [ 0 0 1 ]
+ *
+ *
+ * @return the amount
+ */
+ public float getScaleX() {
+ return this.m00;
+ }
+
+ /** Replies the Y scaling.
+ *
+ *
+ * [ sx 0 0 ]
+ * [ 0 sy 0 ]
+ * [ 0 0 1 ]
+ *
+ *
+ * @return the amount
+ */
+ public float getScaleY() {
+ return this.m11;
+ }
+
+ /** Replies the scaling vector.
+ *
+ *
+ * [ sx 0 0 ]
+ * [ 0 sy 0 ]
+ * [ 0 0 1 ]
+ *
+ *
+ * @return the amount
+ */
+ public Vector2f getScaleVector() {
+ return new Vector2f(this.m00, this.m11);
+ }
+
+ /** Set the shearing elements.
+ *
+ * This function changes only the elements of
+ * the matrix related to the shearing (m01,
+ * m10). The scaling and the translation are not changed.
+ *
+ * After a call to this function, the matrix will
+ * contains (? means any value):
+ *
+ * [ ? shx ? ]
+ * [ shy ? ? ]
+ * [ ? ? ? ]
+ *
+ *
+ * @param shx
+ * @param shy
+ * @see #makeShearMatrix(float, float)
+ */
+ public void setShear(float shx, float shy) {
+ this.m01 = shx;
+ this.m10 = shy;
+ }
+
+ /** Set the shearing elements.
+ *
+ * This function changes only the elements of
+ * the matrix related to the shearing (m01,
+ * m10). The scaling and the translation are not changed.
+ *
+ * After a call to this function, the matrix will
+ * contains (? means any value):
+ *
+ * [ ? t.x ? ]
+ * [ t.y ? ? ]
+ * [ ? ? ? ]
+ *
+ *
+ * @param t
+ * @see #makeShearMatrix(float, float)
+ */
+ public void setShear(Tuple2D t) {
+ this.m01 = t.getX();
+ this.m10 = t.getY();
+ }
+
+ /** Concatenates this transform with a shearing transformation.
+ *
+ * This function is equivalent to:
+ *
+ * this = this * [ 1 shx 0 ]
+ * [ shy 1 0 ]
+ * [ 0 0 1 ]
+ *
+ *
+ * @param shx
+ * @param shy
+ */
+ public void shear(float shx, float shy) {
+ float M0, M1;
+ M0 = this.m00;
+ M1 = this.m01;
+
+ this.m00 = M0 + M1 * shy;
+ this.m01 = M0 * shx + M1;
+
+ M0 = this.m10;
+ M1 = this.m11;
+ this.m10 = M0 + M1 * shy;
+ this.m11 = M0 * shx + M1;
+ }
+
+ /** Concatenates this transform with a shearing transformation.
+ *
+ * This function is equivalent to:
+ *
+ * this = this * [ 1 t.x 0 ]
+ * [ t.y 1 0 ]
+ * [ 0 0 1 ]
+ *
+ *
+ * @param t
+ */
+ public void shear(Tuple2D t) {
+ shear(t.getX(), t.getY());
+ }
+
+ /** Replies the X shearing.
+ *
+ *
+ * [ 0 shx 0 ]
+ * [ shy 0 0 ]
+ * [ 0 0 1 ]
+ *
+ *
+ * @return the amount
+ */
+ public float getShearX() {
+ return this.m01;
+ }
+
+ /** Replies the Y shearing.
+ *
+ *
+ * [ 0 shx 0 ]
+ * [ shy 0 0 ]
+ * [ 0 0 1 ]
+ *
+ *
+ * @return the amount
+ */
+ public float getShearY() {
+ return this.m10;
+ }
+
+ /** Replies the shearing vector.
+ *
+ *
+ * [ 0 shx 0 ]
+ * [ shy 0 0 ]
+ * [ 0 0 1 ]
+ *
+ *
+ * @return the amount
+ */
+ public Vector2f getShearVector() {
+ return new Vector2f(this.m01, this.m10);
+ }
+
+ /**
+ * Sets the value of this matrix to a counter clockwise rotation about the x
+ * axis, and no translation
+ *
+ * This function changes all the elements of
+ * the matrix, icluding the translation.
+ *
+ * After a call to this function, the matrix will
+ * contains (? means any value):
+ *
+ * [ cos(theta) -sin(theta) 0 ]
+ * [ sin(theta) cos(theta) 0 ]
+ * [ 0 0 1 ]
+ *
+ *
+ * @param angle
+ * the angle to rotate about the X axis in radians
+ * @see #setRotation(float)
+ */
+ public final void makeRotationMatrix(float angle) {
+ float sinAngle, cosAngle;
+
+ sinAngle = (float)Math.sin(angle);
+ cosAngle = (float)Math.cos(angle);
+
+ this.m00 = cosAngle;
+ this.m01 = -sinAngle;
+ this.m02 = 0f;
+
+ this.m10 = sinAngle;
+ this.m11 = cosAngle;
+ this.m12 = 0f;
+
+ this.m20 = 0f;
+ this.m21 = 0f;
+ this.m22 = 1f;
+ }
+
+ /**
+ * Sets the value of this matrix to the given translation, without rotation.
+ *
+ * This function changes all the elements of
+ * the matrix including the scaling and the shearing.
+ *
+ * After a call to this function, the matrix will
+ * contains (? means any value):
+ *
+ * [ 1 0 x ]
+ * [ 0 1 y ]
+ * [ 0 0 1 ]
+ *
+ *
+ * @param dx is the translation along X.
+ * @param dy is the translation along Y.
+ * @see #setTranslation(float, float)
+ * @see #setTranslation(Tuple2D)
+ */
+ public final void makeTranslationMatrix(float dx, float dy) {
+ this.m00 = 1f;
+ this.m01 = 0f;
+ this.m02 = dx;
+
+ this.m10 = 0f;
+ this.m11 = 1f;
+ this.m12 = dy;
+
+ this.m20 = 0f;
+ this.m21 = 0f;
+ this.m22 = 1f;
+ }
+
+ /**
+ * Sets the value of this matrix to the given scaling, without rotation.
+ *
+ * This function changes all the elements of
+ * the matrix, including the shearing and the
+ * translation.
+ *
+ * After a call to this function, the matrix will
+ * contains (? means any value):
+ *
+ * [ sx 0 0 ]
+ * [ 0 sy 0 ]
+ * [ 0 0 1 ]
+ *
+ *
+ * @param sx is the scaling along X.
+ * @param sy is the scaling along Y.
+ * @see #setScale(float, float)
+ * @see #setScale(Tuple2D)
+ */
+ public final void makeScaleMatrix(float sx, float sy) {
+ this.m00 = sx;
+ this.m01 = 0f;
+ this.m02 = 0f;
+
+ this.m10 = 0f;
+ this.m11 = sy;
+ this.m12 = 0f;
+
+ this.m20 = 0f;
+ this.m21 = 0f;
+ this.m22 = 1f;
+ }
+
+ /**
+ * Sets the value of this matrix to the given scaling, without rotation.
+ *
+ * This function changes all the elements of
+ * the matrix incuding the scaling and the
+ * translation.
+ *
+ * After a call to this function, the matrix will
+ * contains (? means any value):
+ *
+ * [ 1 shx 0 ]
+ * [ shy 1 0 ]
+ * [ 0 0 1 ]
+ *
+ *
+ * @param shx is the shearing along X.
+ * @param shy is the shearing along Y.
+ * @see #setShear(float, float)
+ * @see #setShear(Tuple2D)
+ */
+ public final void makeShearMatrix(float shx, float shy) {
+ this.m00 = 1f;
+ this.m01 = shx;
+ this.m02 = 0f;
+
+ this.m10 = shy;
+ this.m11 = 1f;
+ this.m12 = 0f;
+
+ this.m20 = 0f;
+ this.m21 = 0f;
+ this.m22 = 1f;
+ }
+
+ /**
+ * Multiply this matrix by the tuple t and place the result back into the
+ * tuple (t = this*t).
+ *
+ * @param t
+ * the tuple to be multiplied by this matrix and then replaced
+ */
+ public void transform(Tuple2D t) {
+ float x, y;
+ x = this.m00 * t.getX() + this.m01 * t.getY() + this.m02;
+ y = this.m10 * t.getX() + this.m11 * t.getY() + this.m12;
+ t.set(x, y);
+ }
+
+ /** Multiply this matrix by the tuple and return the result.
+ *
+ * This function is equivalent to:
+ *
+ * result = this * [ x ]
+ * [ y ]
+ * [ 1 ]
+ *
+ *
+ * @param x
+ * @param y
+ * @return the transformation result
+ */
+ public Point2D transform(float x, float y) {
+ float rx, ry;
+ rx = this.m00 * x + this.m01 * y + this.m02;
+ ry = this.m10 * x + this.m11 * y + this.m12;
+ return new Point2f(rx, ry);
+ }
+
+ /**
+ * Multiply this matrix by the tuple t and and place the result into the
+ * tuple "result".
+ *
+ * This function is equivalent to:
+ *
+ * result = this * [ t.x ]
+ * [ t.y ]
+ * [ 1 ]
+ *
+ *
+ * @param t
+ * the tuple to be multiplied by this matrix
+ * @param result
+ * the tuple into which the product is placed
+ */
+ public void transform(Tuple2D t, Tuple2D result) {
+ result.set(
+ this.m00 * t.getX() + this.m01 * t.getY() + this.m02,
+ this.m10 * t.getX() + this.m11 * t.getY() + this.m12);
+ }
+
+ /**
+ * Returns an Transform2D object representing the
+ * inverse transformation.
+ * The inverse transform Tx' of this transform Tx
+ * maps coordinates transformed by Tx back
+ * to their original coordinates.
+ * In other words, Tx'(Tx(p)) = p = Tx(Tx'(p)).
+ *
+ * If this transform maps all coordinates onto a point or a line
+ * then it will not have an inverse, since coordinates that do
+ * not lie on the destination point or line will not have an inverse
+ * mapping.
+ * The determinant method can be used to determine if this
+ * transform has no inverse, in which case an exception will be
+ * thrown if the createInverse method is called.
+ * @return a new Transform2D object representing the
+ * inverse transformation.
+ * @see #determinant()
+ * @throws SingularMatrixException if the matrix cannot be inverted.
+ */
+ public Transform2D createInverse() {
+ float det = this.m00 * this.m11 - this.m01 * this.m10;
+ if (Math.abs(det) <= Double.MIN_VALUE) {
+ throw new SingularMatrixException("Determinant is "+det); //$NON-NLS-1$
+ }
+ return new Transform2D(
+ this.m11 / det,
+ -this.m01 / det,
+ (this.m01 * this.m12 - this.m11 * this.m02) / det,
+ -this.m10 / det,
+ this.m00 / det,
+ (this.m10 * this.m02 - this.m00 * this.m12) / det);
+ }
+
+ /**
+ * Set the components of the transformation.
+ *
+ * @param m00
+ * the [0][0] element
+ * @param m01
+ * the [0][1] element
+ * @param m02
+ * the [0][2] element
+ * @param m10
+ * the [1][0] element
+ * @param m11
+ * the [1][1] element
+ * @param m12
+ * the [1][2] element
+ */
+ public void set(float m00, float m01, float m02, float m10, float m11, float m12) {
+ set(m00, m01, m02, m10, m11, m12, 0f, 0f, 1f);
+ }
+
+ /**
+ * Invert this transformation.
+ * The inverse transform Tx' of this transform Tx
+ * maps coordinates transformed by Tx back
+ * to their original coordinates.
+ * In other words, Tx'(Tx(p)) = p = Tx(Tx'(p)).
+ *
+ * If this transform maps all coordinates onto a point or a line
+ * then it will not have an inverse, since coordinates that do
+ * not lie on the destination point or line will not have an inverse
+ * mapping.
+ * The determinant method can be used to determine if this
+ * transform has no inverse, in which case an exception will be
+ * thrown if the createInverse method is called.
+ * @see #determinant()
+ * @throws SingularMatrixException if the matrix cannot be inverted.
+ */
+ @Override
+ public void invert() {
+ float det = this.m00 * this.m11 - this.m01 * this.m10;
+ if (Math.abs(det) <= Double.MIN_VALUE) {
+ throw new SingularMatrixException("Determinant is "+det); //$NON-NLS-1$
+ }
+ set(
+ this.m11 / det,
+ -this.m01 / det,
+ (this.m01 * this.m12 - this.m11 * this.m02) / det,
+ -this.m10 / det,
+ this.m00 / det,
+ (this.m10 * this.m02 - this.m00 * this.m12) / det);
+ }
+
+ /**
+ * Invert this transformation.
+ * The inverse transform Tx' of this transform Tx
+ * maps coordinates transformed by Tx back
+ * to their original coordinates.
+ * In other words, Tx'(Tx(p)) = p = Tx(Tx'(p)).
+ *
+ * If this transform maps all coordinates onto a point or a line
+ * then it will not have an inverse, since coordinates that do
+ * not lie on the destination point or line will not have an inverse
+ * mapping.
+ * The determinant method can be used to determine if this
+ * transform has no inverse, in which case an exception will be
+ * thrown if the createInverse method is called.
+ * @param m is the matrix to invert
+ * @see #determinant()
+ * @throws SingularMatrixException if the matrix cannot be inverted.
+ */
+ @Override
+ public void invert(Matrix3f m) {
+ float det = m.m00 * m.m11 - m.m01 * m.m10;
+ if (Math.abs(det) <= Double.MIN_VALUE) {
+ throw new SingularMatrixException("Determinant is "+det); //$NON-NLS-1$
+ }
+ set(
+ m.m11 / det,
+ -m.m01 / det,
+ (m.m01 * m.m12 - m.m11 * m.m02) / det,
+ -m.m10 / det,
+ m.m00 / det,
+ (m.m10 * m.m02 - m.m00 * m.m12) / det);
+ }
+
+}
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Tuple2f.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Tuple2f.java
new file mode 100644
index 000000000..18887d0ca
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Tuple2f.java
@@ -0,0 +1,608 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2012 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.continuous;
+
+import org.arakhne.afc.math.geometry2d.Tuple2D;
+
+/** 2D tuple with 2 floating-point numbers.
+ *
+ * @param is the implementation type of the tuple.
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class Tuple2f> implements Tuple2D {
+
+ private static final long serialVersionUID = 6447733811545555778L;
+
+ /** x coordinate.
+ */
+ protected float x;
+
+ /** y coordinate.
+ */
+ protected float y;
+
+ /**
+ */
+ public Tuple2f() {
+ this.x = this.y = 0;
+ }
+
+ /**
+ * @param tuple is the tuple to copy.
+ */
+ public Tuple2f(Tuple2D tuple) {
+ this.x = tuple.getX();
+ this.y = tuple.getY();
+ }
+
+ /**
+ * @param tuple is the tuple to copy.
+ */
+ public Tuple2f(int[] tuple) {
+ this.x = tuple[0];
+ this.y = tuple[1];
+ }
+
+ /**
+ * @param tuple is the tuple to copy.
+ */
+ public Tuple2f(float[] tuple) {
+ this.x = tuple[0];
+ this.y = tuple[1];
+ }
+
+ /**
+ * @param x
+ * @param y
+ */
+ public Tuple2f(int x, int y) {
+ this.x = x;
+ this.y = y;
+ }
+
+ /**
+ * @param x
+ * @param y
+ */
+ public Tuple2f(float x, float y) {
+ this.x = x;
+ this.y = y;
+ }
+
+ /** {@inheritDoc}
+ */
+ @SuppressWarnings("unchecked")
+ @Override
+ public T clone() {
+ try {
+ return (T)super.clone();
+ }
+ catch(CloneNotSupportedException e) {
+ throw new Error(e);
+ }
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void absolute() {
+ this.x = Math.abs(this.x);
+ this.y = Math.abs(this.y);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void absolute(T t) {
+ t.set(Math.abs(this.x), Math.abs(this.y));
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void add(int x, int y) {
+ this.x += x;
+ this.y += y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void add(float x, float y) {
+ this.x += x;
+ this.y += y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void addX(int x) {
+ this.x += x;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void addX(float x) {
+ this.x += x;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void addY(int y) {
+ this.y += y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void addY(float y) {
+ this.y += y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void clamp(int min, int max) {
+ if (this.x < min) this.x = min;
+ else if (this.x > max) this.x = max;
+ if (this.y < min) this.y = min;
+ else if (this.y > max) this.y = max;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void clamp(float min, float max) {
+ clamp(min, max);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void clampMin(int min) {
+ if (this.x < min) this.x = min;
+ if (this.y < min) this.y = min;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void clampMin(float min) {
+ clampMin(min);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void clampMax(int max) {
+ if (this.x > max) this.x = max;
+ if (this.y > max) this.y = max;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void clampMax(float max) {
+ clampMax(max);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void clamp(int min, int max, T t) {
+ if (this.x < min) t.setX(min);
+ else if (this.x > max) t.setX(max);
+ if (this.y < min) t.setY(min);
+ else if (this.y > max) t.setY(max);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void clamp(float min, float max, T t) {
+ clamp(min, max, t);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void clampMin(int min, T t) {
+ if (this.x < min) t.setX(min);
+ if (this.y < min) t.setY(min);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void clampMin(float min, T t) {
+ clampMin(min, t);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void clampMax(int max, T t) {
+ if (this.x > max) t.setX(max);
+ if (this.y > max) t.setY(max);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void clampMax(float max, T t) {
+ clampMax(max, t);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void get(T t) {
+ t.set(this.x, this.y);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void get(int[] t) {
+ t[0] = (int)this.x;
+ t[1] = (int)this.y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void get(float[] t) {
+ t[0] = this.x;
+ t[1] = this.y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void negate(T t1) {
+ this.x = -t1.getX();
+ this.y = -t1.getY();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void negate() {
+ this.x = -this.x;
+ this.y = -this.y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void scale(int s, T t1) {
+ this.x = s * t1.getX();
+ this.y = s * t1.getY();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void scale(float s, T t1) {
+ this.x = (s * t1.getX());
+ this.y = (s * t1.getY());
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void scale(int s) {
+ this.x = s * this.x;
+ this.y = s * this.y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void scale(float s) {
+ this.x = (s * this.x);
+ this.y = (s * this.y);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void set(Tuple2D t1) {
+ this.x = t1.getX();
+ this.y = t1.getY();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void set(int x, int y) {
+ this.x = x;
+ this.y = y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void set(float x, float y) {
+ this.x = x;
+ this.y = y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void set(int[] t) {
+ this.x = t[0];
+ this.y = t[1];
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void set(float[] t) {
+ this.x = t[0];
+ this.y = t[1];
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float getX() {
+ return this.x;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public int x() {
+ return (int)this.x;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void setX(int x) {
+ this.x = x;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void setX(float x) {
+ this.x = x;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float getY() {
+ return this.y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public int y() {
+ return (int)this.y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void setY(int y) {
+ this.y = y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void setY(float y) {
+ this.y = y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void sub(int x, int y) {
+ this.x -= x;
+ this.y -= y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void subX(int x) {
+ this.x -= x;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void subY(int y) {
+ this.y -= y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void sub(float x, float y) {
+ this.x -= x;
+ this.y -= y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void subX(float x) {
+ this.x -= x;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void subY(float y) {
+ this.y -= y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void interpolate(T t1, T t2, float alpha) {
+ this.x = ((1f-alpha)*t1.getX() + alpha*t2.getX());
+ this.y = ((1f-alpha)*t1.getY() + alpha*t2.getY());
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void interpolate(T t1, float alpha) {
+ this.x = ((1f-alpha)*this.x + alpha*t1.getX());
+ this.y = ((1f-alpha)*this.y + alpha*t1.getY());
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean equals(Tuple2D t1) {
+ try {
+ return(this.x == t1.getX() && this.y == t1.getY());
+ }
+ catch (NullPointerException e2) {
+ return false;
+ }
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @SuppressWarnings("unchecked")
+ @Override
+ public boolean equals(Object t1) {
+ try {
+ T t2 = (T) t1;
+ return(this.x == t2.getX() && this.y == t2.getY());
+ }
+ catch(AssertionError e) {
+ throw e;
+ }
+ catch (Throwable e2) {
+ return false;
+ }
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean epsilonEquals(T t1, float epsilon) {
+ float diff;
+
+ diff = this.x - t1.getX();
+ if(Float.isNaN(diff)) return false;
+ if((diff<0?-diff:diff) > epsilon) return false;
+
+ diff = this.y - t1.getY();
+ if(Float.isNaN(diff)) return false;
+ if((diff<0?-diff:diff) > epsilon) return false;
+
+ return true;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public int hashCode() {
+ int bits = 1;
+ bits = 31 * bits + Float.floatToIntBits(this.x);
+ bits = 31 * bits + Float.floatToIntBits(this.y);
+ return bits ^ (bits >> 32);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public String toString() {
+ return "(" //$NON-NLS-1$
+ +this.x
+ +";" //$NON-NLS-1$
+ +this.y
+ +")"; //$NON-NLS-1$
+ }
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Tuple2fComparator.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Tuple2fComparator.java
new file mode 100644
index 000000000..b8fe94fda
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Tuple2fComparator.java
@@ -0,0 +1,54 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2012 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.continuous;
+
+import java.util.Comparator;
+
+/**
+ * Comparator of Tuple2f.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class Tuple2fComparator implements Comparator> {
+
+ /**
+ */
+ public Tuple2fComparator() {
+ //
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public int compare(Tuple2f o1, Tuple2f o2) {
+ if (o1==o2) return 0;
+ if (o1==null) return Integer.MIN_VALUE;
+ if (o2==null) return Integer.MAX_VALUE;
+ int cmp = Float.compare(o1.getX(), o2.getX());
+ if (cmp!=0) return cmp;
+ return Float.compare(o1.getY(), o2.getY());
+ }
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/UnmodifiablePoint2f.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/UnmodifiablePoint2f.java
new file mode 100644
index 000000000..d7cb0ca7f
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/UnmodifiablePoint2f.java
@@ -0,0 +1,104 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2012 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.continuous;
+
+import org.arakhne.afc.math.geometry2d.Tuple2D;
+
+/** This class implements a Point2f that cannot be modified by
+ * the setters.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class UnmodifiablePoint2f extends Point2f {
+
+ private static final long serialVersionUID = -8670105082548919880L;
+
+ /**
+ */
+ public UnmodifiablePoint2f() {
+ super();
+ }
+
+ /**
+ * @param x
+ * @param y
+ */
+ public UnmodifiablePoint2f(float x, float y) {
+ super(x, y);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public UnmodifiablePoint2f clone() {
+ return (UnmodifiablePoint2f)super.clone();
+ }
+
+ @Override
+ public void set(float x, float y) {
+ //
+ }
+
+ @Override
+ public void set(float[] t) {
+ //
+ }
+
+ @Override
+ public void set(int x, int y) {
+ //
+ }
+
+ @Override
+ public void set(int[] t) {
+ //
+ }
+
+ @Override
+ public void set(Tuple2D t1) {
+ //
+ }
+
+ @Override
+ public void setX(float x) {
+ //
+ }
+
+ @Override
+ public void setX(int x) {
+ //
+ }
+
+ @Override
+ public void setY(float y) {
+ //
+ }
+
+ @Override
+ public void setY(int y) {
+ //
+ }
+
+}
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Vector2f.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Vector2f.java
new file mode 100644
index 000000000..f3d3adb7c
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Vector2f.java
@@ -0,0 +1,307 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2012 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.continuous;
+
+import org.arakhne.afc.math.MathUtil;
+import org.arakhne.afc.math.Matrix2f;
+import org.arakhne.afc.math.geometry.GeometryUtil;
+import org.arakhne.afc.math.geometry2d.Point2D;
+import org.arakhne.afc.math.geometry2d.Tuple2D;
+import org.arakhne.afc.math.geometry2d.Vector2D;
+
+/** 2D Vector with 2 floating-point values.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class Vector2f extends Tuple2f implements Vector2D {
+
+ private static final long serialVersionUID = -2062941509400877679L;
+
+ /**
+ */
+ public Vector2f() {
+ //
+ }
+
+ /**
+ * @param tuple is the tuple to copy.
+ */
+ public Vector2f(Tuple2D tuple) {
+ super(tuple);
+ }
+
+ /**
+ * @param tuple is the tuple to copy.
+ */
+ public Vector2f(int[] tuple) {
+ super(tuple);
+ }
+
+ /**
+ * @param tuple is the tuple to copy.
+ */
+ public Vector2f(float[] tuple) {
+ super(tuple);
+ }
+
+ /**
+ * @param x
+ * @param y
+ */
+ public Vector2f(int x, int y) {
+ super(x,y);
+ }
+
+ /**
+ * @param x
+ * @param y
+ */
+ public Vector2f(float x, float y) {
+ super(x,y);
+ }
+
+ /**
+ * @param x
+ * @param y
+ */
+ public Vector2f(double x, double y) {
+ super((float)x,(float)y);
+ }
+
+ /**
+ * @param x
+ * @param y
+ */
+ public Vector2f(long x, long y) {
+ super(x,y);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public Vector2f clone() {
+ return (Vector2f)super.clone();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float angle(Vector2D v1) {
+ double vDot = dot(v1) / ( length()*v1.length() );
+ if( vDot < -1.) vDot = -1.;
+ if( vDot > 1.) vDot = 1.;
+ return((float) (Math.acos( vDot )));
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float dot(Vector2D v1) {
+ return (this.x*v1.getX() + this.y*v1.getY());
+ }
+
+ /**
+ * Multiply this vector, transposed, by the given matrix and replies the resulting vector.
+ *
+ * @param m
+ * @return transpose(this * m)
+ */
+ public final Vector2f mul(Matrix2f m) {
+ Vector2f r = new Vector2f();
+ r.x = this.getX() * m.m00 + this.getY() * m.m01;
+ r.y = this.getX() * m.m10 + this.getY() * m.m11;
+ return r;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void perpendicularize() {
+ // Based on the cross product in 3D of (vx,vy,0)x(0,0,1), right-handed
+ //set(getY(), -getX());
+ // Based on the cross product in 3D of (vx,vy,0)x(0,0,1), left-handed
+ set(-getY(), getX());
+ }
+
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float length() {
+ return (float) Math.sqrt(this.x*this.x + this.y*this.y);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float lengthSquared() {
+ return (this.x*this.x + this.y*this.y);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void normalize(Vector2D v1) {
+ float norm = 1f / v1.length();
+ this.x = (int)(v1.getX()*norm);
+ this.y = (int)(v1.getY()*norm);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void normalize() {
+ float norm;
+ norm = (float)(1./Math.sqrt(this.x*this.x + this.y*this.y));
+ this.x *= norm;
+ this.y *= norm;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float signedAngle(Vector2D v) {
+ return GeometryUtil.signedAngle(getX(), getY(), v.getX(), v.getY());
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void turnVector(float angle) {
+ float sin = (float)Math.sin(angle);
+ float cos = (float)Math.cos(angle);
+ float x = cos * getX() + sin * getY();
+ float y = -sin * getX() + cos * getY();
+ set(x,y);
+ }
+
+ /** Replies the orientation vector, which is corresponding
+ * to the given angle on a trigonometric circle.
+ *
+ * @param angle is the angle in radians to translate.
+ * @return the orientation vector which is corresponding to the given angle.
+ */
+ public static Vector2f toOrientationVector(float angle) {
+ return new Vector2f(
+ (float)Math.cos(angle),
+ (float)Math.sin(angle));
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public float getOrientationAngle() {
+ float angle = (float)Math.acos(getX());
+ if (getY()<0f) angle = -angle;
+ return MathUtil.clampRadian(angle);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void add(Vector2D t1, Vector2D t2) {
+ this.x = t1.getX() + t2.getX();
+ this.y = t1.getY() + t2.getY();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void add(Vector2D t1) {
+ this.x += t1.getX();
+ this.y += t1.getY();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void scaleAdd(int s, Vector2D t1, Vector2D t2) {
+ this.x = s * t1.getX() + t2.getX();
+ this.y = s * t1.getY() + t2.getY();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void scaleAdd(float s, Vector2D t1, Vector2D t2) {
+ this.x = s * t1.getX() + t2.getX();
+ this.y = s * t1.getY() + t2.getY();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void scaleAdd(int s, Vector2D t1) {
+ this.x = s * this.x + t1.getX();
+ this.y = s * this.y + t1.getY();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void scaleAdd(float s, Vector2D t1) {
+ this.x = s * this.x + t1.getX();
+ this.y = s * this.y + t1.getY();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void sub(Vector2D t1, Vector2D t2) {
+ this.x = t1.getX() - t2.getX();
+ this.y = t1.getY() - t2.getY();
+ }
+
+ @Override
+ public void sub(Point2D t1, Point2D t2) {
+ this.x = t1.getX() - t2.getX();
+ this.y = t1.getY() - t2.getY();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void sub(Vector2D t1) {
+ this.x -= t1.getX();
+ this.y -= t1.getY();
+ }
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/AbstractRectangularShape2i.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/AbstractRectangularShape2i.java
new file mode 100644
index 000000000..53267dc27
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/AbstractRectangularShape2i.java
@@ -0,0 +1,297 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.discrete;
+
+import org.arakhne.afc.math.geometry2d.Point2D;
+
+
+
+/** 2D rectangular shape with integer points.
+ *
+ * @param is the type of the shape implemented by the instance of this class.
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public abstract class AbstractRectangularShape2i extends AbstractShape2i {
+
+ private static final long serialVersionUID = -2716430612894131964L;
+
+ /** Lowest x-coordinate covered by the rectangular shape. */
+ protected int minx = 0;
+ /** Lowest y-coordinate covered by the rectangular shape. */
+ protected int miny = 0;
+ /** Highest x-coordinate covered by the rectangular shape. */
+ protected int maxx = 0;
+ /** Highest x-coordinate covered by the rectangular shape. */
+ protected int maxy = 0;
+
+ /**
+ */
+ public AbstractRectangularShape2i() {
+ //
+ }
+
+ @Override
+ public void clear() {
+ this.minx = this.miny = this.maxx = this.maxy = 0;
+ }
+
+ /** Replies if this rectangle is empty or not.
+ * A rectangle is empty when the two corners
+ * of the rectangle are at the same location.
+ *
+ * @return true if the two corners are
+ * at the same location; false otherwise.
+ */
+ @Override
+ public boolean isEmpty() {
+ return this.minx==this.maxx && this.miny==this.maxy;
+ }
+
+ /** Set the coordinates of this shape from the bounds of the given shape.
+ *
+ * @param shape
+ */
+ public void set(AbstractRectangularShape2i shape) {
+ setFromCorners(shape.getMinX(), shape.getMinY(), shape.getMaxX(), shape.getMaxY());
+ }
+
+ /** Change the frame of the rectangle.
+ *
+ * @param x
+ * @param y
+ * @param width
+ * @param height
+ */
+ public void set(int x, int y, int width, int height) {
+ setFromCorners(x, y, x+width, y+height);
+ }
+
+ /** Change the frame of te rectangle.
+ *
+ * @param min is the min corner of the rectangle.
+ * @param max is the max corner of the rectangle.
+ */
+ public void set(Point2D min, Point2D max) {
+ setFromCorners(min.x(), min.y(), max.x(), max.y());
+ }
+
+ /** Change the frame of the rectangle.
+ *
+ * @param x1 is the coordinate of the first corner.
+ * @param y1 is the coordinate of the first corner.
+ * @param x2 is the coordinate of the second corner.
+ * @param y2 is the coordinate of the second corner.
+ */
+ public void setFromCorners(int x1, int y1, int x2, int y2) {
+ if (x1Shape
+ * based on the specified center point coordinates and corner point
+ * coordinates. The framing rectangle is used by the subclasses of
+ * RectangularShape to define their geometry.
+ *
+ * @param centerX the X coordinate of the specified center point
+ * @param centerY the Y coordinate of the specified center point
+ * @param cornerX the X coordinate of the specified corner point
+ * @param cornerY the Y coordinate of the specified corner point
+ */
+ public void setFromCenter(int centerX, int centerY, int cornerX, int cornerY) {
+ int dx = centerX - cornerX;
+ int dy = centerY - cornerY;
+ setFromCorners(cornerX, cornerY, centerX + dx, centerY + dy);
+ }
+
+ /** Replies the min X.
+ *
+ * @return the min x.
+ */
+ public int getMinX() {
+ return this.minx;
+ }
+
+ /** Replies the max x.
+ *
+ * @return the max x.
+ */
+ public int getMaxX() {
+ return this.maxx;
+ }
+
+ /** Replies the min y.
+ *
+ * @return the min y.
+ */
+ public int getMinY() {
+ return this.miny;
+ }
+
+ /** Replies the max y.
+ *
+ * @return the max y.
+ */
+ public int getMaxY() {
+ return this.maxy;
+ }
+
+ /** Replies the width.
+ *
+ * @return the width.
+ */
+ public int getWidth() {
+ return this.maxx - this.minx;
+ }
+
+ /** Replies the height.
+ *
+ * @return the height.
+ */
+ public int getHeight() {
+ return this.maxy - this.miny;
+ }
+
+ /** Set the min X.
+ *
+ * @param x the min x.
+ */
+ public void setMinX(int x) {
+ int o = this.maxx;
+ if (ox) {
+ this.maxx = o;
+ this.minx = x;
+ }
+ else {
+ this.maxx = x;
+ }
+ }
+
+ /** Set the max Y.
+ *
+ * @param y the max y.
+ */
+ public void setMaxY(int y) {
+ int o = this.miny;
+ if (o>y) {
+ this.maxy = o;
+ this.miny = y;
+ }
+ else {
+ this.maxy = y;
+ }
+ }
+
+ /** Change the width of the rectangle, not the min corner.
+ *
+ * @param width
+ */
+ public void setWidth(int width) {
+ this.maxx = this.minx + Math.max(0, width);
+ }
+
+ /** Change the height of the rectangle, not the min corner.
+ *
+ * @param height
+ */
+ public void setHeight(int height) {
+ this.maxy = this.miny + Math.max(0, height);
+ }
+
+ @Override
+ public void translate(int dx, int dy) {
+ this.minx += dx;
+ this.miny += dy;
+ this.maxx += dx;
+ this.maxy += dy;
+ }
+
+ /** Inflate this rectangle with the given amounts.
+ *
+ * @param left
+ * @param top
+ * @param right
+ * @param bottom
+ */
+ public void inflate(int left, int top, int right, int bottom) {
+ this.minx -= left;
+ this.miny -= top;
+ this.maxx += right;
+ this.maxy += bottom;
+ }
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/AbstractShape2i.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/AbstractShape2i.java
new file mode 100644
index 000000000..add9ccf0a
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/AbstractShape2i.java
@@ -0,0 +1,85 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.discrete;
+
+import org.arakhne.afc.math.geometry2d.Point2D;
+import org.arakhne.afc.math.geometry2d.continuous.Transform2D;
+
+/** 2D shape with integer points.
+ *
+ * @param is the type of the shape implemented by the instance of this class.
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public abstract class AbstractShape2i implements Shape2i {
+
+ private static final long serialVersionUID = -3663448743772835647L;
+
+ /**
+ */
+ public AbstractShape2i() {
+ //
+ }
+
+ /** {@inheritDoc}
+ */
+ @SuppressWarnings("unchecked")
+ @Override
+ public T clone() {
+ try {
+ return (T)super.clone();
+ }
+ catch (CloneNotSupportedException e) {
+ throw new Error(e);
+ }
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public Shape2i createTransformedShape(Transform2D transform) {
+ return new Path2i(getPathIterator(transform));
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public final boolean contains(Point2D p) {
+ return contains(p.x(), p.y());
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public final float distance(Point2D p) {
+ return (float)Math.sqrt(distanceSquared(p));
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public final PathIterator2i getPathIterator() {
+ return getPathIterator(null);
+ }
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Circle2i.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Circle2i.java
new file mode 100644
index 000000000..0295c14c1
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Circle2i.java
@@ -0,0 +1,966 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.discrete;
+
+import java.util.Iterator;
+import java.util.NoSuchElementException;
+import java.util.Set;
+import java.util.TreeSet;
+
+import org.arakhne.afc.math.geometry.PathWindingRule;
+import org.arakhne.afc.math.geometry2d.Point2D;
+import org.arakhne.afc.math.geometry2d.continuous.Transform2D;
+
+
+
+/** 2D circle with integer coordinates.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class Circle2i extends AbstractShape2i {
+
+ private static final long serialVersionUID = -2396327310912728347L;
+
+ /**
+ * ArcIterator.btan(Math.PI/2)
+ */
+ static final float CTRL_VAL = 0.5522847498307933f;
+
+ /**
+ * ctrlpts contains the control points for a set of 4 cubic
+ * bezier curves that approximate a circle of radius 0.5
+ * centered at 0.5, 0.5
+ */
+ static final float PCV = 0.5f + CTRL_VAL * 0.5f;
+ /**
+ * ctrlpts contains the control points for a set of 4 cubic
+ * bezier curves that approximate a circle of radius 0.5
+ * centered at 0.5, 0.5
+ */
+ static final float NCV = 0.5f - CTRL_VAL * 0.5f;
+ /**
+ * ctrlpts contains the control points for a set of 4 cubic
+ * bezier curves that approximate a circle of radius 0.5
+ * centered at 0.5, 0.5
+ */
+ static float CTRL_PTS[][] = {
+ { 1.0f, PCV, PCV, 1.0f, 0.5f, 1.0f },
+ { NCV, 1.0f, 0.0f, PCV, 0.0f, 0.5f },
+ { 0.0f, NCV, NCV, 0.0f, 0.5f, 0.0f },
+ { PCV, 0.0f, 1.0f, NCV, 1.0f, 0.5f }
+ };
+
+ /** Replies if the given point is inside the circle.
+ *
+ * @param cx is the x-coordinate of the circle center
+ * @param cy is the y-coordinate of the circle center
+ * @param cr is the radius of the circle center
+ * @param x is the x-coordinate of the point
+ * @param y is the y-coordinate of the point
+ * @return true if the point is inside the circle.
+ */
+ public static boolean contains(int cx, int cy, int cr, int x, int y) {
+ int vx = x - cx;
+ int vy = y - cy;
+
+ if (vx>=-cr && vx<=cr
+ && vy>=-cr && vy<=cr) {
+ int octant;
+ boolean xpos = (vx>=0);
+ boolean ypos = (vy>=0);
+ if (xpos) {
+ if (ypos) {
+ octant = 0;
+ }
+ else {
+ octant = 2;
+ }
+ }
+ else {
+ if (ypos) {
+ octant = 6;
+ }
+ else {
+ octant = 4;
+ }
+ }
+
+ int px, py, ccw, cpx, cpy;
+ boolean allNull = true;
+ Point2i p;
+ CirclePerimeterIterator iterator = new CirclePerimeterIterator(
+ cx, cy, cr, octant, octant+2, false);
+
+ while (iterator.hasNext()) {
+ p = iterator.next();
+
+ // Trivial case
+ if (p.x()==x && p.y()==y) return true;
+
+ px = cy - p.y();
+ py = p.x() - cx;
+ cpx = x - p.x();
+ cpy = y - p.y();
+ ccw = cpx * py - cpy * px;
+
+ if (ccw>0) return false;
+ if (ccw<0) allNull = false;
+ }
+
+ return !allNull;
+ }
+
+ return false;
+ }
+
+ /** Replies if the given point is inside the quadrant of the given circle.
+ *
+ * @param cx is the x-coordinate of the circle center
+ * @param cy is the y-coordinate of the circle center
+ * @param cr is the radius of the circle center
+ * @param quadrant is the quadrant:
+ *
+ * | quadrant | x | y |
+ *
+ *
+ * | 0 | ≥cx | ≥cy |
+ * | 1 | ≥cx | <cy |
+ * | 2 | <cx | ≥cy |
+ * | 3 | <cx | <cy |
+ *
+ *
+ * @param x is the x-coordinate of the point
+ * @param y is the y-coordinate of the point
+ * @return true if the point is inside the circle.
+ */
+ public static boolean contains(int cx, int cy, int cr, int quadrant, int x, int y) {
+ int vx = x - cx;
+ int vy = y - cy;
+
+ if (vx>=-cr && vx<=cr
+ && vy>=-cr && vy<=cr) {
+ int octant;
+ boolean xpos = (vx>=0);
+ boolean ypos = (vy>=0);
+ if (xpos) {
+ if (ypos) {
+ octant = 0;
+ }
+ else {
+ octant = 2;
+ }
+ }
+ else {
+ if (ypos) {
+ octant = 6;
+ }
+ else {
+ octant = 4;
+ }
+ }
+
+ if (quadrant*2!=octant) return false;
+
+ int px, py, ccw, cpx, cpy;
+ Point2i p;
+ CirclePerimeterIterator iterator = new CirclePerimeterIterator(
+ cx, cy, cr, octant, octant+2, false);
+
+ while (iterator.hasNext()) {
+ p = iterator.next();
+ px = cy - p.y();
+ py = p.x() - cx;
+ cpx = x - p.x();
+ cpy = y - p.y();
+ ccw = cpx * py - cpy * px;
+
+ if (ccw>0) return false;
+ }
+
+ return true;
+ }
+
+ return false;
+ }
+
+ /** Replies the closest point in a circle to a point.
+ *
+ * @param cx is the center of the circle
+ * @param cy is the center of the circle
+ * @param cr is the radius of the circle
+ * @param x is the point
+ * @param y is the point
+ * @return the closest point in the circle to the point.
+ */
+ public static Point2i computeClosestPointTo(int cx, int cy, int cr, int x, int y) {
+ int vx = x - cx;
+ int vy = y - cy;
+
+ int octant;
+ boolean xpos = (vx>=0);
+ boolean ypos = (vy>=0);
+ if (xpos) {
+ if (ypos) {
+ octant = 0;
+ }
+ else {
+ octant = 2;
+ }
+ }
+ else {
+ if (ypos) {
+ octant = 6;
+ }
+ else {
+ octant = 4;
+ }
+ }
+
+ int d, px, py, cpx, cpy, ccw;
+ Point2i p;
+ CirclePerimeterIterator iterator = new CirclePerimeterIterator(
+ cx, cy, cr, octant, octant+2, false);
+
+ boolean isInside = true;
+ int minDist = Integer.MAX_VALUE;
+ Point2i close = new Point2i();
+
+ while (iterator.hasNext()) {
+ p = iterator.next();
+ px = cy - p.y();
+ py = p.x() - cx;
+ cpx = x - p.x();
+ cpy = y - p.y();
+ ccw = cpx * py - cpy * px;
+ if (ccw>=0) {
+ isInside = false;
+ d = cpx*cpx + cpy*cpy;
+ if (dtrue if the two shapes are intersecting; otherwise
+ * false
+ */
+ public static boolean intersectsCircleCircle(int x1, int y1, int radius1, int x2, int y2, int radius2) {
+ Point2i c = computeClosestPointTo(x1, y1, radius1, x2, y2);
+ return contains(x2, y2, radius2, c.x(), c.y());
+ }
+
+ /** Replies if a circle and a rectangle are intersecting.
+ *
+ * @param x1 is the center of the circle
+ * @param y1 is the center of the circle
+ * @param radius is the radius of the circle
+ * @param x2 is the first corner of the rectangle.
+ * @param y2 is the first corner of the rectangle.
+ * @param x3 is the second corner of the rectangle.
+ * @param y3 is the second corner of the rectangle.
+ * @return true if the two shapes are intersecting; otherwise
+ * false
+ */
+ public static boolean intersectsCircleRectangle(int x1, int y1, int radius, int x2, int y2, int x3, int y3) {
+ Point2i c = Rectangle2i.computeClosestPoint(x2, y2, x3, y3, x1, y1);
+ return contains(x1, y1, radius, c.x(), c.y());
+ }
+
+ /** Replies if a circle and a segment are intersecting.
+ *
+ * @param x1 is the center of the circle
+ * @param y1 is the center of the circle
+ * @param radius is the radius of the circle
+ * @param x2 is the first point of the segment.
+ * @param y2 is the first point of the segment.
+ * @param x3 is the second point of the segment.
+ * @param y3 is the second point of the segment.
+ * @return true if the two shapes are intersecting; otherwise
+ * false
+ */
+ public static boolean intersectsCircleSegment(int x1, int y1, int radius, int x2, int y2, int x3, int y3) {
+ Point2i p = Segment2i.computeClosestPointTo(x2, y2, x3, y3, x1, y1);
+ return contains(x1, y1, radius, p.x(), p.y());
+ }
+
+ /** X-coordinate of the center of the circle. */
+ protected int cx = 0;
+ /** Y-coordinate of the center of the circle. */
+ protected int cy = 0;
+ /** Radius of the circle. */
+ protected int radius = 0;
+
+ /**
+ */
+ public Circle2i() {
+ //
+ }
+
+ /**
+ * @param center
+ * @param radius
+ */
+ public Circle2i(Point2D center, int radius) {
+ set(center, radius);
+ }
+
+ /**
+ * @param x
+ * @param y
+ * @param radius
+ */
+ public Circle2i(int x, int y, int radius) {
+ set(x, y, radius);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public void clear() {
+ this.cx = this.cy = 0;
+ this.radius = 0;
+ }
+
+ /** Replies if this circle is empty.
+ * The circle is empty when the radius is nul.
+ *
+ * @return true if the radius is nul;
+ * otherwise false.
+ */
+ @Override
+ public boolean isEmpty() {
+ return this.radius<=0;
+ }
+
+ /** Change the frame of the circle.
+ *
+ * @param x
+ * @param y
+ * @param radius
+ */
+ public void set(int x, int y, int radius) {
+ this.cx = x;
+ this.cy = y;
+ this.radius = Math.abs(radius);
+ }
+
+ /** Change the frame of te circle.
+ *
+ * @param center
+ * @param radius
+ */
+ public void set(Point2D center, int radius) {
+ this.cx = center.x();
+ this.cy = center.y();
+ this.radius = Math.abs(radius);
+ }
+
+ /** Replies the center X.
+ *
+ * @return the center x.
+ */
+ public int getX() {
+ return this.cx;
+ }
+
+ /** Replies the center y.
+ *
+ * @return the center y.
+ */
+ public int getY() {
+ return this.cy;
+ }
+
+ /** Replies the center.
+ *
+ * @return a copy of the center.
+ */
+ public Point2i getCenter() {
+ return new Point2i(this.cx, this.cy);
+ }
+
+ /** Replies the radius.
+ *
+ * @return the radius.
+ */
+ public int getRadius() {
+ return this.radius;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public Rectangle2i toBoundingBox() {
+ Rectangle2i r = new Rectangle2i();
+ r.setFromCorners(
+ this.cx-this.radius,
+ this.cy-this.radius,
+ this.cx+this.radius,
+ this.cy+this.radius);
+ return r;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public float distanceSquared(Point2D p) {
+ Point2i c = getClosestPointTo(p);
+ return c.distanceSquared(p);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public float distanceL1(Point2D p) {
+ Point2i c = getClosestPointTo(p);
+ return c.distanceL1(p);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public float distanceLinf(Point2D p) {
+ Point2i c = getClosestPointTo(p);
+ return c.distanceLinf(p);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public Point2i getClosestPointTo(Point2D p) {
+ return computeClosestPointTo(this.cx, this.cy, this.radius, p.x(), p.y());
+ }
+
+ @Override
+ public boolean intersects(Rectangle2i s) {
+ return intersectsCircleRectangle(
+ getX(), getY(), getRadius(),
+ s.getMinX(), s.getMinY(),
+ s.getMaxX(), s.getMaxY());
+ }
+
+ @Override
+ public boolean intersects(Circle2i s) {
+ return intersectsCircleCircle(
+ getX(), getY(), getRadius(),
+ s.getX(), s.getY(), s.getRadius());
+ }
+
+ @Override
+ public boolean intersects(Segment2i s) {
+ return intersectsCircleSegment(
+ getX(), getY(), getRadius(),
+ s.getX1(), s.getY1(),
+ s.getX2(), s.getY2());
+ }
+
+ @Override
+ public Shape2i createTransformedShape(Transform2D transform) {
+ if (transform==null || transform.isIdentity()) return clone();
+ return new Path2i(getPathIterator(transform));
+ }
+
+ @Override
+ public void translate(int dx, int dy) {
+ this.cx += dx;
+ this.cy += dy;
+ }
+
+ @Override
+ public boolean contains(int x, int y) {
+ return contains(this.cx, this.cy, this.radius, x, y);
+ }
+
+ private static void m(int[] quadrants, int k, int x, int y) {
+ if (x>0) {
+ if (y>0) {
+ quadrants[0] |= k;
+ }
+ else {
+ quadrants[1] |= k;
+ }
+ }
+ else {
+ if (y>0) {
+ quadrants[3] |= k;
+ }
+ else {
+ quadrants[2] |= k;
+ }
+ }
+ }
+
+ @Override
+ public boolean contains(Rectangle2i r) {
+ int vx1 = r.getMinX() - this.cx;
+ int vy1 = r.getMinY() - this.cy;
+ int vx2 = r.getMaxX() - this.cx;
+ int vy2 = r.getMaxY() - this.cy;
+
+ if (vx1>=-this.radius && vx1<=this.radius && vy1>=-this.radius && vy1<=this.radius &&
+ vx2>=-this.radius && vx2<=this.radius && vy2>=-this.radius && vy2<=this.radius) {
+ int[] quadrants = new int[4];
+ int[] x = new int[] {vx1, vx2, vx2, vx1};
+ int[] y = new int[] {vy1, vy1, vy2, vy2};
+ for(int i=0; i<4; ++i) {
+ m(quadrants, (1<0) return false;
+ }
+ }
+ }
+ }
+ }
+
+ return true;
+ }
+
+ return false;
+ }
+
+ /** Replies the points of the circle perimeters starting by the first octant.
+ *
+ * @return the points on the perimeters.
+ */
+ @Override
+ public Iterator getPointIterator() {
+ return new CirclePerimeterIterator(this.cx, this.cy, this.radius, 0, 8, true);
+ }
+
+ /** Replies the points of the circle perimeters starting by the first octant.
+ *
+ * @param firstOctantIndex is the index of the first octant (see figure) to treat.
+ * @param nbOctants is the number of octants to traverse (greater than zero).
+ * @return the points on the perimeters.
+ */
+ public Iterator getPointIterator(int firstOctantIndex, int nbOctants) {
+ return getPointIterator(this.cx, this.cy, this.radius, firstOctantIndex, nbOctants);
+ }
+
+ /** Replies the points of the circle perimeters starting by the first octant.
+ *
+ * @param cx is the center of the radius.
+ * @param cy is the center of the radius.
+ * @param radius is the radius of the radius.
+ * @param firstOctantIndex is the index of the first octant (see figure) to treat.
+ * @param nbOctants is the number of octants to traverse (greater than zero).
+ * @return the points on the perimeters.
+ */
+ public static Iterator getPointIterator(int cx, int cy, int radius, int firstOctantIndex, int nbOctants) {
+ int startOctant, maxOctant;
+ if (firstOctantIndex<=0)
+ startOctant = 0;
+ else if (firstOctantIndex>=8)
+ startOctant = 7;
+ else
+ startOctant = firstOctantIndex;
+ maxOctant = startOctant + nbOctants;
+ if (maxOctant>8) maxOctant = 8;
+ return new CirclePerimeterIterator(
+ cx, cy, radius,
+ startOctant, maxOctant, true);
+ }
+
+ @Override
+ public PathIterator2i getPathIterator(Transform2D transform) {
+ if (transform==null || transform.isIdentity())
+ return new CopyPathIterator(this.cx, this.cy, this.radius);
+ return new TransformPathIterator(this.cx, this.cy, this.radius, transform);
+ }
+
+ /** Iterates on points on the perimeter of a circle.
+ *
+ * The rastrerization is based on a Bresenham algorithm.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private static class CirclePerimeterIterator implements Iterator {
+
+ private final int cx;
+ private final int cy;
+ private final int cr;
+
+ private final boolean skip;
+ private final int maxOctant;
+
+ private int currentOctant;
+ private int x, y, d;
+
+ private Point2i next = null;
+
+ private final Set junctionPoint = new TreeSet(new Tuple2iComparator());
+
+ /**
+ * @param x
+ * @param y
+ * @param r
+ * @param initialOctant
+ * @param maxOctant
+ * @param skip
+ */
+ public CirclePerimeterIterator(int x, int y, int r, int initialOctant, int maxOctant, boolean skip) {
+ assert(r>=0);
+ this.cx = x;
+ this.cy = y;
+ this.cr = r;
+ this.skip = skip;
+ this.maxOctant = maxOctant;
+ this.currentOctant = initialOctant;
+ reset();
+ searchNext();
+ }
+
+ private void reset() {
+ this.x = 0;
+ this.y = this.cr;
+ this.d = 3 - 2 * this.cr;
+ if (this.skip && (this.currentOctant==3 || this.currentOctant==4 || this.currentOctant==6 || this.currentOctant==7)) {
+ // skip the first point because already replied in previous octant
+ if (this.d<=0) {
+ this.d += 4 * this.x + 6;
+ }
+ else {
+ this.d += 4 * (this.x - this.y) + 10;
+ --this.y;
+ }
+ ++this.x;
+ }
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean hasNext() {
+ return this.next!=null;
+ }
+
+ private void searchNext() {
+ if (this.currentOctant>=this.maxOctant) {
+ this.next = null;
+ }
+ else {
+ this.next = new Point2i();
+ while (true) {
+ switch(this.currentOctant) {
+ case 0:
+ this.next.set(this.cx + this.x, this.cy + this.y);
+ break;
+ case 1:
+ this.next.set(this.cx + this.y, this.cy + this.x);
+ break;
+ case 2:
+ this.next.set(this.cx + this.x, this.cy - this.y);
+ break;
+ case 3:
+ this.next.set(this.cx + this.y, this.cy - this.x);
+ break;
+ case 4:
+ this.next.set(this.cx - this.x, this.cy - this.y);
+ break;
+ case 5:
+ this.next.set(this.cx - this.y, this.cy - this.x);
+ break;
+ case 6:
+ this.next.set(this.cx - this.x, this.cy + this.y);
+ break;
+ case 7:
+ this.next.set(this.cx - this.y, this.cy + this.x);
+ break;
+ default:
+ throw new NoSuchElementException();
+ }
+
+ if (this.d<=0) {
+ this.d += 4 * this.x + 6;
+ }
+ else {
+ this.d += 4 * (this.x - this.y) + 10;
+ --this.y;
+ }
+ ++this.x;
+
+ if (this.x>this.y) {
+ // The octant is finished.
+ // Save the junction.
+ boolean cont = this.junctionPoint.contains(this.next);
+ if (!cont) this.junctionPoint.add(new Point2i(this.next));
+ // Goto next.
+ ++this.currentOctant;
+ reset();
+ if (this.currentOctant>=this.maxOctant) {
+ if (cont) this.next = null;
+ cont = false;
+ }
+ if (!cont) return;
+ }
+ else {
+ return;
+ }
+ }
+ }
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public Point2i next() {
+ Point2i pixel = this.next;
+ if (pixel==null) throw new NoSuchElementException();
+ searchNext();
+ return pixel;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ } // class CirclePerimeterIterator
+
+ /** Iterator on the path elements of the circle.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private static class CopyPathIterator implements PathIterator2i {
+
+ private final int x;
+ private final int y;
+ private final int r;
+ private int index = 0;
+ private int movex, movey;
+ private int lastx, lasty;
+
+ /**
+ * @param x
+ * @param y
+ * @param r
+ */
+ public CopyPathIterator(int x, int y, int r) {
+ this.r = Math.max(0, r);
+ this.x = x - this.r;
+ this.y = y - this.r;
+ if (this.r<=0f) {
+ this.index = 6;
+ }
+ }
+
+ @Override
+ public boolean hasNext() {
+ return this.index<=5;
+ }
+
+ @Override
+ public PathElement2i next() {
+ if (this.index>5) throw new NoSuchElementException();
+ int idx = this.index;
+ ++this.index;
+ if (idx==0) {
+ int dr = 2 * this.r;
+ float ctrls[] = CTRL_PTS[3];
+ this.movex = (int)(this.x + ctrls[4] * dr);
+ this.movey = (int)(this.y + ctrls[5] * dr);
+ this.lastx = this.movex;
+ this.lasty = this.movey;
+ return new PathElement2i.MovePathElement2i(
+ this.lastx, this.lasty);
+ }
+ else if (idx<5) {
+ int dr = 2 * this.r;
+ float ctrls[] = CTRL_PTS[idx - 1];
+ int ppx = this.lastx;
+ int ppy = this.lasty;
+ this.lastx = (int)(this.x + ctrls[4] * dr);
+ this.lasty = (int)(this.y + ctrls[5] * dr);
+ return new PathElement2i.CurvePathElement2i(
+ ppx, ppy,
+ (int)(this.x + ctrls[0] * dr),
+ (int)(this.y + ctrls[1] * dr),
+ (int)(this.x + ctrls[2] * dr),
+ (int)(this.y + ctrls[3] * dr),
+ this.lastx, this.lasty);
+ }
+ int ppx = this.lastx;
+ int ppy = this.lasty;
+ this.lastx = this.movex;
+ this.lasty = this.movey;
+ return new PathElement2i.ClosePathElement2i(
+ ppx, ppy,
+ this.lastx, this.lasty);
+ }
+
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public PathWindingRule getWindingRule() {
+ return PathWindingRule.NON_ZERO;
+ }
+
+ }
+
+ /** Iterator on the path elements of the circle.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private static class TransformPathIterator implements PathIterator2i {
+
+ private final Point2D p1 = new Point2i();
+ private final Point2D p2 = new Point2i();
+ private final Point2D ptmp1 = new Point2i();
+ private final Point2D ptmp2 = new Point2i();
+ private final Transform2D transform;
+ private final int x;
+ private final int y;
+ private final int r;
+ private int index = 0;
+ private int movex, movey;
+
+ /**
+ * @param x
+ * @param y
+ * @param r
+ * @param transform
+ */
+ public TransformPathIterator(int x, int y, int r, Transform2D transform) {
+ assert(transform!=null);
+ this.transform = transform;
+ this.r = Math.max(0, r);
+ this.x = x - this.r;
+ this.y = y - this.r;
+ if (this.r<=0f) {
+ this.index = 6;
+ }
+ }
+
+ @Override
+ public boolean hasNext() {
+ return this.index<=5;
+ }
+
+ @Override
+ public PathElement2i next() {
+ if (this.index>5) throw new NoSuchElementException();
+ int idx = this.index;
+ ++this.index;
+ if (idx==0) {
+ int dr = 2 * this.r;
+ float ctrls[] = CTRL_PTS[3];
+ this.movex = (int)(this.x + ctrls[4] * dr);
+ this.movey = (int)(this.y + ctrls[5] * dr);
+ this.p2.set(this.movex, this.movey);
+ this.transform.transform(this.p2);
+ return new PathElement2i.MovePathElement2i(
+ this.p2.x(), this.p2.y());
+ }
+ else if (idx<5) {
+ int dr = 2 * this.r;
+ float ctrls[] = CTRL_PTS[idx - 1];
+ this.p1.set(this.p2);
+ this.p2.set(
+ (this.x + ctrls[4] * dr),
+ (this.y + ctrls[5] * dr));
+ this.transform.transform(this.p2);
+ this.ptmp1.set(
+ (this.x + ctrls[0] * dr),
+ (this.y + ctrls[1] * dr));
+ this.transform.transform(this.ptmp1);
+ this.ptmp2.set(
+ (this.x + ctrls[2] * dr),
+ (this.y + ctrls[3] * dr));
+ this.transform.transform(this.ptmp2);
+ return new PathElement2i.CurvePathElement2i(
+ this.p1.x(), this.p1.y(),
+ this.ptmp1.x(), this.ptmp1.y(),
+ this.ptmp2.x(), this.ptmp2.y(),
+ this.p2.x(), this.p2.y());
+ }
+ this.p1.set(this.p2);
+ this.p2.set(this.movex, this.movey);
+ this.transform.transform(this.p2);
+ return new PathElement2i.ClosePathElement2i(
+ this.p1.x(), this.p1.y(),
+ this.p2.x(), this.p2.y());
+ }
+
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public PathWindingRule getWindingRule() {
+ return PathWindingRule.NON_ZERO;
+ }
+
+ }
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Path2i.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Path2i.java
new file mode 100644
index 000000000..07d8fe192
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Path2i.java
@@ -0,0 +1,2568 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2005-09 Stephane GALLAND.
+ * Copyright (C) 2012-13 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.discrete;
+
+import java.lang.ref.SoftReference;
+import java.util.Arrays;
+import java.util.Collection;
+import java.util.Iterator;
+import java.util.NoSuchElementException;
+
+import org.arakhne.afc.math.MathConstants;
+import org.arakhne.afc.math.geometry.GeometryUtil;
+import org.arakhne.afc.math.geometry.PathElementType;
+import org.arakhne.afc.math.geometry.PathWindingRule;
+import org.arakhne.afc.math.geometry2d.Point2D;
+import org.arakhne.afc.math.geometry2d.continuous.Transform2D;
+
+
+/** A generic path with integer coordinates.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class Path2i extends AbstractShape2i {
+
+ private static final long serialVersionUID = -4229773257722403127L;
+
+ /** Multiple of cubic & quad curve size.
+ */
+ static final int GROW_SIZE = 24;
+
+ /**
+ * Calculates the number of times the given path
+ * crosses the given segment extending to the right.
+ *
+ * @param pi is the description of the path.
+ * @param x1 is the first point of the segment.
+ * @param y1 is the first point of the segment.
+ * @param x2 is the first point of the segment.
+ * @param y2 is the first point of the segment.
+ * @return the crossing or {@link MathConstants#SHAPE_INTERSECTS}.
+ */
+ public static int computeCrossingsFromSegment(PathIterator2i pi, int x1, int y1, int x2, int y2) {
+ return computeCrossingsFromSegment(0, pi, x1, y1, x2, y2, true);
+ }
+
+ /**
+ * Calculates the number of times the given path
+ * crosses the given circle extending to the right.
+ *
+ * @param crossings is the initial value for crossing.
+ * @param pi is the description of the path.
+ * @param x1 is the first point of the segment.
+ * @param y1 is the first point of the segment.
+ * @param x2 is the first point of the segment.
+ * @param y2 is the first point of the segment.
+ * @param closeable indicates if the shape is automatically closed or not.
+ * @return the crossing
+ */
+ static int computeCrossingsFromSegment(int crossings, PathIterator2i pi, int x1, int y1, int x2, int y2, boolean closeable) {
+ // Copied from the AWT API
+ if (!pi.hasNext()) return 0;
+ PathElement2i element;
+
+ element = pi.next();
+ if (element.type != PathElementType.MOVE_TO) {
+ throw new IllegalArgumentException("missing initial moveto in path definition"); //$NON-NLS-1$
+ }
+
+ int movx = element.toX;
+ int movy = element.toY;
+ int curx = movx;
+ int cury = movy;
+ int endx, endy;
+ int numCrosses = crossings;
+ while (numCrosses!=MathConstants.SHAPE_INTERSECTS && pi.hasNext()) {
+ element = pi.next();
+ switch (element.type) {
+ case MOVE_TO:
+ movx = curx = element.toX;
+ movy = cury = element.toY;
+ break;
+ case LINE_TO:
+ endx = element.toX;
+ endy = element.toY;
+ numCrosses = Segment2i.computeCrossingsFromSegment(
+ numCrosses,
+ x1, y1, x2, y2,
+ curx, cury,
+ endx, endy);
+ curx = endx;
+ cury = endy;
+ break;
+ case QUAD_TO:
+ {
+ endx = element.toX;
+ endy = element.toY;
+ Path2i localPath = new Path2i();
+ localPath.moveTo(element.fromX, element.fromY);
+ localPath.quadTo(
+ element.ctrlX1, element.ctrlY1,
+ endx, endy);
+ numCrosses = computeCrossingsFromSegment(
+ numCrosses,
+ localPath.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ x1, y1, x2, y2,
+ false);
+ curx = endx;
+ cury = endy;
+ break;
+ }
+ case CURVE_TO:
+ endx = element.toX;
+ endy = element.toY;
+ Path2i localPath = new Path2i();
+ localPath.moveTo(element.fromX, element.fromY);
+ localPath.curveTo(
+ element.ctrlX1, element.ctrlY1,
+ element.ctrlX2, element.ctrlY2,
+ endx, endy);
+ numCrosses = computeCrossingsFromSegment(
+ numCrosses,
+ localPath.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ x1, y1, x2, y2,
+ false);
+ curx = endx;
+ cury = endy;
+ break;
+ case CLOSE:
+ if (cury != movy || curx != movx) {
+ numCrosses = Segment2i.computeCrossingsFromSegment(
+ numCrosses,
+ x1, y1, x2, y2,
+ curx, cury,
+ movx, movy);
+ }
+ curx = movx;
+ cury = movy;
+ break;
+ default:
+ }
+ }
+
+ if (numCrosses!=MathConstants.SHAPE_INTERSECTS && closeable && cury != movy) {
+ numCrosses = Segment2i.computeCrossingsFromSegment(
+ numCrosses,
+ x1, y1, x2, y2,
+ curx, cury,
+ movx, movy);
+ }
+
+ return numCrosses;
+ }
+
+ /**
+ * Calculates the number of times the given path
+ * crosses the given ellipse extending to the right.
+ *
+ * @param pi is the description of the path.
+ * @param cx is the center of the circle.
+ * @param cy is the center of the circle.
+ * @param radius is the radius of the circle.
+ * @return the crossing or {@link MathConstants#SHAPE_INTERSECTS}.
+ */
+ public static int computeCrossingsFromCircle(PathIterator2i pi, int cx, int cy, int radius) {
+ return computeCrossingsFromCircle(0, pi, cx, cy, radius, true);
+ }
+
+ /**
+ * Calculates the number of times the given path
+ * crosses the given circle extending to the right.
+ *
+ * @param crossings is the initial value for crossing.
+ * @param pi is the description of the path.
+ * @param cx is the center of the circle.
+ * @param cy is the center of the circle.
+ * @param radius is the radius of the circle.
+ * @param closeable indicates if the shape is automatically closed or not.
+ * @return the crossing
+ */
+ static int computeCrossingsFromCircle(int crossings, PathIterator2i pi, int cx, int cy, int radius, boolean closeable) {
+ // Copied from the AWT API
+ if (!pi.hasNext()) return 0;
+ PathElement2i element;
+
+ element = pi.next();
+ if (element.type != PathElementType.MOVE_TO) {
+ throw new IllegalArgumentException("missing initial moveto in path definition"); //$NON-NLS-1$
+ }
+
+ int movx = element.toX;
+ int movy = element.toY;
+ int curx = movx;
+ int cury = movy;
+ int endx, endy;
+ int numCrosses = crossings;
+ while (numCrosses!=MathConstants.SHAPE_INTERSECTS && pi.hasNext()) {
+ element = pi.next();
+ switch (element.type) {
+ case MOVE_TO:
+ movx = curx = element.toX;
+ movy = cury = element.toY;
+ break;
+ case LINE_TO:
+ endx = element.toX;
+ endy = element.toY;
+ numCrosses = Segment2i.computeCrossingsFromCircle(
+ numCrosses,
+ cx, cy, radius,
+ curx, cury,
+ endx, endy);
+ curx = endx;
+ cury = endy;
+ break;
+ case QUAD_TO:
+ {
+ endx = element.toX;
+ endy = element.toY;
+ Path2i localPath = new Path2i();
+ localPath.moveTo(element.fromX, element.fromY);
+ localPath.quadTo(
+ element.ctrlX1, element.ctrlY1,
+ endx, endy);
+ numCrosses = computeCrossingsFromCircle(
+ numCrosses,
+ localPath.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ cx, cy, radius,
+ false);
+ curx = endx;
+ cury = endy;
+ break;
+ }
+ case CURVE_TO:
+ endx = element.toX;
+ endy = element.toY;
+ Path2i localPath = new Path2i();
+ localPath.moveTo(element.fromX, element.fromY);
+ localPath.curveTo(
+ element.ctrlX1, element.ctrlY1,
+ element.ctrlX2, element.ctrlY2,
+ endx, endy);
+ numCrosses = computeCrossingsFromCircle(
+ numCrosses,
+ localPath.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ cx, cy, radius,
+ false);
+ curx = endx;
+ cury = endy;
+ break;
+ case CLOSE:
+ if (cury != movy || curx != movx) {
+ numCrosses = Segment2i.computeCrossingsFromCircle(
+ numCrosses,
+ cx, cy, radius,
+ curx, cury,
+ movx, movy);
+ }
+ curx = movx;
+ cury = movy;
+ break;
+ default:
+ }
+ }
+
+ if (numCrosses!=MathConstants.SHAPE_INTERSECTS && closeable && cury != movy) {
+ numCrosses = Segment2i.computeCrossingsFromCircle(
+ numCrosses,
+ cx, cy, radius,
+ curx, cury,
+ movx, movy);
+ }
+
+ return numCrosses;
+ }
+
+ /**
+ * Calculates the number of times the given path
+ * crosses the ray extending to the right from (px,py).
+ * If the point lies on a part of the path,
+ * then no crossings are counted for that intersection.
+ * +1 is added for each crossing where the Y coordinate is increasing
+ * -1 is added for each crossing where the Y coordinate is decreasing
+ * The return value is the sum of all crossings for every segment in
+ * the path.
+ * The path must start with a MOVE_TO, otherwise an exception is
+ * thrown.
+ *
+ * @param pi is the description of the path.
+ * @param px is the reference point to test.
+ * @param py is the reference point to test.
+ * @return the crossing
+ */
+ public static int computeCrossingsFromPoint(PathIterator2i pi, int px, int py) {
+ return computeCrossingsFromPoint(pi, px, py, true);
+ }
+
+ /**
+ * Calculates the number of times the given path
+ * crosses the ray extending to the right from (px,py).
+ * If the point lies on a part of the path,
+ * then no crossings are counted for that intersection.
+ * +1 is added for each crossing where the Y coordinate is increasing
+ * -1 is added for each crossing where the Y coordinate is decreasing
+ * The return value is the sum of all crossings for every segment in
+ * the path.
+ * The path must start with a MOVE_TO, otherwise an exception is
+ * thrown.
+ *
+ * @param pi is the description of the path.
+ * @param px is the reference point to test.
+ * @param py is the reference point to test.
+ * @param autoClose indicates if the shape is automatically assumed as closed.
+ * @return the crossing, or {@link MathConstants#SHAPE_INTERSECTS}
+ */
+ static int computeCrossingsFromPoint(PathIterator2i pi, int px, int py, boolean autoClose) {
+ // Copied and adapted from the AWT API
+ if (!pi.hasNext()) return 0;
+ PathElement2i element;
+
+ element = pi.next();
+ if (element.type != PathElementType.MOVE_TO) {
+ throw new IllegalArgumentException("missing initial moveto in path definition"); //$NON-NLS-1$
+ }
+
+ int movx = element.toX;
+ int movy = element.toY;
+ int curx = movx;
+ int cury = movy;
+ int endx, endy;
+ int crossings = 0;
+
+ while (pi.hasNext()) {
+ element = pi.next();
+ switch (element.type) {
+ case MOVE_TO:
+ movx = curx = element.toX;
+ movy = cury = element.toY;
+ break;
+ case LINE_TO:
+ endx = element.toX;
+ endy = element.toY;
+ crossings = Segment2i.computeCrossingsFromPoint(
+ crossings,
+ px, py,
+ curx, cury,
+ endx, endy);
+ if (crossings==MathConstants.SHAPE_INTERSECTS) {
+ return crossings;
+ }
+ curx = endx;
+ cury = endy;
+ break;
+ case QUAD_TO:
+ endx = element.toX;
+ endy = element.toY;
+ Path2i curve = new Path2i();
+ curve.moveTo(element.fromX, element.fromY);
+ curve.quadTo(element.ctrlX1, element.ctrlY1, endx, endy);
+ int numCrosses = computeCrossingsFromPoint(
+ curve.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ px, py, false);
+ if (numCrosses==MathConstants.SHAPE_INTERSECTS) {
+ return numCrosses;
+ }
+ crossings += numCrosses;
+ curx = endx;
+ cury = endy;
+ break;
+ case CURVE_TO:
+ endx = element.toX;
+ endy = element.toY;
+ curve = new Path2i();
+ curve.moveTo(element.fromX, element.fromY);
+ curve.curveTo(
+ element.ctrlX1, element.ctrlY1,
+ element.ctrlX2, element.ctrlY2,
+ endx, endy);
+ numCrosses = computeCrossingsFromPoint(
+ curve.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ px, py, false);
+ if (numCrosses==MathConstants.SHAPE_INTERSECTS) {
+ return numCrosses;
+ }
+ crossings += numCrosses;
+ curx = endx;
+ cury = endy;
+ break;
+ case CLOSE:
+ if (cury != movy || curx != movx) {
+ crossings = Segment2i.computeCrossingsFromPoint(
+ crossings,
+ px, py,
+ curx, cury,
+ movx, movy);
+ if (crossings==MathConstants.SHAPE_INTERSECTS) {
+ return crossings;
+ }
+ }
+ curx = movx;
+ cury = movy;
+ break;
+ default:
+ }
+ }
+
+ if (autoClose && cury != movy && curx != movx) {
+ crossings = Segment2i.computeCrossingsFromPoint(
+ crossings,
+ px, py,
+ curx, cury,
+ movx, movy);
+ }
+
+ return crossings;
+ }
+
+ /**
+ * Tests if the specified coordinates are inside the closed
+ * boundary of the specified {@link PathIterator2i}.
+ *
+ * This method provides a basic facility for implementors of
+ * the {@link Shape2i} interface to implement support for the
+ * {@link Shape2i#contains(int, int)} method.
+ *
+ * @param pi the specified {@code PathIterator2f}
+ * @param x the specified X coordinate
+ * @param y the specified Y coordinate
+ * @return {@code true} if the specified coordinates are inside the
+ * specified {@code PathIterator2f}; {@code false} otherwise
+ */
+ public static boolean contains(PathIterator2i pi, int x, int y) {
+ // Copied from the AWT API
+ int mask = (pi.getWindingRule() == PathWindingRule.NON_ZERO ? -1 : 1);
+ int cross = computeCrossingsFromPoint(pi, x, y);
+ return ((cross & mask) != 0);
+ }
+
+ /**
+ * Accumulate the number of times the path crosses the shadow
+ * extending to the right of the rectangle. See the comment
+ * for the SHAPE_INTERSECTS constant for more complete details.
+ * The return value is the sum of all crossings for both the
+ * top and bottom of the shadow for every segment in the path,
+ * or the special value SHAPE_INTERSECTS if the path ever enters
+ * the interior of the rectangle.
+ * The path must start with a SEG_MOVETO, otherwise an exception is
+ * thrown.
+ * The caller must check r[xy]{min,max} for NaN values.
+ *
+ * @param pi is the iterator on the path elements.
+ * @param rxmin is the first corner of the rectangle.
+ * @param rymin is the first corner of the rectangle.
+ * @param rxmax is the second corner of the rectangle.
+ * @param rymax is the second corner of the rectangle.
+ * @return the crossings.
+ */
+ public static int computeCrossingsFromRect(PathIterator2i pi,
+ int rxmin, int rymin,
+ int rxmax, int rymax) {
+ return __computeCrossingsFromRect(pi, rxmin, rymin, rxmax, rymax, true, true);
+ }
+
+ private static int crossingHelper1(
+ int crossings,
+ int rxmin, int rymin,
+ int rxmax, int rymax,
+ int curx, int cury,
+ int movx, int movy,
+ boolean intersectingBehavior) {
+ int crosses = Segment2i.computeCrossingsFromRect(crossings,
+ rxmin, rymin,
+ rxmax, rymax,
+ curx, cury,
+ movx, movy);
+ if (!intersectingBehavior && crosses==MathConstants.SHAPE_INTERSECTS) {
+ int x1 = rxmin+1;
+ int x2 = rxmax-1;
+ int y1 = rymin+1;
+ int y2 = rymax-1;
+ crosses = Segment2i.computeCrossingsFromRect(crossings,
+ x1, y1,
+ x2, y2,
+ curx, cury,
+ movx, movy);
+ }
+ return crosses;
+ }
+
+ /**
+ * Accumulate the number of times the path crosses the shadow
+ * extending to the right of the rectangle. See the comment
+ * for the SHAPE_INTERSECTS constant for more complete details.
+ * The return value is the sum of all crossings for both the
+ * top and bottom of the shadow for every segment in the path,
+ * or the special value SHAPE_INTERSECTS if the path ever enters
+ * the interior of the rectangle.
+ * The path must start with a SEG_MOVETO, otherwise an exception is
+ * thrown.
+ * The caller must check r[xy]{min,max} for NaN values.
+ *
+ * @param pi is the iterator on the path elements.
+ * @param rxmin is the first corner of the rectangle.
+ * @param rymin is the first corner of the rectangle.
+ * @param rxmax is the second corner of the rectangle.
+ * @param rymax is the second corner of the rectangle.
+ * @param autoClose indicates if the line from the last point to the last move
+ * point must be include in the crossing computation.
+ * @param intersectingBehavior indicates the function is called to determine if the rectangle
+ * is inside the shape or not. This function determines
+ * {@link MathConstants#SHAPE_INTERSECTS} in a different way if the
+ * function is used for containing or intersecting tests.
+ * @return the crossings count or {@link MathConstants#SHAPE_INTERSECTS}.
+ */
+ static int __computeCrossingsFromRect(PathIterator2i pi,
+ int rxmin, int rymin,
+ int rxmax, int rymax,
+ boolean autoClose,
+ boolean intersectingBehavior) {
+ // Copied from AWT API
+ if (rxmax <= rxmin || rymax <= rymin) return 0;
+ if (!pi.hasNext()) return 0;
+
+ PathElement2i pathElement = pi.next();
+
+ if (pathElement.type != PathElementType.MOVE_TO) {
+ throw new IllegalArgumentException("missing initial moveto in path definition"); //$NON-NLS-1$
+ }
+
+ int curx, cury, movx, movy, endx, endy;
+ curx = movx = pathElement.toX;
+ cury = movy = pathElement.toY;
+ int crossings = 0;
+
+ while (crossings != MathConstants.SHAPE_INTERSECTS
+ && pi.hasNext()) {
+ pathElement = pi.next();
+ switch (pathElement.type) {
+ case MOVE_TO:
+ // Count should always be a multiple of 2 here.
+ // assert((crossings & 1) != 0);
+ movx = curx = pathElement.toX;
+ movy = cury = pathElement.toY;
+ break;
+ case LINE_TO:
+ endx = pathElement.toX;
+ endy = pathElement.toY;
+ crossings = crossingHelper1(crossings,
+ rxmin, rymin, rxmax, rymax,
+ curx, cury, endx, endy,
+ intersectingBehavior);
+ if (crossings==MathConstants.SHAPE_INTERSECTS) {
+ return MathConstants.SHAPE_INTERSECTS;
+ }
+ curx = endx;
+ cury = endy;
+ break;
+ case QUAD_TO:
+ endx = pathElement.toX;
+ endy = pathElement.toY;
+ Path2i curve = new Path2i();
+ curve.moveTo(pathElement.fromX, pathElement.fromY);
+ curve.quadTo(pathElement.ctrlX1, pathElement.ctrlY1, endx, endy);
+ int numCrosses = __computeCrossingsFromRect(
+ curve.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ rxmin, rymin, rxmax, rymax,
+ false, intersectingBehavior);
+ if (numCrosses==MathConstants.SHAPE_INTERSECTS) {
+ return MathConstants.SHAPE_INTERSECTS;
+ }
+ crossings += numCrosses;
+ curx = endx;
+ cury = endy;
+ break;
+ case CURVE_TO:
+ endx = pathElement.toX;
+ endy = pathElement.toY;
+ curve = new Path2i();
+ curve.moveTo(pathElement.fromX, pathElement.fromY);
+ curve.curveTo(pathElement.ctrlX1, pathElement.ctrlY1, pathElement.ctrlX2, pathElement.ctrlY2, endx, endy);
+ numCrosses = __computeCrossingsFromRect(
+ curve.getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO),
+ rxmin, rymin, rxmax, rymax,
+ false, intersectingBehavior);
+ if (numCrosses==MathConstants.SHAPE_INTERSECTS) {
+ return MathConstants.SHAPE_INTERSECTS;
+ }
+ crossings += numCrosses;
+ curx = endx;
+ cury = endy;
+ break;
+ case CLOSE:
+ if (curx != movx || cury != movy) {
+ crossings = crossingHelper1(crossings,
+ rxmin, rymin, rxmax, rymax,
+ curx, cury, movx, movy,
+ intersectingBehavior);
+ if (crossings==MathConstants.SHAPE_INTERSECTS) {
+ return crossings;
+ }
+ }
+ curx = movx;
+ cury = movy;
+ // Count should always be a multiple of 2 here.
+ // assert((crossings & 1) != 0);
+ break;
+ default:
+ }
+ }
+
+ if (autoClose && crossings != MathConstants.SHAPE_INTERSECTS && (curx != movx || cury != movy)) {
+ crossings = crossingHelper1(crossings,
+ rxmin, rymin, rxmax, rymax,
+ curx, cury, movx, movy,
+ intersectingBehavior);
+ }
+
+ // Count should always be a multiple of 2 here.
+ // assert((crossings & 1) != 0);
+ return crossings;
+ }
+
+ /**
+ * Tests if the specified rectangle is inside the closed
+ * boundary of the specified {@link PathIterator2i}.
+ *
+ * This method provides a basic facility for implementors of
+ * the {@link Shape2i} interface to implement support for the
+ * {@link Shape2i#contains(Rectangle2i)} method.
+ *
+ * @param pi the specified {@code PathIterator2f}
+ * @param rx the lowest corner of the rectangle.
+ * @param ry the lowest corner of the rectangle.
+ * @param rwidth is the width of the rectangle.
+ * @param rheight is the width of the rectangle.
+ * @return {@code true} if the specified rectangle is inside the
+ * specified {@code PathIterator2f}; {@code false} otherwise.
+ */
+ public static boolean contains(PathIterator2i pi, int rx, int ry, int rwidth, int rheight) {
+ // Copied and adapted from AWT API
+ if (rwidth <= 0 || rheight <= 0) {
+ return false;
+ }
+ int mask = (pi.getWindingRule() == PathWindingRule.NON_ZERO ? -1 : 2);
+ int crossings = __computeCrossingsFromRect(pi, rx, ry, rx+rwidth, ry+rheight, true, false);
+ return (crossings != MathConstants.SHAPE_INTERSECTS &&
+ (crossings & mask) != 0);
+ }
+
+ /**
+ * Tests if the interior of the specified {@link PathIterator2i}
+ * intersects the interior of a specified set of rectangular
+ * coordinates.
+ *
+ * This method provides a basic facility for implementors of
+ * the {@link Shape2i} interface to implement support for the
+ * {@code intersects()} method.
+ *
+ * This method object may conservatively return true in
+ * cases where the specified rectangular area intersects a
+ * segment of the path, but that segment does not represent a
+ * boundary between the interior and exterior of the path.
+ * Such a case may occur if some set of segments of the
+ * path are retraced in the reverse direction such that the
+ * two sets of segments cancel each other out without any
+ * interior area between them.
+ * To determine whether segments represent true boundaries of
+ * the interior of the path would require extensive calculations
+ * involving all of the segments of the path and the winding
+ * rule and are thus beyond the scope of this implementation.
+ *
+ * @param pi the specified {@code PathIterator}
+ * @param x the specified X coordinate
+ * @param y the specified Y coordinate
+ * @param w the width of the specified rectangular coordinates
+ * @param h the height of the specified rectangular coordinates
+ * @return {@code true} if the specified {@code PathIterator} and
+ * the interior of the specified set of rectangular
+ * coordinates intersect each other; {@code false} otherwise.
+ */
+ public static boolean intersects(PathIterator2i pi, int x, int y, int w, int h) {
+ if (w <= 0f || h <= 0f) {
+ return false;
+ }
+ int mask = (pi.getWindingRule() == PathWindingRule.NON_ZERO ? -1 : 2);
+ int crossings = __computeCrossingsFromRect(pi, x, y, x+w, y+h, true, true);
+ return (crossings == MathConstants.SHAPE_INTERSECTS ||
+ (crossings & mask) != 0);
+ }
+
+ /** Array of types.
+ */
+ PathElementType[] types;
+
+ /** Array of coords.
+ */
+ int[] coords;
+
+ /** Number of types in the array.
+ */
+ int numTypes = 0;
+
+ /** Number of coords in the array.
+ */
+ int numCoords = 0;
+
+ /** Winding rule for the path.
+ */
+ PathWindingRule windingRule;
+
+ /** Indicates if the path is empty.
+ * The path is empty when there is no point inside, or
+ * all the points are at the same coordinate, or
+ * when the path does not represents a drawable path
+ * (a path with a line or a curve).
+ */
+ private Boolean isEmpty = Boolean.TRUE;
+
+ /** Buffer for the bounds of the path.
+ */
+ private SoftReference bounds = null;
+
+ /**
+ */
+ public Path2i() {
+ this(PathWindingRule.NON_ZERO);
+ }
+
+ /**
+ * @param iterator
+ */
+ public Path2i(Iterator iterator) {
+ this(PathWindingRule.NON_ZERO, iterator);
+ }
+
+ /**
+ * @param windingRule
+ */
+ public Path2i(PathWindingRule windingRule) {
+ assert(windingRule!=null);
+ this.types = new PathElementType[GROW_SIZE];
+ this.coords = new int[GROW_SIZE];
+ this.windingRule = windingRule;
+ }
+
+ /**
+ * @param windingRule
+ * @param iterator
+ */
+ public Path2i(PathWindingRule windingRule, Iterator iterator) {
+ assert(windingRule!=null);
+ this.types = new PathElementType[GROW_SIZE];
+ this.coords = new int[GROW_SIZE];
+ this.windingRule = windingRule;
+ add(iterator);
+ }
+
+ @Override
+ public void clear() {
+ this.types = new PathElementType[GROW_SIZE];
+ this.coords = new int[GROW_SIZE];
+ this.windingRule = PathWindingRule.NON_ZERO;
+ this.numCoords = 0;
+ this.numTypes = 0;
+ this.isEmpty = true;
+ this.bounds = null;
+ }
+
+ @Override
+ public String toString() {
+ StringBuilder b = new StringBuilder();
+ b.append("["); //$NON-NLS-1$
+ if (this.numCoords>0) {
+ b.append(this.coords[0]);
+ for(int i=1; i iterator) {
+ PathElement2i element;
+ while (iterator.hasNext()) {
+ element = iterator.next();
+ switch(element.type) {
+ case MOVE_TO:
+ moveTo(element.toX, element.toY);
+ break;
+ case LINE_TO:
+ lineTo(element.toX, element.toY);
+ break;
+ case QUAD_TO:
+ quadTo(element.ctrlX1, element.ctrlY1, element.toX, element.toY);
+ break;
+ case CURVE_TO:
+ curveTo(element.ctrlX1, element.ctrlY1, element.ctrlX2, element.ctrlY2, element.toX, element.toY);
+ break;
+ case CLOSE:
+ closePath();
+ break;
+ default:
+ }
+ }
+ }
+
+ private void ensureSlots(boolean needMove, int n) {
+ if (needMove && this.numTypes==0) {
+ throw new IllegalStateException("missing initial moveto in path definition"); //$NON-NLS-1$
+ }
+ if (this.types.length==this.numTypes) {
+ this.types = Arrays.copyOf(this.types, this.types.length+GROW_SIZE);
+ }
+ while ((this.numCoords+n)>=this.coords.length) {
+ this.coords = Arrays.copyOf(this.coords, this.coords.length+GROW_SIZE);
+ }
+ }
+
+ /**
+ * Adds a point to the path by moving to the specified
+ * coordinates specified in float precision.
+ *
+ * @param x the specified X coordinate
+ * @param y the specified Y coordinate
+ */
+ public void moveTo(int x, int y) {
+ if (this.numTypes>0 && this.types[this.numTypes-1]==PathElementType.MOVE_TO) {
+ this.coords[this.numCoords-2] = x;
+ this.coords[this.numCoords-1] = y;
+ }
+ else {
+ ensureSlots(false, 2);
+ this.types[this.numTypes++] = PathElementType.MOVE_TO;
+ this.coords[this.numCoords++] = x;
+ this.coords[this.numCoords++] = y;
+ }
+ this.bounds = null;
+ }
+
+ /**
+ * Adds a point to the path by drawing a straight line from the
+ * current coordinates to the new specified coordinates
+ * specified in float precision.
+ *
+ * @param x the specified X coordinate
+ * @param y the specified Y coordinate
+ */
+ public void lineTo(int x, int y) {
+ ensureSlots(true, 2);
+ this.types[this.numTypes++] = PathElementType.LINE_TO;
+ this.coords[this.numCoords++] = x;
+ this.coords[this.numCoords++] = y;
+ this.isEmpty = null;
+ this.bounds = null;
+ }
+
+ /**
+ * Adds a curved segment, defined by two new points, to the path by
+ * drawing a Quadratic curve that intersects both the current
+ * coordinates and the specified coordinates {@code (x2,y2)},
+ * using the specified point {@code (x1,y1)} as a quadratic
+ * parametric control point.
+ * All coordinates are specified in float precision.
+ *
+ * @param x1 the X coordinate of the quadratic control point
+ * @param y1 the Y coordinate of the quadratic control point
+ * @param x2 the X coordinate of the final end point
+ * @param y2 the Y coordinate of the final end point
+ */
+ public void quadTo(int x1, int y1, int x2, int y2) {
+ ensureSlots(true, 4);
+ this.types[this.numTypes++] = PathElementType.QUAD_TO;
+ this.coords[this.numCoords++] = x1;
+ this.coords[this.numCoords++] = y1;
+ this.coords[this.numCoords++] = x2;
+ this.coords[this.numCoords++] = y2;
+ this.isEmpty = null;
+ this.bounds = null;
+ }
+
+ /**
+ * Adds a curved segment, defined by three new points, to the path by
+ * drawing a Bézier curve that intersects both the current
+ * coordinates and the specified coordinates {@code (x3,y3)},
+ * using the specified points {@code (x1,y1)} and {@code (x2,y2)} as
+ * Bézier control points.
+ * All coordinates are specified in float precision.
+ *
+ * @param x1 the X coordinate of the first Bézier control point
+ * @param y1 the Y coordinate of the first Bézier control point
+ * @param x2 the X coordinate of the second Bézier control point
+ * @param y2 the Y coordinate of the second Bézier control point
+ * @param x3 the X coordinate of the final end point
+ * @param y3 the Y coordinate of the final end point
+ */
+ public void curveTo(int x1, int y1,
+ int x2, int y2,
+ int x3, int y3) {
+ ensureSlots(true, 6);
+ this.types[this.numTypes++] = PathElementType.CURVE_TO;
+ this.coords[this.numCoords++] = x1;
+ this.coords[this.numCoords++] = y1;
+ this.coords[this.numCoords++] = x2;
+ this.coords[this.numCoords++] = y2;
+ this.coords[this.numCoords++] = x3;
+ this.coords[this.numCoords++] = y3;
+ this.isEmpty = null;
+ this.bounds = null;
+ }
+
+ /**
+ * Closes the current subpath by drawing a straight line back to
+ * the coordinates of the last {@code moveTo}. If the path is already
+ * closed or if the previous coordinates are for a {@code moveTo}
+ * then this method has no effect.
+ */
+ public void closePath() {
+ if (this.numTypes<=0 ||
+ (this.types[this.numTypes-1]!=PathElementType.CLOSE
+ &&this.types[this.numTypes-1]!=PathElementType.MOVE_TO)) {
+ ensureSlots(true, 0);
+ this.types[this.numTypes++] = PathElementType.CLOSE;
+ }
+ }
+
+ /** Replies an iterator on the path elements.
+ *
+ * Only {@link PathElementType#MOVE_TO},
+ * {@link PathElementType#LINE_TO}, and
+ * {@link PathElementType#CLOSE} types are returned by the iterator.
+ *
+ * The amount of subdivision of the curved segments is controlled by the
+ * flatness parameter, which specifies the maximum distance that any point
+ * on the unflattened transformed curve can deviate from the returned
+ * flattened path segments. Note that a limit on the accuracy of the
+ * flattened path might be silently imposed, causing very small flattening
+ * parameters to be treated as larger values. This limit, if there is one,
+ * is defined by the particular implementation that is used.
+ *
+ * The iterator for this class is not multi-threaded safe.
+ *
+ * @param flatness is the maximum distance that the line segments used to approximate
+ * the curved segments are allowed to deviate from any point on the original curve.
+ * @return an iterator on the path elements.
+ */
+ public PathIterator2i getPathIterator(float flatness) {
+ return new FlatteningPathIterator(getWindingRule(), getPathIterator(null), flatness, 10);
+ }
+
+ /** Replies an iterator on the path elements.
+ *
+ * Only {@link PathElementType#MOVE_TO},
+ * {@link PathElementType#LINE_TO}, and
+ * {@link PathElementType#CLOSE} types are returned by the iterator.
+ *
+ * The amount of subdivision of the curved segments is controlled by the
+ * flatness parameter, which specifies the maximum distance that any point
+ * on the unflattened transformed curve can deviate from the returned
+ * flattened path segments. Note that a limit on the accuracy of the
+ * flattened path might be silently imposed, causing very small flattening
+ * parameters to be treated as larger values. This limit, if there is one,
+ * is defined by the particular implementation that is used.
+ *
+ * The iterator for this class is not multi-threaded safe.
+ *
+ * @param transform is an optional affine Transform2D to be applied to the
+ * coordinates as they are returned in the iteration, or null if
+ * untransformed coordinates are desired.
+ * @param flatness is the maximum distance that the line segments used to approximate
+ * the curved segments are allowed to deviate from any point on the original curve.
+ * @return an iterator on the path elements.
+ */
+ public PathIterator2i getPathIterator(Transform2D transform, float flatness) {
+ return new FlatteningPathIterator(getWindingRule(), getPathIterator(transform), flatness, 10);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public PathIterator2i getPathIterator(Transform2D transform) {
+ if (transform == null) {
+ return new CopyPathIterator();
+ }
+ return new TransformPathIterator(transform);
+ }
+
+ /** Transform the current path.
+ * This function changes the current path.
+ *
+ * @param transform is the affine transformation to apply.
+ * @see #createTransformedShape(Transform2D)
+ */
+ public void transform(Transform2D transform) {
+ if (transform!=null) {
+ Point2D p = new Point2i();
+ for(int i=0; ixmax) xmax = this.coords[i];
+ if (this.coords[i+1]>ymax) ymax = this.coords[i+1];
+ }
+ bb = new Rectangle2i();
+ bb.setFromCorners(xmin, ymin, xmax, ymax);
+ this.bounds = new SoftReference(bb);
+ }
+ return bb;
+ }
+
+ @Override
+ public Point2D getClosestPointTo(Point2D p) {
+ Point2D solution = new Point2i();
+ float bestDist = Float.POSITIVE_INFINITY;
+ Point2D candidate;
+ PathIterator2i pi = getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO);
+ PathElement2i pe;
+
+ Segment2i seg = new Segment2i();
+ int mask = (pi.getWindingRule() == PathWindingRule.NON_ZERO ? -1 : 1);
+ int crossings = 0;
+ boolean isClosed = false;
+ int moveX, moveY, currentX, currentY;
+ moveX = moveY = currentX = currentY = 0;
+
+ while (pi.hasNext()) {
+ pe = pi.next();
+
+ candidate = null;
+
+ currentX = pe.toX;
+ currentY = pe.toY;
+
+ switch(pe.type) {
+ case MOVE_TO:
+ moveX = pe.toX;
+ moveY = pe.toY;
+ isClosed = false;
+ break;
+ case LINE_TO:
+ {
+ seg.set(pe.fromX, pe.fromY, pe.toX, pe.toY);
+ isClosed = false;
+ candidate = seg.getClosestPointTo(p);
+ if (crossings!=MathConstants.SHAPE_INTERSECTS) {
+ crossings = Segment2i.computeCrossingsFromPoint(
+ crossings,
+ p.x(), p.y(),
+ pe.fromX, pe.fromY, pe.toX, pe.toY);
+ }
+ break;
+ }
+ case CLOSE:
+ isClosed = true;
+ if (!pe.isEmpty()) {
+ seg.set(pe.fromX, pe.fromY, pe.toX, pe.toY);
+ candidate = seg.getClosestPointTo(p);
+ if (crossings!=MathConstants.SHAPE_INTERSECTS) {
+ crossings = Segment2i.computeCrossingsFromPoint(
+ crossings,
+ p.x(), p.y(),
+ pe.fromX, pe.fromY, pe.toX, pe.toY);
+ }
+ }
+ break;
+ case QUAD_TO:
+ case CURVE_TO:
+ default:
+ throw new IllegalStateException();
+ }
+
+ if (candidate!=null) {
+ float d = p.distanceSquared(candidate);
+ if (d<=0f) return candidate;
+ if (d> 32));
+ }
+
+ /** Replies the coordinates of this path in an array of
+ * integers.
+ *
+ * @return the coordinates.
+ */
+ public final int[] toIntArray() {
+ return toIntArray(null);
+ }
+
+ /** Replies the coordinates of this path in an array of
+ * integers.
+ *
+ * @param transform is the transformation to apply to all the coordinates.
+ * @return the coordinates.
+ */
+ public int[] toIntArray(Transform2D transform) {
+ if (transform==null) {
+ return Arrays.copyOf(this.coords, this.numCoords);
+ }
+ Point2i p = new Point2i();
+ int[] clone = new int[this.numCoords];
+ for(int i=0; i toCollection() {
+ return new PointCollection();
+ }
+
+ /** Replies the coordinate at the given index.
+ * The index is in [0;{@link #size()}*2).
+ *
+ * @param index
+ * @return the coordinate at the given index.
+ */
+ public float getCoordAt(int index) {
+ return this.coords[index];
+ }
+
+ /** Replies the point at the given index.
+ * The index is in [0;{@link #size()}).
+ *
+ * @param index
+ * @return the point at the given index.
+ */
+ public Point2i getPointAt(int index) {
+ return new Point2i(
+ this.coords[index*2],
+ this.coords[index*2+1]);
+ }
+
+ /** Replies the number of points in the path.
+ *
+ * @return the number of points in the path.
+ */
+ public int size() {
+ return this.numCoords/2;
+ }
+
+ /** Replies if this path is empty.
+ * The path is empty when there is no point inside, or
+ * all the points are at the same coordinate, or
+ * when the path does not represents a drawable path
+ * (a path with a line or a curve).
+ *
+ * @return true if the path does not contain
+ * a coordinate; otherwise false.
+ */
+ @Override
+ public boolean isEmpty() {
+ if (this.isEmpty==null) {
+ this.isEmpty = Boolean.TRUE;
+ PathIterator2i pi = getPathIterator();
+ PathElement2i pe;
+ while (this.isEmpty()==Boolean.TRUE && pi.hasNext()) {
+ pe = pi.next();
+ if (pe.isDrawable()) {
+ this.isEmpty = Boolean.FALSE;
+ }
+ }
+ }
+ return this.isEmpty;
+ }
+
+ /** Replies if the given points exists in the coordinates of this path.
+ *
+ * @param p
+ * @return true if the point is a control point of the path.
+ */
+ boolean containsPoint(Point2D p) {
+ float x, y;
+ for(int i=0; itrue if the point was removed; false otherwise.
+ */
+ boolean remove(int x, int y) {
+ for(int i=0, j=0; i0) {
+ switch(this.types[this.numTypes-1]) {
+ case CLOSE:
+ // no coord to remove
+ break;
+ case MOVE_TO:
+ case LINE_TO:
+ this.numCoords -= 2;
+ break;
+ case CURVE_TO:
+ this.numCoords -= 6;
+ break;
+ case QUAD_TO:
+ this.numCoords -= 4;
+ break;
+ default:
+ throw new IllegalStateException();
+ }
+ --this.numTypes;
+ this.isEmpty = null;
+ this.bounds = null;
+ }
+ }
+
+ /** Change the coordinates of the last inserted point.
+ *
+ * @param x
+ * @param y
+ */
+ public void setLastPoint(int x, int y) {
+ if (this.numCoords>=2) {
+ this.coords[this.numCoords-2] = x;
+ this.coords[this.numCoords-1] = y;
+ this.bounds = null;
+ }
+ }
+
+ /** Replies the points along the path.
+ *
+ * This function is equivalent to a
+ * call to {@link #getPathIterator(float)}
+ * with the default flatness.
+ *
+ * @return the points
+ */
+ @Override
+ public Iterator getPointIterator() {
+ PathIterator2i pathIterator = getPathIterator(MathConstants.SPLINE_APPROXIMATION_RATIO);
+ return new PixelIterator(pathIterator);
+ }
+
+ /** A path iterator that does not transform the coordinates.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private class CopyPathIterator implements PathIterator2i {
+
+ private final Point2D p1 = new Point2i();
+ private final Point2D p2 = new Point2i();
+ private int iType = 0;
+ private int iCoord = 0;
+ private int movex, movey;
+
+ /**
+ */
+ public CopyPathIterator() {
+ //
+ }
+
+ @Override
+ public boolean hasNext() {
+ return this.iType=Path2i.this.numTypes) {
+ throw new NoSuchElementException();
+ }
+ PathElement2i element = null;
+ switch(Path2i.this.types[type]) {
+ case MOVE_TO:
+ if (this.iCoord+2>Path2i.this.numCoords) {
+ throw new NoSuchElementException();
+ }
+ this.movex = Path2i.this.coords[this.iCoord++];
+ this.movey = Path2i.this.coords[this.iCoord++];
+ this.p2.set(this.movex, this.movey);
+ element = new PathElement2i.MovePathElement2i(
+ this.p2.x(), this.p2.y());
+ break;
+ case LINE_TO:
+ if (this.iCoord+2>Path2i.this.numCoords) {
+ throw new NoSuchElementException();
+ }
+ this.p1.set(this.p2);
+ this.p2.set(
+ Path2i.this.coords[this.iCoord++],
+ Path2i.this.coords[this.iCoord++]);
+ element = new PathElement2i.LinePathElement2i(
+ this.p1.x(), this.p1.y(),
+ this.p2.x(), this.p2.y());
+ break;
+ case QUAD_TO:
+ {
+ if (this.iCoord+4>Path2i.this.numCoords) {
+ throw new NoSuchElementException();
+ }
+ this.p1.set(this.p2);
+ int ctrlx = Path2i.this.coords[this.iCoord++];
+ int ctrly = Path2i.this.coords[this.iCoord++];
+ this.p2.set(
+ Path2i.this.coords[this.iCoord++],
+ Path2i.this.coords[this.iCoord++]);
+ element = new PathElement2i.QuadPathElement2i(
+ this.p1.x(), this.p1.y(),
+ ctrlx, ctrly,
+ this.p2.x(), this.p2.y());
+ }
+ break;
+ case CURVE_TO:
+ {
+ if (this.iCoord+6>Path2i.this.numCoords) {
+ throw new NoSuchElementException();
+ }
+ this.p1.set(this.p2);
+ int ctrlx1 = Path2i.this.coords[this.iCoord++];
+ int ctrly1 = Path2i.this.coords[this.iCoord++];
+ int ctrlx2 = Path2i.this.coords[this.iCoord++];
+ int ctrly2 = Path2i.this.coords[this.iCoord++];
+ this.p2.set(
+ Path2i.this.coords[this.iCoord++],
+ Path2i.this.coords[this.iCoord++]);
+ element = new PathElement2i.CurvePathElement2i(
+ this.p1.x(), this.p1.y(),
+ ctrlx1, ctrly1,
+ ctrlx2, ctrly2,
+ this.p2.x(), this.p2.y());
+ }
+ break;
+ case CLOSE:
+ this.p1.set(this.p2);
+ this.p2.set(this.movex, this.movey);
+ element = new PathElement2i.ClosePathElement2i(
+ this.p1.x(), this.p1.y(),
+ this.p2.x(), this.p2.y());
+ break;
+ default:
+ }
+ if (element==null)
+ throw new NoSuchElementException();
+
+ ++this.iType;
+
+ return element;
+ }
+
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public PathWindingRule getWindingRule() {
+ return Path2i.this.getWindingRule();
+ }
+
+ } // class CopyPathIterator
+
+ /** A path iterator that transforms the coordinates.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private class TransformPathIterator implements PathIterator2i {
+
+ private final Transform2D transform;
+ private final Point2D p1 = new Point2i();
+ private final Point2D p2 = new Point2i();
+ private final Point2D ptmp1 = new Point2i();
+ private final Point2D ptmp2 = new Point2i();
+ private int iType = 0;
+ private int iCoord = 0;
+ private int movex, movey;
+
+ /**
+ * @param transform
+ */
+ public TransformPathIterator(Transform2D transform) {
+ assert(transform!=null);
+ this.transform = transform;
+ }
+
+ @Override
+ public boolean hasNext() {
+ return this.iType=Path2i.this.numTypes) {
+ throw new NoSuchElementException();
+ }
+ PathElement2i element = null;
+ switch(Path2i.this.types[this.iType++]) {
+ case MOVE_TO:
+ this.movex = Path2i.this.coords[this.iCoord++];
+ this.movey = Path2i.this.coords[this.iCoord++];
+ this.p2.set(this.movex, this.movey);
+ this.transform.transform(this.p2);
+ element = new PathElement2i.MovePathElement2i(
+ this.p2.x(), this.p2.y());
+ break;
+ case LINE_TO:
+ this.p1.set(this.p2);
+ this.p2.set(
+ Path2i.this.coords[this.iCoord++],
+ Path2i.this.coords[this.iCoord++]);
+ this.transform.transform(this.p2);
+ element = new PathElement2i.LinePathElement2i(
+ this.p1.x(), this.p1.y(),
+ this.p2.x(), this.p2.y());
+ break;
+ case QUAD_TO:
+ {
+ this.p1.set(this.p2);
+ this.ptmp1.set(
+ Path2i.this.coords[this.iCoord++],
+ Path2i.this.coords[this.iCoord++]);
+ this.transform.transform(this.ptmp1);
+ this.p2.set(
+ Path2i.this.coords[this.iCoord++],
+ Path2i.this.coords[this.iCoord++]);
+ this.transform.transform(this.p2);
+ element = new PathElement2i.QuadPathElement2i(
+ this.p1.x(), this.p1.y(),
+ this.ptmp1.x(), this.ptmp1.y(),
+ this.p2.x(), this.p2.y());
+ }
+ break;
+ case CURVE_TO:
+ {
+ this.p1.set(this.p2);
+ this.ptmp1.set(
+ Path2i.this.coords[this.iCoord++],
+ Path2i.this.coords[this.iCoord++]);
+ this.transform.transform(this.ptmp1);
+ this.ptmp2.set(
+ Path2i.this.coords[this.iCoord++],
+ Path2i.this.coords[this.iCoord++]);
+ this.transform.transform(this.ptmp2);
+ this.p2.set(
+ Path2i.this.coords[this.iCoord++],
+ Path2i.this.coords[this.iCoord++]);
+ this.transform.transform(this.p2);
+ element = new PathElement2i.CurvePathElement2i(
+ this.p1.x(), this.p1.y(),
+ this.ptmp1.x(), this.ptmp1.y(),
+ this.ptmp2.x(), this.ptmp2.y(),
+ this.p2.x(), this.p2.y());
+ }
+ break;
+ case CLOSE:
+ this.p1.set(this.p2);
+ this.p2.set(this.movex, this.movey);
+ this.transform.transform(this.p2);
+ element = new PathElement2i.ClosePathElement2i(
+ this.p1.x(), this.p1.y(),
+ this.p2.x(), this.p2.y());
+ break;
+ default:
+ }
+ if (element==null)
+ throw new NoSuchElementException();
+ return element;
+ }
+
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public PathWindingRule getWindingRule() {
+ return Path2i.this.getWindingRule();
+ }
+
+ } // class TransformPathIterator
+
+ /** An collection of the points of the path.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private class PointCollection implements Collection {
+
+ /**
+ */
+ public PointCollection() {
+ //
+ }
+
+ @Override
+ public int size() {
+ return Path2i.this.size();
+ }
+
+ @Override
+ public boolean isEmpty() {
+ return Path2i.this.size()<=0;
+ }
+
+ @Override
+ public boolean contains(Object o) {
+ if (o instanceof Point2D) {
+ return Path2i.this.containsPoint((Point2D)o);
+ }
+ return false;
+ }
+
+ @Override
+ public Iterator iterator() {
+ return new PointIterator();
+ }
+
+ @Override
+ public Object[] toArray() {
+ return Path2i.this.toPointArray();
+ }
+
+ @SuppressWarnings("unchecked")
+ @Override
+ public T[] toArray(T[] a) {
+ Iterator iterator = new PointIterator();
+ for(int i=0; i c) {
+ for(Object obj : c) {
+ if ((!(obj instanceof Point2D))
+ ||(!Path2i.this.containsPoint((Point2D)obj))) {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ @Override
+ public boolean addAll(Collection c) {
+ boolean changed = false;
+ for(Point2D pts : c) {
+ if (add(pts)) {
+ changed = true;
+ }
+ }
+ return changed;
+ }
+
+ @Override
+ public boolean removeAll(Collection c) {
+ boolean changed = false;
+ for(Object obj : c) {
+ if (obj instanceof Point2D) {
+ Point2D pts = (Point2D)obj;
+ if (Path2i.this.remove(pts.x(), pts.y())) {
+ changed = true;
+ }
+ }
+ }
+ return changed;
+ }
+
+ @Override
+ public boolean retainAll(Collection c) {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public void clear() {
+ Path2i.this.clear();
+ }
+
+ } // class PointCollection
+
+ /** Iterator on the points of the path.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private class PointIterator implements Iterator {
+
+ private int index = 0;
+ private Point2D lastReplied = null;
+
+ /**
+ */
+ public PointIterator() {
+ //
+ }
+
+ @Override
+ public boolean hasNext() {
+ return this.index {
+
+ private final PathIterator2i pathIterator;
+ private Iterator lineIterator = null;
+ private Point2i next = null;
+
+ public PixelIterator(PathIterator2i pi) {
+ this.pathIterator = pi;
+ searchNext();
+ }
+
+ private void searchNext() {
+ Point2i old = this.next;
+ this.next = null;
+ while (this.pathIterator.hasNext() && (this.lineIterator==null || !this.lineIterator.hasNext())) {
+ this.lineIterator = null;
+ PathElement2i elt = this.pathIterator.next();
+ if (elt.isDrawable()) {
+ switch(elt.type) {
+ case LINE_TO:
+ this.lineIterator = new Segment2i(
+ elt.fromX, elt.fromY,
+ elt.toX, elt.toY).getPointIterator();
+ break;
+ case MOVE_TO:
+ case CLOSE:
+ case CURVE_TO:
+ case QUAD_TO:
+ default:
+ throw new IllegalStateException();
+ }
+ }
+ }
+ if (this.lineIterator!=null && this.lineIterator.hasNext()) {
+ this.next = this.lineIterator.next();
+ while (this.next.equals(old)) {
+ this.next = this.lineIterator.next();
+ }
+ }
+ }
+
+ @Override
+ public boolean hasNext() {
+ return this.next!=null;
+ }
+
+ @Override
+ public Point2i next() {
+ Point2i n = this.next;
+ if (n==null) throw new NoSuchElementException();
+ searchNext();
+ return n;
+ }
+
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ } // class PixelIterator
+
+ /** A path iterator that is flattening the path.
+ * This iterator was copied from AWT FlatteningPathIterator.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private static class FlatteningPathIterator implements PathIterator2i {
+
+ /** Winding rule of the path.
+ */
+ private final PathWindingRule windingRule;
+
+ /** The source iterator.
+ */
+ private final Iterator pathIterator;
+
+ /**
+ * Square of the flatness parameter for testing against squared lengths.
+ */
+ private final float squaredFlatness;
+
+ /**
+ * Maximum number of recursion levels.
+ */
+ private final int limit;
+
+ /** The recursion level at which each curve being held in storage was generated.
+ */
+ private int levels[];
+
+ /** The cache of interpolated coords.
+ * Note that this must be long enough
+ * to store a full cubic segment and
+ * a relative cubic segment to avoid
+ * aliasing when copying the coords
+ * of a curve to the end of the array.
+ * This is also serendipitously equal
+ * to the size of a full quad segment
+ * and 2 relative quad segments.
+ */
+ private float hold[] = new float[14];
+
+ /** The index of the last curve segment being held for interpolation.
+ */
+ private int holdEnd;
+
+ /**
+ * The index of the curve segment that was last interpolated. This
+ * is the curve segment ready to be returned in the next call to
+ * next().
+ */
+ private int holdIndex;
+
+ /** The ending x of the last segment.
+ */
+ private float currentX;
+
+ /** The ending y of the last segment.
+ */
+ private float currentY;
+
+ /** The x of the last move segment.
+ */
+ private float moveX;
+
+ /** The y of the last move segment.
+ */
+ private float moveY;
+
+ /** The index of the entry in the
+ * levels array of the curve segment
+ * at the holdIndex
+ */
+ private int levelIndex;
+
+ /** True when iteration is done.
+ */
+ private boolean done;
+
+ /** The type of the path element.
+ */
+ private PathElementType holdType;
+
+ /** The x of the last move segment replied by next.
+ */
+ private int lastNextX;
+
+ /** The y of the last move segment replied by next.
+ */
+ private int lastNextY;
+
+ /**
+ * @param windingRule is the winding rule of the path.
+ * @param pathIterator is the path iterator that may be used to initialize the path.
+ * @param flatness the maximum allowable distance between the
+ * control points and the flattened curve
+ * @param limit the maximum number of recursive subdivisions
+ * allowed for any curved segment
+ */
+ public FlatteningPathIterator(PathWindingRule windingRule, Iterator pathIterator, float flatness, int limit) {
+ assert(windingRule!=null);
+ assert(flatness>=0f);
+ assert(limit>=0);
+ this.windingRule = windingRule;
+ this.pathIterator = pathIterator;
+ this.squaredFlatness = flatness * flatness;
+ this.limit = limit;
+ this.levels = new int[limit + 1];
+ searchNext(true);
+ }
+
+ /**
+ * Ensures that the hold array can hold up to (want) more values.
+ * It is currently holding (hold.length - holdIndex) values.
+ */
+ private void ensureHoldCapacity(int want) {
+ if (this.holdIndex - want < 0) {
+ int have = this.hold.length - this.holdIndex;
+ int newsize = this.hold.length + GROW_SIZE;
+ float newhold[] = new float[newsize];
+ System.arraycopy(this.hold, this.holdIndex,
+ newhold, this.holdIndex + GROW_SIZE,
+ have);
+ this.hold = newhold;
+ this.holdIndex += GROW_SIZE;
+ this.holdEnd += GROW_SIZE;
+ }
+ }
+
+ /**
+ * Returns the square of the flatness, or maximum distance of a
+ * control point from the line connecting the end points, of the
+ * quadratic curve specified by the control points stored in the
+ * indicated array at the indicated index.
+ * @param coords an array containing coordinate values
+ * @param offset the index into coords from which to
+ * to start getting the values from the array
+ * @return the flatness of the quadratic curve that is defined by the
+ * values in the specified array at the specified index.
+ */
+ private static float getQuadSquaredFlatness(float coords[], int offset) {
+ return GeometryUtil.distanceSquaredPointLine(
+ coords[offset + 2], coords[offset + 3],
+ coords[offset + 0], coords[offset + 1],
+ coords[offset + 4], coords[offset + 5]);
+ }
+
+ /**
+ * Subdivides the quadratic curve specified by the coordinates
+ * stored in the src array at indices
+ * srcoff through srcoff + 5
+ * and stores the resulting two subdivided curves into the two
+ * result arrays at the corresponding indices.
+ * Either or both of the left and right
+ * arrays can be null or a reference to the same array
+ * and offset as the src array.
+ * Note that the last point in the first subdivided curve is the
+ * same as the first point in the second subdivided curve. Thus,
+ * it is possible to pass the same array for left and
+ * right and to use offsets such that
+ * rightoff equals leftoff + 4 in order
+ * to avoid allocating extra storage for this common point.
+ * @param src the array holding the coordinates for the source curve
+ * @param srcoff the offset into the array of the beginning of the
+ * the 6 source coordinates
+ * @param left the array for storing the coordinates for the first
+ * half of the subdivided curve
+ * @param leftoff the offset into the array of the beginning of the
+ * the 6 left coordinates
+ * @param right the array for storing the coordinates for the second
+ * half of the subdivided curve
+ * @param rightoff the offset into the array of the beginning of the
+ * the 6 right coordinates
+ */
+ private static void subdivideQuad(float src[], int srcoff,
+ float left[], int leftoff,
+ float right[], int rightoff) {
+ float x1 = src[srcoff + 0];
+ float y1 = src[srcoff + 1];
+ float ctrlx = src[srcoff + 2];
+ float ctrly = src[srcoff + 3];
+ float x2 = src[srcoff + 4];
+ float y2 = src[srcoff + 5];
+ if (left != null) {
+ left[leftoff + 0] = x1;
+ left[leftoff + 1] = y1;
+ }
+ if (right != null) {
+ right[rightoff + 4] = x2;
+ right[rightoff + 5] = y2;
+ }
+ x1 = (x1 + ctrlx) / 2f;
+ y1 = (y1 + ctrly) / 2f;
+ x2 = (x2 + ctrlx) / 2f;
+ y2 = (y2 + ctrly) / 2f;
+ ctrlx = (x1 + x2) / 2f;
+ ctrly = (y1 + y2) / 2f;
+ if (left != null) {
+ left[leftoff + 2] = x1;
+ left[leftoff + 3] = y1;
+ left[leftoff + 4] = ctrlx;
+ left[leftoff + 5] = ctrly;
+ }
+ if (right != null) {
+ right[rightoff + 0] = ctrlx;
+ right[rightoff + 1] = ctrly;
+ right[rightoff + 2] = x2;
+ right[rightoff + 3] = y2;
+ }
+ }
+
+ /**
+ * Returns the square of the flatness of the cubic curve specified
+ * by the control points stored in the indicated array at the
+ * indicated index. The flatness is the maximum distance
+ * of a control point from the line connecting the end points.
+ * @param coords an array containing coordinates
+ * @param offset the index of coords from which to begin
+ * getting the end points and control points of the curve
+ * @return the square of the flatness of the CubicCurve2D
+ * specified by the coordinates in coords at
+ * the specified offset.
+ */
+ private static float getCurveSquaredFlatness(float coords[], int offset) {
+ return Math.max(
+ GeometryUtil.distanceSquaredPointSegment(
+ coords[offset + 0],
+ coords[offset + 1],
+ coords[offset + 6],
+ coords[offset + 7],
+ coords[offset + 2],
+ coords[offset + 3], null),
+ GeometryUtil.distanceSquaredPointSegment(
+ coords[offset + 0],
+ coords[offset + 1],
+ coords[offset + 6],
+ coords[offset + 7],
+ coords[offset + 4], coords[offset + 5], null));
+ }
+
+ /**
+ * Subdivides the cubic curve specified by the coordinates
+ * stored in the src array at indices srcoff
+ * through (srcoff + 7) and stores the
+ * resulting two subdivided curves into the two result arrays at the
+ * corresponding indices.
+ * Either or both of the left and right
+ * arrays may be null or a reference to the same array
+ * as the src array.
+ * Note that the last point in the first subdivided curve is the
+ * same as the first point in the second subdivided curve. Thus,
+ * it is possible to pass the same array for left
+ * and right and to use offsets, such as rightoff
+ * equals (leftoff + 6), in order
+ * to avoid allocating extra storage for this common point.
+ * @param src the array holding the coordinates for the source curve
+ * @param srcoff the offset into the array of the beginning of the
+ * the 6 source coordinates
+ * @param left the array for storing the coordinates for the first
+ * half of the subdivided curve
+ * @param leftoff the offset into the array of the beginning of the
+ * the 6 left coordinates
+ * @param right the array for storing the coordinates for the second
+ * half of the subdivided curve
+ * @param rightoff the offset into the array of the beginning of the
+ * the 6 right coordinates
+ */
+ private static void subdivideCurve(
+ float src[], int srcoff,
+ float left[], int leftoff,
+ float right[], int rightoff) {
+ float x1 = src[srcoff + 0];
+ float y1 = src[srcoff + 1];
+ float ctrlx1 = src[srcoff + 2];
+ float ctrly1 = src[srcoff + 3];
+ float ctrlx2 = src[srcoff + 4];
+ float ctrly2 = src[srcoff + 5];
+ float x2 = src[srcoff + 6];
+ float y2 = src[srcoff + 7];
+ if (left != null) {
+ left[leftoff + 0] = x1;
+ left[leftoff + 1] = y1;
+ }
+ if (right != null) {
+ right[rightoff + 6] = x2;
+ right[rightoff + 7] = y2;
+ }
+ x1 = (x1 + ctrlx1) / 2f;
+ y1 = (y1 + ctrly1) / 2f;
+ x2 = (x2 + ctrlx2) / 2f;
+ y2 = (y2 + ctrly2) / 2f;
+ float centerx = (ctrlx1 + ctrlx2) / 2f;
+ float centery = (ctrly1 + ctrly2) / 2f;
+ ctrlx1 = (x1 + centerx) / 2f;
+ ctrly1 = (y1 + centery) / 2f;
+ ctrlx2 = (x2 + centerx) / 2f;
+ ctrly2 = (y2 + centery) / 2f;
+ centerx = (ctrlx1 + ctrlx2) / 2f;
+ centery = (ctrly1 + ctrly2) / 2f;
+ if (left != null) {
+ left[leftoff + 2] = x1;
+ left[leftoff + 3] = y1;
+ left[leftoff + 4] = ctrlx1;
+ left[leftoff + 5] = ctrly1;
+ left[leftoff + 6] = centerx;
+ left[leftoff + 7] = centery;
+ }
+ if (right != null) {
+ right[rightoff + 0] = centerx;
+ right[rightoff + 1] = centery;
+ right[rightoff + 2] = ctrlx2;
+ right[rightoff + 3] = ctrly2;
+ right[rightoff + 4] = x2;
+ right[rightoff + 5] = y2;
+ }
+ }
+
+ private void searchNext(boolean isFirst) {
+ do {
+ flattening();
+ }
+ while (!this.done && !isFirst && isSame());
+ }
+
+ private boolean isSame() {
+ PathElementType type = this.holdType;
+ int x, y;
+ if (type==PathElementType.CLOSE) {
+ x = Math.round(this.moveX);
+ y = Math.round(this.moveY);
+ }
+ else {
+ x = Math.round(this.hold[this.holdIndex + 0]);
+ y = Math.round(this.hold[this.holdIndex + 1]);
+ }
+ return x==this.lastNextX && y==this.lastNextY;
+ }
+
+ private void flattening() {
+ int level;
+
+ if (this.holdIndex >= this.holdEnd) {
+ if (!this.pathIterator.hasNext()) {
+ this.done = true;
+ return;
+ }
+ PathElement2i pathElement = this.pathIterator.next();
+ this.holdType = pathElement.type;
+ pathElement.toArray(this.hold);
+ this.levelIndex = 0;
+ this.levels[0] = 0;
+ }
+
+ switch (this.holdType) {
+ case MOVE_TO:
+ case LINE_TO:
+ this.currentX = this.hold[0];
+ this.currentY = this.hold[1];
+ if (this.holdType == PathElementType.MOVE_TO) {
+ this.moveX = this.currentX;
+ this.moveY = this.currentY;
+ }
+ this.holdIndex = 0;
+ this.holdEnd = 0;
+ break;
+ case CLOSE:
+ this.currentX = this.moveX;
+ this.currentY = this.moveY;
+ this.holdIndex = 0;
+ this.holdEnd = 0;
+ break;
+ case QUAD_TO:
+ if (this.holdIndex >= this.holdEnd) {
+ // Move the coordinates to the end of the array.
+ this.holdIndex = this.hold.length - 6;
+ this.holdEnd = this.hold.length - 2;
+ this.hold[this.holdIndex + 0] = this.currentX;
+ this.hold[this.holdIndex + 1] = this.currentY;
+ this.hold[this.holdIndex + 2] = this.hold[0];
+ this.hold[this.holdIndex + 3] = this.hold[1];
+ this.hold[this.holdIndex + 4] = this.currentX = this.hold[2];
+ this.hold[this.holdIndex + 5] = this.currentY = this.hold[3];
+ }
+
+ level = this.levels[this.levelIndex];
+ while (level < this.limit) {
+ if (getQuadSquaredFlatness(this.hold, this.holdIndex) < this.squaredFlatness) {
+ break;
+ }
+
+ ensureHoldCapacity(4);
+ subdivideQuad(
+ this.hold, this.holdIndex,
+ this.hold, this.holdIndex - 4,
+ this.hold, this.holdIndex);
+ this.holdIndex -= 4;
+
+ // Now that we have subdivided, we have constructed
+ // two curves of one depth lower than the original
+ // curve. One of those curves is in the place of
+ // the former curve and one of them is in the next
+ // set of held coordinate slots. We now set both
+ // curves level values to the next higher level.
+ level++;
+ this.levels[this.levelIndex] = level;
+ this.levelIndex++;
+ this.levels[this.levelIndex] = level;
+ }
+
+ // This curve segment is flat enough, or it is too deep
+ // in recursion levels to try to flatten any more. The
+ // two coordinates at holdIndex+4 and holdIndex+5 now
+ // contain the endpoint of the curve which can be the
+ // endpoint of an approximating line segment.
+ this.holdIndex += 4;
+ this.levelIndex--;
+ break;
+ case CURVE_TO:
+ if (this.holdIndex >= this.holdEnd) {
+ // Move the coordinates to the end of the array.
+ this.holdIndex = this.hold.length - 8;
+ this.holdEnd = this.hold.length - 2;
+ this.hold[this.holdIndex + 0] = this.currentX;
+ this.hold[this.holdIndex + 1] = this.currentY;
+ this.hold[this.holdIndex + 2] = this.hold[0];
+ this.hold[this.holdIndex + 3] = this.hold[1];
+ this.hold[this.holdIndex + 4] = this.hold[2];
+ this.hold[this.holdIndex + 5] = this.hold[3];
+ this.hold[this.holdIndex + 6] = this.currentX = this.hold[4];
+ this.hold[this.holdIndex + 7] = this.currentY = this.hold[5];
+ }
+
+ level = this.levels[this.levelIndex];
+ while (level < this.limit) {
+ if (getCurveSquaredFlatness(this.hold,this. holdIndex) < this.squaredFlatness) {
+ break;
+ }
+
+ ensureHoldCapacity(6);
+ subdivideCurve(
+ this.hold, this.holdIndex,
+ this.hold, this.holdIndex - 6,
+ this.hold, this.holdIndex);
+ this.holdIndex -= 6;
+
+ // Now that we have subdivided, we have constructed
+ // two curves of one depth lower than the original
+ // curve. One of those curves is in the place of
+ // the former curve and one of them is in the next
+ // set of held coordinate slots. We now set both
+ // curves level values to the next higher level.
+ level++;
+ this.levels[this.levelIndex] = level;
+ this.levelIndex++;
+ this.levels[this.levelIndex] = level;
+ }
+
+ // This curve segment is flat enough, or it is too deep
+ // in recursion levels to try to flatten any more. The
+ // two coordinates at holdIndex+6 and holdIndex+7 now
+ // contain the endpoint of the curve which can be the
+ // endpoint of an approximating line segment.
+ this.holdIndex += 6;
+ this.levelIndex--;
+ break;
+ default:
+ }
+ }
+
+ @Override
+ public boolean hasNext() {
+ return !this.done;
+ }
+
+ @Override
+ public PathElement2i next() {
+ if (this.done) {
+ throw new NoSuchElementException("flattening iterator out of bounds"); //$NON-NLS-1$
+ }
+
+ PathElement2i element;
+ PathElementType type = this.holdType;
+ if (type!=PathElementType.CLOSE) {
+ int x = Math.round(this.hold[this.holdIndex + 0]);
+ int y = Math.round(this.hold[this.holdIndex + 1]);
+ if (type == PathElementType.MOVE_TO) {
+ element = new PathElement2i.MovePathElement2i(x, y);
+ }
+ else {
+ element = new PathElement2i.LinePathElement2i(
+ this.lastNextX, this.lastNextY,
+ x, y);
+ }
+ this.lastNextX = x;
+ this.lastNextY = y;
+ }
+ else {
+ int x = Math.round(this.moveX);
+ int y = Math.round(this.moveY);
+ element = new PathElement2i.ClosePathElement2i(
+ this.lastNextX, this.lastNextY,
+ x, y);
+ this.lastNextX = x;
+ this.lastNextY = y;
+ }
+
+ searchNext(false);
+
+ return element;
+ }
+
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public PathWindingRule getWindingRule() {
+ return this.windingRule;
+ }
+
+ } // class FlatteningPathIterator
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/PathElement2i.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/PathElement2i.java
new file mode 100644
index 000000000..602b05d3e
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/PathElement2i.java
@@ -0,0 +1,480 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2005-09 Stephane GALLAND.
+ * Copyright (C) 2012 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.discrete;
+
+import java.io.Serializable;
+
+import org.arakhne.afc.math.geometry.PathElementType;
+
+/** An element of the path.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public abstract class PathElement2i implements Serializable {
+
+ private static final long serialVersionUID = 7757419973445894032L;
+
+ /** Create an instance of path element.
+ *
+ * @param type is the type of the new element.
+ * @param lastX is the coordinate of the last point.
+ * @param lastY is the coordinate of the last point.
+ * @param coords are the coordinates.
+ * @return the instance of path element.
+ */
+ public static PathElement2i newInstance(PathElementType type, int lastX, int lastY, int[] coords) {
+ switch(type) {
+ case MOVE_TO:
+ return new MovePathElement2i(coords[0], coords[1]);
+ case LINE_TO:
+ return new LinePathElement2i(lastX, lastY, coords[0], coords[1]);
+ case QUAD_TO:
+ return new QuadPathElement2i(lastX, lastY, coords[0], coords[1], coords[2], coords[3]);
+ case CURVE_TO:
+ return new CurvePathElement2i(lastX, lastY, coords[0], coords[1], coords[2], coords[3], coords[4], coords[5]);
+ case CLOSE:
+ return new ClosePathElement2i(lastX, lastY, coords[0], coords[1]);
+ default:
+ }
+ throw new IllegalArgumentException();
+ }
+
+ /** Type of the path element.
+ */
+ public final PathElementType type;
+
+ /** Source point.
+ */
+ public final int fromX;
+
+ /** Source point.
+ */
+ public final int fromY;
+
+ /** Target point.
+ */
+ public final int toX;
+
+ /** Target point.
+ */
+ public final int toY;
+
+ /** First control point.
+ */
+ public final int ctrlX1;
+
+ /** First control point.
+ */
+ public final int ctrlY1;
+
+ /** Second control point.
+ */
+ public final int ctrlX2;
+
+ /** Second control point.
+ */
+ public final int ctrlY2;
+
+ /**
+ * @param type is the type of the element.
+ * @param fromx is the source point.
+ * @param fromy is the source point.
+ * @param ctrlx1 is the first control point.
+ * @param ctrly1 is the first control point.
+ * @param ctrlx2 is the first control point.
+ * @param ctrly2 is the first control point.
+ * @param tox is the target point.
+ * @param toy is the target point.
+ */
+ public PathElement2i(PathElementType type, int fromx, int fromy, int ctrlx1, int ctrly1, int ctrlx2, int ctrly2, int tox, int toy) {
+ assert(type!=null);
+ this.type = type;
+ this.fromX = fromx;
+ this.fromY = fromy;
+ this.ctrlX1 = ctrlx1;
+ this.ctrlY1 = ctrly1;
+ this.ctrlX2 = ctrlx2;
+ this.ctrlY2 = ctrly2;
+ this.toX = tox;
+ this.toY = toy;
+ }
+
+ /** Replies if the element is empty, ie. the points are the same.
+ *
+ * @return true if the points are
+ * the same; otherwise false.
+ */
+ public abstract boolean isEmpty();
+
+ /** Replies if the element is not empty and its drawable.
+ * Only the path elements that may produce pixels on the screen
+ * must reply true in this function.
+ *
+ * @return true if the path element
+ * is drawable; otherwise false.
+ */
+ public abstract boolean isDrawable();
+
+ /** Copy the coords into the given array, except the source point.
+ *
+ * @param array
+ */
+ public abstract void toArray(int[] array);
+
+ /** Copy the coords into the given array, except the source point.
+ *
+ * @param array
+ */
+ public abstract void toArray(float[] array);
+
+ /** Copy the coords into an array, except the source point.
+ *
+ * @return the array of the points, except the source point.
+ */
+ public abstract int[] toArray();
+
+ /** An element of the path that represents a MOVE_TO.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ public static class MovePathElement2i extends PathElement2i {
+
+ private static final long serialVersionUID = -8591881826671557331L;
+
+ /**
+ * @param x
+ * @param y
+ */
+ public MovePathElement2i(int x, int y) {
+ super(PathElementType.MOVE_TO,
+ 0, 0, 0, 0, 0, 0,
+ x, y);
+ }
+
+ @Override
+ public boolean isEmpty() {
+ return (this.fromX==this.toX) && (this.fromY==this.toY);
+ }
+
+ @Override
+ public boolean isDrawable() {
+ return false;
+ }
+
+ @Override
+ public void toArray(int[] array) {
+ array[0] = this.toX;
+ array[1] = this.toY;
+ }
+
+ @Override
+ public void toArray(float[] array) {
+ array[0] = this.toX;
+ array[1] = this.toY;
+ }
+
+ @Override
+ public int[] toArray() {
+ return new int[] {this.toX, this.toY};
+ }
+
+ @Override
+ public String toString() {
+ return "MOVE("+ //$NON-NLS-1$
+ this.toX+"x"+ //$NON-NLS-1$
+ this.toY+")"; //$NON-NLS-1$
+ }
+
+ }
+
+ /** An element of the path that represents a LINE_TO.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ public static class LinePathElement2i extends PathElement2i {
+
+ private static final long serialVersionUID = 497492389885992535L;
+
+ /**
+ * @param fromx
+ * @param fromy
+ * @param tox
+ * @param toy
+ */
+ public LinePathElement2i(int fromx, int fromy, int tox, int toy) {
+ super(PathElementType.LINE_TO,
+ fromx, fromy,
+ 0, 0, 0, 0,
+ tox, toy);
+ }
+
+ @Override
+ public boolean isEmpty() {
+ return (this.fromX==this.toX) && (this.fromY==this.toY);
+ }
+
+ @Override
+ public boolean isDrawable() {
+ return !isEmpty();
+ }
+
+ @Override
+ public void toArray(int[] array) {
+ array[0] = this.toX;
+ array[1] = this.toY;
+ }
+
+ @Override
+ public void toArray(float[] array) {
+ array[0] = this.toX;
+ array[1] = this.toY;
+ }
+
+ @Override
+ public int[] toArray() {
+ return new int[] {this.toX, this.toY};
+ }
+
+ @Override
+ public String toString() {
+ return "LINE("+ //$NON-NLS-1$
+ this.toX+"x"+ //$NON-NLS-1$
+ this.toY+")"; //$NON-NLS-1$
+ }
+
+ }
+
+ /** An element of the path that represents a QUAD_TO.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ public static class QuadPathElement2i extends PathElement2i {
+
+ private static final long serialVersionUID = 6341899683730854257L;
+
+ /**
+ * @param fromx
+ * @param fromy
+ * @param ctrlx
+ * @param ctrly
+ * @param tox
+ * @param toy
+ */
+ public QuadPathElement2i(int fromx, int fromy, int ctrlx, int ctrly, int tox, int toy) {
+ super(PathElementType.QUAD_TO,
+ fromx, fromy,
+ ctrlx, ctrly,
+ 0, 0,
+ tox, toy);
+ }
+
+ @Override
+ public boolean isEmpty() {
+ return (this.fromX==this.toX) && (this.fromY==this.toY) &&
+ (this.ctrlX1==this.toX) && (this.ctrlY1==this.toY);
+ }
+
+ @Override
+ public boolean isDrawable() {
+ return !isEmpty();
+ }
+
+ @Override
+ public void toArray(int[] array) {
+ array[0] = this.ctrlX1;
+ array[1] = this.ctrlY1;
+ array[2] = this.toX;
+ array[3] = this.toY;
+ }
+
+ @Override
+ public void toArray(float[] array) {
+ array[0] = this.ctrlX1;
+ array[1] = this.ctrlY1;
+ array[2] = this.toX;
+ array[3] = this.toY;
+ }
+
+ @Override
+ public int[] toArray() {
+ return new int[] {this.ctrlX1, this.ctrlY1, this.toX, this.toY};
+ }
+
+ @Override
+ public String toString() {
+ return "QUAD("+ //$NON-NLS-1$
+ this.ctrlX1+"x"+ //$NON-NLS-1$
+ this.ctrlY1+"|"+ //$NON-NLS-1$
+ this.toX+"x"+ //$NON-NLS-1$
+ this.toY+")"; //$NON-NLS-1$
+ }
+
+ }
+
+ /** An element of the path that represents a CURVE_TO.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ public static class CurvePathElement2i extends PathElement2i {
+
+ private static final long serialVersionUID = 1043302430176113524L;
+
+ /**
+ * @param fromx
+ * @param fromy
+ * @param ctrlx1
+ * @param ctrly1
+ * @param ctrlx2
+ * @param ctrly2
+ * @param tox
+ * @param toy
+ */
+ public CurvePathElement2i(int fromx, int fromy, int ctrlx1, int ctrly1, int ctrlx2, int ctrly2, int tox, int toy) {
+ super(PathElementType.CURVE_TO,
+ fromx, fromy,
+ ctrlx1, ctrly1,
+ ctrlx2, ctrly2,
+ tox, toy);
+ }
+
+ @Override
+ public boolean isEmpty() {
+ return (this.fromX==this.toX) && (this.fromY==this.toY) &&
+ (this.ctrlX1==this.toX) && (this.ctrlY1==this.toY) &&
+ (this.ctrlX2==this.toX) && (this.ctrlY2==this.toY);
+ }
+
+ @Override
+ public boolean isDrawable() {
+ return !isEmpty();
+ }
+
+ @Override
+ public void toArray(int[] array) {
+ array[0] = this.ctrlX1;
+ array[1] = this.ctrlY1;
+ array[2] = this.ctrlX2;
+ array[3] = this.ctrlY2;
+ array[4] = this.toX;
+ array[5] = this.toY;
+ }
+
+ @Override
+ public void toArray(float[] array) {
+ array[0] = this.ctrlX1;
+ array[1] = this.ctrlY1;
+ array[2] = this.ctrlX2;
+ array[3] = this.ctrlY2;
+ array[4] = this.toX;
+ array[5] = this.toY;
+ }
+
+ @Override
+ public int[] toArray() {
+ return new int[] {this.ctrlX1, this.ctrlY1, this.ctrlX2, this.ctrlY2, this.toX, this.toY};
+ }
+
+ @Override
+ public String toString() {
+ return "CURVE("+ //$NON-NLS-1$
+ this.ctrlX1+"x"+ //$NON-NLS-1$
+ this.ctrlY1+"|"+ //$NON-NLS-1$
+ this.ctrlX2+"x"+ //$NON-NLS-1$
+ this.ctrlY2+"|"+ //$NON-NLS-1$
+ this.toX+"x"+ //$NON-NLS-1$
+ this.toY+")"; //$NON-NLS-1$
+ }
+
+ }
+
+ /** An element of the path that represents a CLOSE.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ public static class ClosePathElement2i extends PathElement2i {
+
+ private static final long serialVersionUID = 2745123226508569279L;
+
+ /**
+ * @param fromx
+ * @param fromy
+ * @param tox
+ * @param toy
+ */
+ public ClosePathElement2i(int fromx, int fromy, int tox, int toy) {
+ super(PathElementType.CLOSE,
+ fromx, fromy,
+ 0, 0, 0, 0,
+ tox, toy);
+ }
+
+ @Override
+ public boolean isEmpty() {
+ return (this.fromX==this.toX) && (this.fromY==this.toY);
+ }
+
+ @Override
+ public boolean isDrawable() {
+ return false;
+ }
+
+ @Override
+ public void toArray(int[] array) {
+ //
+ }
+
+ @Override
+ public void toArray(float[] array) {
+ //
+ }
+
+ @Override
+ public int[] toArray() {
+ return new int[0];
+ }
+
+ @Override
+ public String toString() {
+ return "CLOSE"; //$NON-NLS-1$
+ }
+
+ }
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/PathIterator2i.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/PathIterator2i.java
new file mode 100644
index 000000000..645f04a49
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/PathIterator2i.java
@@ -0,0 +1,44 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2005-09 Stephane GALLAND.
+ * Copyright (C) 2012 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.discrete;
+
+import java.util.Iterator;
+
+import org.arakhne.afc.math.geometry.PathWindingRule;
+
+
+/** This interface describes an interator on path elements.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public interface PathIterator2i extends Iterator {
+
+ /** Replies the winding rule for the path.
+ *
+ * @return the winding rule for the path.
+ */
+ public PathWindingRule getWindingRule();
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Point2i.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Point2i.java
new file mode 100644
index 000000000..79d224d0f
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Point2i.java
@@ -0,0 +1,209 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2011 Janus Core Developers
+ * Copyright (C) 2012 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.discrete;
+
+import org.arakhne.afc.math.geometry2d.Point2D;
+import org.arakhne.afc.math.geometry2d.Tuple2D;
+import org.arakhne.afc.math.geometry2d.Vector2D;
+
+/** 2D Point with 2 integers.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class Point2i extends Tuple2i implements Point2D {
+
+ private static final long serialVersionUID = 6087683508168847436L;
+
+ /**
+ */
+ public Point2i() {
+ //
+ }
+
+ /**
+ * @param tuple is the tuple to copy.
+ */
+ public Point2i(Tuple2D tuple) {
+ super(tuple);
+ }
+
+ /**
+ * @param tuple is the tuple to copy.
+ */
+ public Point2i(int[] tuple) {
+ super(tuple);
+ }
+
+ /**
+ * @param tuple is the tuple to copy.
+ */
+ public Point2i(float[] tuple) {
+ super(tuple);
+ }
+
+ /**
+ * @param x
+ * @param y
+ */
+ public Point2i(int x, int y) {
+ super(x,y);
+ }
+
+ /**
+ * @param x
+ * @param y
+ */
+ public Point2i(float x, float y) {
+ super(x,y);
+ }
+
+ /**
+ * @param x
+ * @param y
+ */
+ public Point2i(double x, double y) {
+ super((float)x,(float)y);
+ }
+
+ /**
+ * @param x
+ * @param y
+ */
+ public Point2i(long x, long y) {
+ super(x,y);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public Point2i clone() {
+ return (Point2i)super.clone();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float distanceSquared(Point2D p1) {
+ float dx, dy;
+ dx = this.x-p1.getX();
+ dy = this.y-p1.getY();
+ return (dx*dx+dy*dy);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float distance(Point2D p1) {
+ float dx, dy;
+ dx = this.x-p1.getX();
+ dy = this.y-p1.getY();
+ return (float)Math.sqrt(dx*dx+dy*dy);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float distanceL1(Point2D p1) {
+ return (Math.abs(this.x-p1.getX()) + Math.abs(this.y-p1.getY()));
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float distanceLinf(Point2D p1) {
+ return (Math.max( Math.abs(this.x-p1.getX()), Math.abs(this.y-p1.getY())));
+ }
+
+ @Override
+ public void add(Point2D t1, Vector2D t2) {
+ this.x = (int)(t1.getX() + t2.getX());
+ this.y = (int)(t1.getY() + t2.getY());
+ }
+
+ @Override
+ public void add(Vector2D t1, Point2D t2) {
+ this.x = (int)(t1.getX() + t2.getX());
+ this.y = (int)(t1.getY() + t2.getY());
+ }
+
+ @Override
+ public void add(Vector2D t1) {
+ this.x = (int)(this.x + t1.getX());
+ this.y = (int)(this.y + t1.getY());
+ }
+
+ @Override
+ public void scaleAdd(int s, Vector2D t1, Point2D t2) {
+ this.x = (int)(s * t1.getX() + t2.getX());
+ this.y = (int)(s * t1.getY() + t2.getY());
+ }
+
+ @Override
+ public void scaleAdd(float s, Vector2D t1, Point2D t2) {
+ this.x = (int)(s * t1.getX() + t2.getX());
+ this.y = (int)(s * t1.getY() + t2.getY());
+ }
+
+ @Override
+ public void scaleAdd(int s, Point2D t1, Vector2D t2) {
+ this.x = (int)(s * t1.getX() + t2.getX());
+ this.y = (int)(s * t1.getY() + t2.getY());
+ }
+
+ @Override
+ public void scaleAdd(float s, Point2D t1, Vector2D t2) {
+ this.x = (int)(s * t1.getX() + t2.getX());
+ this.y = (int)(s * t1.getY() + t2.getY());
+ }
+
+ @Override
+ public void scaleAdd(int s, Vector2D t1) {
+ this.x = (int)(s * this.x + t1.getX());
+ this.y = (int)(s * this.y + t1.getY());
+ }
+
+ @Override
+ public void scaleAdd(float s, Vector2D t1) {
+ this.x = (int)(s * this.x + t1.getX());
+ this.y = (int)(s * this.y + t1.getY());
+ }
+
+ @Override
+ public void sub(Point2D t1, Vector2D t2) {
+ this.x = (int)(t1.getX() - t1.getX());
+ this.y = (int)(t1.getY() - t1.getY());
+ }
+
+ @Override
+ public void sub(Vector2D t1) {
+ this.x = (int)(this.x - t1.getX());
+ this.y = (int)(this.y - t1.getY());
+ }
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Rectangle2i.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Rectangle2i.java
new file mode 100644
index 000000000..034e01d0c
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Rectangle2i.java
@@ -0,0 +1,720 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2011 Janus Core Developers
+ * Copyright (C) 2012-13 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.discrete;
+
+import java.util.Iterator;
+import java.util.NoSuchElementException;
+
+import org.arakhne.afc.math.geometry.PathWindingRule;
+import org.arakhne.afc.math.geometry2d.Point2D;
+import org.arakhne.afc.math.geometry2d.continuous.Transform2D;
+
+
+
+/** 2D rectangle with integer coordinates.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class Rectangle2i extends AbstractRectangularShape2i {
+
+ private static final long serialVersionUID = 9061018868216880896L;
+
+ /** Replies if two rectangles are intersecting.
+ *
+ * @param x1 is the first corner of the first rectangle.
+ * @param y1 is the first corner of the first rectangle.
+ * @param x2 is the second corner of the first rectangle.
+ * @param y2 is the second corner of the first rectangle.
+ * @param x3 is the first corner of the second rectangle.
+ * @param y3 is the first corner of the second rectangle.
+ * @param x4 is the second corner of the second rectangle.
+ * @param y4 is the second corner of the second rectangle.
+ * @return true if the two shapes are intersecting; otherwise
+ * false
+ */
+ public static boolean intersectsRectangleRectangle(int x1, int y1, int x2, int y2, int x3, int y3, int x4, int y4) {
+ assert(x1<=x2);
+ assert(y1<=y2);
+ assert(x3<=x4);
+ assert(y3<=y4);
+ return x2 > x3
+ &&
+ x1 < x4
+ &&
+ y2 > y3
+ &&
+ y1 < y4;
+ }
+
+ private static int code(int x, int y, int minx, int miny, int maxx, int maxy) {
+ int code = 0;
+ if (xmaxx) code |= 0x4;
+ if (ymaxy) code |= 0x1;
+ return code;
+ }
+
+ /** Replies if a rectangle is intersecting a segment.
+ *
+ * The intersection test is partly based on the Cohen-Sutherland
+ * classification of the segment.
+ * This classification permits to detect the base cases;
+ * and to run a clipping-like algorithm for the intersection
+ * detection.
+ *
+ * @param x1 is the first corner of the rectangle.
+ * @param y1 is the first corner of the rectangle.
+ * @param x2 is the second corner of the rectangle.
+ * @param y2 is the second corner of the rectangle.
+ * @param x3 is the first point of the segment.
+ * @param y3 is the first point of the segment.
+ * @param x4 is the second point of the segment.
+ * @param y4 is the second point of the segment.
+ * @return true if the two shapes are intersecting; otherwise
+ * false
+ */
+ public static boolean intersectsRectangleSegment(int x1, int y1, int x2, int y2, int x3, int y3, int x4, int y4) {
+ int c1 = code(x3, y3, x1, y1, x2, y2);
+ int c2 = code(x4, y4, x1, y1, x2, y2);
+
+ if (c1==0x0 || c2==0x0) return true;
+ if ((c1&c2)!=0x0) return false;
+
+ int sx1 = x3;
+ int sy1 = y3;
+ int sx2 = x4;
+ int sy2 = y4;
+
+ Point2i pts = new Point2i();
+ Segment2i.LineIterator iterator = new Segment2i.LineIterator(sx1, sy1, sx2, sy2);
+
+ while (iterator.hasNext() && c1!=0x0 && c2!=0x0 && (c1&c2)==0) {
+ if ((c1&0x1)!=0) {
+ do {
+ iterator.next(pts);
+ sy1 = pts.y();
+ }
+ while (iterator.hasNext() && sy1!=y2);
+ if (sy1!=y2) return false;
+ sx1 = pts.x();
+ }
+ else if ((c1&0x2)!=0) {
+ do {
+ iterator.next(pts);
+ sy1 = pts.y();
+ }
+ while (iterator.hasNext() && sy1!=y1);
+ if (sy1!=y1) return false;
+ sx1 = pts.x();
+ }
+ else if ((c1&0x4)!=0) {
+ do {
+ iterator.next(pts);
+ sx1 = pts.x();
+ }
+ while (iterator.hasNext() && sx1!=x2);
+ if (sx1!=x2) return false;
+ sy1 = pts.y();
+ }
+ else {
+ do {
+ iterator.next(pts);
+ sx1 = pts.x();
+ }
+ while (iterator.hasNext() && sx1!=x1);
+ if (sx1!=x1) return false;
+ sy1 = pts.y();
+ }
+ c1 = code(sx1, sy1, x1, y1, x2, y2);
+ }
+
+ return c1==0x0 || c2==0x0;
+ }
+
+ /** Compute the closest point on the rectangle from the given point.
+ *
+ * @param minx is the x-coordinate of the lowest coordinate of the rectangle.
+ * @param miny is the y-coordinate of the lowest coordinate of the rectangle.
+ * @param maxx is the x-coordinate of the highest coordinate of the rectangle.
+ * @param maxy is the y-coordinate of the highest coordinate of the rectangle.
+ * @param px is the x-coordinate of the point.
+ * @param py is the y-coordinate of the point.
+ * @return the closest point.
+ */
+ public static Point2i computeClosestPoint(int minx, int miny, int maxx, int maxy, int px, int py) {
+ int x;
+ int same = 0;
+ if (pxmaxx) {
+ x = maxx;
+ }
+ else {
+ x = px;
+ ++same;
+ }
+ int y;
+ if (pymaxy) {
+ y = maxy;
+ }
+ else {
+ y = py;
+ ++same;
+ }
+ if (same==2) {
+ return new Point2i(px,py);
+ }
+ return new Point2i(x,y);
+ }
+
+ /**
+ */
+ public Rectangle2i() {
+ //
+ }
+
+ /**
+ * @param min is the min corner of the rectangle.
+ * @param max is the max corner of the rectangle.
+ */
+ public Rectangle2i(Point2i min, Point2i max) {
+ setFromCorners(min.x(), min.y(), max.x(), max.y());
+ }
+
+ /**
+ * @param x
+ * @param y
+ * @param width
+ * @param height
+ */
+ public Rectangle2i(int x, int y, int width, int height) {
+ setFromCorners(x, y, x+width, y+height);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public Rectangle2i toBoundingBox() {
+ return clone();
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public float distanceSquared(Point2D p) {
+ int dx;
+ if (p.x()this.maxx) {
+ dx = p.x() - this.maxx;
+ }
+ else {
+ dx = 0;
+ }
+ int dy;
+ if (p.y()this.maxy) {
+ dy = p.y() - this.maxy;
+ }
+ else {
+ dy = 0;
+ }
+ return dx*dx+dy*dy;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public float distanceL1(Point2D p) {
+ int dx;
+ if (p.x()this.maxx) {
+ dx = p.x() - this.maxx;
+ }
+ else {
+ dx = 0;
+ }
+ int dy;
+ if (p.y()this.maxy) {
+ dy = p.y() - this.maxy;
+ }
+ else {
+ dy = 0;
+ }
+ return dx + dy;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public float distanceLinf(Point2D p) {
+ int dx;
+ if (p.x()this.maxx) {
+ dx = p.x() - this.maxx;
+ }
+ else {
+ dx = 0;
+ }
+ int dy;
+ if (p.y()this.maxy) {
+ dy = p.y() - this.maxy;
+ }
+ else {
+ dy = 0;
+ }
+ return Math.max(dx, dy);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public Point2i getClosestPointTo(Point2D p) {
+ return computeClosestPoint(this.minx, this.miny, this.maxx, this.maxy, p.x(), p.y());
+ }
+
+ @Override
+ public boolean intersects(Rectangle2i s) {
+ return intersectsRectangleRectangle(
+ getMinX(), getMinY(),
+ getMaxX(), getMaxY(),
+ s.getMinX(), s.getMinY(),
+ s.getMaxX(), s.getMaxY());
+ }
+
+ @Override
+ public boolean intersects(Circle2i s) {
+ return Circle2i.intersectsCircleRectangle(
+ s.getX(), s.getY(),
+ s.getRadius(),
+ getMinX(), getMinY(),
+ getMaxX(), getMaxY());
+ }
+
+ @Override
+ public boolean intersects(Segment2i s) {
+ return intersectsRectangleSegment(
+ this.minx, this.miny, this.maxx, this.maxy,
+ s.getX1(), s.getY1(), s.getX2(), s.getY2());
+ }
+
+ @Override
+ public PathIterator2i getPathIterator(Transform2D transform) {
+ if (transform==null) {
+ return new CopyPathIterator(
+ getMinX(), getMinY(),
+ getMaxX(), getMaxY());
+ }
+ return new TransformPathIterator(
+ getMinX(), getMinY(),
+ getMaxX(), getMaxY(),
+ transform);
+ }
+
+ @Override
+ public boolean contains(int x, int y) {
+ return x>=this.minx && x<=this.maxx && y>=this.miny && y<=this.maxy;
+ }
+
+ @Override
+ public boolean contains(Rectangle2i r) {
+ return r.getMinX()>=getMinX() && r.getMaxX()<=getMaxX()
+ && r.getMinY()>=getMinY() && r.getMaxY()<=getMaxY();
+ }
+
+ /** Replies the points on the bounds of the rectangle starting from
+ * the top border.
+ *
+ * @return the points on the bounds of the rectangle.
+ */
+ @Override
+ public Iterator getPointIterator() {
+ return getPointIterator(Side.TOP);
+ }
+
+ /** Replies the points on the bounds of the rectangle.
+ *
+ * @param startingBorder is the first border to reply.
+ * @return the points on the bounds of the rectangle.
+ */
+ public Iterator getPointIterator(Side startingBorder) {
+ return new RectangleSideIterator(this.minx, this.miny, this.maxx, this.maxy, startingBorder);
+ }
+
+ /** Iterator on the path elements of the rectangle.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private static class CopyPathIterator implements PathIterator2i {
+
+ private final int x1;
+ private final int y1;
+ private final int x2;
+ private final int y2;
+ private int index = 0;
+
+ /**
+ * @param x1
+ * @param y1
+ * @param x2
+ * @param y2
+ */
+ public CopyPathIterator(int x1, int y1, int x2, int y2) {
+ this.x1 = Math.min(x1, x2);
+ this.y1 = Math.min(y1, y2);
+ this.x2 = Math.max(x1, x2);
+ this.y2 = Math.max(y1, y2);
+ if (Math.abs(this.x1-this.x2)<=0 || Math.abs(this.y1-this.y2)<=0) {
+ this.index = 6;
+ }
+ }
+
+ @Override
+ public boolean hasNext() {
+ return this.index<=5;
+ }
+
+ @Override
+ public PathElement2i next() {
+ int idx = this.index;
+ ++this.index;
+ switch(idx) {
+ case 0:
+ return new PathElement2i.MovePathElement2i(
+ this.x1, this.y1);
+ case 1:
+ return new PathElement2i.LinePathElement2i(
+ this.x1, this.y1,
+ this.x2, this.y1);
+ case 2:
+ return new PathElement2i.LinePathElement2i(
+ this.x2, this.y1,
+ this.x2, this.y2);
+ case 3:
+ return new PathElement2i.LinePathElement2i(
+ this.x2, this.y2,
+ this.x1, this.y2);
+ case 4:
+ return new PathElement2i.LinePathElement2i(
+ this.x1, this.y2,
+ this.x1, this.y1);
+ case 5:
+ return new PathElement2i.ClosePathElement2i(
+ this.x1, this.y1,
+ this.x1, this.y1);
+ default:
+ throw new NoSuchElementException();
+ }
+ }
+
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public PathWindingRule getWindingRule() {
+ return PathWindingRule.NON_ZERO;
+ }
+
+ }
+
+ /** Iterator on the path elements of the rectangle.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private static class TransformPathIterator implements PathIterator2i {
+
+ private final Transform2D transform;
+ private final int x1;
+ private final int y1;
+ private final int x2;
+ private final int y2;
+ private int index = 0;
+
+ private final Point2D p1 = new Point2i();
+ private final Point2D p2 = new Point2i();
+
+ /**
+ * @param x1
+ * @param y1
+ * @param x2
+ * @param y2
+ * @param transform
+ */
+ public TransformPathIterator(int x1, int y1, int x2, int y2, Transform2D transform) {
+ this.transform = transform;
+ this.x1 = Math.min(x1, x2);
+ this.y1 = Math.min(y1, y2);
+ this.x2 = Math.max(x1, x2);
+ this.y2 = Math.max(y1, y2);
+ if (Math.abs(this.x1-this.x2)<=0 || Math.abs(this.y1-this.y2)<=0) {
+ this.index = 6;
+ }
+ }
+
+ @Override
+ public boolean hasNext() {
+ return this.index<=5;
+ }
+
+ @Override
+ public PathElement2i next() {
+ int idx = this.index;
+ ++this.index;
+ switch(idx) {
+ case 0:
+ this.p2.set(this.x1, this.y1);
+ if (this.transform!=null) {
+ this.transform.transform(this.p2);
+ }
+ return new PathElement2i.MovePathElement2i(
+ this.p2.x(), this.p2.y());
+ case 1:
+ this.p1.set(this.p2);
+ this.p2.set(this.x2, this.y1);
+ if (this.transform!=null) {
+ this.transform.transform(this.p2);
+ }
+ return new PathElement2i.LinePathElement2i(
+ this.p1.x(), this.p1.y(),
+ this.p2.x(), this.p2.y());
+ case 2:
+ this.p1.set(this.p2);
+ this.p2.set(this.x2, this.y2);
+ if (this.transform!=null) {
+ this.transform.transform(this.p2);
+ }
+ return new PathElement2i.LinePathElement2i(
+ this.p1.x(), this.p1.y(),
+ this.p2.x(), this.p2.y());
+ case 3:
+ this.p1.set(this.p2);
+ this.p2.set(this.x1, this.y2);
+ if (this.transform!=null) {
+ this.transform.transform(this.p2);
+ }
+ return new PathElement2i.LinePathElement2i(
+ this.p1.x(), this.p1.y(),
+ this.p2.x(), this.p2.y());
+ case 4:
+ this.p1.set(this.p2);
+ this.p2.set(this.x1, this.y1);
+ if (this.transform!=null) {
+ this.transform.transform(this.p2);
+ }
+ return new PathElement2i.LinePathElement2i(
+ this.p1.x(), this.p1.y(),
+ this.p2.x(), this.p2.y());
+ case 5:
+ return new PathElement2i.ClosePathElement2i(
+ this.p2.x(), this.p2.y(),
+ this.p2.x(), this.p2.y());
+ default:
+ throw new NoSuchElementException();
+ }
+ }
+
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public PathWindingRule getWindingRule() {
+ return PathWindingRule.NON_ZERO;
+ }
+
+ }
+
+ /** Sides of a rectangle.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ public static enum Side {
+ /** Top.
+ */
+ TOP,
+ /** Right.
+ */
+ RIGHT,
+ /** Bottom.
+ */
+ BOTTOM,
+ /** Left.
+ */
+ LEFT;
+ }
+
+ /** Iterates on points on the sides of a rectangle.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private static class RectangleSideIterator implements Iterator {
+
+ private final int x0;
+ private final int y0;
+ private final int x1;
+ private final int y1;
+ private final Side firstSide;
+
+ private Side currentSide;
+ private int i;
+
+ /**
+ * @param x1
+ * @param y1
+ * @param x2
+ * @param y2
+ * @param firstSide
+ */
+ public RectangleSideIterator(int x1, int y1, int x2, int y2, Side firstSide) {
+ assert(x1<=x2 && y1<=y2);
+ this.firstSide = firstSide;
+ this.x0 = x1;
+ this.y0 = y1;
+ this.x1 = x2;
+ this.y1 = y2;
+
+ this.currentSide = (x2>x1 && y2>y1) ? this.firstSide : null;
+ this.i = 0;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean hasNext() {
+ return this.currentSide!=null;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public Point2i next() {
+ int x, y;
+
+ switch(this.currentSide) {
+ case TOP:
+ x = this.x0+this.i;
+ y = this.y0;
+ break;
+ case RIGHT:
+ x = this.x1;
+ y = this.y0+this.i+1;
+ break;
+ case BOTTOM:
+ x = this.x1-this.i-1;
+ y = this.y1;
+ break;
+ case LEFT:
+ x = this.x0;
+ y = this.y1-this.i-1;
+ break;
+ default:
+ throw new NoSuchElementException();
+ }
+
+ ++ this.i;
+ Side newSide = null;
+
+ switch(this.currentSide) {
+ case TOP:
+ if (x>=this.x1) {
+ newSide = Side.RIGHT;
+ this.i = 0;
+ }
+ break;
+ case RIGHT:
+ if (y>=this.y1) {
+ newSide = Side.BOTTOM;
+ this.i = 0;
+ }
+ break;
+ case BOTTOM:
+ if (x<=this.x0) {
+ newSide = Side.LEFT;
+ this.i = 0;
+ }
+ break;
+ case LEFT:
+ if (y<=this.y0+1) {
+ newSide = Side.TOP;
+ this.i = 0;
+ }
+ break;
+ default:
+ throw new NoSuchElementException();
+ }
+
+ if (newSide!=null) {
+ this.currentSide = (this.firstSide==newSide) ? null : newSide;
+ }
+
+ return new Point2i(x,y);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ } // class RectangleIterator
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Segment2i.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Segment2i.java
new file mode 100644
index 000000000..88941b931
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Segment2i.java
@@ -0,0 +1,1267 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.discrete;
+
+import java.util.Iterator;
+import java.util.NoSuchElementException;
+
+import org.arakhne.afc.math.MathConstants;
+import org.arakhne.afc.math.geometry.GeometryUtil;
+import org.arakhne.afc.math.geometry.PathWindingRule;
+import org.arakhne.afc.math.geometry2d.Point2D;
+import org.arakhne.afc.math.geometry2d.continuous.Transform2D;
+
+
+
+/** 2D line segment with integer coordinates.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class Segment2i extends AbstractShape2i {
+
+ /**
+ * Calculates the number of times the line from (x0,y0) to (x1,y1)
+ * crosses the ellipse (ex0,ey0) to (ex1,ey1) extending to the right.
+ *
+ * When the line (x0;y0) to (x1;y1) is crossing one of the up or
+ * bottom borders of the shadow of the circle, the crossings
+ * count is increased or decreased, depending if the line is
+ * going down or up, respectively.
+ * In the following figure, the circle is represented.
+ * The "shadow" is the projection of the circle on the right.
+ * The red lines represent the up and bottom borders.
+ *
+ * 
+ *
+ *
+ * @param crossings is the initial value for the number of crossings.
+ * @param cx is the center of the circle to extend.
+ * @param cy is the center of the circle to extend.
+ * @param radius is the radius of the circle to extend.
+ * @param x0 is the first point of the line.
+ * @param y0 is the first point of the line.
+ * @param x1 is the second point of the line.
+ * @param y1 is the secondpoint of the line.
+ * @return the crossing, or {@link MathConstants#SHAPE_INTERSECTS}.
+ */
+ public static int computeCrossingsFromCircle(
+ int crossings,
+ int cx, int cy,
+ int radius,
+ int x0, int y0,
+ int x1, int y1) {
+ int numCrosses = crossings;
+
+ int xmin = cx - Math.abs(radius);
+ int xmax = cx + Math.abs(radius);
+ int ymin = cy - Math.abs(radius);
+ int ymax = cy + Math.abs(radius);
+
+ // The line is entirely on the top or on the bottom of the shadow
+ if (y0ymax && y1>ymax) return numCrosses;
+ // The line is entierly on the left of the shadow.
+ if (x0xmax && x1>xmax) {
+ // The line is entirely at the right of the center of the shadow.
+ // We may use the standard "rectangle" crossing computation
+ if (y0ymax) ++numCrosses;
+ }
+ else {
+ if (y1ymax) --numCrosses;
+ }
+ }
+ else if (Circle2i.intersectsCircleSegment(
+ cx, cy, radius,
+ x0, y0, x1, y1)) {
+ return MathConstants.SHAPE_INTERSECTS;
+ }
+ else {
+ numCrosses = computeCrossingsFromPoint(numCrosses, cx, ymin, x0, y0, x1, y1, true, false);
+ numCrosses = computeCrossingsFromPoint(numCrosses, cx, ymax, x0, y0, x1, y1, false, true);
+ }
+
+ return numCrosses;
+ }
+
+ /**
+ * Calculates the number of times the line from (x0,y0) to (x1,y1)
+ * crosses the segment (sx0,sy0) to (sx1,sy1) extending to the right.
+ *
+ * When the line (x0;y0) to (x1;y1) is crossing one of the up or
+ * bottom borders of the shadow of the segment, the crossings
+ * count is increased or decreased, depending if the line is
+ * going down or up, respectively.
+ * In the following figure, the segment is represented.
+ * The "shadow" is the projection of the segment on the right.
+ * The red lines represent the up and bottom borders.
+ *
+ * 
+ *
+ *
+ * @param crossings is the initial value for the number of crossings.
+ * @param sx1 is the first point of the segment to extend.
+ * @param sy1 is the first point of the segment to extend.
+ * @param sx2 is the second point of the segment to extend.
+ * @param sy2 is the second point of the segment to extend.
+ * @param x0 is the first point of the line.
+ * @param y0 is the first point of the line.
+ * @param x1 is the second point of the line.
+ * @param y1 is the secondpoint of the line.
+ * @return the crossing, or {@link MathConstants#SHAPE_INTERSECTS}.
+ */
+ public static int computeCrossingsFromSegment(
+ int crossings,
+ int sx1, int sy1,
+ int sx2, int sy2,
+ int x0, int y0,
+ int x1, int y1) {
+ /* CAUTION:
+ * --------
+ * In the comment of this function, it is assumed that y0<=y1,
+ * to simplify the explanations.
+ * The source code is handled y0<=y1 and y0>y1.
+ */
+ int numCrosses = crossings;
+
+ int xmin = Math.min(sx1, sx2);
+ int xmax = Math.max(sx1, sx2);
+ int ymin = Math.min(sy1, sy2);
+ int ymax = Math.max(sy1, sy2);
+
+ // The line is entirely below or up to the shadow of the segment
+ if (y0ymax && y1>ymax) return numCrosses;
+ // The line is entirely at te left of the segment
+ if (x0xmax && x1>xmax) {
+ // The line is entirely at the right of the shadow
+ if (y0ymax) ++numCrosses;
+ }
+ else {
+ if (y1ymax) --numCrosses;
+ }
+ }
+ else if (intersectsSegmentSegment(x0, y0, x1, y1, sx1, sy1, sx2, sy2, true, true, null)) {
+ return MathConstants.SHAPE_INTERSECTS;
+ }
+ else {
+ // The line is intersectly partly the bounding rectangle of the segment.
+ // We must determine on which side of the segment the points of the line are.
+ // If side1 is positive, the first point of the line is on the side of the shadow, relatively to the segment
+ // If it is negative, the first point is on the opposite side of the shadow, relatively to the segment.
+ // If it is nul, the point is on line colinear to the segment.
+ // Same for side2 and the second point of the line.
+ int side1, side2;
+ boolean firstIsTop = (sy1<=sy2);
+ if (firstIsTop) {
+ side1 = GeometryUtil.getPointSideOfLine(sx1, sy1, sx2, sy2, x0, y0, 0f);
+ side2 = GeometryUtil.getPointSideOfLine(sx1, sy1, sx2, sy2, x1, y1, 0f);
+ }
+ else {
+ side1 = GeometryUtil.getPointSideOfLine(sx2, sy2, sx1, sy1, x0, y0, 0f);
+ side2 = GeometryUtil.getPointSideOfLine(sx2, sy2, sx1, sy1, x1, y1, 0f);
+ }
+ if (side1>=0 || side2>=0) {
+ // At least one point is on the side of the shadow.
+ // Now we compute the intersection with the up and bottom borders.
+ // Intersection is obtained by computed the crossing value from
+ // the two points of the segment.
+ int n1, n2;
+ n1 = computeCrossingsFromPoint(0, sx1, sy1, x0, y0, x1, y1, firstIsTop, !firstIsTop);
+ n2 = computeCrossingsFromPoint(0, sx2, sy2, x0, y0, x1, y1, !firstIsTop, firstIsTop);
+
+ // The total crossing value must be updated with the border's crossing values.
+ numCrosses += n1 + n2;
+ }
+ }
+
+ return numCrosses;
+ }
+
+ /**
+ * Accumulate the number of times the line crosses the shadow
+ * extending to the right of the rectangle.
+ *
+ * When the line (x0;y0) to (x1;y1) is intersecting the rectangle,
+ * the value {@link MathConstants#SHAPE_INTERSECTS} is returned.
+ * When the line (x0;y0) to (x1;y1) is crossing one of the up or
+ * bottom borders of the shadow of the rectangle, the crossings
+ * count is increased or decreased, depending if the line is
+ * going down or up, respectively.
+ * In the following figure, the rectangle is represented.
+ * The "shadow" is the projection of the rectangle on the right.
+ * The red lines represent the up and bottom borders.
+ *
+ * 
+ *
+ *
+ * @param crossings is the initial value for the number of crossings.
+ * @param rxmin is the first corner of the rectangle.
+ * @param rymin is the first corner of the rectangle.
+ * @param rxmax is the second corner of the rectangle.
+ * @param rymax is the second corner of the rectangle.
+ * @param x0 is the first point of the line.
+ * @param y0 is the first point of the line.
+ * @param x1 is the second point of the line.
+ * @param y1 is the secondpoint of the line.
+ * @return the crossing, or {@link MathConstants#SHAPE_INTERSECTS}.
+ */
+ public static int computeCrossingsFromRect(
+ int crossings,
+ int rxmin, int rymin,
+ int rxmax, int rymax,
+ int x0, int y0,
+ int x1, int y1) {
+ int numCrosses = crossings;
+ // The line is horizontal, only SHAPE_INTERSECT may be replies
+ if (y0==y1) {
+ if (y0>=rymin && y0<=rymax &&
+ (x0>=rxmin || x1>=rxmin) &&
+ (x0<=rxmax || x1<=rxmax)) {
+ return MathConstants.SHAPE_INTERSECTS;
+ }
+ return crossings;
+ }
+ // The line is entirely at the top or at the bottom of the rectangle
+ if (y0 > rymax && y1 > rymax) return numCrosses;
+ if (y0 < rymin && y1 < rymin) return numCrosses;
+ // The line is entirely on the left of the rectangle
+ if (x0 < rxmin && x1 < rxmin) return numCrosses;
+
+ if (x0 > rxmax && x1 > rxmax) {
+ // Line is entirely to the right of the rect
+ // and the vertical ranges of the two overlap by a non-empty amount
+ // Thus, this line segment is partially in the "right-shadow"
+ // Path may have done a complete crossing
+ // Or path may have entered or exited the right-shadow
+ if (y0 < y1) {
+ // y-increasing line segment...
+ // We know that y0 < rymax and y1 > rymin
+ if (y0 < rymin) ++numCrosses;
+ if (y1 > rymax) ++numCrosses;
+ }
+ else if (y1 < y0) {
+ // y-decreasing line segment...
+ // We know that y1 < rymax and y0 > rymin
+ if (y1 < rymin) --numCrosses;
+ if (y0 > rymax) --numCrosses;
+ }
+ }
+ else {
+ // Remaining case:
+ // Both x and y ranges overlap by a non-empty amount
+ // First do trivial INTERSECTS rejection of the cases
+ // where one of the endpoints is inside the rectangle.
+ if ((x0 > rxmin && x0 < rxmax && y0 > rymin && y0 < rymax) ||
+ (x1 > rxmin && x1 < rxmax && y1 > rymin && y1 < rymax)) {
+ return MathConstants.SHAPE_INTERSECTS;
+ }
+
+ // Otherwise calculate the y intercepts and see where
+ // they fall with respect to the rectangle
+ LineIterator iterator;
+ int ymaxline;
+ if (y0<=y1) {
+ iterator = new LineIterator(x0, y0, x1, y1);
+ ymaxline = y1;
+ }
+ else {
+ iterator = new LineIterator(x1, y1, x0, y0);
+ ymaxline = y0;
+ }
+ Point2i p = new Point2i();
+ Integer xintercept1 = null;
+ Integer xintercept2 = null;
+ boolean cont = true;
+ while (iterator.hasNext() && cont) {
+ iterator.next(p);
+ if (p.y()==rymin && (xintercept1==null || xintercept1>p.x())) {
+ xintercept1 = p.x();
+ }
+ if (p.y()==rymax && (xintercept2==null || xintercept2>p.x())) {
+ xintercept2 = p.x();
+ }
+ cont = (p.y()<=ymaxline);
+ }
+
+ if (xintercept1!=null && xintercept2!=null) {
+ if (xintercept1rxmax && xintercept2>rxmax) {
+ // the intersection points are entirely on the right
+ if (y0 < y1) {
+ // y-increasing line segment...
+ // We know that y0 < rymax and y1 > rymin
+ if (y0 <= rymin) ++numCrosses;
+ if (y1 >= rymax) ++numCrosses;
+ }
+ else if (y1 < y0) {
+ // y-decreasing line segment...
+ // We know that y1 < rymax and y0 > rymin
+ if (y1 <= rymin) --numCrosses;
+ if (y0 >= rymax) --numCrosses;
+ }
+ }
+ else {
+ return MathConstants.SHAPE_INTERSECTS;
+ }
+ }
+ else if (xintercept1!=null) {
+ // Only the top line of the rectangle is intersecting the segment
+ if (xintercept1rxmax) {
+ if (y0 < y1) {
+ // y-increasing line segment...
+ // We know that y0 < rymax and y1 > rymin
+ if (y0 <= rymin) ++numCrosses;
+ }
+ else if (y1 < y0) {
+ // y-decreasing line segment...
+ // We know that y1 < rymax and y0 > rymin
+ if (y1 <= rymin) --numCrosses;
+ }
+ }
+ else {
+ return MathConstants.SHAPE_INTERSECTS;
+ }
+ }
+ else if (xintercept2!=null) {
+ // Only the bottom line of the rectangle is intersecting the segment
+ if (xintercept2rxmax) {
+ if (y0 < y1) {
+ // y-increasing line segment...
+ // We know that y0 < rymax and y1 > rymin
+ if (y0 <= rymax) ++numCrosses;
+ }
+ else if (y1 < y0) {
+ // y-decreasing line segment...
+ // We know that y1 < rymax and y0 > rymin
+ if (y1 <= rymax) --numCrosses;
+ }
+ }
+ else {
+ return MathConstants.SHAPE_INTERSECTS;
+ }
+ }
+ }
+
+ return numCrosses;
+ }
+
+ /**
+ * Calculates the number of times the line from (x0,y0) to (x1,y1)
+ * crosses the up/bottom borders of the ray extending to the right from (px,py).
+ * +x is returned for a crossing where the Y coordinate is increasing.
+ * -x is returned for a crossing where the Y coordinate is decreasing.
+ * x is the number of border crossed by the lines.
+ *
+ * The borders of the segment are the two side limits between the cells covered by the segment
+ * and the adjacents cells (not covered by the segment).
+ * In the following figure, the point (px;py) is represented.
+ * The "shadow line" is the projection of (px;py) on the right.
+ * The red lines represent the up and bottom borders.
+ *
+ * 
+ *
+ *
+ * @param crossing is the initial value of the crossing.
+ * @param px is the reference point to test.
+ * @param py is the reference point to test.
+ * @param x0 is the first point of the line.
+ * @param y0 is the first point of the line.
+ * @param x1 is the second point of the line.
+ * @param y1 is the secondpoint of the line.
+ * @return the crossing, {@link MathConstants#SHAPE_INTERSECTS}
+ */
+ public static int computeCrossingsFromPoint(
+ int crossing,
+ int px, int py,
+ int x0, int y0,
+ int x1, int y1) {
+ return computeCrossingsFromPoint(crossing, px, py, x0, y0, x1, y1, true, true);
+ }
+
+ /**
+ * Calculates the number of times the line from (x0,y0) to (x1,y1)
+ * crosses the up/bottom borders of the ray extending to the right from (px,py).
+ * +x is returned for a crossing where the Y coordinate is increasing.
+ * -x is returned for a crossing where the Y coordinate is decreasing.
+ * x is the number of border crossed by the lines.
+ *
+ * The borders of the segment are the two side limits between the cells covered by the segment
+ * and the adjacents cells (not covered by the segment).
+ * In the following figure, the point (px;py) is represented.
+ * The "shadow line" is the projection of (px;py) on the right.
+ * The red lines represent the up and bottom borders.
+ *
+ *
+ *
+ *
+ * @param crossing is the initial value of the crossing.
+ * @param px is the reference point to test.
+ * @param py is the reference point to test.
+ * @param x0 is the first point of the line.
+ * @param y0 is the first point of the line.
+ * @param x1 is the second point of the line.
+ * @param y1 is the secondpoint of the line.
+ * @param enableTopBorder indicates if the top border must be enabled in the crossing computation.
+ * @param enableBottomBorder indicates if the bottom border must be enabled in the crossing computation.
+ * @return the crossing; or {@link MathConstants#SHAPE_INTERSECTS} if the segment is on the point.
+ */
+ public static int computeCrossingsFromPoint(
+ int crossing,
+ int px, int py,
+ int x0, int y0,
+ int x1, int y1,
+ boolean enableTopBorder,
+ boolean enableBottomBorder) {
+ // The line is horizontal, impossible to intersect the borders.
+ if (y0==y1) return crossing;
+ // The line does cross the shadow line
+ if (pyy0 && py>y1) return crossing;
+ // The line is entirely on the left of the point
+ if (px>x0 && px>x1) return crossing;
+
+ // General case: try to detect crossing
+
+ LineIterator iterator = new LineIterator(x0, y0, x1, y1);
+
+ Point2i p = new Point2i();
+ while (iterator.hasNext()) {
+ iterator.next(p);
+ if (p.y()==py) {
+ if (p.x()==px)
+ return MathConstants.SHAPE_INTERSECTS;
+ if (p.x()>px) {
+ // Found an intersection
+ int numCrosses = crossing;
+ if (y0<=y1) {
+ if (y0py && enableBottomBorder) ++numCrosses;
+ }
+ else {
+ if (y0>py && enableBottomBorder) --numCrosses;
+ if (y1true if the two shapes are intersecting; otherwise
+ * false
+ */
+ public static boolean intersectsSegmentSegment(int x1, int y1, int x2, int y2, int x3, int y3, int x4, int y4) {
+ int side1 = GeometryUtil.getPointSideOfLine(x1, y1, x2, y2, x3, y3, 0f);
+ int side2 = GeometryUtil.getPointSideOfLine(x1, y1, x2, y2, x4, y4, 0f);
+ if ((side1*side2)<=0) {
+ return intersectsSegmentSegment1(x1, y1, x2, y2, x3, y3, x4, y4, true, true, null)!=0;
+ }
+ return false;
+ }
+
+ /** Replies if two segments are intersecting pixel per pixel.
+ * This function does not determine if the segments' lines
+ * are intersecting because using the pixel-based test.
+ * This function uses the pixels of the segments that are
+ * computed according to a Bresenham line algorithm.
+ *
+ * @param x1 is the first point of the first segment.
+ * @param y1 is the first point of the first segment.
+ * @param x2 is the second point of the first segment.
+ * @param y2 is the second point of the first segment.
+ * @param x3 is the first point of the second segment.
+ * @param y3 is the first point of the second segment.
+ * @param x4 is the second point of the second segment.
+ * @param y4 is the second point of the second segment.
+ * @param enableThirdPoint indicates if the intersection on the third point is computed.
+ * @param enableFourthPoint indicates if the intersection on the fourth point is computed.
+ * @param intersectionPoint are the coordinates of the intersection, if exist.
+ * @return true if the two segments are intersecting; otherwise
+ * false
+ */
+ public static boolean intersectsSegmentSegment(int x1, int y1, int x2, int y2, int x3, int y3, int x4, int y4, boolean enableThirdPoint, boolean enableFourthPoint, Point2D intersectionPoint) {
+ return intersectsSegmentSegment1(x1, y1, x2, y2, x3, y3, x4, y4, enableThirdPoint, enableFourthPoint, intersectionPoint)!=0;
+ }
+
+ /** Replies if two segments are intersecting pixel per pixel.
+ * This function does not determine if the segments' lines
+ * are intersecting because using the pixel-based test.
+ * This function uses the pixels of the segments that are
+ * computed according to a Bresenham line algorithm.
+ *
+ * @param x1 is the first point of the first segment.
+ * @param y1 is the first point of the first segment.
+ * @param x2 is the second point of the first segment.
+ * @param y2 is the second point of the first segment.
+ * @param x3 is the first point of the second segment.
+ * @param y3 is the first point of the second segment.
+ * @param x4 is the second point of the second segment.
+ * @param y4 is the second point of the second segment.
+ * @param enableThirdPoint indicates if the intersection on the third point is computed.
+ * @param enableFourthPoint indicates if the intersection on the fourth point is computed.
+ * @param intersectionPoint are the coordinates of the intersection, if exist.
+ * @return an integer value; if 0 the two segments are not intersecting;
+ * 1 if the two segments are intersecting and the segment 2 has pixels on both
+ * sides of the segment 1; 2 if the segments are intersecting and the segment 2
+ * is only in one side of the segment 1.
+ */
+ static int intersectsSegmentSegment1(int x1, int y1, int x2, int y2, int x3, int y3, int x4, int y4, boolean enableThirdPoint, boolean enableFourthPoint, Point2D intersectionPoint) {
+ LineIterator it1;
+ if (x1max1) max1 = p1.y();
+ }
+ else {
+ break;
+ }
+ }
+
+ while (it2.hasNext()) {
+ it2.next(p2);
+ isFirstPointOfSecondSegment = false;
+ if (p2.x()==x) {
+ if (p2.y()max2) max2 = p2.y();
+ }
+ else {
+ break;
+ }
+ }
+
+ if (max2>=min1 && max1>=min2) {
+ if (intersectionPoint!=null) {
+ intersectionPoint.set(x, Math.max(min1, min2));
+ }
+ return !isFirstPointOfSecondSegment && (it2.hasNext()) ? 1 : 2;
+ }
+ }
+ while (it1.hasNext() && it2.hasNext());
+
+ if (enableFourthPoint && p1.equals(p2)) {
+ if (intersectionPoint!=null) {
+ intersectionPoint.set(p1);
+ }
+ return !isFirstPointOfSecondSegment && (it2.hasNext()) ? 1 : 2;
+ }
+ }
+
+ return 0;
+ }
+
+ private static final long serialVersionUID = -82425036308183925L;
+
+ /** X-coordinate of the first point. */
+ protected int ax = 0;
+ /** Y-coordinate of the first point. */
+ protected int ay = 0;
+ /** X-coordinate of the second point. */
+ protected int bx = 0;
+ /** Y-coordinate of the second point. */
+ protected int by = 0;
+
+ /**
+ */
+ public Segment2i() {
+ //
+ }
+
+ /**
+ * @param a
+ * @param b
+ */
+ public Segment2i(Point2D a, Point2D b) {
+ set(a, b);
+ }
+
+ /**
+ * @param x1
+ * @param y1
+ * @param x2
+ * @param y2
+ */
+ public Segment2i(int x1, int y1, int x2, int y2) {
+ set(x1, y1, x2, y2);
+ }
+
+ @Override
+ public void clear() {
+ this.ax = this.ay = this.bx = this.by = 0;
+ }
+
+ /**
+ * Replies if this segment is empty.
+ * The segment is empty when the two
+ * points are equal.
+ *
+ * @return true if the two points are
+ * equal.
+ */
+ @Override
+ public boolean isEmpty() {
+ return this.ax==this.bx && this.ay==this.by;
+ }
+
+ /** Change the line.
+ *
+ * @param x1
+ * @param y1
+ * @param x2
+ * @param y2
+ */
+ public void set(int x1, int y1, int x2, int y2) {
+ this.ax = x1;
+ this.ay = y1;
+ this.bx = x2;
+ this.by = y2;
+ }
+
+ /** Change the line.
+ *
+ * @param a
+ * @param b
+ */
+ public void set(Point2D a, Point2D b) {
+ this.ax = a.x();
+ this.ay = a.y();
+ this.bx = b.x();
+ this.by = b.y();
+ }
+
+ /** Replies the X of the first point.
+ *
+ * @return the x of the first point.
+ */
+ public int getX1() {
+ return this.ax;
+ }
+
+ /** Replies the Y of the first point.
+ *
+ * @return the y of the first point.
+ */
+ public int getY1() {
+ return this.ay;
+ }
+
+ /** Replies the X of the second point.
+ *
+ * @return the x of the second point.
+ */
+ public int getX2() {
+ return this.bx;
+ }
+
+ /** Replies the Y of the second point.
+ *
+ * @return the y of the second point.
+ */
+ public int getY2() {
+ return this.by;
+ }
+
+ /** Replies the first point.
+ *
+ * @return the first point.
+ */
+ public Point2D getP1() {
+ return new Point2i(this.ax, this.ay);
+ }
+
+ /** Replies the second point.
+ *
+ * @return the second point.
+ */
+ public Point2D getP2() {
+ return new Point2i(this.bx, this.by);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public Rectangle2i toBoundingBox() {
+ Rectangle2i r = new Rectangle2i();
+ r.setFromCorners(
+ this.ax,
+ this.ay,
+ this.bx,
+ this.by);
+ return r;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public float distanceSquared(Point2D p) {
+ Point2D closestPoint = getClosestPointTo(p);
+ return closestPoint.distanceSquared(p);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public float distanceL1(Point2D p) {
+ Point2D closestPoint = getClosestPointTo(p);
+ return closestPoint.distanceL1(p);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public float distanceLinf(Point2D p) {
+ Point2D closestPoint = getClosestPointTo(p);
+ return closestPoint.distanceLinf(p);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public boolean contains(int x, int y) {
+ if (x>=this.ax && x<=this.bx && y>=this.ay && y<=this.by) {
+ if (this.ax==this.bx || this.ay==this.by) {
+ return true;
+ }
+
+ int minDist = Integer.MAX_VALUE;
+ int d;
+ int a,b;
+ Point2i p = new Point2i();
+ LineIterator iterator = new LineIterator(this.ax, this.ay, this.bx, this.by);
+ while (iterator.hasNext()) {
+ iterator.next(p);
+ a = Math.abs(x-p.x());
+ b = Math.abs(y-p.y());
+ d = a*a + b*b ;
+ if (d==0) return true;
+ if (d>minDist) {
+ return false;
+ }
+ minDist = d;
+ }
+ }
+ return false;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public boolean contains(Rectangle2i r) {
+ return r.isEmpty() && contains(r.getMinX(), r.getMinY());
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public Point2i getClosestPointTo(Point2D p) {
+ return computeClosestPointTo(this.ax, this.ay, this.bx, this.by, p.x(), p.y());
+ }
+
+ /** Replies the closest point in a circle to a point.
+ *
+ * @param ax is the x-coordinate of the first point of the segment
+ * @param ay is the y-coordinate of the first point of the segment
+ * @param bx is the x-coordinate of the second point of the segment
+ * @param by is the y-coordinate of the second point of the segment
+ * @param px is the x-coordinate of the point
+ * @param py is the x-coordinate of the point
+ * @return the closest point in the segment to the point.
+ */
+ public static Point2i computeClosestPointTo(int ax, int ay, int bx, int by, int px, int py) {
+ // Special case
+ // 0 1 2 3 4 5 6 7 8 9 10
+ // 5) | | | | | | | | | | |X|
+ // 4) | | |O| | | | | |X|X| |
+ // 3) | | | | | | |X|X| | | |
+ // 2) | | | | |X|X| | | | | |
+ // 1) | | |X|X| | | | | | | |
+ // 0) |X|X| | | | | | | | | |
+ //
+ // The closest point to point O is (4;2) even
+ // if the distance is increasing between (2;1)
+ // and (4;2). The algo must take this special
+ // case into account.
+
+ int minDist = Integer.MAX_VALUE;
+ int d;
+ int a,b;
+ boolean oneBestFound = false;
+ Point2i solution = new Point2i(ax, ay);
+ Point2i cp = new Point2i();
+ LineIterator iterator = new LineIterator(ax, ay, bx, by);
+ while (iterator.hasNext()) {
+ iterator.next(cp);
+ a = Math.abs(px-cp.x());
+ b = Math.abs(py-cp.y());
+ d = a*a + b*b ;
+ if (d==0) {
+ // We are sure that the closest point was found
+ return cp;
+ }
+ if (d>minDist) {
+ // here we have found a good candidate, but
+ // but due to the rasterization the optimal solution
+ // may be one pixel after the already found.
+ // See the special case configuration at the beginning
+ // of this function.
+ if (oneBestFound) return solution;
+ oneBestFound = true;
+ }
+ else {
+ minDist = d;
+ solution.set(cp);
+ // here we have found a good candidate, but
+ // but due to the rasterization the optimal solution
+ // may be one pixel after the already found.
+ // See the special case configuration at the beginning
+ // of this function.
+ if (oneBestFound) return solution;
+ }
+ }
+ return solution;
+ }
+
+ @Override
+ public void translate(int dx, int dy) {
+ this.ax += dx;
+ this.ay += dy;
+ this.bx += dx;
+ this.by += dy;
+ }
+
+ @Override
+ public PathIterator2i getPathIterator(Transform2D transform) {
+ return new SegmentPathIterator(
+ this.ax, this.ay, this.bx, this.by,
+ transform);
+ }
+
+ /** Replies an iterator on the points of the segment.
+ *
+ * The Bresenham line algorithm is an algorithm which determines which points in
+ * an n-dimensional raster should be plotted in order to form a close
+ * approximation to a straight line between two given points. It is
+ * commonly used to draw lines on a computer screen, as it uses only
+ * integer addition, subtraction and bit shifting, all of which are
+ * very cheap operations in standard computer architectures. It is one of the
+ * earliest algorithms developed in the field of computer graphics. A minor extension
+ * to the original algorithm also deals with drawing circles.
+ *
+ * While algorithms such as Wu's algorithm are also frequently used in modern
+ * computer graphics because they can support antialiasing, the speed and
+ * simplicity of Bresenham's line algorithm mean that it is still important.
+ * The algorithm is used in hardware such as plotters and in the graphics
+ * chips of modern graphics cards. It can also be found in many software
+ * graphics libraries. Because the algorithm is very simple, it is often
+ * implemented in either the firmware or the hardware of modern graphics cards.
+ *
+ * @return an iterator on the points along the Bresenham line.
+ */
+ @Override
+ public Iterator getPointIterator() {
+ return new LineIterator(this.ax, this.ay, this.bx, this.by);
+ }
+
+ @Override
+ public boolean equals(Object obj) {
+ if (obj == this) {
+ return true;
+ }
+ if (obj instanceof Segment2i) {
+ Segment2i rr2d = (Segment2i) obj;
+ return ((this.ax == rr2d.getX1()) &&
+ (this.ay == rr2d.getY1()) &&
+ (this.bx == rr2d.getX2()) &&
+ (this.by == rr2d.getY2()));
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ long bits = 1L;
+ bits = 31L * bits + this.ax;
+ bits = 31L * bits + this.ay;
+ bits = 31L * bits + this.bx;
+ bits = 31L * bits + this.by;
+ return (int) (bits ^ (bits >> 32));
+ }
+
+ /** Transform the current segment.
+ * This function changes the current segment.
+ *
+ * @param transform is the affine transformation to apply.
+ * @see #createTransformedShape(Transform2D)
+ */
+ public void transform(Transform2D transform) {
+ Point2i p = new Point2i(this.ax, this.ay);
+ transform.transform(p);
+ this.ax = p.x();
+ this.ay = p.y();
+ p.set(this.bx, this.by);
+ transform.transform(p);
+ this.bx = p.x();
+ this.by = p.y();
+ }
+
+ @Override
+ public Shape2i createTransformedShape(Transform2D transform) {
+ Point2D p1 = transform.transform(this.ax, this.ay);
+ Point2D p2 = transform.transform(this.bx, this.by);
+ return new Segment2i(p1, p2);
+ }
+
+ @Override
+ public boolean intersects(Rectangle2i s) {
+ return Rectangle2i.intersectsRectangleSegment(
+ s.getMinX(), s.getMinY(),
+ s.getMaxX(), s.getMaxY(),
+ getX1(), getY1(),
+ getX2(), getY2());
+ }
+
+ @Override
+ public boolean intersects(Circle2i s) {
+ return Circle2i.intersectsCircleSegment(
+ s.getX(), s.getY(),
+ s.getRadius(),
+ getX1(), getY1(),
+ getX2(), getY2());
+ }
+
+ @Override
+ public boolean intersects(Segment2i s) {
+ return intersectsSegmentSegment(
+ getX1(), getY1(), getX2(), getY2(),
+ s.getX1(), s.getY1(), s.getX2(), s.getY2());
+ }
+
+ @Override
+ public String toString() {
+ StringBuilder b = new StringBuilder();
+ b.append("["); //$NON-NLS-1$
+ b.append(getX1());
+ b.append(";"); //$NON-NLS-1$
+ b.append(getY1());
+ b.append("|"); //$NON-NLS-1$
+ b.append(getX2());
+ b.append(";"); //$NON-NLS-1$
+ b.append(getY2());
+ b.append("]"); //$NON-NLS-1$
+ return b.toString();
+ }
+
+ /** Iterator on the path elements of the segment.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ private static class SegmentPathIterator implements PathIterator2i {
+
+ private final Point2D p1 = new Point2i();
+ private final Point2D p2 = new Point2i();
+ private final Transform2D transform;
+ private final int x1;
+ private final int y1;
+ private final int x2;
+ private final int y2;
+ private int index = 0;
+
+ /**
+ * @param x1
+ * @param y1
+ * @param x2
+ * @param y2
+ * @param transform
+ */
+ public SegmentPathIterator(int x1, int y1, int x2, int y2, Transform2D transform) {
+ this.transform = transform;
+ this.x1 = x1;
+ this.y1 = y1;
+ this.x2 = x2;
+ this.y2 = y2;
+ if (this.x1==this.x2 && this.y1==this.y2) {
+ this.index = 2;
+ }
+ }
+
+ @Override
+ public boolean hasNext() {
+ return this.index<=1;
+ }
+
+ @Override
+ public PathElement2i next() {
+ if (this.index>1) throw new NoSuchElementException();
+ int idx = this.index;
+ ++this.index;
+ switch(idx) {
+ case 0:
+ this.p2.set(this.x1, this.y1);
+ if (this.transform!=null) {
+ this.transform.transform(this.p2);
+ }
+ return new PathElement2i.MovePathElement2i(
+ this.p2.x(), this.p2.y());
+ case 1:
+ this.p1.set(this.p2);
+ this.p2.set(this.x2, this.y2);
+ if (this.transform!=null) {
+ this.transform.transform(this.p2);
+ }
+ return new PathElement2i.LinePathElement2i(
+ this.p1.x(), this.p1.y(),
+ this.p2.x(), this.p2.y());
+ default:
+ throw new NoSuchElementException();
+ }
+ }
+
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ @Override
+ public PathWindingRule getWindingRule() {
+ return PathWindingRule.NON_ZERO;
+ }
+
+ }
+
+ /** The Bresenham line algorithm is an algorithm which determines which points in
+ * an n-dimensional raster should be plotted in order to form a close
+ * approximation to a straight line between two given points. It is
+ * commonly used to draw lines on a computer screen, as it uses only
+ * integer addition, subtraction and bit shifting, all of which are
+ * very cheap operations in standard computer architectures. It is one of the
+ * earliest algorithms developed in the field of computer graphics. A minor extension
+ * to the original algorithm also deals with drawing circles.
+ *
+ * While algorithms such as Wu's algorithm are also frequently used in modern
+ * computer graphics because they can support antialiasing, the speed and
+ * simplicity of Bresenham's line algorithm mean that it is still important.
+ * The algorithm is used in hardware such as plotters and in the graphics
+ * chips of modern graphics cards. It can also be found in many software
+ * graphics libraries. Because the algorithm is very simple, it is often
+ * implemented in either the firmware or the hardware of modern graphics cards.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+ static class LineIterator implements Iterator {
+
+ private final boolean steep;
+ private final int ystep;
+ private final int xstep;
+ private final int deltax;
+ private final int deltay;
+ private final int x1;
+ private int y, x;
+ private int error;
+
+ /**
+ * @param x0 is the x-coordinate of the first point of the Bresenham line.
+ * @param y0 is the y-coordinate of the first point of the Bresenham line.
+ * @param x1 is the x-coordinate of the last point of the Bresenham line.
+ * @param y1 is the y-coordinate of the last point of the Bresenham line.
+ */
+ public LineIterator(int x0, int y0, int x1, int y1) {
+ int _x0 = x0;
+ int _y0 = y0;
+ int _x1 = x1;
+ int _y1 = y1;
+
+ this.steep = Math.abs(_y1 - _y0) > Math.abs(_x1 - _x0);
+
+ int swapv;
+ if (this.steep) {
+ //swap(x0, y0);
+ swapv = _x0;
+ _x0 = _y0;
+ _y0 = swapv;
+ //swap(x1, y1);
+ swapv = _x1;
+ _x1 = _y1;
+ _y1 = swapv;
+ }
+ /*if (_x0 > _x1) {
+ //swap(x0, x1);
+ swapv = _x0;
+ _x0 = _x1;
+ _x1 = swapv;
+ //swap(y0, y1);
+ swapv = _y0;
+ _y0 = _y1;
+ _y1 = swapv;
+ }*/
+
+ this.deltax = Math.abs(_x1 - _x0);
+ this.deltay = Math.abs(_y1 - _y0);
+ this.error = this.deltax / 2;
+ this.y = _y0;
+
+ if (_x0 < _x1) this.xstep = 1;
+ else this.xstep = -1;
+
+ if (_y0 < _y1) this.ystep = 1;
+ else this.ystep = -1;
+
+ this.x1 = _x1;
+ this.x = _x0;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean hasNext() {
+ return ((this.xstep>0) && (this.x <= this.x1))
+ ||((this.xstep<0) && (this.x1 <= this.x));
+ }
+
+ /** Replies the next point in the given parameter.
+ *
+ * @param p
+ */
+ public void next(Point2i p) {
+ if (this.steep) {
+ p.set(this.y, this.x);
+ }
+ else {
+ p.set(this.x, this.y);
+ }
+
+ this.error = this.error - this.deltay;
+
+ if (this.error < 0) {
+ this.y = this.y + this.ystep;
+ this.error = this.error + this.deltax;
+ }
+
+ this.x += this.xstep;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public Point2i next() {
+ Point2i p = new Point2i();
+ next(p);
+ return p;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void remove() {
+ throw new UnsupportedOperationException();
+ }
+
+ } // class LineIterator
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Shape2i.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Shape2i.java
new file mode 100644
index 000000000..10224b78a
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Shape2i.java
@@ -0,0 +1,158 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.discrete;
+
+import java.util.Iterator;
+
+import org.arakhne.afc.math.geometry2d.Point2D;
+import org.arakhne.afc.math.geometry2d.Shape2D;
+import org.arakhne.afc.math.geometry2d.continuous.Transform2D;
+
+/** 2D shape with integer points.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public interface Shape2i extends Shape2D {
+
+ /** Replies the bounding box of this shape.
+ *
+ * @return the bounding box of this shape.
+ */
+ public abstract Rectangle2i toBoundingBox();
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public Shape2i clone();
+
+ /** Replies the minimal distance from this shape to the given point.
+ *
+ * @param p
+ * @return the minimal distance between this shape and the point.
+ */
+ public float distance(Point2D p);
+
+ /** Replies the squared value of the minimal distance from this shape to the given point.
+ *
+ * @param p
+ * @return squared value of the minimal distance between this shape and the point.
+ */
+ public float distanceSquared(Point2D p);
+
+ /**
+ * Computes the L-1 (Manhattan) distance between this shape and
+ * point p1. The L-1 distance is equal to abs(x1-x2) + abs(y1-y2).
+ * @param p the point
+ * @return the distance.
+ */
+ public float distanceL1(Point2D p);
+
+ /**
+ * Computes the L-infinite distance between this shape and
+ * point p1. The L-infinite distance is equal to
+ * MAX[abs(x1-x2), abs(y1-y2)].
+ * @param p the point
+ * @return the distance.
+ */
+ public float distanceLinf(Point2D p);
+
+ /** Replies if this shape is intersecting the given rectangle.
+ *
+ * @param s
+ * @return true if this shape is intersecting the given shape;
+ * false if there is no intersection.
+ */
+ public boolean intersects(Rectangle2i s);
+
+ /** Replies if this shape is intersecting the given circle.
+ *
+ * @param s
+ * @return true if this shape is intersecting the given shape;
+ * false if there is no intersection.
+ */
+ public boolean intersects(Circle2i s);
+
+ /** Replies if this shape is intersecting the given segment.
+ *
+ * @param s
+ * @return true if this shape is intersecting the given shape;
+ * false if there is no intersection.
+ */
+ public boolean intersects(Segment2i s);
+
+ /** Replies the elements of the paths.
+ *
+ * @param transform is the transformation to apply to the path.
+ * @return the elements of the path.
+ */
+ public PathIterator2i getPathIterator(Transform2D transform);
+
+ /** Replies the elements of the paths.
+ *
+ * @return the elements of the path.
+ */
+ public PathIterator2i getPathIterator();
+
+ /** Replies an iterator on the points covered by this shape.
+ *
+ * The implementation of the iterator depends on the shape type.
+ * There is no warranty about the order of the points.
+ *
+ * @return an iterator on the points.
+ */
+ public Iterator getPointIterator();
+
+ /** Apply the transformation to the shape and reply the result.
+ * This function does not change the current shape.
+ *
+ * @param transform is the transformation to apply to the shape.
+ * @return the result of the transformation.
+ */
+ public Shape2i createTransformedShape(Transform2D transform);
+
+ /** Translate the shape.
+ *
+ * @param dx
+ * @param dy
+ */
+ public void translate(int dx, int dy);
+
+ /** Replies if the given point is inside this shape.
+ *
+ * @param x
+ * @param y
+ * @return true if the given point is inside this
+ * shape, otherwise false.
+ */
+ public boolean contains(int x, int y);
+
+ /** Replies if the given rectangle is inside this shape.
+ *
+ * @param r
+ * @return true if the given rectangle is inside this
+ * shape, otherwise false.
+ */
+ public boolean contains(Rectangle2i r);
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Tuple2i.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Tuple2i.java
new file mode 100644
index 000000000..9df74de36
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Tuple2i.java
@@ -0,0 +1,617 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2011 Janus Core Developers
+ * Copyright (C) 2012 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.discrete;
+
+import org.arakhne.afc.math.geometry2d.Tuple2D;
+
+/** 2D tuple with 2 integers.
+ *
+ * @param is the implementation type of the tuple.
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class Tuple2i> implements Tuple2D {
+
+ private static final long serialVersionUID = -7779997414431055683L;
+
+ /** x coordinate.
+ */
+ protected int x;
+
+ /** y coordinate.
+ */
+ protected int y;
+
+ /**
+ */
+ public Tuple2i() {
+ this.x = this.y = 0;
+ }
+
+ /**
+ * @param tuple is the tuple to copy.
+ */
+ public Tuple2i(Tuple2i tuple) {
+ this.x = tuple.x;
+ this.y = tuple.y;
+ }
+
+ /**
+ * @param tuple is the tuple to copy.
+ */
+ public Tuple2i(Tuple2D tuple) {
+ this.x = (int)tuple.getX();
+ this.y = (int)tuple.getY();
+ }
+
+ /**
+ * @param tuple is the tuple to copy.
+ */
+ public Tuple2i(int[] tuple) {
+ this.x = tuple[0];
+ this.y = tuple[1];
+ }
+
+ /**
+ * @param tuple is the tuple to copy.
+ */
+ public Tuple2i(float[] tuple) {
+ this.x = (int)tuple[0];
+ this.y = (int)tuple[1];
+ }
+
+ /**
+ * @param x
+ * @param y
+ */
+ public Tuple2i(int x, int y) {
+ this.x = x;
+ this.y = y;
+ }
+
+ /**
+ * @param x
+ * @param y
+ */
+ public Tuple2i(float x, float y) {
+ this.x = (int)x;
+ this.y = (int)y;
+ }
+
+ /** {@inheritDoc}
+ */
+ @SuppressWarnings("unchecked")
+ @Override
+ public T clone() {
+ try {
+ return (T)super.clone();
+ }
+ catch(CloneNotSupportedException e) {
+ throw new Error(e);
+ }
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void absolute() {
+ this.x = Math.abs(this.x);
+ this.y = Math.abs(this.y);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void absolute(T t) {
+ t.set(Math.abs(this.x), Math.abs(this.y));
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void add(int x, int y) {
+ this.x += x;
+ this.y += y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void add(float x, float y) {
+ this.x += x;
+ this.y += y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void addX(int x) {
+ this.x += x;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void addX(float x) {
+ this.x += x;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void addY(int y) {
+ this.y += y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void addY(float y) {
+ this.y += y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void clamp(int min, int max) {
+ if (this.x < min) this.x = min;
+ else if (this.x > max) this.x = max;
+ if (this.y < min) this.y = min;
+ else if (this.y > max) this.y = max;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void clamp(float min, float max) {
+ clamp((int)min, (int)max);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void clampMin(int min) {
+ if (this.x < min) this.x = min;
+ if (this.y < min) this.y = min;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void clampMin(float min) {
+ clampMin((int)min);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void clampMax(int max) {
+ if (this.x > max) this.x = max;
+ if (this.y > max) this.y = max;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void clampMax(float max) {
+ clampMax((int)max);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void clamp(int min, int max, T t) {
+ if (this.x < min) t.setX(min);
+ else if (this.x > max) t.setX(max);
+ if (this.y < min) t.setY(min);
+ else if (this.y > max) t.setY(max);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void clamp(float min, float max, T t) {
+ clamp((int)min, (int)max, t);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void clampMin(int min, T t) {
+ if (this.x < min) t.setX(min);
+ if (this.y < min) t.setY(min);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void clampMin(float min, T t) {
+ clampMin((int)min, t);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void clampMax(int max, T t) {
+ if (this.x > max) t.setX(max);
+ if (this.y > max) t.setY(max);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void clampMax(float max, T t) {
+ clampMax((int)max, t);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void get(T t) {
+ t.set(this.x, this.y);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void get(int[] t) {
+ t[0] = this.x;
+ t[1] = this.y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void get(float[] t) {
+ t[0] = this.x;
+ t[1] = this.y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void negate(T t1) {
+ this.x = -t1.x();
+ this.y = -t1.y();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void negate() {
+ this.x = -this.x;
+ this.y = -this.y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void scale(int s, T t1) {
+ this.x = (int)(s * t1.getX());
+ this.y = (int)(s * t1.getY());
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void scale(float s, T t1) {
+ this.x = (int)(s * t1.getX());
+ this.y = (int)(s * t1.getY());
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void scale(int s) {
+ this.x = s * this.x;
+ this.y = s * this.y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void scale(float s) {
+ this.x = (int)(s * this.x);
+ this.y = (int)(s * this.y);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void set(Tuple2D t1) {
+ this.x = t1.x();
+ this.y = t1.y();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void set(int x, int y) {
+ this.x = x;
+ this.y = y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void set(float x, float y) {
+ this.x = (int)x;
+ this.y = (int)y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void set(int[] t) {
+ this.x = t[0];
+ this.y = t[1];
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void set(float[] t) {
+ this.x = (int)t[0];
+ this.y = (int)t[1];
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float getX() {
+ return this.x;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public int x() {
+ return this.x;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void setX(int x) {
+ this.x = x;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void setX(float x) {
+ this.x = (int)x;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float getY() {
+ return this.y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public int y() {
+ return this.y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void setY(int y) {
+ this.y = y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void setY(float y) {
+ this.y = (int)y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void sub(int x, int y) {
+ this.x -= x;
+ this.y -= y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void subX(int x) {
+ this.x -= x;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void subY(int y) {
+ this.y -= y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void sub(float x, float y) {
+ this.x -= x;
+ this.y -= y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void subX(float x) {
+ this.x -= x;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void subY(float y) {
+ this.y -= y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void interpolate(T t1, T t2, float alpha) {
+ this.x = (int)((1f-alpha)*t1.getX() + alpha*t2.getX());
+ this.y = (int)((1f-alpha)*t1.getY() + alpha*t2.getY());
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void interpolate(T t1, float alpha) {
+ this.x = (int)((1f-alpha)*this.x + alpha*t1.getX());
+ this.y = (int)((1f-alpha)*this.y + alpha*t1.getY());
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean equals(Tuple2D t1) {
+ try {
+ return(this.x == t1.x() && this.y == t1.y());
+ }
+ catch (NullPointerException e2) {
+ return false;
+ }
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @SuppressWarnings("unchecked")
+ @Override
+ public boolean equals(Object t1) {
+ try {
+ T t2 = (T) t1;
+ return(this.x == t2.x() && this.y == t2.y());
+ }
+ catch(AssertionError e) {
+ throw e;
+ }
+ catch (Throwable e2) {
+ return false;
+ }
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean epsilonEquals(T t1, float epsilon) {
+ float diff;
+
+ diff = this.x - t1.getX();
+ if(Float.isNaN(diff)) return false;
+ if((diff<0?-diff:diff) > epsilon) return false;
+
+ diff = this.y - t1.getY();
+ if(Float.isNaN(diff)) return false;
+ if((diff<0?-diff:diff) > epsilon) return false;
+
+ return true;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public int hashCode() {
+ int bits = 1;
+ bits = 31 * bits + this.x;
+ bits = 31 * bits + this.y;
+ return bits ^ (bits >> 32);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public String toString() {
+ return "(" //$NON-NLS-1$
+ +this.x
+ +";" //$NON-NLS-1$
+ +this.y
+ +")"; //$NON-NLS-1$
+ }
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Tuple2iComparator.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Tuple2iComparator.java
new file mode 100644
index 000000000..09786f261
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Tuple2iComparator.java
@@ -0,0 +1,55 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2011 Janus Core Developers
+ * Copyright (C) 2012 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.discrete;
+
+import java.util.Comparator;
+
+/**
+ * Comparator of Tuple2i.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class Tuple2iComparator implements Comparator> {
+
+ /**
+ */
+ public Tuple2iComparator() {
+ //
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public int compare(Tuple2i o1, Tuple2i o2) {
+ if (o1==o2) return 0;
+ if (o1==null) return Integer.MIN_VALUE;
+ if (o2==null) return Integer.MAX_VALUE;
+ int cmp = o1.x() - o2.x();
+ if (cmp!=0) return cmp;
+ return o1.y() - o2.y();
+ }
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/UnmodifiablePoint2i.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/UnmodifiablePoint2i.java
new file mode 100644
index 000000000..23216ffc5
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/UnmodifiablePoint2i.java
@@ -0,0 +1,104 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2012 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.discrete;
+
+import org.arakhne.afc.math.geometry2d.Tuple2D;
+
+/** This class implements a Point2i that cannot be modified by
+ * the setters.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class UnmodifiablePoint2i extends Point2i {
+
+ private static final long serialVersionUID = -6561225929333955341L;
+
+ /**
+ */
+ public UnmodifiablePoint2i() {
+ super();
+ }
+
+ /**
+ * @param x
+ * @param y
+ */
+ public UnmodifiablePoint2i(float x, float y) {
+ super(x, y);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public UnmodifiablePoint2i clone() {
+ return (UnmodifiablePoint2i)super.clone();
+ }
+
+ @Override
+ public void set(float x, float y) {
+ //
+ }
+
+ @Override
+ public void set(float[] t) {
+ //
+ }
+
+ @Override
+ public void set(int x, int y) {
+ //
+ }
+
+ @Override
+ public void set(int[] t) {
+ //
+ }
+
+ @Override
+ public void set(Tuple2D t1) {
+ //
+ }
+
+ @Override
+ public void setX(float x) {
+ //
+ }
+
+ @Override
+ public void setX(int x) {
+ //
+ }
+
+ @Override
+ public void setY(float y) {
+ //
+ }
+
+ @Override
+ public void setY(int y) {
+ //
+ }
+
+}
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Vector2i.java b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Vector2i.java
new file mode 100644
index 000000000..e56ff24f9
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Vector2i.java
@@ -0,0 +1,279 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2011 Janus Core Developers
+ * Copyright (C) 2012 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry2d.discrete;
+
+import org.arakhne.afc.math.MathUtil;
+import org.arakhne.afc.math.geometry2d.Point2D;
+import org.arakhne.afc.math.geometry2d.Tuple2D;
+import org.arakhne.afc.math.geometry2d.Vector2D;
+import org.arakhne.afc.math.geometry2d.continuous.Vector2f;
+
+/** 2D Vector with 2 integers.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class Vector2i extends Tuple2i implements Vector2D {
+
+ private static final long serialVersionUID = -4528846627184370639L;
+
+ /**
+ */
+ public Vector2i() {
+ //
+ }
+
+ /**
+ * @param tuple is the tuple to copy.
+ */
+ public Vector2i(Tuple2D tuple) {
+ super(tuple);
+ }
+
+ /**
+ * @param tuple is the tuple to copy.
+ */
+ public Vector2i(int[] tuple) {
+ super(tuple);
+ }
+
+ /**
+ * @param tuple is the tuple to copy.
+ */
+ public Vector2i(float[] tuple) {
+ super(tuple);
+ }
+
+ /**
+ * @param x
+ * @param y
+ */
+ public Vector2i(int x, int y) {
+ super(x,y);
+ }
+
+ /**
+ * @param x
+ * @param y
+ */
+ public Vector2i(float x, float y) {
+ super(x,y);
+ }
+
+ /**
+ * @param x
+ * @param y
+ */
+ public Vector2i(double x, double y) {
+ super((float)x,(float)y);
+ }
+
+ /**
+ * @param x
+ * @param y
+ */
+ public Vector2i(long x, long y) {
+ super(x,y);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public Vector2i clone() {
+ return (Vector2i)super.clone();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float angle(Vector2D v1) {
+ double vDot = dot(v1) / ( length()*v1.length() );
+ if( vDot < -1.) vDot = -1.;
+ if( vDot > 1.) vDot = 1.;
+ return((float) (Math.acos( vDot )));
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float dot(Vector2D v1) {
+ return (this.x*v1.getX() + this.y*v1.getY());
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float length() {
+ return (float) Math.sqrt(this.x*this.x + this.y*this.y);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float lengthSquared() {
+ return (this.x*this.x + this.y*this.y);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void normalize(Vector2D v1) {
+ float norm;
+ norm = (float) (1./Math.sqrt(v1.getX()*v1.getX() + v1.getY()*v1.getY()));
+ this.x = (int)(v1.getX()*norm);
+ this.y = (int)(v1.getY()*norm);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void normalize() {
+ float norm;
+ norm = (float)(1./Math.sqrt(this.x*this.x + this.y*this.y));
+ this.x *= norm;
+ this.y *= norm;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float signedAngle(Vector2D v) {
+ assert(v!=null);
+ Vector2f a = new Vector2f(this);
+ if (a.length()==0) return Float.NaN;
+ Vector2f b = new Vector2f(v);
+ if (b.length()==0) return Float.NaN;
+ a.normalize();
+ b.normalize();
+
+ float cos = a.getX() * b.getX() + a.getY() * b.getY();
+ // A x B = |A|.|B|.sin(theta).N = sin(theta) (where N is the unit vector perpendicular to plane AB)
+ float sin = a.getX()*b.getY() - a.getY()*b.getX();
+
+ float angle = (float)Math.atan2(sin, cos);
+
+ return angle;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void turnVector(float angle) {
+ float sin = (float)Math.sin(angle);
+ float cos = (float)Math.cos(angle);
+ float x = cos * getX() + sin * getY();
+ float y = -sin * getX() + cos * getY();
+ set(x,y);
+ }
+
+ @Override
+ public void add(Vector2D t1, Vector2D t2) {
+ this.x = (int)(t1.getX() + t2.getX());
+ this.y = (int)(t1.getY() + t2.getY());
+ }
+
+ @Override
+ public void add(Vector2D t1) {
+ this.x = (int)(this.x + t1.getX());
+ this.y = (int)(this.y + t1.getY());
+ }
+
+ @Override
+ public void scaleAdd(int s, Vector2D t1, Vector2D t2) {
+ this.x = (int)(s * t1.getX() + t2.getX());
+ this.y = (int)(s * t1.getY() + t2.getY());
+ }
+
+ @Override
+ public void scaleAdd(float s, Vector2D t1, Vector2D t2) {
+ this.x = (int)(s * t1.getX() + t2.getX());
+ this.y = (int)(s * t1.getY() + t2.getY());
+ }
+
+ @Override
+ public void scaleAdd(int s, Vector2D t1) {
+ this.x = (int)(s * this.x + t1.getX());
+ this.y = (int)(s * this.y + t1.getY());
+ }
+
+ @Override
+ public void scaleAdd(float s, Vector2D t1) {
+ this.x = (int)(s * this.x + t1.getX());
+ this.y = (int)(s * this.y + t1.getY());
+ }
+
+ @Override
+ public void sub(Vector2D t1, Vector2D t2) {
+ this.x = (int)(t1.getX() - t2.getX());
+ this.y = (int)(t1.getY() - t2.getY());
+ }
+
+ @Override
+ public void sub(Point2D t1, Point2D t2) {
+ this.x = (int)(t1.getX() - t2.getX());
+ this.y = (int)(t1.getY() - t2.getY());
+ }
+
+ @Override
+ public void sub(Vector2D t1) {
+ this.x = (int)(this.x - t1.getX());
+ this.y = (int)(this.y - t1.getY());
+ }
+
+ /** Replies the orientation vector, which is corresponding
+ * to the given angle on a trigonometric circle.
+ *
+ * @param angle is the angle in radians to translate.
+ * @return the orientation vector which is corresponding to the given angle.
+ */
+ public static Vector2i toOrientationVector(float angle) {
+ return new Vector2i(
+ (float)Math.cos(angle),
+ (float)Math.sin(angle));
+ }
+
+ @Override
+ public float getOrientationAngle() {
+ float angle = (float)Math.acos(getX());
+ if (getY()<0f) angle = -angle;
+ return MathUtil.clampRadian(angle);
+ }
+
+ @Override
+ public void perpendicularize() {
+ // Based on the cross product in 3D of (vx,vy,0)x(0,0,1), right-handed
+ //set(y(), -x());
+ // Based on the cross product in 3D of (vx,vy,0)x(0,0,1), left-handed
+ set(-y(), x());
+ }
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/Point3D.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/Point3D.java
new file mode 100644
index 000000000..a86e7abc3
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/Point3D.java
@@ -0,0 +1,182 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2012 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry3d;
+
+/** 3D Point.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public interface Point3D extends Tuple3D {
+
+ /**
+ * Computes the square of the distance between this point and point p1.
+ * @param p1 the other point
+ * @return the distance.
+ */
+ public int distanceSquared(Point3D p1);
+
+ /**
+ * Computes the square of the distance between this point and point p1.
+ * @param p1 the other point
+ * @return the distance.
+ */
+ public float getDistanceSquared(Point3D p1);
+
+ /**
+ * Computes the distance between this point and point p1.
+ * @param p1 the other point
+ * @return the distance.
+ */
+ public int distance(Point3D p1);
+
+ /**
+ * Computes the distance between this point and point p1.
+ * @param p1 the other point
+ * @return the distance.
+ */
+ public float getDistance(Point3D p1);
+
+ /**
+ * Computes the L-1 (Manhattan) distance between this point and
+ * point p1. The L-1 distance is equal to abs(x1-x2) + abs(y1-y2).
+ * @param p1 the other point
+ * @return the distance.
+ */
+ public int distanceL1(Point3D p1);
+
+ /**
+ * Computes the L-1 (Manhattan) distance between this point and
+ * point p1. The L-1 distance is equal to abs(x1-x2) + abs(y1-y2).
+ * @param p1 the other point
+ * @return the distance.
+ */
+ public float getDistanceL1(Point3D p1);
+
+ /**
+ * Computes the L-infinite distance between this point and
+ * point p1. The L-infinite distance is equal to
+ * MAX[abs(x1-x2), abs(y1-y2)].
+ * @param p1 the other point
+ * @return the distance.
+ */
+ public int distanceLinf(Point3D p1);
+
+ /**
+ * Computes the L-infinite distance between this point and
+ * point p1. The L-infinite distance is equal to
+ * MAX[abs(x1-x2), abs(y1-y2)].
+ * @param p1 the other point
+ * @return the distance.
+ */
+ public float getDistanceLinf(Point3D p1);
+
+ /**
+ * Sets the value of this tuple to the sum of tuples t1 and t2.
+ * @param t1 the first tuple
+ * @param t2 the second tuple
+ */
+ public void add(Point3D t1, Vector3D t2);
+
+ /**
+ * Sets the value of this tuple to the sum of tuples t1 and t2.
+ * @param t1 the first tuple
+ * @param t2 the second tuple
+ */
+ public void add(Vector3D t1, Point3D t2);
+
+ /**
+ * Sets the value of this tuple to the sum of itself and t1.
+ * @param t1 the other tuple
+ */
+ public void add(Vector3D t1);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of tuple t1 plus tuple t2 (this = s*t1 + t2).
+ * @param s the scalar value
+ * @param t1 the tuple to be multipled
+ * @param t2 the tuple to be added
+ */
+ public void scaleAdd(int s, Vector3D t1, Point3D t2);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of tuple t1 plus tuple t2 (this = s*t1 + t2).
+ * @param s the scalar value
+ * @param t1 the tuple to be multipled
+ * @param t2 the tuple to be added
+ */
+ public void scaleAdd(float s, Vector3D t1, Point3D t2);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of tuple t1 plus tuple t2 (this = s*t1 + t2).
+ * @param s the scalar value
+ * @param t1 the tuple to be multipled
+ * @param t2 the tuple to be added
+ */
+ public void scaleAdd(int s, Point3D t1, Vector3D t2);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of tuple t1 plus tuple t2 (this = s*t1 + t2).
+ * @param s the scalar value
+ * @param t1 the tuple to be multipled
+ * @param t2 the tuple to be added
+ */
+ public void scaleAdd(float s, Point3D t1, Vector3D t2);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of itself and then adds tuple t1 (this = s*this + t1).
+ * @param s the scalar value
+ * @param t1 the tuple to be added
+ */
+ public void scaleAdd(int s, Vector3D t1);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of itself and then adds tuple t1 (this = s*this + t1).
+ * @param s the scalar value
+ * @param t1 the tuple to be added
+ */
+ public void scaleAdd(float s, Vector3D t1);
+
+
+ /**
+ * Sets the value of this tuple to the difference
+ * of tuples t1 and t2 (this = t1 - t2).
+ * @param t1 the first tuple
+ * @param t2 the second tuple
+ */
+ public void sub(Point3D t1, Vector3D t2);
+
+ /**
+ * Sets the value of this tuple to the difference
+ * of itself and t1 (this = this - t1).
+ * @param t1 the other tuple
+ */
+ public void sub(Vector3D t1);
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/Shape3D.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/Shape3D.java
new file mode 100644
index 000000000..e1e67f2cb
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/Shape3D.java
@@ -0,0 +1,74 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2013 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry3d;
+
+import java.io.Serializable;
+
+import org.arakhne.afc.math.geometry2d.Point2D;
+
+/** 3D shape.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public interface Shape3D
+extends Cloneable, Serializable {
+
+ /** Replies if this shape is empty.
+ * The semantic associated to the state "empty"
+ * depends on the implemented shape. See the
+ * subclasses for details.
+ *
+ * @return true if the shape is empty;
+ * false otherwise.
+ */
+ public boolean isEmpty();
+
+ /** Clone this shape.
+ *
+ * @return the clone.
+ */
+ public Shape3D clone();
+
+ /** Reset this shape to be equivalent to
+ * an just-created instance of this shape type.
+ */
+ public void clear();
+
+ /** Replies if the given point is inside this shape.
+ *
+ * @param p
+ * @return true if the given shape is intersecting this
+ * shape, otherwise false.
+ */
+ public boolean contains(Point3D p);
+
+ /** Replies the point on the shape that is closest to the given point.
+ *
+ * @param p
+ * @return the closest point on the shape; or the point itself
+ * if it is inside the shape.
+ */
+ public Point2D getClosestPointTo(Point3D p);
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/Tuple3D.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/Tuple3D.java
new file mode 100644
index 000000000..127d951aa
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/Tuple3D.java
@@ -0,0 +1,484 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2012 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry3d;
+
+import java.io.Serializable;
+
+/** 3D tuple.
+ *
+ * @param is the type of data that can be added or substracted to this tuple.
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public interface Tuple3D>
+extends Cloneable, Serializable {
+
+ /** Clone this point.
+ *
+ * @return the clone.
+ */
+ public TT clone();
+
+ /**
+ * Sets each component of this tuple to its absolute value.
+ */
+ public void absolute();
+
+ /**
+ * Sets each component of the tuple parameter to its absolute
+ * value and places the modified values into this tuple.
+ * @param t the source tuple, which will not be modified
+ */
+ public void absolute(TT t);
+
+ /**
+ * Sets the value of this tuple to the sum of itself and x and y.
+ * @param x
+ * @param y
+ * @param z
+ */
+ public void add(int x, int y, int z);
+
+ /**
+ * Sets the value of this tuple to the sum of itself and x and y.
+ * @param x
+ * @param y
+ * @param z
+ */
+ public void add(float x, float y, float z);
+
+ /**
+ * Sets the x value of this tuple to the sum of itself and x.
+ * @param x
+ */
+ public void addX(int x);
+
+ /**
+ * Sets the x value of this tuple to the sum of itself and x.
+ * @param x
+ */
+ public void addX(float x);
+
+ /**
+ * Sets the y value of this tuple to the sum of itself and y.
+ * @param y
+ */
+ public void addY(int y);
+
+ /**
+ * Sets the y value of this tuple to the sum of itself and y.
+ * @param y
+ */
+ public void addY(float y);
+
+ /**
+ * Sets the z value of this tuple to the sum of itself and z.
+ * @param z
+ */
+ public void addZ(int z);
+
+ /**
+ * Sets the z value of this tuple to the sum of itself and z.
+ * @param z
+ */
+ public void addZ(float z);
+
+ /**
+ * Clamps this tuple to the range [low, high].
+ * @param min the lowest value in this tuple after clamping
+ * @param max the highest value in this tuple after clamping
+ */
+ public void clamp(int min, int max);
+
+ /**
+ * Clamps this tuple to the range [low, high].
+ * @param min the lowest value in this tuple after clamping
+ * @param max the highest value in this tuple after clamping
+ */
+ public void clamp(float min, float max);
+
+ /**
+ * Clamps the minimum value of this tuple to the min parameter.
+ * @param min the lowest value in this tuple after clamping
+ */
+ public void clampMin(int min);
+
+ /**
+ * Clamps the minimum value of this tuple to the min parameter.
+ * @param min the lowest value in this tuple after clamping
+ */
+ public void clampMin(float min);
+
+ /**
+ * Clamps the maximum value of this tuple to the max parameter.
+ * @param max the highest value in the tuple after clamping
+ */
+ public void clampMax(int max);
+
+ /**
+ * Clamps the maximum value of this tuple to the max parameter.
+ * @param max the highest value in the tuple after clamping
+ */
+ public void clampMax(float max);
+
+ /**
+ * Clamps the tuple parameter to the range [low, high] and
+ * places the values into this tuple.
+ * @param min the lowest value in the tuple after clamping
+ * @param max the highest value in the tuple after clamping
+ * @param t the source tuple, which will not be modified
+ */
+ public void clamp(int min, int max, TT t);
+
+ /**
+ * Clamps the tuple parameter to the range [low, high] and
+ * places the values into this tuple.
+ * @param min the lowest value in the tuple after clamping
+ * @param max the highest value in the tuple after clamping
+ * @param t the source tuple, which will not be modified
+ */
+ public void clamp(float min, float max, TT t);
+
+ /**
+ * Clamps the minimum value of the tuple parameter to the min
+ * parameter and places the values into this tuple.
+ * @param min the lowest value in the tuple after clamping
+ * @param t the source tuple, which will not be modified
+ */
+ public void clampMin(int min, TT t);
+
+ /**
+ * Clamps the minimum value of the tuple parameter to the min
+ * parameter and places the values into this tuple.
+ * @param min the lowest value in the tuple after clamping
+ * @param t the source tuple, which will not be modified
+ */
+ public void clampMin(float min, TT t);
+
+ /**
+ * Clamps the maximum value of the tuple parameter to the max
+ * parameter and places the values into this tuple.
+ * @param max the highest value in the tuple after clamping
+ * @param t the source tuple, which will not be modified
+ */
+ public void clampMax(int max, TT t);
+
+ /**
+ * Clamps the maximum value of the tuple parameter to the max
+ * parameter and places the values into this tuple.
+ * @param max the highest value in the tuple after clamping
+ * @param t the source tuple, which will not be modified
+ */
+ public void clampMax(float max, TT t);
+
+ /**
+ * Copies the values of this tuple into the tuple t.
+ * @param t is the target tuple
+ */
+ public void get(TT t);
+
+ /**
+ * Copies the value of the elements of this tuple into the array t.
+ * @param t the array that will contain the values of the vector
+ */
+ public void get(int[] t);
+
+ /**
+ * Copies the value of the elements of this tuple into the array t.
+ * @param t the array that will contain the values of the vector
+ */
+ public void get(float[] t);
+
+ /**
+ * Sets the value of this tuple to the negation of tuple t1.
+ * @param t1 the source tuple
+ */
+ public void negate(TT t1);
+
+ /**
+ * Negates the value of this tuple in place.
+ */
+ public void negate();
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of tuple t1.
+ * @param s the scalar value
+ * @param t1 the source tuple
+ */
+ public void scale(int s, TT t1);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of tuple t1.
+ * @param s the scalar value
+ * @param t1 the source tuple
+ */
+ public void scale(float s, TT t1);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of the scale factor with this.
+ * @param s the scalar value
+ */
+ public void scale(int s);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of the scale factor with this.
+ * @param s the scalar value
+ */
+ public void scale(float s);
+
+ /**
+ * Sets the value of this tuple to the value of tuple t1.
+ * @param t1 the tuple to be copied
+ */
+ public void set(Tuple3D t1);
+
+ /**
+ * Sets the value of this tuple to the specified x and y
+ * coordinates.
+ * @param x the x coordinate
+ * @param y the y coordinate
+ * @param z the z coordinate
+ */
+ public void set(int x, int y, int z);
+
+ /**
+ * Sets the value of this tuple to the specified x and y
+ * coordinates.
+ * @param x the x coordinate
+ * @param y the y coordinate
+ * @param z the z coordinate
+ */
+ public void set(float x, float y, float z);
+
+ /**
+ * Sets the value of this tuple from the 2 values specified in
+ * the array.
+ * @param t the array of length 2 containing xy in order
+ */
+ public void set(int[] t);
+
+ /**
+ * Sets the value of this tuple from the 2 values specified in
+ * the array.
+ * @param t the array of length 2 containing xy in order
+ */
+ public void set(float[] t);
+
+ /**
+ * Get the x coordinate.
+ *
+ * @return the x coordinate.
+ */
+ public float getX();
+
+ /**
+ * Get the x coordinate.
+ *
+ * @return the x coordinate.
+ */
+ public int x();
+
+ /**
+ * Set the x coordinate.
+ *
+ * @param x value to x coordinate.
+ */
+ public void setX(int x);
+
+ /**
+ * Set the x coordinate.
+ *
+ * @param x value to x coordinate.
+ */
+ public void setX(float x);
+
+ /**
+ * Get the y coordinate.
+ *
+ * @return the y coordinate.
+ */
+ public float getY();
+
+ /**
+ * Get the y coordinate.
+ *
+ * @return the y coordinate.
+ */
+ public int y();
+
+ /**
+ * Set the y coordinate.
+ *
+ * @param y value to y coordinate.
+ */
+ public void setY(int y);
+
+ /**
+ * Set the y coordinate.
+ *
+ * @param y value to y coordinate.
+ */
+ public void setY(float y);
+
+ /**
+ * Get the z coordinate.
+ *
+ * @return the z coordinate.
+ */
+ public float getZ();
+
+ /**
+ * Get the z coordinate.
+ *
+ * @return the z coordinate.
+ */
+ public int z();
+
+ /**
+ * Set the z coordinate.
+ *
+ * @param z value to z coordinate.
+ */
+ public void setZ(int z);
+
+ /**
+ * Set the z coordinate.
+ *
+ * @param z value to z coordinate.
+ */
+ public void setZ(float z);
+
+ /**
+ * Sets the value of this tuple to the difference of itself and x, y and z.
+ * @param x
+ * @param y
+ * @param z
+ */
+ public void sub(int x, int y, int z);
+
+ /**
+ * Sets the value of this tuple to the difference of itself and x, y and z.
+ * @param x
+ * @param y
+ * @param z
+ */
+ public void sub(float x, float y, float z);
+
+ /**
+ * Sets the x value of this tuple to the difference of itself and x.
+ * @param x
+ */
+ public void subX(int x);
+
+ /**
+ * Sets the x value of this tuple to the difference of itself and x.
+ * @param x
+ */
+ public void subX(float x);
+
+ /**
+ * Sets the y value of this tuple to the difference of itself and y.
+ * @param y
+ */
+ public void subY(int y);
+
+ /**
+ * Sets the y value of this tuple to the difference of itself and y.
+ * @param y
+ */
+ public void subY(float y);
+
+ /**
+ * Sets the z value of this tuple to the difference of itself and z.
+ * @param z
+ */
+ public void subZ(int z);
+
+ /**
+ * Sets the z value of this tuple to the difference of itself and z.
+ * @param z
+ */
+ public void subZ(float z);
+
+ /**
+ * Linearly interpolates between tuples t1 and t2 and places the
+ * result into this tuple: this = (1-alpha)*t1 + alpha*t2.
+ * @param t1 the first tuple
+ * @param t2 the second tuple
+ * @param alpha the alpha interpolation parameter
+ */
+ public void interpolate(TT t1, TT t2, float alpha);
+
+ /**
+ * Linearly interpolates between this tuple and tuple t1 and
+ * places the result into this tuple: this = (1-alpha)*this + alpha*t1.
+ * @param t1 the first tuple
+ * @param alpha the alpha interpolation parameter
+ */
+ public void interpolate(TT t1, float alpha);
+
+ /**
+ * Returns true if all of the data members of Tuple2f t1 are
+ * equal to the corresponding data members in this Tuple2f.
+ * @param t1 the vector with which the comparison is made
+ * @return true or false
+ */
+ public boolean equals(Tuple3D t1);
+
+ /**
+ * Returns true if the Object t1 is of type Tuple2f and all of the
+ * data members of t1 are equal to the corresponding data members in
+ * this Tuple2f.
+ * @param t1 the object with which the comparison is made
+ * @return true or false
+ */
+ @Override
+ public boolean equals(Object t1);
+
+ /**
+ * Returns true if the L-infinite distance between this tuple
+ * and tuple t1 is less than or equal to the epsilon parameter,
+ * otherwise returns false. The L-infinite
+ * distance is equal to MAX[abs(x1-x2), abs(y1-y2)].
+ * @param t1 the tuple to be compared to this tuple
+ * @param epsilon the threshold value
+ * @return true or false
+ */
+ public boolean epsilonEquals(TT t1, float epsilon);
+
+ /**
+ * Returns a hash code value based on the data values in this
+ * object. Two different Tuple2f objects with identical data values
+ * (i.e., Tuple2f.equals returns true) will return the same hash
+ * code value. Two objects with different data members may return the
+ * same hash value, although this is not likely.
+ * @return the integer hash code value
+ */
+ @Override
+ public int hashCode();
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/Vector3D.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/Vector3D.java
new file mode 100644
index 000000000..d04f714cf
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/Vector3D.java
@@ -0,0 +1,215 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010-2012 Stephane GALLAND.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 3 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry3d;
+
+
+/** 3D Vector.
+ *
+ * @author $Author: galland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public interface Vector3D extends Tuple3D {
+
+ /**
+ * Sets the value of this tuple to the sum of tuples t1 and t2.
+ * @param t1 the first tuple
+ * @param t2 the second tuple
+ */
+ public void add(Vector3D t1, Vector3D t2);
+
+
+ /**
+ * Sets the value of this tuple to the sum of itself and t1.
+ * @param t1 the other tuple
+ */
+ public void add(Vector3D t1);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of tuple t1 plus tuple t2 (this = s*t1 + t2).
+ * @param s the scalar value
+ * @param t1 the tuple to be multipled
+ * @param t2 the tuple to be added
+ */
+ public void scaleAdd(int s, Vector3D t1, Vector3D t2);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of tuple t1 plus tuple t2 (this = s*t1 + t2).
+ * @param s the scalar value
+ * @param t1 the tuple to be multipled
+ * @param t2 the tuple to be added
+ */
+ public void scaleAdd(float s, Vector3D t1, Vector3D t2);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of itself and then adds tuple t1 (this = s*this + t1).
+ * @param s the scalar value
+ * @param t1 the tuple to be added
+ */
+ public void scaleAdd(int s, Vector3D t1);
+
+ /**
+ * Sets the value of this tuple to the scalar multiplication
+ * of itself and then adds tuple t1 (this = s*this + t1).
+ * @param s the scalar value
+ * @param t1 the tuple to be added
+ */
+ public void scaleAdd(float s, Vector3D t1);
+
+
+ /**
+ * Sets the value of this tuple to the difference
+ * of tuples t1 and t2 (this = t1 - t2).
+ * @param t1 the first tuple
+ * @param t2 the second tuple
+ */
+ public void sub(Vector3D t1, Vector3D t2);
+
+ /**
+ * Sets the value of this tuple to the difference
+ * of tuples t1 and t2 (this = t1 - t2).
+ * @param t1 the first tuple
+ * @param t2 the second tuple
+ */
+ public void sub(Point3D t1, Point3D t2);
+
+ /**
+ * Sets the value of this tuple to the difference
+ * of itself and t1 (this = this - t1).
+ * @param t1 the other tuple
+ */
+ public void sub(Vector3D t1);
+
+ /**
+ * Computes the dot product of the this vector and vector v1.
+ * @param v1 the other vector
+ * @return the dot product.
+ */
+ public float dot(Vector3D v1);
+
+ /**
+ * Computes the cross product of the this vector and vector v1.
+ * The coordinate system's standard depends on the underlying
+ * implementation of the API.
+ * One of {@link #crossLeftHand(Vector3D)} or {@link #crossRightHand(Vector3D)}
+ * will be invoked by this function.
+ *
+ * @param v1 the other vector
+ * @return the dot product.
+ * @see #crossLeftHand(Vector3D)
+ * @see #crossRightHand(Vector3D)
+ */
+ public Vector3D cross(Vector3D v1);
+
+ /**
+ * Computes the cross product of the vectors v1 and v2 and
+ * put the result in this vector.
+ * The coordinate system's standard depends on the underlying
+ * implementation of the API.
+ * One of {@link #crossLeftHand(Vector3D, Vector3D)} or
+ * {@link #crossRightHand(Vector3D, Vector3D)}
+ * will be invoked by this function.
+ *
+ * @param v1
+ * @param v2
+ * @see #crossLeftHand(Vector3D, Vector3D)
+ * @see #crossRightHand(Vector3D, Vector3D)
+ */
+ public void cross(Vector3D v1, Vector3D v2);
+
+ /**
+ * Computes the cross product of the this vector and vector v1
+ * as if the vectors are inside a left-hand coordinate system.
+ * @param v1 the other vector
+ * @return the dot product.
+ */
+ public Vector3D crossLeftHand(Vector3D v1);
+
+ /**
+ * Computes the cross product of the vectors v1 and v2
+ * as if the vectors are inside a left-hand coordinate system;
+ * and put the result in this vector.
+ * @param v1
+ * @param v2
+ */
+ public void crossLeftHand(Vector3D v1, Vector3D v2);
+
+ /**
+ * Computes the cross product of the this vector and vector v1
+ * as if the vectors are inside a left-hand coordinate system.
+ * @param v1 the other vector
+ * @return the dot product.
+ */
+ public Vector3D crossRightHand(Vector3D v1);
+
+ /**
+ * Computes the cross product of the vectors v1 and v2
+ * as if the vectors are inside a left-hand coordinate system;
+ * and put the result in this vector.
+ * @param v1
+ * @param v2
+ */
+ public void crossRightHand(Vector3D v1, Vector3D v2);
+
+ /**
+ * Returns the length of this vector.
+ * @return the length of this vector
+ */
+ public float length();
+
+ /**
+ * Returns the squared length of this vector.
+ * @return the squared length of this vector
+ */
+ public float lengthSquared();
+
+ /**
+ * Sets the value of this vector to the normalization of vector v1.
+ * @param v1 the un-normalized vector
+ */
+ public void normalize(Vector3D v1);
+
+ /**
+ * Normalizes this vector in place.
+ */
+ public void normalize();
+
+
+ /**
+ * Returns the angle in radians between this vector and the vector
+ * parameter; the return value is constrained to the range [0,PI].
+ * @param v1 the other vector
+ * @return the angle in radians in the range [0,PI]
+ */
+ public float angle(Vector3D v1);
+
+ /** Turn this vector about the given rotation angle.
+ *
+ * @param axis is the axis of rotation.
+ * @param angle is the rotation angle in radians.
+ */
+ public void turnVector(Vector3D axis, float angle);
+
+}
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/AbstractOrthoPlane.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/AbstractOrthoPlane.java
new file mode 100644
index 000000000..5d1747e72
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/AbstractOrthoPlane.java
@@ -0,0 +1,248 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2013 Christophe BOHRHAUER.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+package org.arakhne.afc.math.geometry3d.continuous;
+
+import org.arakhne.afc.math.MathUtil;
+import org.arakhne.afc.math.geometry.continuous.object4d.AxisAngle4f;
+
+/** This class represents a 3D plane which is colinear to the axis.
+ *
+ * @author $Author: sgalland$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public abstract class AbstractOrthoPlane extends AbstractPlane implements OrthoPlane {
+
+ private static final long serialVersionUID = 8460135045561998626L;
+
+ /** Indicates if this plane is oriented to the positve side.
+ * If true, the normal of the plane is directed
+ * to the positive infinity.
+ */
+ protected boolean isPositive = true;
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public void negate() {
+ this.isPositive = !this.isPositive;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public void absolute() {
+ this.isPositive = true;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public final Plane normalize() {
+ return this;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public final void setScale(float sx, float sy, float sz) {
+ //
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public final void scale(float sx, float sy, float sz) {
+ //
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public final Tuple3f getScale() {
+ return new Vector3f(1,1,1);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public final void setRotation(Quaternion quaternion) {
+ //
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public final void setRotation(AxisAngle4f quaternion) {
+ //
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public final void setRotation(Quaternion quaternion, Point3f pivot) {
+ //
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public final void setRotation(AxisAngle4f quaternion, Point3f pivot) {
+ //
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public final void rotate(AxisAngle4f quaternion) {
+ //
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public final void rotate(Quaternion quaternion) {
+ //
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public final void rotate(AxisAngle4f quaternion, Point3f pivot) {
+ //
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public final void rotate(Quaternion quaternion, Point3f pivot) {
+ //
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public final AxisAngle4f getAxisAngle() {
+ return new AxisAngle4f();
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public final void setPivot(float x, float y, float z) {
+ //
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public final void setPivot(Point3f point) {
+ //
+ }
+
+ /**
+ * Classifies a point with respect to the plane.
+ *
+ * Classifying a point with respect to a plane is done by passing
+ * the (x, y, z) values of the point into the plane equation,
+ * Ax + By + Cz + D = 0. The result of this operation is the
+ * distance from the plane to the point along the plane's normal vector.
+ * It will be positive if the point is on the side of the
+ * plane pointed to by the normal Vec3f, negative otherwise.
+ * If the result is 0, the point is on the plane.
+ */
+ @Override
+ public final PlanarClassificationType classifies(Tuple3f vec) {
+ float d = distanceTo(vec);
+ int cmp = MathUtil.epsilonDistanceSign(d);
+ if (cmp<0) return PlanarClassificationType.BEHIND;
+ if (cmp>0) return PlanarClassificationType.IN_FRONT_OF;
+ return PlanarClassificationType.COINCIDENT;
+ }
+
+ /**
+ * Classifies a point with respect to the plane.
+ *
+ * Classifying a point with respect to a plane is done by passing
+ * the (x, y, z) values of the point into the plane equation,
+ * Ax + By + Cz + D = 0. The result of this operation is the
+ * distance from the plane to the point along the plane's normal vector.
+ * It will be positive if the point is on the side of the
+ * plane pointed to by the normal Vec3f, negative otherwise.
+ * If the result is 0, the point is on the plane.
+ */
+ @Override
+ public final PlanarClassificationType classifies(float x, float y, float z) {
+ float d = distanceTo(x,y,z);
+ int cmp = MathUtil.epsilonDistanceSign(d);
+ if (cmp<0) return PlanarClassificationType.BEHIND;
+ if (cmp>0) return PlanarClassificationType.IN_FRONT_OF;
+ return PlanarClassificationType.COINCIDENT;
+ }
+
+ /**
+ * Classifies a sphere with respect to the plane.
+ *
+ * Classifying a point with respect to a plane is done by passing
+ * the (x, y, z) values of the point into the plane equation,
+ * Ax + By + Cz + D = 0. The result of this operation is the
+ * distance from the plane to the point along the plane's normal vector.
+ * It will be positive if the point is on the side of the
+ * plane pointed to by the normal Vec3f, negative otherwise.
+ * If the result is 0, the point is on the plane.
+ */
+ @Override
+ public final PlanarClassificationType classifies(float x, float y, float z, float radius) {
+ float d = distanceTo(x,y,z);
+ float epsilon = MathUtil.getDistanceEpsilon();
+ if (d<-radius-epsilon) return PlanarClassificationType.BEHIND;
+ if (d>radius+epsilon) return PlanarClassificationType.IN_FRONT_OF;
+ return PlanarClassificationType.COINCIDENT;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public final boolean intersects(Tuple3f vec) {
+ return MathUtil.epsilonEqualsZero(distanceTo(vec));
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public final boolean intersects(float x, float y, float z) {
+ return MathUtil.epsilonEqualsZero(distanceTo(x,y,z));
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public final boolean intersects(float x, float y, float z, float radius) {
+ float d = Math.abs(distanceTo(x,y,z));
+ float epsilon = MathUtil.getDistanceEpsilon();
+ return (d=0) buf.append('+');
+ buf.append(b);
+ buf.append(".y "); //$NON-NLS-1$
+ float c = getEquationComponentC();
+ if (c>=0) buf.append('+');
+ buf.append(c);
+ buf.append(".z "); //$NON-NLS-1$
+ float d = getEquationComponentD();
+ if (d>=0) buf.append('+');
+ buf.append(d);
+ buf.append("=0]"); //$NON-NLS-1$
+ return buf.toString();
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public PlanarClassificationType classifies(Tuple3f vec) {
+ PlanarClassificationType c;
+ float distance = distanceTo(vec);
+ int cmp = MathUtil.epsilonDistanceSign(distance);
+ if (cmp<0)
+ c = PlanarClassificationType.BEHIND;
+ else if (cmp>0)
+ c = PlanarClassificationType.IN_FRONT_OF;
+ else
+ c = PlanarClassificationType.COINCIDENT;
+ return c;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public PlanarClassificationType classifies(float x, float y, float z) {
+ PlanarClassificationType c;
+ float distance = distanceTo(x,y,z);
+ int cmp = MathUtil.epsilonDistanceSign(distance);
+ if (cmp>0)
+ c = PlanarClassificationType.IN_FRONT_OF;
+ else if (cmp<0)
+ c = PlanarClassificationType.BEHIND;
+ else
+ c = PlanarClassificationType.COINCIDENT;
+ return c;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public PlanarClassificationType classifies(float lx, float ly, float lz, float ux, float uy, float uz) {
+ assert(lx<=ux);
+ assert(ly<=uy);
+ assert(lz<=uz);
+
+ Vector3f normal = getNormal();
+ float minx, miny, minz, maxx, maxy, maxz;
+
+ // X axis
+ if(normal.getX()>=0.) {
+ minx = lx;
+ maxx = ux;
+ }
+ else {
+ minx = ux;
+ maxx = lx;
+ }
+
+ // Y axis
+ if(normal.getY()>=0.) {
+ miny = ly;
+ maxy = uy;
+ }
+ else {
+ miny = uy;
+ maxy = ly;
+ }
+
+ // Z axis
+ if(normal.getZ()>=0.) {
+ minz = lz;
+ maxz = uz;
+ }
+ else {
+ minz = uz;
+ maxz = lz;
+ }
+
+ float d = getEquationComponentD();
+
+ if ((MathUtil.dotProduct(normal.getX(), normal.getY(), normal.getZ(), minx, miny, minz)+d)>0.)
+ return PlanarClassificationType.IN_FRONT_OF;
+
+ if ((MathUtil.dotProduct(normal.getX(), normal.getY(), normal.getZ(), maxx, maxy, maxz)+d)>=0.)
+ return PlanarClassificationType.COINCIDENT;
+
+ return PlanarClassificationType.BEHIND;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public PlanarClassificationType classifies(float x, float y, float z, float radius) {
+ float distance = distanceTo(x,y,z);
+ if (!MathUtil.epsilonEqualsDistance((distance<0) ? -distance : distance,radius)) {
+ if (distance<-radius) return PlanarClassificationType.BEHIND;
+ if (distance>radius) return PlanarClassificationType.IN_FRONT_OF;
+ }
+ return PlanarClassificationType.COINCIDENT;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public PlanarClassificationType classifies(Plane otherPlane) {
+ float distance = distanceTo(otherPlane);
+
+ // The distance could not be computed
+ // the planes intersect.
+ // Planes intersect also when the distance
+ // is null
+ int cmp = MathUtil.epsilonDistanceSign(distance);
+
+ if ((distance==Double.NaN)||
+ (cmp==0))
+ return PlanarClassificationType.COINCIDENT;
+
+ if (cmp>0) return PlanarClassificationType.IN_FRONT_OF;
+ return PlanarClassificationType.BEHIND;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public boolean intersects(Tuple3f vec) {
+ float distance = distanceTo(vec);
+ return MathUtil.epsilonDistanceSign(distance)==0;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public boolean intersects(float x, float y, float z) {
+ float distance = distanceTo(x,y,z);
+ return MathUtil.epsilonDistanceSign(distance)==0;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public boolean intersects(float lx, float ly, float lz, float ux, float uy, float uz) {
+ return classifies(lx, ly, lz, ux, uy, uz) == PlanarClassificationType.COINCIDENT;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public boolean intersects(float x, float y, float z, float radius) {
+ float distance = distanceTo(x,y,z);
+ return (MathUtil.epsilonEqualsZero(distance));
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public boolean intersects(Plane otherPlane) {
+ float distance = distanceTo(otherPlane);
+ if (Double.isNaN(distance)) return true;
+ // The distance could not be computed
+ // the planes intersect.
+ // Planes intersect also when the distance
+ // is null
+ return MathUtil.epsilonDistanceSign(distance)==0;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public final float distanceTo(Tuple3f v) {
+ return distanceTo(v.getX(), v.getY(), v.getZ());
+ }
+
+ /**
+ * Compute the distance between two colinear planes.
+ *
+ * @param p is the plane to compute the distance to.
+ * @return the distance from the plane to the point along the plane's normal Vec3f.
+ * It will be positive if the point is on the side of the plane pointed to by the normal Vec3f, negative otherwise.
+ * If the result is 0, the point is on the plane. This function could replies
+ * {@link Double#NaN} if the planes are not colinear.
+ */
+ public float distanceTo(Plane p) {
+ // Compute the normales
+ Vector3f oNormal = p.getNormal();
+ oNormal.normalize();
+ Vector3f mNormal = getNormal();
+ mNormal.normalize();
+
+ float dotProduct = oNormal.dot(mNormal);
+
+ if (MathUtil.epsilonEqualsDistance(Math.abs(dotProduct),1)) {
+ // Planes are colinear.
+ // The problem could be restricted to a 1D problem.
+
+ // Compute the coordinate of this pane
+ // assuming the origin is (0,0,0)
+ float c1 = -distanceTo(0,0,0);
+
+ // Compute the coordinate of the other pane
+ // assuming the origin is (0,0,0)
+ float c2 = -p.distanceTo(0,0,0);
+
+ if (dotProduct==-1) {
+ // The planes have not the same orientation.
+ // Reverse one coordinate.
+ c2 = -c2;
+ }
+
+ return c2 - c1;
+
+ }
+ return Float.NaN;
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public Line3f getIntersection(Plane plane) {
+ Vector3f n1 = getNormal();
+ Vector3f n2 = plane.getNormal();
+ Vector3f u = new Vector3f();
+
+ u.cross(n1, n2);
+ float ulength = u.length();
+ if (MathUtil.epsilonEqualsZeroDistance(ulength)) {
+ // planes are parallel
+ return null;
+ }
+
+ // u is both perpendicular to the two normals,
+ // so it is parallel to both planes
+
+ // ((d2 n1 - d1 n2) x (n1 x n2))
+ // -----------------------------
+ // | n1 x n2 | ^2
+ //
+ // same as:
+ //
+ // ((d2 n1 - d1 n2) x u)
+ // ---------------------
+ // ulength ^2
+ n1.scale(plane.getEquationComponentD());
+ n2.scale(getEquationComponentD());
+ n1.sub(n2);
+ n2.cross(n1, u);
+ n2.scale(1.f/(ulength*ulength)); // n2 contains intersection point
+
+ u.normalize();
+
+ return new Line3f(new Point3f(n2), u);
+ }
+
+ /** {@inheritDoc}
+ */
+ @Override
+ public Point3f getIntersection(Line3f line) {
+ Vector3f n = getNormal();
+ Vector3f u = line.getDirection();
+
+ float s = n.dot(u);
+ if (MathUtil.epsilonEqualsZeroTrigo(s)) {
+ // line and plane are parallel
+ return null;
+ }
+
+ // Assuming L: P0 + si * u
+ //
+ // -(a x0 + b y0 + c z0 + d)
+ // si = -------------------------
+ // n.u
+ Point3f p0 = line.getP0();
+
+ float si = -(
+ getEquationComponentA()*p0.getX()
+ +
+ getEquationComponentB()*p0.getY()
+ +
+ getEquationComponentC()*p0.getZ()
+ +
+ getEquationComponentD()
+ )/s;
+
+ u.scale(si);
+ p0.add(u);
+
+ return p0;
+ }
+
+ }
\ No newline at end of file
diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/AxisAlignedBox.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/AxisAlignedBox.java
new file mode 100644
index 000000000..12f5ca96e
--- /dev/null
+++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/AxisAlignedBox.java
@@ -0,0 +1,1232 @@
+/*
+ * $Id$
+ *
+ * Copyright (c) 2013 Christophe BOHRHAUER
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * This program is free software; you can redistribute it and/or modify
+ */
+
+package org.arakhne.afc.math.geometry3d.continuous;
+
+import java.util.Arrays;
+import java.util.Collection;
+
+import org.arakhne.afc.math.geometry.IntersectionType;
+import org.arakhne.afc.math.geometry.IntersectionUtil;
+import org.arakhne.afc.math.geometry.continuous.euclide.EuclidianPoint3D;
+import org.arakhne.afc.math.geometry.continuous.object2d.bounds.MinimumBoundingRectangle;
+import org.arakhne.afc.math.geometry.system.CoordinateSystem3D;
+import org.arakhne.afc.sizediterator.SizedIterator;
+import org.arakhne.afc.util.ArrayUtil;
+
+/**
+ * An Axis Aligned Bounding Box.
+ *
+ * All the transformations on this bounding box are
+ * relative to the box's center.
+ *
+ * @author $Author: sgalland$
+ * @author $Author: ngaud$
+ * @author $Author: cbohrhauer$
+ * @version $FullVersion$
+ * @mavengroupid $GroupId$
+ * @mavenartifactid $ArtifactId$
+ */
+public class AxisAlignedBox
+extends AbstractCombinableBounds3D
+implements AlignedCombinableBounds3D, OrientedCombinableBounds3D, TranslatableBounds3D {
+
+ private static final long serialVersionUID = 7615135756504420528L;
+
+ /**
+ * The lower corner of this bounding box
+ */
+ EuclidianPoint3D lower = new EuclidianPoint3D();
+
+ /**
+ * The upper corner of this bounding box
+ */
+ EuclidianPoint3D upper = new EuclidianPoint3D();
+
+ private boolean isBoundInit;
+
+ /**
+ * The vertices that composes this box, it is compiled when it is required not before
+ */
+ private transient Vector3f[] vertices = null;
+
+ /** Uninitialized bouding box.
+ */
+ public AxisAlignedBox() {
+ this.isBoundInit = false;
+ }
+
+ /**
+ * @param lx is the lower point of the box.
+ * @param ly is the lower point of the box.
+ * @param lz is the lower point of the box.
+ * @param ux is the upper point of the box.
+ * @param uy is the upper point of the box.
+ * @param uz is the upper point of the box.
+ */
+ public AxisAlignedBox(float lx, float ly, float lz, float ux, float uy, float uz) {
+ this.lower.set(lx,ly,lz);
+ this.upper.set(ux,uy,uz);
+ this.isBoundInit = true;
+ checkBounds();
+ }
+
+ /**
+ * @param lower is the lower point of the box.
+ * @param upper is the upper point of the box.
+ */
+ public AxisAlignedBox(Tuple3f lower, Tuple3f upper) {
+ this.lower.set(lower);
+ this.upper.set(upper);
+ this.isBoundInit = true;
+ checkBounds();
+ }
+
+ /**
+ * @param bboxList are the boxes to combine to initialize this bounding object.
+ */
+ public AxisAlignedBox(Collection bboxList) {
+ combineBounds(false, bboxList);
+ this.isBoundInit = true;
+ }
+
+ /**
+ * @param bboxList are the boxes to combine to initialize this bounding object.
+ */
+ public AxisAlignedBox(CombinableBounds3D... bboxList) {
+ combineBounds(false, Arrays.asList(bboxList));
+ this.isBoundInit = true;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public final BoundingPrimitiveType3D getBoundType() {
+ return BoundingPrimitiveType3D.ALIGNED_BOX;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public AxisAlignedBox clone() {
+ AxisAlignedBox clone = (AxisAlignedBox)super.clone();
+ clone.lower = (EuclidianPoint3D)this.lower.clone();
+ clone.upper = (EuclidianPoint3D)this.upper.clone();
+ return clone;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void reset() {
+ if (this.isBoundInit) {
+ this.isBoundInit = false;
+ this.lower.set(0,0,0);
+ this.upper.set(0,0,0);
+ this.vertices = null;
+ }
+ }
+
+ /** {@inheritDoc}
+ *
+ * @param o {@inheritDoc}
+ * @return {@inheritDoc}
+ */
+ @Override
+ public boolean equals(Object o) {
+ if (!isInit()) return false;
+ if (this==o) return true;
+ if (o instanceof AxisAlignedBox) {
+ return (this.lower.equals(((AxisAlignedBox)o).lower) && this.upper.equals(((AxisAlignedBox)o).upper));
+ }
+ return false;
+ }
+
+ /** Check if the following constraints are respected:
+ *
+ * - {@code lower.x <= upper.x}
+ * - {@code lower.y <= upper.y}
+ * - {@code lower.z <= upper.z}
+ *
+ */
+ protected void checkBounds() {
+ if (this.upper.getA() < this.lower.getA()) {
+ float t = this.upper.getA();
+ this.upper.setA(this.lower.getA());
+ this.lower.setA(t);
+ }
+ if (this.upper.getB() < this.lower.getB()) {
+ float t = this.upper.getB();
+ this.upper.setB(this.lower.getB());
+ this.lower.setB(t);
+ }
+ if (this.upper.getC() < this.lower.getC()) {
+ float t = this.upper.getC();
+ this.upper.setC(this.lower.getC());
+ this.lower.setC(t);
+ }
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean isEmpty() {
+ return !isInit() || (((this.upper.getA() - this.lower.getA()) <= 0)
+ || ((this.upper.getB() - this.lower.getB()) <= 0) || ((this.upper.getC() - this.lower.getC()) <= 0));
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean isInit() {
+ return this.isBoundInit;
+ }
+
+ /**
+ * Sets the lower corner of this bounding box
+ *
+ * @param point
+ */
+ public void setLower(Tuple3f point) {
+ boolean init = this.isBoundInit;
+ this.isBoundInit = true;
+ this.lower.set(point);
+ if (!init) this.upper.set(point);
+ this.vertices = null;
+ checkBounds();
+ }
+
+ /**
+ * Sets the lower corner of this bounding box
+ *
+ * @param x
+ * @param y
+ * @param z
+ */
+ public void setLower(float x, float y, float z) {
+ boolean init = this.isBoundInit;
+ this.isBoundInit = true;
+ this.lower.set(x, y, z);
+ if (!init) this.upper.set(x,y,z);
+ this.vertices = null;
+ checkBounds();
+ }
+
+ /**
+ * Sets the upper corner of this bounding box
+ *
+ * @param point
+ */
+ public void setUpper(Tuple3f point) {
+ boolean init = this.isBoundInit;
+ this.isBoundInit = true;
+ this.upper.set(point);
+ if (!init) this.lower.set(point);
+ this.vertices = null;
+ checkBounds();
+ }
+
+ /**
+ * Sets the upper corner of this bounding box
+ *
+ * @param x
+ * @param y
+ * @param z
+ */
+ public void setUpper(float x, float y, float z) {
+ boolean init = this.isBoundInit;
+ this.isBoundInit = true;
+ this.upper.set(x, y, z);
+ if (!init) this.lower.set(x,y,z);
+ this.vertices = null;
+ checkBounds();
+ }
+
+ /**
+ * Sets the lower and upper corners of this bounding box
+ *
+ * @param lower
+ * @param upper
+ */
+ public void set(Tuple3f lower, Tuple3f upper) {
+ this.isBoundInit = true;
+ this.lower.set(lower);
+ this.upper.set(upper);
+ this.vertices = null;
+ checkBounds();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void getLowerUpper(Point3f lower, Point3f upper) {
+ assert(lower!=null);
+ assert(upper!=null);
+ lower.set(this.lower);
+ upper.set(this.upper);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public EuclidianPoint3D getLower() {
+ return this.lower;
+ }
+
+ /**
+ * Gets the lower corner of this bounding box
+ *
+ * @param point is the object to set with the coordinates of the lower point.
+ */
+ public void getLower(Tuple3f point) {
+ point.set(this.lower);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public EuclidianPoint3D getUpper() {
+ return this.upper;
+ }
+
+ /**
+ * Gets the upper corner of this bounding box
+ *
+ * @param point is the object to set with the coordinates of the upper point.
+ */
+ public void getUpper(Tuple3f point) {
+ point.set(this.upper);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public EuclidianPoint3D getCenter() {
+ return new EuclidianPoint3D((this.upper.getA() + this.lower.getA()) / 2.f,
+ (this.upper.getB() + this.lower.getB()) / 2.f,
+ (this.upper.getC() + this.lower.getC()) / 2.f);
+ }
+
+ /**
+ * Gets the upper corner of this bounding box
+ *
+ * @param point is the object to set with the coordinates of the center point.
+ */
+ public void getCenter(Tuple3f point) {
+ point.set((this.upper.getA() + this.lower.getA()) / 2.f,
+ (this.upper.getB() + this.lower.getB()) / 2.f,
+ (this.upper.getC() + this.lower.getC()) / 2.f);
+ }
+
+ private void ensureVertices() {
+ if(this.vertices==null) {
+ this.vertices = new Vector3f[8];
+ float sx = getSizeX()/2.f;
+ float sy = getSizeY()/2.f;
+ float sz = getSizeZ()/2.f;
+
+ this.vertices[0] = new Vector3f( sx, sy, sz);
+ this.vertices[1] = new Vector3f(-sx, sy, sz);
+ this.vertices[2] = new Vector3f(-sx,-sy, sz);
+ this.vertices[3] = new Vector3f( sx,-sy, sz);
+ this.vertices[4] = new Vector3f( sx,-sy,-sz);
+ this.vertices[5] = new Vector3f( sx, sy,-sz);
+ this.vertices[6] = new Vector3f(-sx, sy,-sz);
+ this.vertices[7] = new Vector3f(-sx,-sy,-sz);
+ }
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public SizedIterator getLocalOrientedBoundVertices() {
+ ensureVertices();
+ return ArrayUtil.sizedIterator(this.vertices);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public SizedIterator getGlobalOrientedBoundVertices() {
+ return new LocalToGlobalVertexIterator(getCenter(), getLocalOrientedBoundVertices());
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public Vector3f[] getOrientedBoundAxis() {
+ return new Vector3f[] {
+ new Vector3f(1.,0.,0.),
+ new Vector3f(0.,1.,0.),
+ new Vector3f(0.,0.,1.)
+ };
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public Vector3f getOrientedBoundExtentVector() {
+ return new Vector3f(
+ getSizeX()/2.,
+ getSizeY()/2.,
+ getSizeZ()/2.);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float[] getOrientedBoundExtents() {
+ return new float[] {
+ getSizeX()/2.f,
+ getSizeY()/2.f,
+ getSizeZ()/2.f
+ };
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public EuclidianPoint3D getGlobalVertexAt(int index) {
+ switch(index) {
+ case 0:
+ return new EuclidianPoint3D(
+ this.upper.getA(),
+ this.upper.getB(),
+ this.upper.getC());
+ case 1:
+ return new EuclidianPoint3D(
+ this.upper.getA(),
+ this.upper.getB(),
+ this.lower.getC());
+ case 2:
+ return new EuclidianPoint3D(
+ this.upper.getA(),
+ this.lower.getB(),
+ this.upper.getC());
+ case 3:
+ return new EuclidianPoint3D(
+ this.upper.getA(),
+ this.lower.getB(),
+ this.lower.getC());
+ case 4:
+ return new EuclidianPoint3D(
+ this.lower.getA(),
+ this.upper.getB(),
+ this.upper.getC());
+ case 5:
+ return new EuclidianPoint3D(
+ this.lower.getA(),
+ this.upper.getB(),
+ this.lower.getC());
+ case 6:
+ return new EuclidianPoint3D(
+ this.lower.getA(),
+ this.lower.getB(),
+ this.upper.getC());
+ case 7:
+ return new EuclidianPoint3D(
+ this.lower.getA(),
+ this.lower.getB(),
+ this.lower.getC());
+ default:
+ throw new IndexOutOfBoundsException(Integer.toString(index));
+ }
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public Vector3f getLocalVertexAt(int index) {
+ ensureVertices();
+ return this.vertices[index];
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public int getVertexCount() {
+ ensureVertices();
+ return this.vertices.length;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public Vector3f getR() {
+ return new Vector3f(1.,0.,0.);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public Vector3f getS() {
+ return new Vector3f(0.,1.,0.);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public Vector3f getT() {
+ return new Vector3f(0.,0.,1.);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float getRExtent() {
+ return getSizeX() / 2.f;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float getSExtent() {
+ return getSizeY() / 2.f;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float getTExtent() {
+ return getSizeZ() / 2.f;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float getSizeX() {
+ if (!this.isBoundInit) return Float.NaN;
+ return this.upper.getA() - this.lower.getA();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float getSizeY() {
+ if (!this.isBoundInit) return Float.NaN;
+ return this.upper.getB() - this.lower.getB();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float getSizeZ() {
+ if (!this.isBoundInit) return Float.NaN;
+ return this.upper.getC() - this.lower.getC();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public Vector3f getSize() {
+ if (!this.isBoundInit) return null;
+ return new Vector3f(this.upper.getA() - this.lower.getA(), this.upper.getB()
+ - this.lower.getB(), this.upper.getC() - this.lower.getC());
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public String toString() {
+ StringBuilder sb = new StringBuilder(64);
+ sb.append("BoundingBox["); //$NON-NLS-1$
+ sb.append(this.lower.getA());
+ sb.append(", "); //$NON-NLS-1$
+ sb.append(this.lower.getB());
+ sb.append(", "); //$NON-NLS-1$
+ sb.append(this.lower.getC());
+ sb.append(", "); //$NON-NLS-1$
+ sb.append(this.upper.getA());
+ sb.append(", "); //$NON-NLS-1$
+ sb.append(this.upper.getB());
+ sb.append(", "); //$NON-NLS-1$
+ sb.append(this.upper.getC());
+ sb.append(']');
+ return sb.toString();
+ }
+
+ /**
+ * Add the point into the bounds.
+ */
+ @Override
+ protected void combinePoints(boolean isAlreadyInit, Collection pointList) {
+ if (pointList == null || pointList.isEmpty())
+ return;
+
+ boolean init = isAlreadyInit;
+ float minx, miny, minz;
+ float maxx, maxy, maxz;
+
+ minx = this.lower.getA();
+ miny = this.lower.getB();
+ minz = this.lower.getC();
+ maxx = this.upper.getA();
+ maxy = this.upper.getB();
+ maxz = this.upper.getC();
+
+ for (Tuple3f t : pointList) {
+ if (t!=null) {
+ if (!init) {
+ init = true;
+ minx = t.getX();
+ miny = t.getY();
+ minz = t.getZ();
+ maxx = t.getX();
+ maxy = t.getY();
+ maxz = t.getZ();
+ } else {
+ if (t.getX() < minx)
+ minx = t.getX();
+ if (t.getY() < miny)
+ miny = t.getY();
+ if (t.getZ() < minz)
+ minz = t.getZ();
+ if (t.getX() > maxx)
+ maxx = t.getX();
+ if (t.getY() > maxy)
+ maxy = t.getY();
+ if (t.getZ() > maxz)
+ maxz = t.getZ();
+ }
+ }
+ }
+
+ this.lower.set(minx, miny, minz);
+ this.upper.set(maxx, maxy, maxz);
+ this.isBoundInit = true;
+ this.vertices = null;
+ }
+
+ /**
+ * Add the bounds into the bounds.
+ */
+ @Override
+ protected void combineBounds(boolean isInit,
+ Collection bounds) {
+ if (bounds == null || bounds.isEmpty())
+ return;
+
+ boolean init = isInit;
+ float minx, miny, minz;
+ float maxx, maxy, maxz;
+
+ minx = this.lower.getA();
+ miny = this.lower.getB();
+ minz = this.lower.getC();
+ maxx = this.upper.getA();
+ maxy = this.upper.getB();
+ maxz = this.upper.getC();
+
+ Point3f lowerPt, upperPt;
+
+ for (Bounds3D b : bounds) {
+ if (b!=null && b.isInit()) {
+ lowerPt = b.getLower();
+ upperPt = b.getUpper();
+ if (!init) {
+ init = true;
+ minx = lowerPt.getX();
+ miny = lowerPt.getY();
+ minz = lowerPt.getZ();
+ maxx = upperPt.getX();
+ maxy = upperPt.getY();
+ maxz = upperPt.getZ();
+ } else {
+ if (lowerPt.getX() < minx)
+ minx = lowerPt.getX();
+ if (lowerPt.getY() < miny)
+ miny = lowerPt.getY();
+ if (lowerPt.getZ() < minz)
+ minz = lowerPt.getZ();
+ if (upperPt.getX() > maxx)
+ maxx = upperPt.getX();
+ if (upperPt.getY() > maxy)
+ maxy = upperPt.getY();
+ if (upperPt.getZ() > maxz)
+ maxz = upperPt.getZ();
+ }
+ }
+ }
+
+ this.lower.set(minx, miny, minz);
+ this.upper.set(maxx, maxy, maxz);
+ this.isBoundInit = true;
+ this.vertices = null;
+ }
+
+ /** Replies the voxel that corresponds to the
+ * upper (max z), left (max y), front (min x)
+ * corner.
+ *
+ * @return the frontal upper-left quarter of this box.
+ * @see CoordinateSystem3D#XYZ_RIGHT_HAND
+ */
+ public AxisAlignedBox getNorthWestFrontVoxel() {
+ float midX = (this.lower.getA()+this.upper.getA())/2.f;
+ float midY = (this.lower.getB()+this.upper.getB())/2.f;
+ float midZ = (this.lower.getC()+this.upper.getC())/2.f;
+ return new AxisAlignedBox(
+ this.lower.getA(),midY,midZ,
+ midX,this.upper.getB(),this.upper.getC());
+ }
+
+ /** Replies the voxel that corresponds to the
+ * upper (max z), left (max y), back (max x)
+ * corner.
+ *
+ * @return the background upper-left quarter of this box.
+ * @see CoordinateSystem3D#XYZ_RIGHT_HAND
+ */
+ public AxisAlignedBox getNorthWestBackVoxel() {
+ float midX = (this.lower.getA()+this.upper.getA())/2.f;
+ float midY = (this.lower.getB()+this.upper.getB())/2.f;
+ float midZ = (this.lower.getC()+this.upper.getC())/2.f;
+ return new AxisAlignedBox(
+ midX,midY,midZ,
+ this.upper.getA(),this.upper.getB(),this.upper.getC());
+ }
+
+ /** Replies the voxel that corresponds to the
+ * upper (max z), right (min y), front (min x)
+ * corner.
+ *
+ * @return the frontal upper-right quarter of this box.
+ * @see CoordinateSystem3D#XYZ_RIGHT_HAND
+ */
+ public AxisAlignedBox getNorthEastFrontVoxel() {
+ float midX = (this.lower.getA()+this.upper.getA())/2.f;
+ float midY = (this.lower.getB()+this.upper.getB())/2.f;
+ float midZ = (this.lower.getC()+this.upper.getC())/2.f;
+ return new AxisAlignedBox(
+ this.lower.getA(),this.lower.getB(),midZ,
+ midX,midY,this.upper.getC());
+ }
+
+ /** Replies the voxel that corresponds to the
+ * upper (max z), right (min y), back (max x)
+ * corner.
+ *
+ * @return the background upper-right quarter of this box.
+ * @see CoordinateSystem3D#XYZ_RIGHT_HAND
+ */
+ public AxisAlignedBox getNorthEastBackVoxel() {
+ float midX = (this.lower.getA()+this.upper.getA())/2.f;
+ float midY = (this.lower.getB()+this.upper.getB())/2.f;
+ float midZ = (this.lower.getC()+this.upper.getC())/2.f;
+ return new AxisAlignedBox(
+ midX,this.lower.getB(),midZ,
+ this.upper.getA(),midY,this.upper.getC());
+ }
+
+ /** Replies the voxel that corresponds to the
+ * lower (min z), left (max y), front (min x)
+ * corner.
+ *
+ * @return the frontal lower-left quarter of this box.
+ * @see CoordinateSystem3D#XYZ_RIGHT_HAND
+ */
+ public AxisAlignedBox getSouthWestFrontVoxel() {
+ float midX = (this.lower.getA()+this.upper.getA())/2.f;
+ float midY = (this.lower.getB()+this.upper.getB())/2.f;
+ float midZ = (this.lower.getC()+this.upper.getC())/2.f;
+ return new AxisAlignedBox(
+ this.lower.getA(),midY,this.lower.getC(),
+ midX,this.upper.getB(),midZ);
+ }
+
+ /** Replies the voxel that corresponds to the
+ * lower (min z), left (max y), back (max x)
+ * corner.
+ *
+ * @return the background lower-left quarter of this box.
+ * @see CoordinateSystem3D#XYZ_RIGHT_HAND
+ */
+ public AxisAlignedBox getSouthWestBackVoxel() {
+ float midX = (this.lower.getA()+this.upper.getA())/2.f;
+ float midY = (this.lower.getB()+this.upper.getB())/2.f;
+ float midZ = (this.lower.getC()+this.upper.getC())/2.f;
+ return new AxisAlignedBox(
+ midX,midY,this.lower.getC(),
+ this.upper.getA(),this.upper.getB(),midZ);
+ }
+
+ /** Replies the voxel that corresponds to the
+ * lower (min z), right (min y), front (min x)
+ * corner.
+ *
+ * @return the frontal lower-right quarter of this box.
+ * @see CoordinateSystem3D#XYZ_RIGHT_HAND
+ */
+ public AxisAlignedBox getSouthEastFrontVoxel() {
+ float midX = (this.lower.getA()+this.upper.getA())/2.f;
+ float midY = (this.lower.getB()+this.upper.getB())/2.f;
+ float midZ = (this.lower.getC()+this.upper.getC())/2.f;
+ return new AxisAlignedBox(
+ this.lower.getA(),this.lower.getB(),this.lower.getC(),
+ midX,midY,midZ);
+ }
+
+ /** Replies the voxel that corresponds to the
+ * lower (min z), right (min y), back (max x)
+ * corner.
+ *
+ * @return the background lower-right quarter of this box.
+ * @see CoordinateSystem3D#XYZ_RIGHT_HAND
+ */
+ public AxisAlignedBox getSouthEastBackVoxel() {
+ float midX = (this.lower.getA()+this.upper.getA())/2.f;
+ float midY = (this.lower.getB()+this.upper.getB())/2.f;
+ float midZ = (this.lower.getC()+this.upper.getC())/2.f;
+ return new AxisAlignedBox(
+ midX,this.lower.getB(),this.lower.getC(),
+ this.upper.getA(),midY,midZ);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public float distanceSquared(Point3f p) {
+ if (!isInit()) return Float.NaN;
+ float d1 = 0;
+ if (p.getX()this.upper.getA()) {
+ d1 = p.getX() - this.upper.getA();
+ d1 = d1*d1;
+ }
+ float d2 = 0;
+ if (p.getY()this.upper.getB()) {
+ d2 = p.getY() - this.upper.getB();
+ d2 = d2*d2;
+ }
+ float d3 = 0;
+ if (p.getZ()