From 8a76d540fe85d7ac889d5b03e2c4a8ef2b7e39cb Mon Sep 17 00:00:00 2001 From: Maxime GROLLEAU Date: Mon, 2 Dec 2013 11:59:13 +0100 Subject: [PATCH] Reorganizing package arborescence --- .../java/org/arakhne/afc/math/Matrix2f.java | 1255 ++++++ .../java/org/arakhne/afc/math/Matrix3f.java | 3496 +++++++++++++++++ .../java/org/arakhne/afc/math/Matrix4f.java | 1960 +++++++++ .../afc/math/SingularMatrixException.java | 63 + .../afc/math/geometry/ClassifierUtil.java | 1437 +++++++ .../math/geometry/IntersectionClassifier.java | 98 + .../geometry/IntersectionClassifier2D.java | 80 + .../geometry/IntersectionClassifier3D.java | 112 + .../afc/math/geometry/IntersectionType.java | 323 ++ .../afc/math/geometry/IntersectionUtil.java | 2343 +++++++++++ .../math/geometry/PointFilterXYIterable.java | 62 + .../math/geometry/PointFilterXYIterator.java | 88 + .../math/geometry/PointFilterXZIterable.java | 63 + .../math/geometry/PointFilterXZIterator.java | 87 + .../math/geometry/PointFilterYZIterable.java | 63 + .../math/geometry/PointFilterYZIterator.java | 87 + .../geometry/system/CoordinateSystem.java | 65 + .../geometry/system/CoordinateSystem2D.java | 401 ++ .../geometry/system/CoordinateSystem3D.java | 1205 ++++++ .../system/CoordinateSystemConstants.java | 228 ++ .../CoordinateSystemNotFoundException.java | 42 + .../arakhne/afc/math/geometry2d/Path2D.java | 93 + .../afc/math/geometry2d/PathElement2D.java | 60 + .../arakhne/afc/math/geometry2d/Point2D.java | 151 + .../arakhne/afc/math/geometry2d/Shape2D.java | 103 + .../arakhne/afc/math/geometry2d/Tuple2D.java | 426 ++ .../arakhne/afc/math/geometry2d/Vector2D.java | 183 + .../AbstractRectangularShape2f.java | 392 ++ .../continuous/AbstractShape2f.java | 110 + .../math/geometry2d/continuous/Circle2f.java | 579 +++ .../math/geometry2d/continuous/Ellipse2f.java | 457 +++ .../continuous/OrientedRectangle2f.java | 899 +++++ .../math/geometry2d/continuous/Path2f.java | 3121 +++++++++++++++ .../geometry2d/continuous/PathElement2f.java | 451 +++ .../geometry2d/continuous/PathIterator2f.java | 54 + .../geometry2d/continuous/PathShadow2f.java | 459 +++ .../math/geometry2d/continuous/Point2f.java | 208 + .../geometry2d/continuous/Rectangle2f.java | 708 ++++ .../continuous/RoundRectangle2f.java | 618 +++ .../math/geometry2d/continuous/Segment2f.java | 485 +++ .../math/geometry2d/continuous/Shape2f.java | 228 ++ .../geometry2d/continuous/Transform2D.java | 875 +++++ .../math/geometry2d/continuous/Tuple2f.java | 608 +++ .../continuous/Tuple2fComparator.java | 54 + .../continuous/UnmodifiablePoint2f.java | 104 + .../math/geometry2d/continuous/Vector2f.java | 307 ++ .../discrete/AbstractRectangularShape2i.java | 297 ++ .../geometry2d/discrete/AbstractShape2i.java | 85 + .../math/geometry2d/discrete/Circle2i.java | 966 +++++ .../afc/math/geometry2d/discrete/Path2i.java | 2568 ++++++++++++ .../geometry2d/discrete/PathElement2i.java | 480 +++ .../geometry2d/discrete/PathIterator2i.java | 44 + .../afc/math/geometry2d/discrete/Point2i.java | 209 + .../math/geometry2d/discrete/Rectangle2i.java | 720 ++++ .../math/geometry2d/discrete/Segment2i.java | 1267 ++++++ .../afc/math/geometry2d/discrete/Shape2i.java | 158 + .../afc/math/geometry2d/discrete/Tuple2i.java | 617 +++ .../discrete/Tuple2iComparator.java | 55 + .../discrete/UnmodifiablePoint2i.java | 104 + .../math/geometry2d/discrete/Vector2i.java | 279 ++ .../arakhne/afc/math/geometry3d/Point3D.java | 182 + .../arakhne/afc/math/geometry3d/Shape3D.java | 74 + .../arakhne/afc/math/geometry3d/Tuple3D.java | 484 +++ .../arakhne/afc/math/geometry3d/Vector3D.java | 215 + .../continuous/AbstractOrthoPlane.java | 248 ++ .../geometry3d/continuous/AbstractPlane.java | 359 ++ .../geometry3d/continuous/AxisAlignedBox.java | 1232 ++++++ .../math/geometry3d/continuous/Capsule.java | 1209 ++++++ .../geometry3d/continuous/EulerAngle.java | 300 ++ .../math/geometry3d/continuous/Line3f.java | 402 ++ .../geometry3d/continuous/OrientedBox.java | 1479 +++++++ .../geometry3d/continuous/OrthoPlane.java | 35 + .../continuous/PlanarClassificationType.java | 206 + .../afc/math/geometry3d/continuous/Plane.java | 144 + .../math/geometry3d/continuous/Plane4f.java | 898 +++++ .../continuous/PlaneClassifier.java | 135 + .../math/geometry3d/continuous/Point3f.java | 267 ++ .../geometry3d/continuous/Quaternion.java | 712 ++++ .../math/geometry3d/continuous/Shape3f.java | 179 + .../math/geometry3d/continuous/Sphere.java | 936 +++++ .../geometry3d/continuous/Transform3D.java | 447 +++ .../geometry3d/continuous/Triangle3f.java | 1017 +++++ .../math/geometry3d/continuous/Tuple3f.java | 710 ++++ .../continuous/Tuple3fComparator.java | 56 + .../continuous/UnmodifiablePoint3f.java | 115 + .../math/geometry3d/continuous/Vector3f.java | 320 ++ .../math/geometry3d/continuous/XYPlane.java | 238 ++ .../math/geometry3d/continuous/XZPlane.java | 213 + .../math/geometry3d/continuous/YZPlane.java | 213 + .../continuous/convexhull/ConvexHull.java | 156 + .../convexhull/ConvexHullAlgorithm.java | 56 + .../convexhull/DivideAndConquerAlgorithm.java | 768 ++++ .../continuous/convexhull/HullEdge3D.java | 112 + .../continuous/convexhull/HullObject.java | 65 + .../continuous/convexhull/HullTriangle3D.java | 624 +++ .../continuous/convexhull/HullVertex3D.java | 102 + .../continuous/spline/AbstractSpline.java | 49 + .../geometry3d/continuous/spline/BSpline.java | 212 + .../continuous/spline/BezierSpline.java | 127 + .../continuous/spline/CatMullRomSpline.java | 251 ++ .../continuous/spline/CoxDeBourSpline.java | 180 + .../continuous/spline/CubicBezierSpline.java | 106 + .../continuous/spline/HermiteSpline.java | 229 ++ .../continuous/spline/LagrangeSpline.java | 85 + .../geometry3d/continuous/spline/Spline.java | 59 + .../afc/math/geometry3d/discrete/Point3i.java | 255 ++ .../afc/math/geometry3d/discrete/Tuple3i.java | 728 ++++ .../discrete/Tuple3iComparator.java | 57 + .../discrete/UnmodifiablePoint3i.java | 115 + .../math/geometry3d/discrete/Vector3i.java | 283 ++ 110 files changed, 50784 insertions(+) create mode 100644 core/math/src/main/java/org/arakhne/afc/math/Matrix2f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/Matrix3f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/Matrix4f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/SingularMatrixException.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry/ClassifierUtil.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry/IntersectionClassifier.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry/IntersectionClassifier2D.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry/IntersectionClassifier3D.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry/IntersectionType.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry/IntersectionUtil.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry/PointFilterXYIterable.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry/PointFilterXYIterator.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry/PointFilterXZIterable.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry/PointFilterXZIterator.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry/PointFilterYZIterable.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry/PointFilterYZIterator.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry/system/CoordinateSystem.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry/system/CoordinateSystem2D.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry/system/CoordinateSystem3D.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry/system/CoordinateSystemConstants.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry/system/CoordinateSystemNotFoundException.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/Path2D.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/PathElement2D.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/Point2D.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/Shape2D.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/Tuple2D.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/Vector2D.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/AbstractRectangularShape2f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/AbstractShape2f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Circle2f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Ellipse2f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/OrientedRectangle2f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Path2f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/PathElement2f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/PathIterator2f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/PathShadow2f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Point2f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Rectangle2f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/RoundRectangle2f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Segment2f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Shape2f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Transform2D.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Tuple2f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Tuple2fComparator.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/UnmodifiablePoint2f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/continuous/Vector2f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/AbstractRectangularShape2i.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/AbstractShape2i.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Circle2i.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Path2i.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/PathElement2i.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/PathIterator2i.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Point2i.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Rectangle2i.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Segment2i.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Shape2i.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Tuple2i.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Tuple2iComparator.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/UnmodifiablePoint2i.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry2d/discrete/Vector2i.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/Point3D.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/Shape3D.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/Tuple3D.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/Vector3D.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/AbstractOrthoPlane.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/AbstractPlane.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/AxisAlignedBox.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Capsule.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/EulerAngle.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Line3f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/OrientedBox.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/OrthoPlane.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/PlanarClassificationType.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Plane.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Plane4f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/PlaneClassifier.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Point3f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Quaternion.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Shape3f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Sphere.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Transform3D.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Triangle3f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Tuple3f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Tuple3fComparator.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/UnmodifiablePoint3f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Vector3f.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/XYPlane.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/XZPlane.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/YZPlane.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/ConvexHull.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/ConvexHullAlgorithm.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/DivideAndConquerAlgorithm.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/HullEdge3D.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/HullObject.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/HullTriangle3D.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/HullVertex3D.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/AbstractSpline.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/BSpline.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/BezierSpline.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/CatMullRomSpline.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/CoxDeBourSpline.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/CubicBezierSpline.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/HermiteSpline.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/LagrangeSpline.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/Spline.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/discrete/Point3i.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/discrete/Tuple3i.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/discrete/Tuple3iComparator.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/discrete/UnmodifiablePoint3i.java create mode 100644 core/math/src/main/java/org/arakhne/afc/math/geometry3d/discrete/Vector3i.java diff --git a/core/math/src/main/java/org/arakhne/afc/math/Matrix2f.java b/core/math/src/main/java/org/arakhne/afc/math/Matrix2f.java new file mode 100644 index 000000000..2754aaa12 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/Matrix2f.java @@ -0,0 +1,1255 @@ +/* + * $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.geometry2d.Vector2D; +import org.arakhne.afc.math.geometry2d.continuous.Point2f; +import org.arakhne.afc.math.geometry2d.continuous.Tuple2f; +import org.arakhne.afc.math.geometry2d.continuous.Vector2f; + +/** + * Is represented internally as a 2x2 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 Matrix2f implements Serializable, Cloneable, MathConstants { + + private static final long serialVersionUID = -181335987517755500L; + + /** + * The first matrix element in the first row. + */ + public float m00; + + /** + * The second matrix element in the first row. + */ + public float m01; + + /** + * The first matrix element in the second row. + */ + public float m10; + + /** + * The second matrix element in the second row. + */ + public float m11; + + /** + * Constructs and initializes a Matrix2d from the specified nine values. + * + * @param m00 + * the [0][0] element + * @param m01 + * the [0][1] element + * @param m10 + * the [1][0] element + * @param m11 + * the [1][1] element + */ + public Matrix2f(float m00, float m01, float m10, float m11) { + this.m00 = m00; + this.m01 = m01; + + this.m10 = m10; + this.m11 = m11; + } + + /** + * Constructs and initializes a Matrix2d from the specified nine- element + * array. + * + * @param v + * the array of length 4 containing in order + */ + public Matrix2f(float[] v) { + this.m00 = v[0]; + this.m01 = v[1]; + + this.m10 = v[2]; + this.m11 = v[3]; + } + + /** + * Constructs a new matrix with the same values as the Matrix2d parameter. + * + * @param m1 + * the source matrix + */ + public Matrix2f(Matrix2f m1) { + this.m00 = m1.m00; + this.m01 = m1.m01; + + this.m10 = m1.m10; + this.m11 = m1.m11; + } + + /** + * Constructs and initializes a Matrix2d to all zeros. + */ + public Matrix2f() { + this.m00 = 0f; + this.m01 = 0f; + + this.m10 = 0f; + this.m11 = 0f; + } + + /** + * Returns a string that contains the values of this Matrix2d. + * + * @return the String representation + */ + @Override + public String toString() { + return this.m00 + ", " //$NON-NLS-1$ + + this.m01 + "\n" //$NON-NLS-1$ + + this.m10 + ", " //$NON-NLS-1$ + + this.m11 + "\n"; //$NON-NLS-1$ + } + + /** + * Sets this Matrix2d to identity. + */ + public final void setIdentity() { + this.m00 = 1f; + this.m01 = 0f; + + this.m10 = 0f; + this.m11 = 1f; + } + + /** + * Sets the specified element of this Matrix2f 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; + default: + throw new ArrayIndexOutOfBoundsException(); + } + break; + + case 1: + switch (column) { + case 0: + this.m10 = value; + break; + case 1: + this.m11 = 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); + default: + break; + } + break; + case 1: + switch (column) { + case 0: + return (this.m10); + case 1: + return (this.m11); + 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, Vector2D v) { + if (row == 0) { + v.set(this.m00, this.m01); + } + else if (row == 1) { + v.set(this.m10, this.m11); + } + 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; + } + else if (row == 1) { + v[0] = this.m10; + v[1] = this.m11; + } + 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, Vector2f v) { + if (column == 0) { + v.set(this.m00, this.m10); + } + else if (column == 1) { + v.set(this.m01, this.m11); + } + 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; + } + else if (column == 1) { + v[0] = this.m01; + v[1] = this.m11; + } + else { + throw new ArrayIndexOutOfBoundsException(); + } + } + + /** + * Sets the specified row of this Matrix2d 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 + */ + public final void setRow(int row, float x, float y) { + switch (row) { + case 0: + this.m00 = x; + this.m01 = y; + break; + + case 1: + this.m10 = x; + this.m11 = y; + break; + + default: + throw new ArrayIndexOutOfBoundsException(); + } + } + + /** + * Sets the specified row of this Matrix2d 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, Vector2f v) { + switch (row) { + case 0: + this.m00 = v.getX(); + this.m01 = v.getY(); + break; + + case 1: + this.m10 = v.getX(); + this.m11 = v.getY(); + break; + + default: + throw new ArrayIndexOutOfBoundsException(); + } + } + + /** + * Sets the specified row of this Matrix2d to the two 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]; + break; + + case 1: + this.m10 = v[0]; + this.m11 = v[1]; + break; + + default: + throw new ArrayIndexOutOfBoundsException(); + } + } + + /** + * Sets the specified column of this Matrix2d to the two values provided. + * + * @param column + * the column number to be modified (zero indexed) + * @param x + * the first row element + * @param y + * the second row element + */ + public final void setColumn(int column, float x, float y) { + switch (column) { + case 0: + this.m00 = x; + this.m10 = y; + break; + + case 1: + this.m01 = x; + this.m11 = y; + break; + + default: + throw new ArrayIndexOutOfBoundsException(); + } + } + + /** + * Sets the specified column of this Matrix2d 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, Vector2f v) { + switch (column) { + case 0: + this.m00 = v.getX(); + this.m10 = v.getY(); + break; + + case 1: + this.m01 = v.getX(); + this.m11 = v.getY(); + break; + + default: + throw new ArrayIndexOutOfBoundsException(); + } + } + + /** + * Sets the specified column of this Matrix2d to the two 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]; + break; + + case 1: + this.m01 = v[0]; + this.m11 = v[1]; + 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.m10 += scalar; + this.m11 += 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, Matrix2f m1) { + this.m00 = m1.m00 + scalar; + this.m01 = m1.m01 + scalar; + + this.m10 = m1.m10 + scalar; + this.m11 = m1.m11 + 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(Matrix2f m1, Matrix2f m2) { + this.m00 = m1.m00 + m2.m00; + this.m01 = m1.m01 + m2.m01; + + this.m10 = m1.m10 + m2.m10; + this.m11 = m1.m11 + m2.m11; + } + + /** + * Sets the value of this matrix to the sum of itself and matrix m1. + * + * @param m1 + * the other matrix + */ + public final void add(Matrix2f m1) { + this.m00 += m1.m00; + this.m01 += m1.m01; + + this.m10 += m1.m10; + this.m11 += m1.m11; + } + + /** + * 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(Matrix2f m1, Matrix2f m2) { + this.m00 = m1.m00 - m2.m00; + this.m01 = m1.m01 - m2.m01; + + this.m10 = m1.m10 - m2.m10; + this.m11 = m1.m11 - m2.m11; + } + + /** + * 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(Matrix2f m1) { + this.m00 -= m1.m00; + this.m01 -= m1.m01; + + this.m10 -= m1.m10; + this.m11 -= m1.m11; + } + + /** + * 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; + } + + /** + * Sets the value of this matrix to the transpose of the argument matrix. + * + * @param m1 + * the matrix to be transposed + */ + public final void transpose(Matrix2f m1) { + if (this != m1) { + this.m00 = m1.m00; + this.m01 = m1.m10; + + this.m10 = m1.m01; + this.m11 = m1.m11; + } + else { + transpose(); + } + } + + /** + * Sets the value of this matrix to the value of the Matrix2d argument. + * + * @param m1 + * the source Matrix2d + */ + public final void set(Matrix2f m1) { + this.m00 = m1.m00; + this.m01 = m1.m01; + + this.m10 = m1.m10; + this.m11 = m1.m11; + } + + /** + * Sets the values in this Matrix2d equal to the row-major array parameter + * (ie, the first two elements of the array will be copied into the first + * row of this matrix, etc.). + * + * @param m + * the double precision array of length 4 + */ + public final void set(float[] m) { + this.m00 = m[0]; + this.m01 = m[1]; + + this.m10 = m[2]; + this.m11 = m[4]; + } + + /** + * Set the components of the matrix. + * + * @param m00 + * the [0][0] element + * @param m01 + * the [0][1] element + * @param m10 + * the [1][0] element + * @param m11 + * the [1][1] element + */ + public void set(float m00, float m01, float m10, float m11) { + this.m00 = m00; + this.m01 = m01; + + this.m10 = m10; + this.m11 = m11; + } + + /** + * Computes the determinant of this matrix. + * + * @return the determinant of the matrix + */ + public final float determinant() { + return this.m00*this.m11 - this.m01*this.m10; + } + + /** + * 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.m10 *= scalar; + this.m11 *= 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, Matrix2f m1) { + this.m00 = scalar * m1.m00; + this.m01 = scalar * m1.m01; + + this.m10 = scalar * m1.m10; + this.m11 = scalar * m1.m11; + } + + /** + * Sets the value of this matrix to the result of multiplying itself with + * matrix m1. + * + * @param m1 + * the other matrix + */ + public final void mul(Matrix2f m1) { + float _m00, _m01, _m10, _m11; + + _m00 = this.m00 * m1.m00 + this.m01 * m1.m10; + _m01 = this.m00 * m1.m01 + this.m01 * m1.m11; + + _m10 = this.m10 * m1.m00 + this.m11 * m1.m10; + _m11 = this.m10 * m1.m01 + this.m11 * m1.m11; + + this.m00 = _m00; + this.m01 = _m01; + this.m10 = _m10; + this.m11 = _m11; + } + + /** + * Multiply this matrix by the given vector and replies the resulting vector. + * + * @param v + * @return this * v + */ + public final Vector2f mul(Vector2D v) { + Vector2f r = new Vector2f(); + r.set( + this.m00 * v.getX() + this.m01 * v.getY(), + this.m10 * v.getX() + this.m11 * v.getY()); + return r; + } + + /** + * 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(Matrix2f m1, Matrix2f m2) { + if (this != m1 && this != m2) { + this.m00 = m1.m00 * m2.m00 + m1.m01 * m2.m10; + this.m01 = m1.m00 * m2.m01 + m1.m01 * m2.m11; + + this.m10 = m1.m10 * m2.m00 + m1.m11 * m2.m10; + this.m11 = m1.m10 * m2.m01 + m1.m11 * m2.m11; + } + else { + float _m00, _m01, _m10, _m11; // vars for temp result matrix + + _m00 = m1.m00 * m2.m00 + m1.m01 * m2.m10; + _m01 = m1.m00 * m2.m01 + m1.m01 * m2.m11; + + _m10 = m1.m10 * m2.m00 + m1.m11 * m2.m10; + _m11 = m1.m10 * m2.m01 + m1.m11 * m2.m11; + + this.m00 = _m00; + this.m01 = _m01; + this.m10 = _m10; + this.m11 = _m11; + } + } + + /** + * 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(Matrix2f m1, Matrix2f m2) { + if (this != m1 && this != m2) { + this.m00 = m1.m00 * m2.m00 + m1.m10 * m2.m01; + this.m01 = m1.m00 * m2.m10 + m1.m10 * m2.m11; + + this.m10 = m1.m01 * m2.m00 + m1.m11 * m2.m01; + this.m11 = m1.m01 * m2.m10 + m1.m11 * m2.m11; + } + else { + float _m00, _m01, _m10, _m11; // vars for temp result matrix + + _m00 = m1.m00 * m2.m00 + m1.m10 * m2.m01; + _m01 = m1.m00 * m2.m10 + m1.m10 * m2.m11; + + _m10 = m1.m01 * m2.m00 + m1.m11 * m2.m01; + _m11 = m1.m01 * m2.m10 + m1.m11 * m2.m11; + + this.m00 = _m00; + this.m01 = _m01; + this.m10 = _m10; + this.m11 = _m11; + } + + } + + /** + * 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(Matrix2f m1, Matrix2f m2) { + if (this != m1 && this != m2) { + this.m00 = m1.m00 * m2.m00 + m1.m01 * m2.m01; + this.m01 = m1.m00 * m2.m10 + m1.m01 * m2.m11; + + this.m10 = m1.m10 * m2.m00 + m1.m11 * m2.m01; + this.m11 = m1.m10 * m2.m10 + m1.m11 * m2.m11; + } + else { + float _m00, _m01, _m10, _m11; // vars for temp result matrix + + _m00 = m1.m00 * m2.m00 + m1.m01 * m2.m01; + _m01 = m1.m00 * m2.m10 + m1.m01 * m2.m11; + + _m10 = m1.m10 * m2.m00 + m1.m11 * m2.m01; + _m11 = m1.m10 * m2.m10 + m1.m11 * m2.m11; + + this.m00 = _m00; + this.m01 = _m01; + this.m10 = _m10; + this.m11 = _m11; + } + } + + /** + * 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(Matrix2f m1, Matrix2f m2) { + if (this != m1 && this != m2) { + this.m00 = m1.m00 * m2.m00 + m1.m10 * m2.m10; + this.m01 = m1.m00 * m2.m01 + m1.m10 * m2.m11; + + this.m10 = m1.m01 * m2.m00 + m1.m11 * m2.m10; + this.m11 = m1.m01 * m2.m01 + m1.m11 * m2.m11; + } + else { + float _m00, _m01, _m10, _m11; // vars for temp result matrix + + _m00 = m1.m00 * m2.m00 + m1.m10 * m2.m10; + _m01 = m1.m00 * m2.m01 + m1.m10 * m2.m11; + + _m10 = m1.m01 * m2.m00 + m1.m11 * m2.m10; + _m11 = m1.m01 * m2.m01 + m1.m11 * m2.m11; + + this.m00 = _m00; + this.m01 = _m01; + this.m10 = _m10; + this.m11 = _m11; + } + } + + /** + * Perform cross product normalization of this matrix. + */ + public final void normalizeCP() { + double mag = 1.0 / Math.sqrt(this.m00 * this.m00 + this.m10 * this.m10); + this.m00 = (float)(this.m00 * mag); + this.m10 = (float)(this.m10 * mag); + + mag = 1.0 / Math.sqrt(this.m01 * this.m01 + this.m11 * this.m11); + this.m01 = (float)(this.m01 * mag); + this.m11 = (float)(this.m11 * mag); + } + + /** + * 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(Matrix2f m1) { + double mag = 1.0 / Math.sqrt(m1.m00 * m1.m00 + m1.m10 * m1.m10); + this.m00 = (float)(m1.m00 * mag); + this.m10 = (float)(m1.m10 * mag); + + mag = 1.0 / Math.sqrt(m1.m01 * m1.m01 + m1.m11 * m1.m11); + this.m01 = (float)(m1.m01 * mag); + this.m11 = (float)(m1.m11 * mag); + } + + /** + * Returns true if all of the data members of Matrix2d m1 are equal to the + * corresponding data members in this Matrix2d. + * + * @param m1 + * the matrix with which the comparison is made + * @return true or false + */ + public boolean equals(Matrix2f m1) { + try { + return (this.m00 == m1.m00 && this.m01 == m1.m01 + && this.m10 == m1.m10 + && this.m11 == m1.m11); + } + catch (NullPointerException e2) { + return false; + } + } + + /** + * Returns true if the Object t1 is of type Matrix2d and all of the data + * members of t1 are equal to the corresponding data members in this + * Matrix2d. + * + * @param t1 + * the matrix with which the comparison is made + * @return true or false + */ + @Override + public boolean equals(Object t1) { + try { + Matrix2f m2 = (Matrix2f) t1; + return (this.m00 == m2.m00 && this.m01 == m2.m01 + && this.m10 == m2.m10 + && this.m11 == m2.m11); + } + catch (ClassCastException e1) { + return false; + } + catch (NullPointerException e2) { + return false; + } + } + + /** + * Returns a hash code value based on the data values in this object. Two + * different Matrix2d objects with identical data values (i.e., + * Matrix2d.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.m10); + bits = 31L * bits + floatToIntBits(this.m11); + 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.m10 = 0f; + this.m11 = 0f; + } + + /** + * Sets this matrix as diagonal. + * + * @param m00 + * the first element of the diagonal + * @param m11 + * the second element of the diagonal + */ + public final void setDiagonal(float m00, float m11) { + this.m00 = m00; + this.m01 = 0f; + this.m10 = 0f; + this.m11 = m11; + } + + /** + * Negates the value of this matrix: this = -this. + */ + public final void negate() { + this.m00 = -this.m00; + this.m01 = -this.m01; + + this.m10 = -this.m10; + this.m11 = -this.m11; + } + + /** + * Sets the value of this matrix equal to the negation of of the Matrix2d + * parameter. + * + * @param m1 + * the source matrix + */ + public final void negate(Matrix2f m1) { + this.m00 = -m1.m00; + this.m01 = -m1.m01; + + this.m10 = -m1.m10; + this.m11 = -m1.m11; + } + + /** + * 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 Matrix2f clone() { + Matrix2f m1 = null; + try { + m1 = (Matrix2f) 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 m00. + */ + 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 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; + } + + /** 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 Vector2f cov(Vector2f... 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 Vector2f cov(Point2f... 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 Vector2f cov(Iterable> tuples) { + setZero(); + + // Compute the mean m + Vector2f m = new Vector2f(); + int count = 0; + for(Tuple2f p : tuples) { + m.add(p.getX(), p.getY()); + ++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(Tuple2f 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 + //cov.m10 += (p.getY() - m.getY()) * (p.getX() - m.getX()); // same as m01 + this.m11 += (p.getY() - m.getY()) * (p.getY() - m.getY()); + } + + this.m00 /= count; + this.m01 /= count; + this.m10 = this.m01; + this.m11 /= 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; + } + + /** + * Compute the eigenvectors of this matrix, assuming it is a symmetric matrix + * according to the Jacobi Cyclic Method. + * + * @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 #eigenVectorsOfSymmetricMatrix(Matrix2f) + */ + public float[] eigenVectorsOfSymmetricMatrix(Matrix2f eigenVectors) { + assert(eigenVectors!=null); + + // Copy values up to the diagonal + float m11 = getElement(0,0); + float m12 = getElement(0,1); + float m22 = getElement(1,1); + + boolean sweepsConsumed = true; + int i; + float u, u2, u2p1, t, c, s; + float ri0, ri1; + + for(int a=0; a + * 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: + *

+ * + * @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 >= 0
    • + *
  • lowerx <= upperx
    • + *
  • lowery <= 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. + *

Dividing axis
+ * 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. + *

Projecting polygons onto a 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. + *

+ * + * + * + * + * + * + * + * + * + * + * + * + * + *
tresult
INSIDEENCLOSING
OUTSIDEOUTSIDE
SPANNINGSPANNING
ENCLOSINGINSIDE
SAMESAME
+ * + * @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. + *

+ * + * + * + * + * + * + * + * + * + * + * + * + * + *
tresult
INSIDEENCLOSING
OUTSIDEOUTSIDE
SPANNINGSPANNING
ENCLOSINGINSIDE
SAMESAME
+ * + * @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. + *

+ * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + *
f1f2result
INSIDEINSIDEINSIDE
INSIDEOUTSIDEINSIDE
INSIDESPANNINGINSIDE
INSIDEENCLOSINGINSIDE
INSIDESAMEINSIDE
OUTSIDEINSIDEINSIDE
OUTSIDEOUTSIDEOUTSIDE
OUTSIDESPANNINGSPANNING
OUTSIDEENCLOSINGSPANNING
OUTSIDESAMESPANNING
SPANNINGINSIDEINSIDE
SPANNINGOUTSIDESPANNING
SPANNINGSPANNINGSPANNING
SPANNINGENCLOSINGSPANNING
SPANNINGSAMESPANNING
ENCLOSINGINSIDEINSIDE
ENCLOSINGOUTSIDESPANNING
ENCLOSINGSPANNINGSPANNING
ENCLOSINGENCLOSINGENCLOSING
ENCLOSINGSAMESPANNING
SAMEINSIDEINSIDE
SAMEOUTSIDESPANNING
SAMESPANNINGSPANNING
SAMEENCLOSINGSPANNING
SAMESAMESAME
+ * + * @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. + *

+ * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + *
thisf2result
INSIDEINSIDEINSIDE
INSIDEOUTSIDEINSIDE
INSIDESPANNINGINSIDE
INSIDEENCLOSINGINSIDE
INSIDESAMEINSIDE
OUTSIDEINSIDEINSIDE
OUTSIDEOUTSIDEOUTSIDE
OUTSIDESPANNINGSPANNING
OUTSIDEENCLOSINGSPANNING
OUTSIDESAMESPANNING
SPANNINGINSIDEINSIDE
SPANNINGOUTSIDESPANNING
SPANNINGSPANNINGSPANNING
SPANNINGENCLOSINGSPANNING
SPANNINGSAMESPANNING
ENCLOSINGINSIDEINSIDE
ENCLOSINGOUTSIDESPANNING
ENCLOSINGSPANNINGSPANNING
ENCLOSINGENCLOSINGENCLOSING
ENCLOSINGSAMEENCLOSING
SAMEINSIDEINSIDE
SAMEOUTSIDESPANNING
SAMESPANNINGSPANNING
SAMEENCLOSINGENCLOSING
SAMESAMESAME
+ * + * @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. + *

+ * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + *
tcresult
INSIDEINSIDEINSIDE
INSIDEOUTSIDEOUTSIDE
INSIDESPANNINGSPANNING
INSIDEENCLOSINGSPANNING
INSIDESAMEINSIDE
OUTSIDEINSIDEOUTSIDE
OUTSIDEOUTSIDEOUTSIDE
OUTSIDESPANNINGOUTSIDE
OUTSIDEENCLOSINGOUTSIDE
OUTSIDESAMEOUTSIDE
SPANNINGINSIDESPANNING
SPANNINGOUTSIDEOUTSIDE
SPANNINGSPANNINGSPANNING
SPANNINGENCLOSINGSPANNING
SPANNINGSAMESPANNING
ENCLOSINGINSIDESPANNING
ENCLOSINGOUTSIDEOUTSIDE
ENCLOSINGSPANNINGSPANNING
ENCLOSINGENCLOSINGENCLOSING
ENCLOSINGSAMEENCLOSING
SAMEINSIDEINSIDE
SAMEOUTSIDEOUTSIDE
SAMESPANNINGSPANNING
SAMEENCLOSINGENCLOSING
SAMESAMESAME
+ * + * @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. + *

+ * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + *
thiscresult
INSIDEINSIDEINSIDE
INSIDEOUTSIDEOUTSIDE
INSIDESPANNINGSPANNING
INSIDEENCLOSINGSPANNING
INSIDESAMESPANNING
OUTSIDEINSIDEOUTSIDE
OUTSIDEOUTSIDEOUTSIDE
OUTSIDESPANNINGOUTSIDE
OUTSIDEENCLOSINGOUTSIDE
OUTSIDESAMEOUTSIDE
SPANNINGINSIDESPANNING
SPANNINGOUTSIDEOUTSIDE
SPANNINGSPANNINGSPANNING
SPANNINGENCLOSINGSPANNING
SPANNINGSAMESPANNING
ENCLOSINGINSIDESPANNING
ENCLOSINGOUTSIDEOUTSIDE
ENCLOSINGSPANNINGSPANNING
ENCLOSINGENCLOSINGENCLOSING
ENCLOSINGSAMEENCLOSING
SAMEINSIDEINSIDE
SAMEOUTSIDEOUTSIDE
SAMESPANNINGSPANNING
SAMEENCLOSINGENCLOSING
SAMESAMESAME
+ * + * @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. + *

+ *

Point-Segment Intersection Picture 1 + * Point-Segment Intersection Picture 2
+ *

+ * 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). + *

+ *

Point-Segment Intersection Picture 3
+ *

+ * 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?forceCoplanarintersectsPointTrangle() Result
truetruetruetrue
truetruefalsetrue
truefalsetruefalse
truefalsefalsefalse
falsetruetruefalse
falsetruefalsetrue
falsefalsetruefalse
falsefalsefalsefalse
+ *

+ * 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. + *

+ *

Point-Segment Intersection Picture 1 + * Point-Segment Intersection Picture 2
+ *

+ * 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). + *

+ *

Point-Segment Intersection Picture 3
+ *

+ * 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?forceCollinearintersectsPointSegment() Result
truetruetruetrue
truetruefalsetrue
truefalsetruefalse
truefalsefalsefalse
falsetruetruefalse
falsetruefalsetrue
falsefalsetruefalse
falsefalsefalsefalse
+ * + * @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] + */ + XY_RIGHT_HAND, + + /** Left handed XY coordinate system. + *

+ * [Left Handed XY Coordinate System] + */ + 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] + *
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] + *
+ * + * @author $Author: sgalland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public enum CoordinateSystem3D implements CoordinateSystem { + + /** Left handed XZY coordinate system. + *

+ * [Left Handed XZY Coordinate System] + */ + XZY_LEFT_HAND(0,1,/* */1,0), + + /** Left handed XYZ coordinate system. + *

+ * [Left Handed XYZ Coordinate System] + */ + XYZ_LEFT_HAND(-1,0,/* */0,1), + + /** Right handed XZY coordinate system. + *

+ * [Right Handed XZY Coordinate System] + */ + XZY_RIGHT_HAND(0,-1,/* */1,0), + + /** Right handed XYZ coordinate system. + *

+ * [Right Handed XYZ Coordinate System] + */ + 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: + * + * + * + * + * + * + * + * + * + *
quadrantxy
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()this.upper.getC()) { + d3 = p.getZ() - this.upper.getC(); + d3 = d3*d3; + } + return d1+d2+d3; + } + + /** + * {@inheritDoc} + */ + @Override + public EuclidianPoint3D nearestPoint(Point3f reference) { + if (!isInit()) return null; + EuclidianPoint3D nearest = new EuclidianPoint3D(); + + if (reference.getX()this.upper.getA()) { + nearest.setA(this.upper.getA()); + } + else { + nearest.setA(reference.getX()); + } + + if (reference.getY()this.upper.getB()) { + nearest.setB(this.upper.getB()); + } + else { + nearest.setB(reference.getY()); + } + + if (reference.getZ()this.upper.getC()) { + nearest.setC(this.upper.getC()); + } + else { + nearest.setC(reference.getZ()); + } + + return nearest; + } + + /** + * {@inheritDoc} + */ + @Override + public float distanceMaxSquared(Point3f p) { + if (!isInit()) return Float.NaN; + float d1 = 0; + if (p.getX()this.upper.getA()) { + d1 = p.getX() - this.lower.getA(); + } + else d1 = Math.max(p.getX()-this.lower.getA(), this.upper.getA()-p.getX()); + + float d2 = 0; + if (p.getY()this.upper.getB()) { + d2 = p.getY() - this.lower.getB(); + } + else d2 = Math.max(p.getY()-this.lower.getB(), this.upper.getB()-p.getY()); + + float d3 = 0; + if (p.getZ()this.upper.getC()) { + d3 = p.getZ() - this.lower.getC(); + } + else d3 = Math.max(p.getZ()-this.lower.getC(), this.upper.getC()-p.getZ()); + + return d1*d1+d2*d2+d3*d3; + } + + /** + * {@inheritDoc} + */ + @Override + public EuclidianPoint3D farestPoint(Point3f reference) { + if (!isInit()) return null; + EuclidianPoint3D farest = new EuclidianPoint3D(); + + if (reference.getX()this.upper.getA()) { + farest.setA(this.lower.getA()); + } + else { + float dl = Math.abs(reference.getX()-this.lower.getA()); + float du = Math.abs(reference.getX()-this.upper.getA()); + if (dl>du) { + farest.setA(this.lower.getA()); + } + else { + farest.setA(this.upper.getA()); + } + } + + if (reference.getY()this.upper.getB()) { + farest.setB(this.lower.getB()); + } + else { + float dl = Math.abs(reference.getY()-this.lower.getB()); + float du = Math.abs(reference.getY()-this.upper.getB()); + if (dl>du) { + farest.setB(this.lower.getB()); + } + else { + farest.setB(this.upper.getB()); + } + } + + if (reference.getZ()this.upper.getC()) { + farest.setC(this.lower.getC()); + } + else { + float dl = Math.abs(reference.getZ()-this.lower.getC()); + float du = Math.abs(reference.getZ()-this.upper.getC()); + if (dl>du) { + farest.setC(this.lower.getC()); + } + else { + farest.setC(this.upper.getC()); + } + } + + return farest; + } + + /** + * {@inheritDoc} + */ + @Override + public void translate(Vector3f v) { + if (isInit()) { + this.lower.setA(this.lower.getA() + v.getX()); + this.lower.setB(this.lower.getB() + v.getY()); + this.lower.setC(this.lower.getC() + v.getZ()); + this.upper.setA(this.upper.getA() + v.getX()); + this.upper.setB(this.upper.getB() + v.getY()); + this.upper.setC(this.upper.getC() + v.getZ()); + this.vertices = null; + } + } + + /** + * {@inheritDoc} + */ + @Override + public void setTranslation(Point3f position) { + if (isInit()) { + Point3f c = getPosition(); + assert(c!=null); + this.lower.sub(c); + this.upper.sub(c); + this.lower.add(position); + this.upper.add(position); + this.vertices = null; + } + } + + /** + * Replies the height of this box. + * + * @return the height of this box. + */ + private float getHeight(CoordinateSystem3D cs) { + return (cs.getHeightCoordinateIndex()==1) + ? getSizeY() + : getSizeZ(); + } + + /** + * {@inheritDoc} + */ + @Override + public void setTranslation(Point3f position, boolean onGround) { + if (isInit()) { + Point3f c = getPosition(); + assert(c!=null); + this.lower.sub(c); + this.upper.sub(c); + if (onGround) { + CoordinateSystem3D cs = CoordinateSystem3D.getDefaultCoordinateSystem(); + Point3f np = new Point3f(position); + cs.addHeight(np, getHeight(cs)/2.f); + this.lower.add(np); + this.upper.add(np); + + } + else { + this.lower.add(position); + this.upper.add(position); + } + this.vertices = null; + } + } + + /** + * {@inheritDoc} + */ + @Override + public MinimumBoundingRectangle toBounds2D() { + return toBounds2D(CoordinateSystem3D.getDefaultCoordinateSystem()); + } + + /** + * {@inheritDoc} + */ + @Override + public MinimumBoundingRectangle toBounds2D(CoordinateSystem3D system) { + if (!isInit()) return new MinimumBoundingRectangle(); + return new MinimumBoundingRectangle( + system.toCoordinateSystem2D(this.lower), + system.toCoordinateSystem2D(this.upper)); + } + + //--------------------------------------------- + // IntersectionClassifier + //--------------------------------------------- + + /** + * {@inheritDoc} + */ + @Override + public IntersectionType classifies(Point3f p) { + if (!isInit()) return IntersectionType.OUTSIDE; + return (this.lower.getA()<=p.getX() && p.getX()<=this.upper.getA() + && + this.lower.getB()<=p.getY() && p.getY()<=this.upper.getB() + && + this.lower.getC()<=p.getZ() && p.getZ()<=this.upper.getC()) + ? IntersectionType.INSIDE + : IntersectionType.OUTSIDE; + } + + /** + * {@inheritDoc} + */ + @Override + public boolean intersects(Point3f p) { + if (!isInit()) return false; + return (this.lower.getA()<=p.getX() && p.getX()<=this.upper.getA() + && + this.lower.getB()<=p.getY() && p.getY()<=this.upper.getB() + && + this.lower.getC()<=p.getZ() && p.getZ()<=this.upper.getC()); + } + + /** + * {@inheritDoc} + */ + @Override + public IntersectionType classifies(Point3f l, Point3f u) { + assert(l.getX()<=u.getX()); + assert(l.getY()<=u.getY()); + assert(l.getZ()<=u.getZ()); + if (!isInit()) return IntersectionType.OUTSIDE; + return IntersectionUtil.classifiesAlignedBoxes( + l, u, this.lower, this.upper); + } + + /** + * {@inheritDoc} + */ + @Override + public boolean intersects(Point3f l, Point3f u) { + assert(l.getX()<=u.getX()); + assert(l.getY()<=u.getY()); + assert(l.getZ()<=u.getZ()); + if (!isInit()) return false; + return IntersectionUtil.intersectsAlignedBoxes( + l, u, this.lower, this.upper); + } + + /** + * {@inheritDoc} + */ + @Override + public IntersectionType classifies(Point3f c, float r) { + assert(c!=null); + assert(r>=0.); + if (!isInit()) return IntersectionType.OUTSIDE; + return IntersectionUtil.classifiesSolidSphereSolidAlignedBox( + c, r, this.lower, this.upper); + } + + /** + * {@inheritDoc} + */ + @Override + public boolean intersects(Point3f c, float r) { + if (!isInit()) return false; + assert(c!=null); + assert(r>=0.); + return IntersectionUtil.intersectsSolidSphereSolidAlignedBox( + c, r, this.lower, this.upper); + } + + /** + * {@inheritDoc} + */ + @Override + public IntersectionType classifies(Plane plane) { + if (!isInit()) return IntersectionType.OUTSIDE; + return (plane.classifies( + this.lower.getA(), this.lower.getB(), this.lower.getC(), + this.upper.getA(), this.upper.getB(), this.upper.getC())== PlanarClassificationType.COINCIDENT) + ? IntersectionType.SPANNING : IntersectionType.OUTSIDE; + } + + /** + * {@inheritDoc} + */ + @Override + public boolean intersects(Plane plane) { + if (!isInit()) return false; + return plane.intersects( + this.lower.getA(), this.lower.getB(), this.lower.getC(), + this.upper.getA(), this.upper.getB(), this.upper.getC()); + } + + /** + * {@inheritDoc} + */ + @Override + public PlanarClassificationType classifiesAgainst(Plane plane) { + if (!isInit()) return null; + return plane.classifies( + this.lower.getA(), this.lower.getB(), this.lower.getC(), + this.upper.getA(), this.upper.getB(), this.upper.getC()); + } + + /** + * {@inheritDoc} + */ + @Override + public IntersectionType classifies(Point3f center, Vector3f[] axis, float[] extent) { + if (!isInit()) return IntersectionType.OUTSIDE; + return IntersectionUtil.classifiesAlignedBoxOrientedBox( + this.lower, this.upper, + center, axis, extent).invert(); + } + + /** + * {@inheritDoc} + */ + @Override + public boolean intersects(Point3f center, Vector3f[] axis, float[] extent) { + if (!isInit()) return false; + return IntersectionUtil.intersectsAlignedBoxOrientedBox( + this.lower, this.upper, + center, axis, extent); + } + +} diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Capsule.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Capsule.java new file mode 100644 index 000000000..ad3df5621 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Capsule.java @@ -0,0 +1,1209 @@ +/* + * $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.Collection; +import java.util.Iterator; +import java.util.LinkedList; +import java.util.NoSuchElementException; + +import org.arakhne.afc.math.MathUtil; +import org.arakhne.afc.math.Matrix4f; +import org.arakhne.afc.math.geometry.ClassifierUtil; +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.BoundingCircle; +import org.arakhne.afc.math.geometry.continuous.object2d.bounds.Bounds2D; +import org.arakhne.afc.math.geometry.continuous.object4d.AxisAngle4f; +import org.arakhne.afc.math.geometry.system.CoordinateSystem3D; +import org.arakhne.afc.math.geometry2d.continuous.OrientedRectangle2f; +import org.arakhne.afc.math.geometry2d.continuous.Point2f; +import org.arakhne.afc.math.geometry2d.continuous.Vector2f; +import org.arakhne.afc.sizediterator.SizedIterator; +import org.arakhne.afc.util.ArrayUtil; +import org.arakhne.afc.util.CollectionUtil; + +/** + * A bounding capsule is a swept sphere (i.e. the volume that a sphere takes as it moves along a straight line segment) containing the object. Capsules can be represented by the radius of the swept sphere and the segment that the sphere is swept across). It has traits similar to a cylinder, but is easier to use, because the intersection test is simpler. A capsule and another object intersect if the distance between the capsule's defining segment and some feature of the other object is smaller than the capsule's radius. For example, two capsules intersect if the distance + * between the capsules' segments is smaller than the sum of their radii. This holds for arbitrarily rotated capsules, which is why they're more appealing than cylinders in practice. + * + * @author $Author: cbohrhauer$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class Capsule extends AbstractCombinableBounds3D implements TranslatableBounds3D, RotatableBounds3D, OrientedCombinableBounds3D { + + private static final long serialVersionUID = -9007714495958355907L; + + /** + * Medial line segment start point, it is also the lower point of the capsule + */ + private Point3f a; + + /** + * Medial line segment endpoint + */ + private Point3f b; + + /** + * Radius + */ + private float radius; + + /** + * The lower corner of the AABB of this capsule + */ + private EuclidianPoint3D lower; + + /** + * The upper corner of the AABB of this capsule + */ + private EuclidianPoint3D upper; + + /** + * The center of the segment AB, also corresponding to the center of the AABB and OBB + */ + private EuclidianPoint3D center; + + /** + * Orthonormal set of vector composing an axis base + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + *
indexexplanation
0R axis
1S axis
2T axis
+ */ + Vector3f[] axis = new Vector3f[3]; + + /** + * Extent of the OBB, cannot be negative. + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + *
indexexplanation
0extent in R axis
1extent in S axis
2extent in T axis
+ */ + float[] extent = new float[3]; + + /** + * The various vertices that compose this capsule + */ + private transient Vector3f[] vertices = null; + + /** + */ + public Capsule() { + this.center = new EuclidianPoint3D(); + } + + /** + * @param a + * is the first point on the capsule's segment. + * @param b + * is the second point on the capsule's segment. + * @param radius + * is the radius of the capsule. + */ + public Capsule(Point3f a, Point3f b, float radius) { + assert ((a != null) && (b != null)); + this.a = a; + this.b = b; + this.radius = radius; + + // A is always the lower point of the capsule reference segment + if (CoordinateSystem3D.getDefaultCoordinateSystem().isZOnUp()) {// Z up + if (this.a.getZ() > this.b.getZ()) {// a not lower, switch with b + Point3f tmp = this.a; + this.a = this.b; + this.b = tmp; + } + } else {// Y up + if (this.a.getY() < this.b.getY()) {// a not lower, switch with b + Point3f tmp = this.a; + this.a = b; + this.b = tmp; + } + } + computeCenter(); + computeAABB(); + //useless computeOBB(); + } + + /** + * {@inheritDoc} + */ + @Override + public final BoundingPrimitiveType3D getBoundType() { + return BoundingPrimitiveType3D.CAPSULE; + } + + @Override + public String toString() { + StringBuilder s= new StringBuilder("Capsule "); //$NON-NLS-1$ + s.append("a: "); //$NON-NLS-1$ + s.append(this.a.toString()); + s.append(" b: "); //$NON-NLS-1$ + s.append(this.b.toString()); + s.append(" radius: "); //$NON-NLS-1$ + s.append(this.radius); + return s.toString(); + } + + /** + * Compute the center point of the capsule and set the center attribute. + */ + private void computeCenter() { + this.center = new EuclidianPoint3D(this.a); + this.center.add(this.b); + this.center.scale(0.5f); + } + + /** + * Compute the AABB around the capsule. + */ + private void computeAABB() { + this.lower = new EuclidianPoint3D(this.a.getX() - this.radius, this.a.getY() - this.radius, this.a.getZ() - this.radius); + this.upper = new EuclidianPoint3D(this.b.getX() + this.radius, this.b.getY() + this.radius, this.b.getZ() + this.radius); + } + + /** + * Compute the OBB around the capsule. + */ + private void computeOBB() { + this.axis = new Vector3f[3]; + + this.axis[0] = new Vector3f(this.b); + this.axis[0].sub(this.a); + + Vector2f y2d = MathUtil.perpendicularVector(this.axis[0].getX(), this.axis[0].getY()); + this.axis[1] = new Vector3f(y2d.getX(), y2d.getY(), this.axis[0].getZ()); + + this.axis[2] = new Vector3f(); + this.axis[2].cross(this.axis[0], this.axis[1]); + + this.extent = new float[3]; + this.extent[0] = this.axis[0].length() / 2 + this.radius; + this.extent[1] = this.radius; + this.extent[2] = this.axis[2].length() / 2 + this.radius; + + this.axis[1].normalize(); + this.axis[2].normalize(); + this.axis[3].normalize(); + } + + /** + * {@inheritDoc} + */ + @Override + public Bounds2D toBounds2D() { + return toBounds2D(CoordinateSystem3D.getDefaultCoordinateSystem()); + } + + /** + * {@inheritDoc} + */ + @Override + public Bounds2D toBounds2D(CoordinateSystem3D system) { + Point2f a2d = system.toCoordinateSystem2D(this.a); + Point2f b2d = system.toCoordinateSystem2D(this.b); + Point2f center2d = system.toCoordinateSystem2D(this.center); + /** + specific configuration when a = b capsule in 2D = circle + */ + if ((this.a.getX() == this.b.getX()) && (this.a.getY() == this.b.getY())) { + return new BoundingCircle(new Point2f(this.a.getX(),this.a.getY()),this.radius); + } + + Vector2f[] axis = new Vector2f[2]; + axis[0] = new Vector2f(b2d); + axis[0].sub(a2d); + axis[1] = MathUtil.perpendicularVector(axis[0]); + float[] extent = new float[2]; + extent[0] = axis[0].length() / 2 + this.radius; + extent[1] = this.radius; + + axis[0].normalize(); + axis[1].normalize(); + return new OrientedRectangle2f(center2d, axis, extent); + } + + /** + * Replies the first point of the capsule's segment. + * + * @return the first point of the capsule's segment. + */ + public Point3f getA() { + return this.a; + } + + /** + * Replies the second point of the capsule's segment. + * + * @return the second point of the capsule's segment. + */ + public Point3f getB() { + return this.b; + } + + /** + * Replies the radius of the capsule. + * + * @return the radius of the capsule. + */ + public float getRadius() { + return this.radius; + } + + /** + * {@inheritDoc} + */ + @Override + public float getSizeX() { + return this.upper.getA() - this.lower.getA(); + } + + /** + * {@inheritDoc} + */ + @Override + public float getSizeY() { + return this.upper.getB() - this.lower.getB(); + } + + /** + * {@inheritDoc} + */ + @Override + public float getSizeZ() { + return this.upper.getC() - this.lower.getC(); + } + + /** + * {@inheritDoc} + */ + @Override + public EuclidianPoint3D getCenter() { + return this.center; + } + + /** + * {@inheritDoc} + */ + @Override + public EuclidianPoint3D getLower() { + return this.lower; + } + + /** + * {@inheritDoc} + */ + @Override + public void getLowerUpper(Point3f lower, Point3f upper) { + lower.set(this.lower); + upper.set(this.upper); + + } + + /** + * {@inheritDoc} + */ + @Override + public Vector3f getSize() { + return new Vector3f(this.upper.getA() - this.lower.getA(), this.upper.getB() - this.lower.getB(), this.upper.getC() - this.lower.getC()); + } + + /** + * {@inheritDoc} + */ + @Override + public EuclidianPoint3D getUpper() { + return this.upper; + } + + /** + * {@inheritDoc} + */ + @Override + public Vector3f getR() { + return this.axis[0]; + } + + /** + * {@inheritDoc} + */ + @Override + public float getRExtent() { + return this.extent[0]; + } + + /** + * {@inheritDoc} + */ + @Override + public Vector3f getS() { + return this.axis[1]; + } + + /** + * {@inheritDoc} + */ + @Override + public float getSExtent() { + return this.extent[1]; + } + + /** + * {@inheritDoc} + */ + @Override + public Vector3f getT() { + return this.axis[2]; + } + + /** + * {@inheritDoc} + */ + @Override + public float getTExtent() { + return this.extent[2]; + } + + /** + * {@inheritDoc} + */ + @Override + public void setTranslation(Point3f position) { + Vector3f translation = new Vector3f(position); + translation.sub(this.center); + translate(translation); + } + + /** + * {@inheritDoc} + */ + @Override + public void setTranslation(Point3f position, boolean onGround) { + if (onGround) {// a is moving, position defines the new position of a + Vector3f translation = new Vector3f(position); + translation.sub(this.a); + translate(translation); + } else { + setTranslation(position); + } + } + + /** + * {@inheritDoc} + */ + @Override + public void translate(Vector3f translation) { + this.a.add(translation); + this.b.add(translation); + this.center.add(translation); + this.lower.add(translation); + this.upper.add(translation); + } + + /** + * {@inheritDoc} + */ + @Override + public void rotate(AxisAngle4f rotation) { + Matrix4f m = new Matrix4f(); + m.set(rotation); + Vector3f translateToOrigin = new Vector3f(this.center); + translateToOrigin.negate(); + // Translate to origin + this.a.add(translateToOrigin); + this.b.add(translateToOrigin); + // Rotate + m.transform(this.a); + m.transform(this.b); + // Translate to initial position + this.a.add(this.center); + this.b.add(this.center); + computeAABB(); + computeOBB(); + } + + /** + * {@inheritDoc} + */ + @Override + public void rotate(Quaternion rotation) { + Matrix4f m = new Matrix4f(); + m.set(rotation); + Vector3f translateToOrigin = new Vector3f(this.center); + translateToOrigin.negate(); + // Translate to origin + this.a.add(translateToOrigin); + this.b.add(translateToOrigin); + // Rotate + m.transform(this.a); + m.transform(this.b); + // Translate to initial position + this.a.add(this.center); + this.b.add(this.center); + computeAABB(); + computeOBB(); + } + + /** + * {@inheritDoc} + */ + @Override + public void setRotation(AxisAngle4f rotation) { + // The orignal capsule is vertical grounded by A (0,0,0) + Vector3f ab = new Vector3f(this.a); + ab.sub(this.b); + + this.a = new Point3f(); + this.b = new Point3f(); + // A is always the lower point of the capsule reference segment + if (CoordinateSystem3D.getDefaultCoordinateSystem().isZOnUp()) {// Z up + this.b.add(new Vector3f(0, 0, ab.length())); + } else {// Y up + this.b.add(new Vector3f(0, ab.length(), 0)); + } + + computeCenter(); + Matrix4f m = new Matrix4f(); + m.set(rotation); + Vector3f translateToOrigin = new Vector3f(this.center); + translateToOrigin.negate(); + // Translate to origin + this.a.add(translateToOrigin); + this.b.add(translateToOrigin); + // Rotate + m.transform(this.a); + m.transform(this.b); + // Translate to initial position + this.a.add(this.center); + this.b.add(this.center); + computeAABB(); + computeOBB(); + } + + /** + * {@inheritDoc} + */ + @Override + public void setRotation(Quaternion rotation) { + // The orignal capsule is vertical grounded by A (0,0,0) + Vector3f ab = new Vector3f(this.a); + ab.sub(this.b); + + this.a = new Point3f(); + this.b = new Point3f(); + // A is always the lower point of the capsule reference segment + if (CoordinateSystem3D.getDefaultCoordinateSystem().isZOnUp()) {// Z up + this.b.add(new Vector3f(0, 0, ab.length())); + } else {// Y up + this.b.add(new Vector3f(0, ab.length(), 0)); + } + + computeCenter(); + Matrix4f m = new Matrix4f(); + m.set(rotation); + Vector3f translateToOrigin = new Vector3f(this.center); + translateToOrigin.negate(); + // Translate to origin + this.a.add(translateToOrigin); + this.b.add(translateToOrigin); + // Rotate + m.transform(this.a); + m.transform(this.b); + // Translate to initial position + this.a.add(this.center); + this.b.add(this.center); + computeAABB(); + computeOBB(); + } + + /** + * {@inheritDoc} + */ + @Override + public boolean isEmpty() { + return false; + } + + /** + * {@inheritDoc} + */ + @Override + public boolean isInit() { + return true; + } + + /** + * {@inheritDoc} + */ + @Override + public float distanceMaxSquared(Point3f reference) { + Vector3f refToA = new Vector3f(this.a); + refToA.sub(reference); + Vector3f refToB = new Vector3f(this.b); + refToB.sub(reference); + float refToAlength = refToA.length(); + float refToBlength = refToB.length(); + + if (refToAlength > refToBlength) { // A farest + return (refToAlength + this.radius) * (refToAlength + this.radius); + } + // B farest + return (refToBlength + this.radius) * (refToBlength + this.radius); + } + + /** + * {@inheritDoc} + */ + @Override + public EuclidianPoint3D farestPoint(Point3f reference) { + Vector3f refToA = new Vector3f(this.a); + refToA.sub(reference); + Vector3f refToB = new Vector3f(this.b); + refToB.sub(reference); + float refToAlength = refToA.length(); + float refToBlength = refToB.length(); + + if (refToAlength > refToBlength) { // A farest + refToA.normalize(); + refToA.scale(this.radius); + + EuclidianPoint3D farest = new EuclidianPoint3D(this.a); + farest.add(refToA); + return farest; + } + + // B farest + refToB.normalize(); + refToB.scale(this.radius); + + EuclidianPoint3D farest = new EuclidianPoint3D(this.b); + farest.add(refToB); + return farest; + } + + /** + * {@inheritDoc} + */ + @Override + public float distanceSquared(Point3f reference) { + EuclidianPoint3D nearestPoint = new EuclidianPoint3D(); + MathUtil.getNearestPointOnSegment(reference.getX(), reference.getY(), reference.getZ(), this.a.getX(), this.a.getY(), this.a.getZ(), this.b.getX(), this.b.getY(), this.b.getZ(), nearestPoint); + + Vector3f v = new Vector3f(nearestPoint); + v.sub(reference); + return (v.length() - this.radius) * (v.length() - this.radius); + } + + /** + * {@inheritDoc} + */ + @Override + public EuclidianPoint3D nearestPoint(Point3f reference) { + EuclidianPoint3D nearestPoint = new EuclidianPoint3D(); + MathUtil.getNearestPointOnSegment(reference.getX(), reference.getY(), reference.getZ(), this.a.getX(), this.a.getY(), this.a.getZ(), this.b.getX(), this.b.getY(), this.b.getZ(), nearestPoint); + + Vector3f v = new Vector3f(nearestPoint); + v.sub(reference); + v.normalize(); + v.scale(this.radius); + nearestPoint.sub(v); + return nearestPoint; + } + + /** + * Computes OBB vertices + * + * @return OBB vertices + */ + private Vector3f[] computeCapsuleVertices() { + this.vertices = new Vector3f[8]; + + Vector3f Rpos = new Vector3f(this.axis[0]); + Rpos.scale(this.extent[0]); + Vector3f Rneg = new Vector3f(Rpos); + Rneg.negate(); + Vector3f Spos = new Vector3f(this.axis[1]); + Spos.scale(this.extent[1]); + Vector3f Sneg = new Vector3f(Spos); + Sneg.negate(); + Vector3f Tpos = new Vector3f(this.axis[2]); + Tpos.scale(this.extent[2]); + Vector3f Tneg = new Vector3f(Tpos); + Tneg.negate(); + + this.vertices[0] = MathUtil.add(Rpos, Spos, Tpos); + this.vertices[1] = MathUtil.add(Rneg, Spos, Tpos); + this.vertices[2] = MathUtil.add(Rneg, Sneg, Tpos); + this.vertices[3] = MathUtil.add(Rpos, Sneg, Tpos); + this.vertices[4] = MathUtil.add(Rpos, Sneg, Tneg); + this.vertices[5] = MathUtil.add(Rpos, Spos, Tneg); + this.vertices[6] = MathUtil.add(Rneg, Spos, Tneg); + this.vertices[7] = MathUtil.add(Rneg, Sneg, Tneg); + + return this.vertices; + } + + /** + * {@inheritDoc} + */ + @Override + public SizedIterator getGlobalOrientedBoundVertices() { + return new LocalToGlobalVertexIterator(getCenter(), getLocalOrientedBoundVertices()); + } + + /** + * {@inheritDoc} + */ + @Override + public EuclidianPoint3D getGlobalVertexAt(int index) { + + Vector3f[] vertices = computeCapsuleVertices(); + + EuclidianPoint3D p = new EuclidianPoint3D(getCenter()); + p.add(vertices[index]); + return p; + } + + /** + * {@inheritDoc} + */ + @Override + public SizedIterator getLocalOrientedBoundVertices() { + return ArrayUtil.sizedIterator(computeCapsuleVertices()); + } + + /** + * {@inheritDoc} + */ + @Override + public Vector3f getLocalVertexAt(int index) { + Vector3f[] vertices = computeCapsuleVertices(); + return vertices[index]; + } + + /** + * {@inheritDoc} + */ + @Override + public Vector3f[] getOrientedBoundAxis() { + return this.axis; + } + + /** + * {@inheritDoc} + */ + @Override + public Vector3f getOrientedBoundExtentVector() { + return new Vector3f(this.extent[0], this.extent[1], this.extent[2]); + } + + /** + * {@inheritDoc} + */ + @Override + public float[] getOrientedBoundExtents() { + return this.extent; + } + + /** + * {@inheritDoc} + */ + @Override + public int getVertexCount() { + return 8; + } + + /** + * Replies if the specified capsule intersects this capsule + * + * @param capsule + * - the cpasule to test + * @return true if intersecting, otherwise false + */ + public boolean intersectsCapsule(Capsule capsule) { + return IntersectionUtil.intersectsCapsuleCapsule(this.a, this.b, this.radius, capsule.getA(), capsule.getB(), capsule.getRadius()); + } + + /** + * Replies if the specified capsule intersects this capsule + * + * @param capsuleA + * - Medial line segment start point of the first capsule + * @param capsuleB + * - Medial line segment end point of the first capsule + * @param capsuleRadius + * - radius of the first capsule + * @return true if intersecting, otherwise false + */ + public boolean intersectsCapsule(Point3f capsuleA, Point3f capsuleB, float capsuleRadius) { + return IntersectionUtil.intersectsCapsuleCapsule(this.a, this.b, this.radius, capsuleA, capsuleB, capsuleRadius); + } + + /** + * {@inheritDoc} + */ + @Override + public IntersectionType classifies(Point3f c, float r) { + return ClassifierUtil.classifySphereCapsule(c, r, this.a, this.b, this.radius); + } + + /** + * {@inheritDoc} + */ + @Override + public IntersectionType classifies(Plane plane) { + + float distanceToA = plane.distanceTo(this.a); + float distanceToB = plane.distanceTo(this.b); + + if (distanceToA * distanceToB > 0) { // two distance of the same sign + if (Math.abs(Math.min(distanceToA, distanceToA)) > this.radius) {// no intersection + return IntersectionType.OUTSIDE; + } + return IntersectionType.SPANNING; + } + return IntersectionType.SPANNING; + } + + /** + * {@inheritDoc} + */ + @Override + public IntersectionType classifies(Point3f center, Vector3f[] axis, float[] extent) { + return ClassifierUtil.classifyOrientedBoxCapsule(center, axis, extent, this.a, this.b, this.radius); + } + + /** + * {@inheritDoc} + */ + @Override + public PlanarClassificationType classifiesAgainst(Plane plane) { + float distanceToA = plane.distanceTo(this.a); + float distanceToB = plane.distanceTo(this.b); + + if (distanceToA * distanceToB > 0) { // two distance of the same sign + if (Math.abs(Math.min(distanceToA, distanceToA)) > this.radius) {// no intersection + if (distanceToA > 0) { + return PlanarClassificationType.IN_FRONT_OF; + } + return PlanarClassificationType.BEHIND; + } + return PlanarClassificationType.COINCIDENT; + } + return PlanarClassificationType.COINCIDENT; + } + + /** + * {@inheritDoc} + */ + @Override + public boolean intersects(Point3f c, float r) { + return IntersectionUtil.intersectsSphereCapsule(c, r, this.a, this.b, this.radius); + } + + /** + * {@inheritDoc} + */ + @Override + public boolean intersects(Plane plane) { + float distanceToA = plane.distanceTo(this.a); + float distanceToB = plane.distanceTo(this.b); + + return ((distanceToA * distanceToB <= 0) // two distance of the same sign + || (Math.abs(Math.min(distanceToA, distanceToA)) <= this.radius)); // no intersection + } + + /** + * {@inheritDoc} + */ + @Override + public boolean intersects(Point3f center, Vector3f[] axis, float[] extent) { + return IntersectionUtil.intersectsOrientedBoxCapsule(center, axis, extent, this.a, this.b, this.radius); + } + + /** + * {@inheritDoc} + */ + @Override + public IntersectionType classifies(Point3f p) { + return ClassifierUtil.classifyPointCapsule(p, this.a, this.b, this.radius); + } + + /** + * Compute intersection between an OBB and a capsule + */ + private boolean intersectsAlignedBoxCapsule(AxisAlignedBox aabb) { + Point3f closestFromA = aabb.nearestPoint(this.a); + Point3f closestFromB = aabb.nearestPoint(this.b); + + float distance = MathUtil.distanceSegmentSegment(this.a.getX(), this.a.getY(), this.b.getX(), this.b.getY(), closestFromA.getX(), closestFromA.getY(), closestFromB.getX(), closestFromB.getY()); + + return (distance <= this.radius); + } + + /** + * Compute intersection between an AABB and a capsule + */ + private IntersectionType classifyAlignedBoxCapsule(AxisAlignedBox aabb) { + Point3f closestFromA = aabb.nearestPoint(this.a); + Point3f closestFromB = aabb.nearestPoint(this.b); + Point3f farestFromA = aabb.farestPoint(this.a); + Point3f farestFromB = aabb.farestPoint(this.b); + + float distanceToNearest = MathUtil.distanceSegmentSegment(this.a.getX(), this.a.getY(), this.b.getX(), this.b.getY(), closestFromA.getX(), closestFromA.getY(), closestFromB.getX(), closestFromB.getY()); + + if (distanceToNearest > this.radius) { + return IntersectionType.OUTSIDE; + } + + float distanceToFarest = MathUtil.distanceSegmentSegment(this.a.getX(), this.a.getY(), this.b.getX(), this.b.getY(), farestFromA.getX(), farestFromA.getY(), farestFromB.getX(), farestFromB.getY()); + if (distanceToFarest < this.radius) { + return IntersectionType.ENCLOSING; + } + + IntersectionType onSphereA = ClassifierUtil.classifiesSolidSphereSolidAlignedBox(this.a, this.radius, aabb.getLower(), aabb.getUpper()); + IntersectionType onSphereB = ClassifierUtil.classifiesSolidSphereSolidAlignedBox(this.b, this.radius, aabb.getLower(), aabb.getUpper()); + + 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; + } + + /** + * {@inheritDoc} + */ + @Override + public IntersectionType classifies(Point3f l, Point3f u) { + return classifyAlignedBoxCapsule(new AxisAlignedBox(l, u)); + } + + /** + * {@inheritDoc} + */ + @Override + public boolean intersects(Point3f p) { + return IntersectionUtil.intersectPointCapsule(p, this.a, this.b, this.radius); + } + + /** + * {@inheritDoc} + */ + @Override + public boolean intersects(Point3f l, Point3f u) { + return intersectsAlignedBoxCapsule(new AxisAlignedBox(l, u)); + } + + @Override + protected void combineBounds(boolean isInit, Collection bounds) { + if (!isInit) { + if (bounds.size() == 1) { + Bounds3D cb = CollectionUtil.firstElement(bounds); + if (cb instanceof OrientedBox) { + OrientedBox obb = (OrientedBox) cb; + this.center = (EuclidianPoint3D) obb.center.clone(); + for (int i = 0; i < 3; ++i) { + this.axis[i] = obb.axis[i].clone(); + this.extent[i] = obb.extent[i]; + } + + this.computeCapsuleVertices(); + this.computeAandBfromOBBData(); + } else if (cb instanceof Sphere) { + Sphere sphere = (Sphere) cb; + this.center = (EuclidianPoint3D) sphere.center.clone(); + this.axis[0] = new Vector3f(1., 0., 0.); + this.axis[1] = new Vector3f(0., 1., 0.); + this.axis[2] = new Vector3f(0., 0., 1.); + for (int i = 0; i < 3; ++i) { + this.extent[i] = sphere.radius; + } + + this.computeCapsuleVertices(); + this.computeAandBfromOBBData(); + } else if (cb instanceof AxisAlignedBox) { + AxisAlignedBox aabb = (AxisAlignedBox) cb; + this.center = (EuclidianPoint3D) aabb.getCenter().clone(); + this.axis[0] = new Vector3f(1., 0., 0.); + this.axis[1] = new Vector3f(0., 1., 0.); + this.axis[2] = new Vector3f(0., 0., 1.); + this.extent[0] = aabb.getSizeX() / 2.f; + this.extent[1] = aabb.getSizeY() / 2.f; + this.extent[2] = aabb.getSizeZ() / 2.f; + + this.computeCapsuleVertices(); + this.computeAandBfromOBBData(); + } else if (cb instanceof ComposedBounds3D) { + ComposedBounds3D ccb = (ComposedBounds3D) cb; + computeBoundingCapsule(new MergedBoundList(ccb)); + } + } else { + computeBoundingCapsule(new MergedBoundList(bounds)); + } + } else { + computeBoundingCapsule(new MergedBoundList(bounds, this)); + } + + } + + @Override + protected void combinePoints(boolean isInit, Collection pointList) { + if (!isInit) { + computeBoundingCapsule(pointList); + } else { + computeBoundingCapsule(new MergedPointList(pointList)); + } + } + + private void computeBoundingCapsule(Iterable vertexList) { + if (this.axis == null || this.axis.length != 3) + this.axis = new Vector3f[3]; + for (int i = 0; i < 3; ++i) { + if (this.axis[i] == null) + this.axis[i] = new Vector3f(); + } + + // Determining the OBB axis, extent and center + OrientedBox.computeOBBCenterAxisExtents(vertexList, this.axis[0], this.axis[1], this.axis[2], this.center, this.extent); + + //FIXME Refine the way to compute the radius + + this.computeAandBfromOBBData(); + this.computeAABB(); + this.computeCapsuleVertices(); + } + + /** + * {@inheritDoc} + */ + @Override + public void reset() { + this.a = null; + this.b = null; + this.radius = 0.0f; + this.center.set(0, 0, 0); + + for (int i = 0; i < 3; ++i) { + this.axis[i].set(0, 0, 0); + this.extent[i] = 0; + } + + this.vertices = null; + this.lower = this.upper = null; + } + + private void computeAandBfromOBBData() { + // FIXME Refine the way to compute the radius + this.radius = Math.max(this.extent[1], this.extent[2]); + this.b = new EuclidianPoint3D(this.center); + + Vector3f r = new Vector3f(this.axis[0]); + r.normalize(); + r.scale(this.extent[0]); + this.b.add(r); + + r.normalize(); + r.scale(-this.extent[0]); + + this.a = new EuclidianPoint3D(this.center); + this.a.add(r); + assert(this.a.getZ() <= this.b.getZ()); + } + + /** + * An iterable on the Capsule vertices and on a list of points. + * + * @author $Author: ngaud$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ + private class MergedPointList implements Iterable { + + private final Collection points; + + /** + * @param pointList + * is a list of point to merge inside the iterable. + */ + public MergedPointList(Collection pointList) { + this.points = pointList; + } + + @Override + public Iterator iterator() { + return CollectionUtil.mergeIterators( + getGlobalOrientedBoundVertices(), + CollectionUtil.sizedIterator(this.points)); + } + + } + + /** + * @author $Author: ngaud$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ + private static class MergedBoundList implements Iterable { + + private final Collection bounds; + private final Capsule capsule; + + /** + * @param bounds + * are the list of bounds to merge inside + */ + public MergedBoundList(Collection bounds) { + this.bounds = bounds; + this.capsule = null; + } + + /** + * @param bounds + * are the list of bounds to merge inside + * @param obb + * is the obb to put back in the merging list. + */ + public MergedBoundList(Collection bounds, Capsule obb) { + this.bounds = bounds; + this.capsule = obb; + } + + @Override + public Iterator iterator() { + Iterator iterator = new ExtractionIterator(this.bounds.iterator()); + + if (this.capsule == null) + return iterator; + + return CollectionUtil.mergeIterators(iterator, this.capsule.getGlobalOrientedBoundVertices()); + } + + /** + * An iterable on vertices which is merging a collection of bounds and optionnally a capsule. + */ + private static class ExtractionIterator implements Iterator { + + private final Iterator bounds; + private LinkedList subBounds = null; + private Iterator vertices; + private Tuple3f next; + + /** + * @param bounds + */ + public ExtractionIterator(Iterator bounds) { + this.bounds = bounds; + searchNext(); + } + + private void searchNext() { + this.next = null; + while (this.vertices == null || !this.vertices.hasNext()) { + Bounds3D cb; + this.vertices = null; + while ((this.subBounds != null && !this.subBounds.isEmpty()) || this.bounds.hasNext()) { + cb = (this.subBounds != null) ? this.subBounds.removeFirst() : this.bounds.next(); + if (cb.isInit()) { + if (cb instanceof OrientedBounds3D) { + this.vertices = ((OrientedBounds3D) cb).getGlobalOrientedBoundVertices(); + break; + } else if (cb instanceof ComposedBounds3D) { + if (this.subBounds == null) + this.subBounds = new LinkedList(); + this.subBounds.addAll((ComposedBounds3D) cb); + } + } + } + if (this.subBounds != null && this.subBounds.isEmpty()) + this.subBounds = null; + if (this.vertices == null) + return; // No more vertex + } + + // One vertex exists! + this.next = this.vertices.next(); + } + + @Override + public boolean hasNext() { + return this.next != null; + } + + @Override + public Tuple3f next() { + if (this.next == null) + throw new NoSuchElementException(); + Tuple3f n = this.next; + searchNext(); + return n; + } + + @Override + public void remove() { + // + } + + } + + } + +} diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/EulerAngle.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/EulerAngle.java new file mode 100644 index 000000000..b68b2c640 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/EulerAngle.java @@ -0,0 +1,300 @@ +/* + * $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.geometry3d.continuous; + +import org.arakhne.afc.math.MathUtil; +import org.arakhne.afc.math.geometry.continuous.object4d.AxisAngle4f; +import org.arakhne.afc.math.geometry.system.CoordinateSystem3D; +import org.arakhne.afc.util.HashCodeUtil; + + +/** + * Represents Euler angles. + *

+ * The term "Euler Angle" is used for any representation of 3 dimensional + * rotations where the rotation is decomposed into 3 separate angles. + *

+ * There is no single set of conventions and standards in this area, + * therefore the following conventions was choosen:

    + *
  • angle applied first: heading;
    • + *
  • angle applied second: attitude;
    • + *
  • angle applied last: bank
    • + *
+ *

+ * Examples: NASA aircraft standard and telescope standard + * [NASA Aircraft Standard] + * [Telescope Standard] + *

+ * You must see {@link CoordinateSystem3D} for more details on rotations + * in a specific 3D coordinate system. + * + * @author $Author: sgalland$ + * @author $Author: ngaud$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class EulerAngle { //TODO Put in quaternion + + /** + * Rotation angle around the top vector. + */ + public float heading; + + /** + * Rotation angle around the left vector. + */ + public float attitude; + + /** + * Rotation angle around the front vector. + */ + public float bank; + + /** + * + */ + public EulerAngle() { + this.heading = this.attitude = this.bank = 0f; + } + + /** + * @param eulers + */ + public EulerAngle(EulerAngle eulers) { + this.heading = eulers.heading; + this.attitude = eulers.attitude; + this.bank = eulers.bank; + } + + /** + * @param heading is the rotation around top vector. + * @param attitude is the rotation around left vector. + * @param bank is the rotation around front vector. + */ + public EulerAngle(float heading, float attitude, float bank) { + this.heading = heading; + this.attitude = attitude; + this.bank = bank; + } + + /** + * @param quaternion + */ + public EulerAngle(Quaternion quaternion) { + MathUtil.quat2euler(quaternion, this); + } + + /** + * @param rotation + */ + public EulerAngle(AxisAngle4f rotation) { + Quaternion q = new Quaternion(); + q.set(rotation); + MathUtil.quat2euler(q, this); + } + + /** Set the heading angle. + * + * @param heading is the rotation around top vector. + */ + public final void setHeading(float heading) { + this.heading = heading; + } + + /** Get the heading angle. + * + * @return the rotation around top vector. + */ + public final float getHeading() { + return this.heading; + } + + /** Set the attitude angle. + * + * @param attitude is the rotation around left vector. + */ + public final void setAttitude(float attitude) { + this.attitude = attitude; + } + + /** Get the attitude angle. + * + * @return the rotation around left vector. + */ + public final float getAttitude() { + return this.attitude; + } + + /** Set the bank angle. + * + * @param bank is the rotation around front vector. + */ + public final void setBank(float bank) { + this.bank = bank; + } + + /** Get the bank angle. + * + * @return the rotation around front vector. + */ + public final float getBank() { + return this.bank; + } + + /** Set the Euler angles. + * + * @param heading is the rotation around top vector. + * @param attitude is the rotation around left vector. + * @param bank is the rotation around front vector. + */ + public final void set(float heading, float attitude, float bank) { + this.heading = heading; + this.attitude = attitude; + this.bank = bank; + } + + /** Set the Euler angles. + * + * @param eulers + */ + public final void set(EulerAngle eulers) { + this.heading = eulers.heading; + this.attitude = eulers.attitude; + this.bank = eulers.bank; + } + + /** Set the Euler angles from a quaternion. + * + * @param quaternion + * @see MathUtil#quat2euler(Quaternion, EulerAngle) + */ + public final void set(Quaternion quaternion) { + MathUtil.quat2euler(quaternion, this); + } + + /** Set the Euler angles from a quaternion. + * + * @param rotation + * @see MathUtil#quat2euler(Quaternion, EulerAngle) + */ + public final void set(AxisAngle4f rotation) { + Quaternion q = new Quaternion(); + q.set(rotation); + MathUtil.quat2euler(q, this); + } + + /** Set the Euler angles from a quaternion. + * + * @param quaternion + * @param system is the coordinate system to use for the conversion + * @see MathUtil#quat2euler(Quaternion, EulerAngle, CoordinateSystem3D) + */ + public final void set(Quaternion quaternion, CoordinateSystem3D system) { + MathUtil.quat2euler(quaternion, this, system); + } + + /** Set the Euler angles from a quaternion. + * + * @param rotation + * @param system is the coordinate system to use for the conversion + * @see MathUtil#quat2euler(Quaternion, EulerAngle, CoordinateSystem3D) + */ + public final void set(AxisAngle4f rotation, CoordinateSystem3D system) { + Quaternion q = new Quaternion(); + q.set(rotation); + MathUtil.quat2euler(q, this, system); + } + + /** Replies the quaternion that is corresponding to this eulers. + * + * @return the quaternion + * @see MathUtil#euler2quat(float, float, float) + */ + public final Quaternion toQuaternion() { + return MathUtil.euler2quat(this.heading, this.attitude, this.bank); + } + + /** Replies the quaternion that is corresponding to this eulers. + * + * @param system is the coordinate system to use for the conversion + * @return the quaternion + * @see MathUtil#euler2quat(float, float, float, CoordinateSystem3D) + */ + public final Quaternion toQuaternion(CoordinateSystem3D system) { + return MathUtil.euler2quat(this.heading, this.attitude, this.bank, system); + } + + /** + * {@inheritDoc} + */ + @Override + public String toString() { + StringBuilder buffer = new StringBuilder(); + buffer.append("heading="); //$NON-NLS-1$ + buffer.append(this.heading); + buffer.append("; attitude="); //$NON-NLS-1$ + buffer.append(this.attitude); + buffer.append("; bank="); //$NON-NLS-1$ + buffer.append(this.bank); + return buffer.toString(); + } + + /** + * {@inheritDoc} + */ + @Override + public boolean equals(Object obj) { + EulerAngle a = null; + Quaternion q = null; + if (obj instanceof EulerAngle) { + a = (EulerAngle)obj; + } + else if (obj instanceof Quaternion) { + q = (Quaternion)obj; + } + else if (obj instanceof AxisAngle4f) { + q = new Quaternion(); + q.set((AxisAngle4f)obj); + } + if (a==null && q!=null) { + a = MathUtil.quat2euler(q); + } + if (a!=null) { + return this.attitude==a.attitude + && this.bank==a.bank + && this.heading==a.heading; + } + return false; + } + + /** + * {@inheritDoc} + */ + @Override + public int hashCode() { + int h = HashCodeUtil.hash(this.attitude); + h = HashCodeUtil.hash(h, this.bank); + h = HashCodeUtil.hash(h, this.heading); + return h; + } + +} diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Line3f.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Line3f.java new file mode 100644 index 000000000..2c316ec0d --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Line3f.java @@ -0,0 +1,402 @@ +/* + * $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.geometry3d.continuous; + +import java.io.Serializable; + +import org.arakhne.afc.math.MathUtil; +import org.arakhne.afc.math.Matrix4f; +import org.arakhne.afc.math.geometry.continuous.transform.Transformable3D; + +/** This class represents a 3D line. + *

+ * The equation of the line is: + * + * + * L⁡t + * = + * P+ + * t. + * + * D + * ⃗ + * + * + * + * for any real-valued t. + * D⃗ is not + * necessarily unit length. + * + * @author $Author: cbohrhauer$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class Line3f implements Serializable, Transformable3D { + + private static final long serialVersionUID = 7914697528608357347L; + + /** First point on the line. + */ + public final Point3f pivot = new Point3f(); + + /** Direction vector. + */ + public final Vector3f d = new Vector3f(); + + /** + */ + public Line3f() { + super(); + } + + /** + * @param p1 is first point on the line + * @param p2 is second point on the line + */ + public Line3f(Point3f p1, Point3f p2) { + this.pivot.set(p1); + this.d.set(p2); + this.d.sub(p1); + this.d.normalize(); + } + + /** + * @param pivot is a point on the line + * @param direction is the direction of the line + */ + public Line3f(Point3f pivot, Vector3f direction) { + this.pivot.set(pivot); + this.d.set(direction); + this.d.normalize(); + } + + /** {@inheritDoc} + */ + @Override + public void setIdentityTransform() { + this.pivot.set(0,0,0); + this.d.set(1,0,0); + } + + /** {@inheritDoc} + */ + @Override + public void setTransform(Transform3D trans) { + setTranslation(trans.m03,trans.m13,trans.m23); + + Quaternion quat = new Quaternion(); + trans.get(quat); + setRotation(quat); + } + + /** {@inheritDoc} + */ + @Override + public void transform(Transform3D_OLD trans) { + translate(trans.m03, trans.m13, trans.m23); + + Quaternion quat = new Quaternion(); + trans.get(quat); + rotate(quat); + } + + /** {@inheritDoc} + */ + @Override + public Transform3D_OLD getTransformMatrix() { + Transform3D_OLD m = new Transform3D_OLD(); + m.setTranslation(new Vector3f(this.pivot.getX(),this.pivot.getY(),this.pivot.getZ())); + m.setRotation(new AxisAngle4f(this.d,0.f)); + return m; + } + + /** {@inheritDoc} + */ + @Override + public void setTranslation(float x, float y, float z) { + this.pivot.set(x,y,z); + } + + /** {@inheritDoc} + */ + @Override + public void translate(float dx, float dy, float dz) { + this.pivot.setX(this.pivot.getX() + dx); + this.pivot.setY(this.pivot.getY() + dy); + this.pivot.setZ(this.pivot.getZ() + dz); + } + + /** {@inheritDoc} + */ + @Override + public void setTranslation(Point3f position) { + this.pivot.set(position); + } + + /** {@inheritDoc} + */ + @Override + public Point3f getTranslation() { + return new Point3f(this.pivot); + } + + /** {@inheritDoc} + */ + @Override + public void translate(Vector3f v) { + this.pivot.setX(this.pivot.getX() + v.getX()); + this.pivot.setY(this.pivot.getY() + v.getY()); + this.pivot.setZ(this.pivot.getZ() + v.getZ()); + } + + /** {@inheritDoc} + */ + @Override + public void setScale(float sx, float sy, float sz) { + // + } + + /** {@inheritDoc} + */ + @Override + public void scale(float sx, float sy, float sz) { + // + } + + /** {@inheritDoc} + */ + @Override + public Tuple3f getScale() { + return new Vector3f(1,1,1); + } + + /** {@inheritDoc} + */ + @Override + public void setRotation(AxisAngle4f quaternion) { + this.d.set(1,0,0); + Matrix4f m = new Matrix4f(); + m.set(quaternion); + m.transform(this.d); + this.d.normalize(); + } + + /** {@inheritDoc} + */ + @Override + public void setRotation(Quaternion quaternion) { + this.d.set(1,0,0); + Matrix4f m = new Matrix4f(); + m.set(quaternion); + m.transform(this.d); + this.d.normalize(); + } + + /** {@inheritDoc} + */ + @Override + public void setRotation(AxisAngle4f quaternion, Point3f pivot) { + this.pivot.set(pivot); + this.d.set(1,0,0); + Matrix4f m = new Matrix4f(); + m.set(quaternion); + m.transform(this.d); + this.d.normalize(); + } + + /** {@inheritDoc} + */ + @Override + public void setRotation(Quaternion quaternion, Point3f pivot) { + this.pivot.set(pivot); + this.d.set(1,0,0); + Matrix4f m = new Matrix4f(); + m.set(quaternion); + m.transform(this.d); + this.d.normalize(); + } + + /** {@inheritDoc} + */ + @Override + public void rotate(AxisAngle4f quaternion) { + Matrix4f m = new Matrix4f(); + m.set(quaternion); + m.transform(this.d); + this.d.normalize(); + } + + /** {@inheritDoc} + */ + @Override + public void rotate(Quaternion quaternion, Point3f pivotPoint) { + if (pivotPoint==this.pivot) rotate(quaternion); + + Matrix4f m = new Matrix4f(); + m.set(quaternion); + + // Rotate the pivot + this.pivot.setX(this.pivot.getX() - pivotPoint.getX()); + this.pivot.setY(this.pivot.getY() - pivotPoint.getY()); + this.pivot.setZ(this.pivot.getZ() - pivotPoint.getZ()); + + m.transform(this.pivot); + + this.pivot.setX(this.pivot.getX() + pivotPoint.getX()); + this.pivot.setY(this.pivot.getY() + pivotPoint.getY()); + this.pivot.setZ(this.pivot.getZ() + pivotPoint.getZ()); + + // Rotate the direction + m.transform(this.d); + this.d.normalize(); + } + + /** {@inheritDoc} + */ + @Override + public void rotate(AxisAngle4f quaternion, Point3f pivotPoint) { + if (pivotPoint==this.pivot) rotate(quaternion); + + Matrix4f m = new Matrix4f(); + m.set(quaternion); + + // Rotate the pivot + this.pivot.setX(this.pivot.getX() - pivotPoint.getX()); + this.pivot.setY(this.pivot.getY() - pivotPoint.getY()); + this.pivot.setZ(this.pivot.getZ() - pivotPoint.getZ()); + + m.transform(this.pivot); + + this.pivot.setX(this.pivot.getX() + pivotPoint.getX()); + this.pivot.setY(this.pivot.getY() + pivotPoint.getY()); + this.pivot.setZ(this.pivot.getZ() + pivotPoint.getZ()); + + // Rotate the direction + m.transform(this.d); + this.d.normalize(); + } + + /** {@inheritDoc} + */ + @Override + public void rotate(Quaternion quaternion) { + Matrix4f m = new Matrix4f(); + m.set(quaternion); + m.transform(this.d); + this.d.normalize(); + } + + /** {@inheritDoc} + */ + @Override + public AxisAngle4f getAxisAngle() { + return new AxisAngle4f(this.d,0); + } + + /** {@inheritDoc} + */ + @Override + public void setPivot(float x, float y, float z) { + this.pivot.set(x,y,z); + } + + /** {@inheritDoc} + */ + @Override + public void setPivot(Point3f point) { + this.pivot.set(point); + } + + /** {@inheritDoc} + */ + @Override + public Point3f getPivot() { + return new Point3f(this.pivot); + } + + /** + * Replies the direction for the line. + * + * @return line direction vector + */ + public Vector3f getDirection() { + return new Vector3f(this.d); + } + + /** + * Replies the first point. + * + * @return the point on the line. + */ + public Point3f getP0() { + return new Point3f(this.pivot); + } + + /** + * Replies the second point. + * + * @return the point on the line. + */ + public Point3f getP1() { + Point3f p = new Point3f(this.pivot); + p.add(this.d); + return p; + } + + /** Replies the distance between this line and the given point. + * + * @param x + * @param y + * @param z + * @return the distance. + */ + public float distance(float x, float y, float z) { + return MathUtil.distancePointLine(x, y, z, + this.pivot.getX(), this.pivot.getY(), this.pivot.getZ(), + this.pivot.getX()+this.d.getX(), + this.pivot.getY()+this.d.getY(), + this.pivot.getZ()+this.d.getZ()); + } + + /** + * {@inheritDoc} + */ + @Override + public String toString() { + StringBuilder b = new StringBuilder(); + b.append("]p:("); //$NON-NLS-1$ + b.append(this.pivot.getX()); + b.append(";"); //$NON-NLS-1$ + b.append(this.pivot.getY()); + b.append(";"); //$NON-NLS-1$ + b.append(this.pivot.getZ()); + b.append("),v:("); //$NON-NLS-1$ + b.append(this.d.getX()); + b.append(";"); //$NON-NLS-1$ + b.append(this.d.getY()); + b.append(";"); //$NON-NLS-1$ + b.append(this.d.getZ()); + b.append(")["); //$NON-NLS-1$ + return b.toString(); + } + +} diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/OrientedBox.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/OrientedBox.java new file mode 100644 index 000000000..e8733d4a1 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/OrientedBox.java @@ -0,0 +1,1479 @@ +/* + * $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 java.util.Iterator; +import java.util.LinkedList; +import java.util.NoSuchElementException; + +import org.arakhne.afc.math.MathUtil; +import org.arakhne.afc.math.Matrix3f; +import org.arakhne.afc.math.Matrix4f; +import org.arakhne.afc.math.geometry.IntersectionType; +import org.arakhne.afc.math.geometry.IntersectionUtil; +import org.arakhne.afc.math.geometry.PointFilterXYIterable; +import org.arakhne.afc.math.geometry.PointFilterXZIterable; +import org.arakhne.afc.math.geometry.PointFilterYZIterable; +import org.arakhne.afc.math.geometry.continuous.euclide.EuclidianPoint3D; +import org.arakhne.afc.math.geometry.continuous.object4d.AxisAngle4f; +import org.arakhne.afc.math.geometry.system.CoordinateSystem3D; +import org.arakhne.afc.math.geometry2d.continuous.OrientedRectangle2f; +import org.arakhne.afc.math.geometry2d.continuous.Point2f; +import org.arakhne.afc.math.geometry2d.continuous.Vector2f; +import org.arakhne.afc.sizediterator.SizedIterator; +import org.arakhne.afc.util.ArrayUtil; +import org.arakhne.afc.util.CollectionUtil; + +/** + * Definition of a fixed Oriented Bounding Box (OBB). + *

+ * 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: sgalland$ + * @author $Author: ngaud$ + * @author $Author: cbohrhauer$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + * @see Mathematics for 3D Game Programming & Computer Graphics + */ +public class OrientedBox extends AbstractCombinableBounds3D implements TranslatableBounds3D, RotatableBounds3D, OrientedCombinableBounds3D { + + private static final long serialVersionUID = 5092008376628616496L; + + /** + * Compute the oriented bounding box center, axis and extent. + * + * @param

+ * is the type of the points. + * @param points + * is the list of the points enclosed by the OBB + * @param R + * is the vector where the R axis of the OBB is put + * @param S + * is the vector where the S axis of the OBB is put + * @param T + * is the the vector where T axis of the OBB is put + * @param center + * is the point which is set with the OBB's center coordinates. + * @param extents + * are the extents of the OBB for the R, S and T axis. + */ + public static

void computeOBBCenterAxisExtents(P[] points, Vector3f R, Vector3f S, Vector3f T, Point3f center, float[] extents) { + computeOBBCenterAxisExtents(Arrays.asList(points), R, S, T, center, extents); + } + + /** + * Compute the oriented bounding box center, axis and extent. + * + * @param points + * is the list of the points enclosed by the OBB + * @param R + * is the vector where the R axis of the OBB is put + * @param S + * is the vector where the S axis of the OBB is put + * @param T + * is the the vector where T axis of the OBB is put + * @param center + * is the point which is set with the OBB's center coordinates. + * @param extents + * are the extents of the OBB for the R, S and T axis. + */ + public static void computeOBBCenterAxisExtents(Iterable points, Vector3f R, Vector3f S, Vector3f T, Point3f center, float[] extents) { + assert (points != null); + assert (center != null); + assert (extents != null && extents.length >= 3); + + Vector3f vecNull = new Vector3f(); + if (R.equals(vecNull) && S.equals(vecNull) && T.equals(vecNull)) { + // Determining the covariance matrix of the points + // and set the center of the box + Matrix3f cov = new Matrix3f(); + MathUtil.cov(cov, points); + + // Determining eigenvectors of covariance matrix and defines RST axis + Matrix3f rst = new Matrix3f();// eigenvectors + + MathUtil.eigenVectorsOfSymmetricMatrix(cov, rst); + rst.getColumn(0, R); + rst.getColumn(1, S); + rst.getColumn(2, T); + }//else we have already set some constraints on OBB axis, dosen't need to compute them again + + float minR = Float.POSITIVE_INFINITY; + float maxR = Float.NEGATIVE_INFINITY; + float minS = Float.POSITIVE_INFINITY; + float maxS = Float.NEGATIVE_INFINITY; + float minT = Float.POSITIVE_INFINITY; + float maxT = Float.NEGATIVE_INFINITY; + + float PdotR; + float PdotS; + float PdotT; + Vector3f v = new Vector3f(); + + for (Tuple3f tuple : points) { + v.set(tuple); + + PdotR = v.dot(R); + PdotS = v.dot(S); + PdotT = v.dot(T); + + if (PdotR < minR) + minR = PdotR; + if (PdotR > maxR) + maxR = PdotR; + if (PdotS < minS) + minS = PdotS; + if (PdotS > maxS) + maxS = PdotS; + if (PdotT < minT) + minT = PdotT; + if (PdotT > maxT) + maxT = PdotT; + } + + float a = (maxR + minR) / 2.f; + float b = (maxS + minS) / 2.f; + float c = (maxT + minT) / 2.f; + + // Set the center of the OBB + center.set(a * R.getX() + b * S.getX() + c * T.getX(), + + a * R.getY() + b * S.getY() + c * T.getY(), + + a * R.getZ() + b * S.getZ() + c * T.getZ()); + + // Compute extents + extents[0] = (maxR - minR) / 2.f; + extents[1] = (maxS - minS) / 2.f; + extents[2] = (maxT - minT) / 2.f; + + // Normalize axis + R.normalize(); + S.normalize(); + T.normalize(); + + // Selection with largest magnitude eigenvalue to identify R + int max = -1; + for (int i = 0; i < 3; i++) { + if (extents[i] > max) { + max = i; + } + } + switch (max) { + case 0: { + // do nothing, right order + } + break; + + case 1: { + Vector3f tmpVec = new Vector3f(R); + R.set(S); + S.set(T); + T.set(tmpVec); + + float tmpD = extents[0]; + extents[0] = extents[1]; + extents[1] = extents[2]; + extents[2] = tmpD; + } + break; + case 2: { + Vector3f tmpVec = new Vector3f(S); + S.set(R); + R.set(T); + T.set(tmpVec); + + float tmpD = extents[1]; + extents[1] = extents[0]; + extents[0] = extents[2]; + extents[2] = tmpD; + + } + break; + } + } + + private boolean isBoundInit; + + /** + * Center of the OBB + */ + EuclidianPoint3D center = new EuclidianPoint3D(); + + /** + * Orthonormal set of vector composing an axis base + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + *
indexexplanation
0R axis
1S axis
2T axis
+ */ + Vector3f[] axis = new Vector3f[3]; + + /** + * Extent of the OBB, cannot be negative. + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + *
indexexplanation
0extent in R axis
1extent in S axis
2extent in T axis
+ */ + float[] extent = new float[3]; + + /** + * Lower point of the axis-aligned bounding box. + */ + private transient EuclidianPoint3D aabbLower = null; + + /** + * Upper point of the axis-aligned bounding box. + */ + private transient EuclidianPoint3D aabbUpper = null; + + /** + * The various vertices that compose this OBB + */ + private transient Vector3f[] vertices = null; + + /** + * Build an empty OBB. + */ + public OrientedBox() { + this.isBoundInit = false; + } + + /** + * Build an OBB from the set of vertex that composes the corresponding object 3D. + * + * @param vertices + */ + public OrientedBox(Tuple3f... vertices) { + this.isBoundInit = false; + combinePoints(false, Arrays.asList(vertices)); + } + + /** + * Build an OBB from the set of vertex that composes the corresponding object 3D. + * + * @param vertices + */ + public OrientedBox(Collection vertices) { + this.isBoundInit = false; + combinePoints(false, vertices); + } + + /** + * Build an OBB. The given parameters are copied. + * + * @param center + * @param axis + * @param extent + */ + public OrientedBox(Point3f center, Vector3f[] axis, float[] extent) { + assert (center != null); + assert (axis != null); + assert (axis[0] != null); + assert (axis[1] != null); + assert (axis[2] != null); + assert (extent != null); + assert (extent[0] >= 0.); + assert (extent[1] >= 0.); + assert (extent[2] >= 0.); + this.center.set(center); + for (int i = 0; i < 3; ++i) { + this.axis[i] = new Vector3f(axis[i]); + this.extent[i] = extent[i]; + } + this.isBoundInit = true; + } + + /** + * Build an OBB. The given parameters are copied. + * + * @param center + * @param axis + * @param extent + */ + public OrientedBox(Point3f center, Vector3f[] axis, Tuple3f extent) { + assert (center != null); + assert (axis != null); + assert (axis[0] != null); + assert (axis[1] != null); + assert (axis[2] != null); + assert (extent != null); + this.center.set(center); + for (int i = 0; i < 3; ++i) { + this.axis[i] = new Vector3f(axis[i]); + } + this.extent[0] = extent.getX(); + this.extent[1] = extent.getY(); + this.extent[2] = extent.getZ(); + this.isBoundInit = true; + } + + /** + * {@inheritDoc} + */ + @Override + public final BoundingPrimitiveType3D getBoundType() { + return BoundingPrimitiveType3D.ORIENTED_BOX; + } + + @Override + public String toString() { + StringBuilder s = new StringBuilder("OBB "); //$NON-NLS-1$ + s.append("center: "); //$NON-NLS-1$ + s.append(this.center.toString()); + s.append(" axis: "); //$NON-NLS-1$ + s.append(this.axis.toString()); + s.append(" extent: "); //$NON-NLS-1$ + s.append(this.extent.toString()); + return s.toString(); + } + + /** + * {@inheritDoc} + */ + @Override + public OrientedBox clone() { + OrientedBox clone = (OrientedBox) super.clone(); + clone.axis = new Vector3f[this.axis.length]; + for (int i = 0; i < this.axis.length; ++i) { + clone.axis[i] = (Vector3f) this.axis[i].clone(); + } + clone.center = (EuclidianPoint3D) this.center.clone(); + clone.extent = this.extent.clone(); + + return clone; + } + + /** + * {@inheritDoc} + */ + @Override + public OrientedRectangle2f toBounds2D() { + return toBounds2D(CoordinateSystem3D.getDefaultCoordinateSystem()); + } + + /** + * {@inheritDoc} + */ + @Override + public OrientedRectangle2f toBounds2D(CoordinateSystem3D system) { + if (!isInit()) + return new OrientedRectangle2f(); + SizedIterator pts = getGlobalOrientedBoundVertices(); + Point2f[] points = new Point2f[pts.totalSize()]; + for (int i = 0; pts.hasNext(); ++i) { + points[i] = system.toCoordinateSystem2D(pts.next()); + } + return new OrientedRectangle2f(points); + } + + /** + * {@inheritDoc} + */ + @Override + public boolean equals(Object o) { + if (!isInit()) + return false; + if (this == o) + return true; + if (o instanceof OrientedBox) { + OrientedBox s = (OrientedBox) o; + if (this.center.equals(s.center)) { + for (int i = 0; i < this.axis.length; ++i) { + if (!this.axis[i].equals(s.axis[i])) + return false; + } + for (int i = 0; i < this.extent.length; ++i) { + if (this.extent[i] != s.extent[i]) + return false; + } + return true; + } + } + return false; + } + + /** + * {@inheritDoc} + */ + @Override + public SizedIterator getLocalOrientedBoundVertices() { + if (this.vertices == null) { + computeVertices(); + } + return ArrayUtil.sizedIterator(this.vertices); + } + + /** + * {@inheritDoc} + */ + @Override + public Vector3f getLocalVertexAt(int index) { + if (this.vertices == null) { + computeVertices(); + } + return this.vertices[index]; + } + + /** + * {@inheritDoc} + */ + @Override + public int getVertexCount() { + if (this.vertices == null) { + computeVertices(); + } + return this.vertices.length; + } + + /** + * {@inheritDoc} + */ + @Override + public SizedIterator getGlobalOrientedBoundVertices() { + return new LocalToGlobalVertexIterator(getCenter(), getLocalOrientedBoundVertices()); + } + + /** + * {@inheritDoc} + */ + @Override + public EuclidianPoint3D getGlobalVertexAt(int index) { + if (this.vertices == null) { + computeVertices(); + } + EuclidianPoint3D p = new EuclidianPoint3D(getCenter()); + p.add(this.vertices[index]); + return p; + } + + /** + * {@inheritDoc} + */ + @Override + public Vector3f[] getOrientedBoundAxis() { + return this.axis; + } + + /** + * {@inheritDoc} + */ + @Override + public Vector3f getOrientedBoundExtentVector() { + return new Vector3f(this.extent[0], this.extent[1], this.extent[2]); + } + + /** + * {@inheritDoc} + */ + @Override + public float[] getOrientedBoundExtents() { + return this.extent; + } + + /** + * {@inheritDoc} + */ + @Override + public Vector3f getR() { + return this.axis[0]; + } + + /** + * {@inheritDoc} + */ + @Override + public Vector3f getS() { + return this.axis[1]; + } + + /** + * {@inheritDoc} + */ + @Override + public Vector3f getT() { + return this.axis[2]; + } + + /** + * {@inheritDoc} + */ + @Override + public float getRExtent() { + return this.extent[0]; + } + + /** + * {@inheritDoc} + */ + @Override + public float getSExtent() { + return this.extent[1]; + } + + /** + * {@inheritDoc} + */ + @Override + public float getTExtent() { + return this.extent[2]; + } + + /** + * {@inheritDoc} + */ + @Override + public EuclidianPoint3D getCenter() { + return this.center; + } + + private void computeVertices() { + this.vertices = new Vector3f[8]; + + Vector3f Rpos = new Vector3f(this.axis[0]); + Rpos.scale(this.extent[0]); + Vector3f Rneg = new Vector3f(Rpos); + Rneg.negate(); + Vector3f Spos = new Vector3f(this.axis[1]); + Spos.scale(this.extent[1]); + Vector3f Sneg = new Vector3f(Spos); + Sneg.negate(); + Vector3f Tpos = new Vector3f(this.axis[2]); + Tpos.scale(this.extent[2]); + Vector3f Tneg = new Vector3f(Tpos); + Tneg.negate(); + + this.vertices[0] = MathUtil.add(Rpos, Spos, Tpos); + this.vertices[1] = MathUtil.add(Rneg, Spos, Tpos); + this.vertices[2] = MathUtil.add(Rneg, Sneg, Tpos); + this.vertices[3] = MathUtil.add(Rpos, Sneg, Tpos); + this.vertices[4] = MathUtil.add(Rpos, Sneg, Tneg); + this.vertices[5] = MathUtil.add(Rpos, Spos, Tneg); + this.vertices[6] = MathUtil.add(Rneg, Spos, Tneg); + this.vertices[7] = MathUtil.add(Rneg, Sneg, Tneg); + + this.aabbLower = new EuclidianPoint3D(); + this.aabbUpper = new EuclidianPoint3D(); + for (int i = 0; i < this.vertices.length; ++i) { + if (this.vertices[i].getX() < this.aabbLower.getA()) + this.aabbLower.setA(this.vertices[i].getX()); + if (this.vertices[i].getY() < this.aabbLower.getB()) + this.aabbLower.setB(this.vertices[i].getY()); + if (this.vertices[i].getZ() < this.aabbLower.getC()) + this.aabbLower.setC(this.vertices[i].getZ()); + if (this.vertices[i].getX() > this.aabbUpper.getA()) + this.aabbUpper.setA(this.vertices[i].getX()); + if (this.vertices[i].getY() > this.aabbUpper.getB()) + this.aabbUpper.setB(this.vertices[i].getY()); + if (this.vertices[i].getZ() > this.aabbUpper.getC()) + this.aabbUpper.setC(this.vertices[i].getZ()); + } + this.aabbLower.add(this.center); + this.aabbUpper.add(this.center); + } + + /** + * {@inheritDoc} + */ + @Override + public void getLowerUpper(Point3f lower, Point3f upper) { + assert (lower != null); + assert (upper != null); + if (this.aabbLower == null || this.aabbUpper == null) { + computeVertices(); + } + lower.set(this.aabbLower); + upper.set(this.aabbUpper); + } + + /** + * {@inheritDoc} + */ + @Override + public EuclidianPoint3D getLower() { + if (this.aabbLower == null) { + computeVertices(); + } + return this.aabbLower; + } + + /** + * {@inheritDoc} + */ + @Override + public EuclidianPoint3D getUpper() { + if (this.aabbUpper == null) { + computeVertices(); + } + return this.aabbUpper; + } + + /** + * {@inheritDoc} + */ + @Override + public float distanceSquared(Point3f p) { + if (!isInit()) + return Float.NaN; + Vector3f v = new Vector3f(); + v.sub(p, this.center);// p - this.center; + float sqDist = 0.f; + float d, excess; + for (int i = 0; i < 3; ++i) { + // Project vector from box center to p on each axis, getting the distance + // of p along that axis, and count any excess distance outside box extents + d = v.dot(this.axis[i]); + excess = 0.f; + if (d < -this.extent[i]) { + excess = -this.extent[i] - d; + } else if (d > this.extent[i]) { + excess = d - this.extent[i]; + } + sqDist += excess * excess; + } + return sqDist; + } + + /** + * {@inheritDoc} + */ + @Override + public EuclidianPoint3D nearestPoint(Point3f reference) { + if (!isInit()) + return null; + EuclidianPoint3D q = new EuclidianPoint3D(); + IntersectionUtil.computeClosestFarestOBBPoints(this.center, this.axis, this.extent, reference, q, null); + return q; + } + + /** + * {@inheritDoc} + */ + @Override + public float distanceMaxSquared(Point3f p) { + if (!isInit()) + return Float.NaN; + Vector3f v = new Vector3f(); + v.sub(p, this.center);// p - this.center; + float sqDist = 0.f; + float d, excess; + for (int i = 0; i < 3; ++i) { + // Project vector from box center to p on each axis, getting the distance + // of p along that axis, and count any excess distance outside box extents + d = v.dot(this.axis[i]); + if (d < 0.) { + excess = this.extent[i] - d; + } else { + excess = d + this.extent[i]; + } + sqDist += excess * excess; + } + return sqDist; + } + + /** + * {@inheritDoc} + */ + @Override + public EuclidianPoint3D farestPoint(Point3f reference) { + if (!isInit()) + return null; + EuclidianPoint3D q = new EuclidianPoint3D(); + IntersectionUtil.computeClosestFarestOBBPoints(this.center, this.axis, this.extent, reference, null, q); + return q; + } + + /** + * {@inheritDoc} + */ + @Override + public boolean isEmpty() { + return !isInit() || this.extent[0] <= 0. || this.extent[1] <= 0. || this.extent[2] <= 0.; + } + + /** + * {@inheritDoc} + */ + @Override + public boolean isInit() { + return this.isBoundInit; + } + + // ---------------------------------------------- + // CombinableBounds + // ---------------------------------------------- + + /** + * Compute the oriented bounding box from the set of vertices and the associated axis-aligned bounding box. + * + * @param constraint + * indicates the plane which must be coplanar to the box, or null if none. + * @param vertexList + * @see MGPCG pages 219-221 + */ + private void computeBoundingBoxes(PlaneConstraint constraint, Iterable vertexList) { + if (this.axis == null || this.axis.length != 3) + this.axis = new Vector3f[3]; + for (int i = 0; i < 3; ++i) { + if (this.axis[i] == null) + this.axis[i] = new Vector3f(); + else + this.axis[i].set(0,0,0); + } + + // boolean treated = false; + + if (constraint != null) { + Vector2f R = new Vector2f(); + Vector2f S = new Vector2f(); + switch (constraint) { + case OXY: + // Determining the OBB axis restricted on OXY plane + OrientedRectangle2f.computeOBRAxis(new PointFilterXYIterable(vertexList), R, S); + this.axis[0].set(new Vector3f(R.getX(), R.getY(), 0.)); + this.axis[1].set(new Vector3f(S.getX(), S.getY(), 0.)); + this.axis[2].set(new Vector3f(0., 0., 1.)); + // treated = true; + break; + case OXZ: + // Determining the OBB axis restricted on OXZ plane + OrientedRectangle2f.computeOBRAxis(new PointFilterXZIterable(vertexList), R, S); + this.axis[0].set(new Vector3f(R.getX(), 0., R.getY())); + this.axis[1].set(new Vector3f(0., 1., 0.)); + this.axis[2].set(new Vector3f(S.getX(), 0., S.getY())); + // treated = true; + break; + case OYZ: + // Determining the OBB axis restricted on OYZ plane + OrientedRectangle2f.computeOBRAxis(new PointFilterYZIterable(vertexList), R, S); + this.axis[0].set(new Vector3f(1., 0., 0.)); + this.axis[1].set(new Vector3f(0., R.getX(), R.getY())); + this.axis[2].set(new Vector3f(0., S.getX(), S.getY())); + // treated = true; + break; + } + } + + /* + * if (!treated) { // Determining the OBB axis computeOBBAxis(vertexList, this.axis[0], this.axis[1], this.axis[2]); } + * + * //Extent and center computation computeOBBCenterExtents(vertexList, this.axis[0], this.axis[1], this.axis[2], this.center, this.extent); + */ + computeOBBCenterAxisExtents(vertexList, this.axis[0], this.axis[1], this.axis[2], this.center, this.extent); + + this.vertices = null; + this.aabbLower = this.aabbUpper = null; + this.isBoundInit = true; + } + + /** + * Set this bounds from a set of points but force this oriented bounding box to be coplanar with the given plane. + * + * @param constraint + * indicates which plane may be coplanar to the box. + * @param points + * are the points used to update this bounding object. + */ + public final void set(PlaneConstraint constraint, Tuple3f... points) { + set(constraint, Arrays.asList(points)); + } + + /** + * Set this bounds from a set of points but force this oriented bounding box to be coplanar with the given plane. + * + * @param constraint + * indicates which plane may be coplanar to the box. + * @param points + * are the points used to update this bounding object. + */ + public void set(PlaneConstraint constraint, Collection points) { + assert (constraint != null); + computeBoundingBoxes(constraint, points); + } + + /** + * Add the points into the bounds. + * + * @param isInit + * must indicates if this bounding object is supposed to be already initialized or not. This parameter is used to improve the performances. + * @param pointList + * are the points used to update the bounding box coordinates + */ + @Override + protected void combinePoints(boolean isInit, Collection pointList) { + if (!isInit) { + computeBoundingBoxes(null, pointList); + } else { + computeBoundingBoxes(null, new MergedPointList(pointList)); + } + } + + /** + * Add the bounds into the bounds. + * + * @param isInit + * must indicates if this bounding object is supposed to be already initialized or not. This parameter is used to improve the performances. + * @param bounds + * are the bounds used to update the bounding box coordinates + */ + @Override + protected void combineBounds(boolean isInit, Collection bounds) { + if (!isInit) { + if (bounds.size() == 1) { + Bounds3D cb = CollectionUtil.firstElement(bounds); + if (cb instanceof OrientedBox) { + OrientedBox obb = (OrientedBox) cb; + this.center = (EuclidianPoint3D) obb.center.clone(); + for (int i = 0; i < 3; ++i) { + this.axis[i] = (Vector3f) obb.axis[i].clone(); + this.extent[i] = obb.extent[i]; + } + this.vertices = null; + this.aabbLower = this.aabbUpper = null; + this.isBoundInit = true; + } else if (cb instanceof Sphere) { + Sphere sphere = (Sphere) cb; + this.center = (EuclidianPoint3D) sphere.center.clone(); + this.axis[0] = new Vector3f(1., 0., 0.); + this.axis[1] = new Vector3f(0., 1., 0.); + this.axis[2] = new Vector3f(0., 0., 1.); + for (int i = 0; i < 3; ++i) { + this.extent[i] = sphere.radius; + } + this.vertices = null; + this.aabbLower = this.aabbUpper = null; + this.isBoundInit = true; + } else if (cb instanceof AxisAlignedBox) { + AxisAlignedBox aabb = (AxisAlignedBox) cb; + this.center = (EuclidianPoint3D) aabb.getCenter().clone(); + this.axis[0] = new Vector3f(1., 0., 0.); + this.axis[1] = new Vector3f(0., 1., 0.); + this.axis[2] = new Vector3f(0., 0., 1.); + this.extent[0] = aabb.getSizeX() / 2.f; + this.extent[1] = aabb.getSizeY() / 2.f; + this.extent[2] = aabb.getSizeZ() / 2.f; + this.vertices = null; + this.aabbLower = this.aabbUpper = null; + this.isBoundInit = true; + } else if (cb instanceof ComposedBounds3D) { + ComposedBounds3D ccb = (ComposedBounds3D) cb; + computeBoundingBoxes(null, new MergedBoundList(ccb)); + } + } else { + computeBoundingBoxes(null, new MergedBoundList(bounds)); + } + } else { + computeBoundingBoxes(null, new MergedBoundList(bounds, this)); + } + } + + /** + * Reset the content of this bounds + */ + @Override + public void reset() { + if (this.isBoundInit) { + this.isBoundInit = false; + this.center.set(0, 0, 0); + + for (int i = 0; i < 3; ++i) { + this.axis[i].set(0, 0, 0); + this.extent[i] = 0; + } + + this.vertices = null; + this.aabbLower = this.aabbUpper = null; + } + } + + /** + * {@inheritDoc} + */ + @Override + public float getSizeX() { + if (!this.isBoundInit) + return Float.NaN; + if (this.aabbLower == null || this.aabbUpper == null) + computeVertices(); + return this.aabbUpper.getA() - this.aabbLower.getA(); + } + + /** + * {@inheritDoc} + */ + @Override + public float getSizeY() { + if (!this.isBoundInit) + return Float.NaN; + if (this.aabbLower == null || this.aabbUpper == null) + computeVertices(); + return this.aabbUpper.getB() - this.aabbLower.getB(); + } + + /** + * {@inheritDoc} + */ + @Override + public float getSizeZ() { + if (!this.isBoundInit) + return Float.NaN; + if (this.aabbLower == null || this.aabbUpper == null) + computeVertices(); + return this.aabbUpper.getC() - this.aabbLower.getC(); + } + + /** + * {@inheritDoc} + */ + @Override + public Vector3f getSize() { + if (!this.isBoundInit) + return null; + if (this.aabbLower == null || this.aabbUpper == null) + computeVertices(); + return new Vector3f(this.aabbUpper.getA() - this.aabbLower.getA(), this.aabbUpper.getB() - this.aabbLower.getB(), this.aabbUpper.getC() - this.aabbLower.getC()); + } + + // -------------------------------------------------------- + // TranslatableBounds + // -------------------------------------------------------- + + @Override + public void translate(Vector3f v) { + if (isInit()) { + this.center.add(v); + if (this.aabbLower != null) + this.aabbLower.add(v); + if (this.aabbUpper != null) + this.aabbUpper.add(v); + if (this.vertices != null) { + for (Vector3f vv : this.vertices) { + if (vv != null) { + vv.add(v); + } + } + } + } + } + + /** + * Replies the height of this box. + * + * @return the height of this box. + */ + private float getHeight(CoordinateSystem3D cs) { + return (cs.getHeightCoordinateIndex() == 1) ? getSizeY() : getSizeZ(); + } + + /** + * {@inheritDoc} + */ + @Override + public void setTranslation(Point3f position, boolean onGround) { + if (isInit()) { + if (onGround) { + CoordinateSystem3D cs = CoordinateSystem3D.getDefaultCoordinateSystem(); + Point3f np = new Point3f(position); + cs.addHeight(np, getHeight(cs) / 2.f); + this.center.set(np); + } else { + this.center.set(position); + } + this.aabbLower = this.aabbUpper = null; + this.vertices = null; + } + } + + /** + * {@inheritDoc} + */ + @Override + public void setTranslation(Point3f position) { + if (isInit()) { + this.center.set(position); + this.aabbLower = this.aabbUpper = null; + this.vertices = null; + } + } + + // -------------------------------------------------------- + // RotatableBounds3D + // -------------------------------------------------------- + + /** + * {@inheritDoc} + */ + @Override + public final void rotate(AxisAngle4f rotation) { + Matrix4f m = new Matrix4f(); + m.set(rotation); + for (Vector3f v : this.axis) { + m.transform(v); + } + this.vertices = null; + this.aabbLower = this.aabbUpper = null; + } + + /** + * {@inheritDoc} + */ + @Override + public void rotate(Quaternion rotation) { + Matrix4f m = new Matrix4f(); + m.set(rotation); + for (Vector3f v : this.axis) { + m.transform(v); + } + this.vertices = null; + this.aabbLower = this.aabbUpper = null; + } + + /** + * {@inheritDoc} + */ + @Override + public void setRotation(AxisAngle4f rotation) { + Matrix4f m = new Matrix4f(); + m.set(rotation); + + // Internal algorithm uses an identity matrix as basis + this.axis[0].set(1.f, 0.f, 0.f); + this.axis[1].set(0.f, 1.f, 0.f); + this.axis[2].set(0.f, 0.f, 1.f); + + for (Vector3f v : this.axis) { + m.transform(v); + } + + this.vertices = null; + this.aabbLower = this.aabbUpper = null; + } + + /** + * {@inheritDoc} + */ + @Override + public void setRotation(Quaternion rotation) { + Matrix4f m = new Matrix4f(); + m.set(rotation); + + // Internal algorithm uses an identity matrix as basis + this.axis[0].set(1.f, 0.f, 0.f); + this.axis[1].set(0.f, 1.f, 0.f); + this.axis[2].set(0.f, 0.f, 1.f); + + for (Vector3f v : this.axis) { + m.transform(v); + } + + this.vertices = null; + this.aabbLower = this.aabbUpper = null; + } + + // -------------------------------------------------------- + // IntersectionClassifier + // -------------------------------------------------------- + + /** + * {@inheritDoc} + */ + @Override + public IntersectionType classifies(Point3f p) { + if (!isInit()) + return IntersectionType.OUTSIDE; + Vector3f d = new Vector3f(); + d.sub(p, this.center); // d = p - center + + // For each OBB axis... + float dist; + for (int i = 0; i < this.axis.length; ++i) { + // ...project d onto that axis to get the distance along the axis of d from the box center + dist = d.dot(this.axis[i]); + // If distance farther than the box extents, return OUTSIDE + if (Math.abs(dist) > this.extent[i]) + return IntersectionType.OUTSIDE; + } + + return IntersectionType.INSIDE; + } + + /** + * {@inheritDoc} + */ + @Override + public boolean intersects(Point3f p) { + if (!isInit()) + return false; + Vector3f d = new Vector3f(); + d.sub(p, this.center); // d = p - center + + // For each OBB axis... + float dist; + for (int i = 0; i < this.axis.length; ++i) { + // ...project d onto that axis to get the distance along the axis of d from the box center + dist = d.dot(this.axis[i]); + // If distance farther than the box extents, return OUTSIDE + if (Math.abs(dist) > this.extent[i]) + return false; + } + + return true; + } + + /** + * {@inheritDoc} + */ + @Override + public IntersectionType classifies(Point3f c, float r) { + if (!isInit()) + return IntersectionType.OUTSIDE; + return IntersectionUtil.classifiesSolidSphereOrientedBox(c, r, this.center, this.axis, this.extent); + } + + /** + * {@inheritDoc} + */ + @Override + public boolean intersects(Point3f c, float r) { + if (!isInit()) + return false; + return IntersectionUtil.intersectsSolidSphereOrientedBox(c, r, this.center, this.axis, this.extent); + } + + /** + * {@inheritDoc} + */ + @Override + public IntersectionType classifies(Plane plane) { + if (!isInit()) + return IntersectionType.OUTSIDE; + + // Compute the effective radius of the obb and + // compare it with the distance between the obb center + // and the plane; source MGPCG pp.235 + Vector3f n = plane.getNormal(); + + float dist = Math.abs(plane.distanceTo(this.center)); + + float effectiveRadius = 0.f; + + for (int i = 0; i < this.axis.length; ++i) { + effectiveRadius += Math.abs(MathUtil.dotProduct(this.axis[i].getX() * this.extent[i], this.axis[i].getY() * this.extent[i], this.axis[i].getZ() * this.extent[i], n.getX(), n.getY(), n.getZ())); + } + + return (MathUtil.epsilonCompareTo(dist, effectiveRadius) > 0) ? IntersectionType.OUTSIDE : IntersectionType.SPANNING; + } + + /** + * {@inheritDoc} + */ + @Override + public boolean intersects(Plane plane) { + if (!isInit()) + return false; + // Compute the effective radius of the obb and + // compare it with the distance between the obb center + // and the plane; source MGPCG pp.235 + Vector3f n = plane.getNormal(); + + float dist = Math.abs(plane.distanceTo(this.center)); + + float effectiveRadius = 0.f; + + for (int i = 0; i < this.axis.length; ++i) { + effectiveRadius += Math.abs(MathUtil.dotProduct(this.axis[i].getX() * this.extent[i], this.axis[i].getY() * this.extent[i], this.axis[i].getZ() * this.extent[i], n.getX(), n.getY(), n.getZ())); + } + + return MathUtil.epsilonCompareTo(dist, effectiveRadius) <= 0; + } + + /** + * {@inheritDoc} + */ + @Override + public PlanarClassificationType classifiesAgainst(Plane plane) { + if (!isInit()) + return null; + // Compute the effective radius of the obb and + // compare it with the distance between the obb center + // and the plane; source MGPCG pp.235 + Vector3f n = plane.getNormal(); + + float dist = plane.distanceTo(this.center); + + float effectiveRadius = 0.f; + + for (int i = 0; i < this.axis.length; ++i) { + effectiveRadius += Math.abs(MathUtil.dotProduct(this.axis[i].getX() * this.extent[i], this.axis[i].getY() * this.extent[i], this.axis[i].getZ() * this.extent[i], n.getX(), n.getY(), n.getZ())); + } + + if (MathUtil.epsilonCompareTo(Math.abs(dist), effectiveRadius) <= 0) + return PlanarClassificationType.COINCIDENT; + + return (dist > 0.) ? PlanarClassificationType.IN_FRONT_OF : PlanarClassificationType.BEHIND; + } + + /** + * {@inheritDoc} + */ + @Override + public IntersectionType classifies(Point3f l, Point3f u) { + if (!isInit()) + return IntersectionType.OUTSIDE; + return IntersectionUtil.classifiesAlignedBoxOrientedBox(l, u, this.center, this.axis, this.extent); + } + + /** + * {@inheritDoc} + */ + @Override + public boolean intersects(Point3f l, Point3f u) { + if (!isInit()) + return false; + return IntersectionUtil.intersectsAlignedBoxOrientedBox(l, u, this.center, this.axis, this.extent); + } + + /** + * {@inheritDoc} + */ + @Override + public IntersectionType classifies(Point3f obbCenter, Vector3f[] obbAxis, float[] obbExtent) { + if (!isInit()) + return IntersectionType.OUTSIDE; + return IntersectionUtil.classifiesOrientedBoxes(obbCenter, obbAxis, obbExtent, this.center, this.axis, this.extent); + } + + /** + * {@inheritDoc} + */ + @Override + public boolean intersects(Point3f obbCenter, Vector3f[] obbAxis, float[] obbExtent) { + if (!isInit()) + return false; + return IntersectionUtil.intersectsOrientedBoxes(obbCenter, obbAxis, obbExtent, this.center, this.axis, this.extent); + } + + /** + * An iterable on the OBB vertices and on a list of points. + * + * @author $Author: sgalland$ + * @author $Author: ngaud$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ + private class MergedPointList implements Iterable { + + private final Collection points; + + /** + * @param pointList + * is a list of point to merge inside the iterable. + */ + public MergedPointList(Collection pointList) { + this.points = pointList; + } + + @Override + public Iterator iterator() { + return CollectionUtil.mergeIterators(getGlobalOrientedBoundVertices(), CollectionUtil.sizedIterator(this.points)); + } + + } + + /** + * An iterable on vertices which is merging a collection of bounds and optionnally an oriented bounding box. + * + * @author $Author: sgalland$ + * @author $Author: ngaud$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ + private static class MergedBoundList implements Iterable { + + private final Collection bounds; + private final OrientedBox obb; + + /** + * @param bounds + * are the list of bounds to merge inside + */ + public MergedBoundList(Collection bounds) { + this.bounds = bounds; + this.obb = null; + } + + /** + * @param bounds + * are the list of bounds to merge inside + * @param obb + * is the obb to put back in the merging list. + */ + public MergedBoundList(Collection bounds, OrientedBox obb) { + this.bounds = bounds; + this.obb = obb; + } + + @Override + public Iterator iterator() { + Iterator iterator = new ExtractionIterator(this.bounds.iterator()); + + if (this.obb == null) + return iterator; + + return CollectionUtil.mergeIterators(iterator, this.obb.getGlobalOrientedBoundVertices()); + } + + /** + * An iterable on vertices which is merging a collection of bounds and optionnally an oriented bounding box. + * + * @author Stéphane GALLAND <stephane.galland@utbm.fr> + * @version $Name$ $Revision$ @mavengroupid fr.utbm.set.sfc fr.utbm.set.sfc + * @mavenartifactid setmath + */ + private static class ExtractionIterator implements Iterator { + + private final Iterator bounds; + private LinkedList subBounds = null; + private Iterator vertices; + private Tuple3f next; + + /** + * @param bounds + */ + public ExtractionIterator(Iterator bounds) { + this.bounds = bounds; + searchNext(); + } + + private void searchNext() { + this.next = null; + while (this.vertices == null || !this.vertices.hasNext()) { + Bounds3D cb; + this.vertices = null; + while ((this.subBounds != null && !this.subBounds.isEmpty()) || this.bounds.hasNext()) { + cb = (this.subBounds != null) ? this.subBounds.removeFirst() : this.bounds.next(); + if (cb.isInit()) { + if (cb instanceof OrientedBounds3D) { + this.vertices = ((OrientedBounds3D) cb).getGlobalOrientedBoundVertices(); + break; + } else if (cb instanceof ComposedBounds3D) { + if (this.subBounds == null) + this.subBounds = new LinkedList(); + this.subBounds.addAll((ComposedBounds3D) cb); + } + } + } + if (this.subBounds != null && this.subBounds.isEmpty()) + this.subBounds = null; + if (this.vertices == null) + return; // No more vertex + } + + // One vertex exists! + this.next = this.vertices.next(); + } + + @Override + public boolean hasNext() { + return this.next != null; + } + + @Override + public Tuple3f next() { + if (this.next == null) + throw new NoSuchElementException(); + Tuple3f n = this.next; + searchNext(); + return n; + } + + @Override + public void remove() { + // + } + + } + + } + + /** + * An iterable on vertices which is merging a collection of bounds and optionnally an oriented bounding box. + * + * @author $Author: sgalland$ + * @author $Author: ngaud$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ + public static enum PlaneConstraint { + /** + * Oriented bounding box may have one face coplanar with Oxy plane + */ + OXY, + /** + * Oriented bounding box may have one face coplanar with Oxz plane + */ + OXZ, + /** + * Oriented bounding box may have one face coplanar with Oyz plane + */ + OYZ, + } + +} diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/OrthoPlane.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/OrthoPlane.java new file mode 100644 index 000000000..45fc07ada --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/OrthoPlane.java @@ -0,0 +1,35 @@ +/* + * $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; + +/** This interface represents a 3D plane which is + * parallel to one of the axis planes. + * + * @author $Author: sgalland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public interface OrthoPlane extends Plane { + + // + +} \ No newline at end of file diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/PlanarClassificationType.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/PlanarClassificationType.java new file mode 100644 index 000000000..128dc079b --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/PlanarClassificationType.java @@ -0,0 +1,206 @@ +/* + * $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.geometry3d.continuous; + +/** + * This enumeration describes a classification from a plane. + *

+ * 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: + *

    + *
  • IN_FRONT_OF + *
  • BEHIND + *
  • COINCIDENT + *
+ * + * @author $Author: sgalland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public enum PlanarClassificationType { + + /** + * An object is in the front of the plane. + */ + IN_FRONT_OF, + /** + * An object is behind the plane. + */ + BEHIND, + /** + * An object is intersecting the plane. + */ + COINCIDENT; + + /** Invert the intersection classification. + *

+ * + * + * + * + * + * + * + * + * + *
tresult
IN_FRONT_OFBEHIND
BEHINDIN_FRONT_OF
COINCIDENTCOINCIDENT
+ * + * @param t + * @return the inverted classification. + */ + public static PlanarClassificationType invert(PlanarClassificationType t) { + switch (t) { + case IN_FRONT_OF: + return PlanarClassificationType.BEHIND; + case BEHIND: + return PlanarClassificationType.IN_FRONT_OF; + default: + return t; + } + } + + /** 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. + *

+ * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + *
f1f2result
BEHINDBEHINDBEHIND
BEHINDCOINCIDENTCOINCIDENT
BEHINDIN_FRONT_OFIN_FRONT_OF
COINCIDENTBEHINDCOINCIDENT
COINCIDENTCOINCIDENTCOINCIDENT
COINCIDENTIN_FRONT_OFCOINCIDENT
IN_FRONT_OFBEHINDIN_FRONT_OF
IN_FRONT_OFCOINCIDENTCOINCIDENT
IN_FRONT_OFIN_FRONT_OFIN_FRONT_OF
+ * + * @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 PlanarClassificationType or(PlanarClassificationType f1, PlanarClassificationType f2) { + if (f1==f2) return f1; + if (f1==COINCIDENT || f2==COINCIDENT) return COINCIDENT; + return IN_FRONT_OF; + } + + /** 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. + *

+ * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + *
thisf2result
BEHINDBEHINDBEHIND
BEHINDCOINCIDENTCOINCIDENT
BEHINDIN_FRONT_OFIN_FRONT_OF
COINCIDENTBEHINDCOINCIDENT
COINCIDENTCOINCIDENTCOINCIDENT
COINCIDENTIN_FRONT_OFCOINCIDENT
IN_FRONT_OFBEHINDIN_FRONT_OF
IN_FRONT_OFCOINCIDENTCOINCIDENT
IN_FRONT_OFIN_FRONT_OFIN_FRONT_OF
+ * + * @param f2 is the classification of E against F2. + * @return the classification of E against the whole object composed of F1 and F2. + */ + public PlanarClassificationType or(PlanarClassificationType 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. + *

+ * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + *
f1f2result
BEHINDBEHINDBEHIND
BEHINDCOINCIDENTCOINCIDENT
BEHINDIN_FRONT_OFCOINCIDENT
COINCIDENTBEHINDCOINCIDENT
COINCIDENTCOINCIDENTCOINCIDENT
COINCIDENTIN_FRONT_OFCOINCIDENT
IN_FRONT_OFBEHINDCOINCIDENT
IN_FRONT_OFCOINCIDENTCOINCIDENT
IN_FRONT_OFIN_FRONT_OFIN_FRONT_OF
+ * + * @param f1 + * @param f2 + * @return the result of the intersection. + */ + public static PlanarClassificationType and(PlanarClassificationType f1, PlanarClassificationType f2) { + if (f1==f2) return f1; + return COINCIDENT; + } + + /** 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. + *

+ * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + *
thisf2result
BEHINDBEHINDBEHIND
BEHINDCOINCIDENTCOINCIDENT
BEHINDIN_FRONT_OFCOINCIDENT
COINCIDENTBEHINDCOINCIDENT
COINCIDENTCOINCIDENTCOINCIDENT
COINCIDENTIN_FRONT_OFCOINCIDENT
IN_FRONT_OFBEHINDCOINCIDENT
IN_FRONT_OFCOINCIDENTCOINCIDENT
IN_FRONT_OFIN_FRONT_OFIN_FRONT_OF
+ * + * @param f2 + * @return the result of the intersection. + */ + public PlanarClassificationType and(PlanarClassificationType f2) { + return and(this,f2); + } + +} \ No newline at end of file diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Plane.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Plane.java new file mode 100644 index 000000000..9ed07b05a --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Plane.java @@ -0,0 +1,144 @@ +/* + * $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.geometry3d.continuous; + +import java.io.Serializable; + +import org.arakhne.afc.math.geometry.continuous.transform.Transformable3D; + + +/** This interface represents a 3D plane. + * + * @author $Author: cbohrhauer$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public interface Plane extends Cloneable, Serializable, Transformable3D, PlaneClassifier { + + /** Replies a clone of this plane. + * + * @return a clone. + */ + public Plane clone(); + + /** + * Returns the normal to this plane. + * + * @return the normal of the plane. + */ + public Vector3f getNormal(); + + /** + * Replies the component a of the plane equation. + * + * @return the component a of the plane equation. + */ + public float getEquationComponentA(); + + /** + * Replies the component b of the plane equation. + * + * @return the component b of the plane equation. + */ + public float getEquationComponentB(); + + /** + * Replies the component c of the plane equation. + * + * @return the component c of the plane equation. + */ + public float getEquationComponentC(); + + /** + * Replies the component d of the plane equation. + * + * @return the component d of the plane equation. + */ + public float getEquationComponentD(); + + /** + * Replies the distance from the given point to the plane. + * + * @param x is the x-coordinate of the point. + * @param y is the y-coordinate of the point. + * @param z is the z-coordinate of the point. + * @return the distance from the plane to the point. + * 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. + */ + public float distanceTo(float x, float y, float z); + + /** + * Replies the distance from the given point to the plane. + * + * @param v is the point. + * @return the distance from the plane to the point. + * 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. + */ + public float distanceTo(Tuple3f v); + + /** + * Negate the normal of the plane. + */ + public void negate(); + + /** + * Make the normal of the plane be absolute, ie. all their + * components are positive or nul. + */ + public void absolute(); + + /** + * Normalizes this plane (i.e. the vector (a,b,c) becomes unit length). + * + * @return this plane + */ + public Plane normalize(); + + /** Set the equation of the plane according to be colinear (if possible) + * to the specified plane. + * + * @param plane is the plane to copy. + */ + public void set(Plane plane); + + /** Replies the intersection between this plane and the specified one. + * + * @param plane is used to compute the intersection. + * @return the intersection segment or null + */ + public Line3f getIntersection(Plane plane); + + /** Replies the intersection between this plane and the specified line. + * + * @param line is used to compute the intersection. + * @return the intersection point or null + */ + public Point3f getIntersection(Line3f line); + + + //TODO public Point3f getProjection(Point3D point) + +} \ No newline at end of file diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Plane4f.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Plane4f.java new file mode 100644 index 000000000..a4c26b2f3 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Plane4f.java @@ -0,0 +1,898 @@ +/* + * $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.geometry3d.continuous; + +import org.arakhne.afc.math.MathUtil; +import org.arakhne.afc.math.Matrix4f; +import org.arakhne.afc.math.geometry.continuous.object4d.AxisAngle4f; +import org.arakhne.afc.math.geometry.system.CoordinateSystem3D; +import org.arakhne.afc.math.transform.Transform3D_OLD; + +/** This class represents a 3D plane. + *

+ * P + * be the point we wish to lie in the plane, and let + * n⃗ + * be a nonzero normal vector to the plane. + * The desired plane is the set of all points + * r such that + * + * + * n + * ⃗ + * + * ⋅ + * + * r + * - + * p + * + * = + * 0 + * . + *

+ * If we write + * + * + * + * n⃗ + * + * = + * + * + * a + * b + * c + * + * + * + * ,
+ * + * + * r + * = + * + * xyz + * + * + * , and + * d as the dot product + * + * + * + * n⃗ + * + * ⋅ + * p + * =-d + * + * ,
+ * then the plane Π is determined by the condition:
+ * + * + * Π:a.x + * + + * b.y + * + + * c.z + * + + * d=0 + * + * + *

+ * The normal to the plane is the vector + * abc. + * + * Given three points in space + * x1y1z1, + * x2y2z2 and + * x3y3z3, + * the equation of the plane through these points is + * given by the following determinants. + *

+ * + * + * a= + * + * + * + * 1 + * y1 + * z1 + * + * + * 1 + * y2 + * z2 + * + * + * 1 + * y3 + * z3 + * + * + * + * + * + * + * + * b= + * + * + * + * x1 + * 1 + * z1 + * + * + * x2 + * 1 + * z2 + * + * + * x3 + * 1 + * z3 + * + * + * + * + * + * + * + * c= + * + * + * + * x1 + * y1 + * 1 + * + * + * x2 + * y2 + * 1 + * + * + * x3 + * y3 + * 1 + * + * + * + * + * + * + * + * d=- + * + * + * + * x1 + * y1 + * z1 + * + * + * x2 + * y2 + * z2 + * + * + * x3 + * y3 + * z3 + * + * + * + * + * + * + * @author $Author: cbohrhauer$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public final class Plane4f extends AbstractPlane { + + private static final long serialVersionUID = 2621838558308018582L; + + /** equation coefficient. + */ + private float a; + + /** equation coefficient. + */ + private float b; + + /** equation coefficient. + */ + private float c; + + /** equation coefficient. + */ + private float d; + + /** Cached pivot point. + */ + private transient Point3f cachedPivot = null; + + /** + * + */ + public Plane4f() { + this.a = 1; + this.b = 0; + this.c = 0; + this.d = 0; + } + + /** + * @param a is the plane equation coefficient + * @param b is the plane equation coefficient + * @param c is the plane equation coefficient + * @param d is the plane equation coefficient + */ + public Plane4f(float a, float b, float c, float d) { + this.a = a; + this.b = b; + this.c = c; + this.d = d; + normalize(); + } + + /** + * @param normal is the normal of the plane. + * @param p is a point which lies on the plane. + */ + public Plane4f(Vector3f normal, Point3f p) { + this(normal.getX(), normal.getY(), normal.getZ(), p.getX(), p.getY(), p.getZ()); + } + + /** + * @param a is the plane equation coefficient + * @param b is the plane equation coefficient + * @param c is the plane equation coefficient + * @param px is the x coordinate of a point which lies on the plane. + * @param py is the x coordinate of a point which lies on the plane. + * @param pz is the x coordinate of a point which lies on the plane. + */ + public Plane4f(float a, float b, float c, float px, float py, float pz) { + this.a = a; + this.b = b; + this.c = c; + normalize(); + setPivot(px,py,pz); + } + + /** + * @param plane is the plane to copy + */ + public Plane4f(Plane plane) { + this.a = plane.getEquationComponentA(); + this.b = plane.getEquationComponentB(); + this.c = plane.getEquationComponentC(); + this.d = plane.getEquationComponentD(); + normalize(); + } + + /** + * @param p1 is a point on the plane + * @param p2 is a point on the plane + * @param p3 is a point on the plane + */ + public Plane4f(Tuple3f p1, Tuple3f p2, Tuple3f p3) { + set(p1.getX(), p1.getY(), p1.getZ(), p2.getX(), p2.getY(), p2.getZ(), p3.getX(), p3.getY(), p3.getZ()); + } + + /** + * @param p1x is a point on the plane + * @param p1y is a point on the plane + * @param p1z is a point on the plane + * @param p2x is a point on the plane + * @param p2y is a point on the plane + * @param p2z is a point on the plane + * @param p3x is a point on the plane + * @param p3y is a point on the plane + * @param p3z is a point on the plane + */ + public Plane4f(float p1x, float p1y, float p1z, float p2x, float p2y, float p2z, float p3x, float p3y, float p3z) { + set(p1x, p1y, p1z, p2x, p2y, p2z, p3x, p3y, p3z); + } + + /** Replies a clone of this plane. + * + * @return a clone. + */ + @Override + public Plane4f clone() { + return (Plane4f)super.clone(); + } + + /** Clear buffered values. + */ + protected void clearBufferedValues() { + this.cachedPivot = null; + } + + /** {@inheritDoc} + */ + @Override + public void negate() { + this.a = -this.a; + this.b = -this.b; + this.c = -this.c; + this.d = -this.d; + clearBufferedValues(); + } + + /** {@inheritDoc} + */ + @Override + public void absolute() { + if (this.a<0) this.a = -this.a; + if (this.b<0) this.b = -this.b; + if (this.c<0) this.c = -this.c; + clearBufferedValues(); + } + + /** {@inheritDoc} + */ + @Override + public Plane normalize() { + float t = (float) Math.sqrt(this.a*this.a+this.b*this.b+this.c*this.c); + this.a /= t; + this.b /= t; + this.c /= t; + this.d /= t; + clearBufferedValues(); + return this; + } + + /** {@inheritDoc} + */ + @Override + public void set(Plane plane) { + this.a = plane.getEquationComponentA(); + this.b = plane.getEquationComponentB(); + this.c = plane.getEquationComponentC(); + this.d = plane.getEquationComponentD(); + normalize(); + } + + /** Set this pane to be coplanar with all the three specified points. + * + * @param p1x is a point on the plane + * @param p1y is a point on the plane + * @param p1z is a point on the plane + * @param p2x is a point on the plane + * @param p2y is a point on the plane + * @param p2z is a point on the plane + * @param p3x is a point on the plane + * @param p3y is a point on the plane + * @param p3z is a point on the plane + */ + public void set(float p1x, float p1y, float p1z, float p2x, float p2y, float p2z, float p3x, float p3y, float p3z) { + Vector3f v = new Vector3f(); + if (CoordinateSystem3D.getDefaultCoordinateSystem().isLeftHanded()) { + MathUtil.crossProductLeftHand( + p2x-p1x, p2y-p1y, p2z-p1z, + p3x-p1x, p3y-p1y, p3z-p1z, + v); + } + else { + MathUtil.crossProductRightHand( + p2x-p1x, p2y-p1y, p2z-p1z, + p3x-p1x, p3y-p1y, p3z-p1z, + v); + } + this.a = v.getX(); + this.b = v.getY(); + this.c = v.getZ(); + this.d = - (this.a * p1x + this.b * p1y + this.c * p1z); + normalize(); + } + + /** Set this pane to be coplanar with all the three specified points. + * + * @param p1 is a point on the plane + * @param p2 is a point on the plane + * @param p3 is a point on the plane + */ + public final void set(Point3f p1, Point3f p2, Point3f p3) { + set(p1.getX(), p1.getY(), p1.getZ(), p2.getX(), p2.getY(), p2.getZ(), p3.getX(), p3.getY(), p3.getZ()); + } + + /** Set this pane with the given factors. + * + * @param a is the first factor of the plane equation. + * @param b is the first factor of the plane equation. + * @param c is the first factor of the plane equation. + * @param d is the first factor of the plane equation. + */ + public void set(float a, float b, float c, float d) { + this.a = a; + this.b = b; + this.c = c; + this.d = d; + clearBufferedValues(); + } + + /** {@inheritDoc} + */ + @Override + public Vector3f getNormal() { + return new Vector3f(this.a,this.b,this.c); + } + + /** {@inheritDoc} + */ + @Override + public float getEquationComponentA() { + return this.a; + } + + /** {@inheritDoc} + */ + @Override + public float getEquationComponentB() { + return this.b; + } + + /** {@inheritDoc} + */ + @Override + public float getEquationComponentC() { + return this.c; + } + + /** {@inheritDoc} + */ + @Override + public float getEquationComponentD() { + return this.d; + } + + /** {@inheritDoc} + */ + @Override + public float distanceTo(float x, float y, float z) { + return this.a * x + this.b * y + this.c * z + this.d; + } + + /** {@inheritDoc} + */ + @Override + public void setIdentityTransform() { + this.a = 0; + this.b = 0; + this.c = 1; + this.d = 0; + clearBufferedValues(); + } + + /** {@inheritDoc} + */ + @Override + public void setTransform(Transform3D_OLD trans) { + // Update the plane equation according + // to the desired normal (computed from + // the identity vector and the rotations). + Quaternion quat = new Quaternion(); + trans.get(quat); + Matrix4f m = new Matrix4f(); + m.set(quat); + Vector3f v = new Vector3f(0,0,1); // identity vector + m.transform(v); + this.a = v.getX(); + this.b = v.getY(); + this.c = v.z(); + + // a.x + b.y + c.z + d = 0 + // where (x,y,z) is the translation point + this.d = - (this.a*trans.m03 + this.b*trans.m13 + this.c*trans.m23); + + clearBufferedValues(); + } + + /** {@inheritDoc} + */ + @Override + public final void transform(Transform3D_OLD trans) { + transform(trans,getPivotReference()); + } + + /** + * Transforms the plane object by the given transform. + * + * @param trans + * @param refPoint is the point to project on the point to obtain the pivot. + */ + public void transform(Transform3D_OLD trans, Point3f refPoint) { + Vector3f v = new Vector3f(this.a,this.b,this.c); + trans.transform(v); + + // Update the plane equation according + // to the desired normal (computed from + // the identity vector and the rotations). + this.a = v.getX(); + this.b = v.getY(); + this.c = v.getZ(); + + // a.x + b.y + c.z + d = 0 + // where (x,y,z) is the translation point + this.d = - (this.a*(refPoint.getX()+trans.m03) + + this.b*(refPoint.getY()+trans.m13) + + this.c*(refPoint.getZ()+trans.m23)); + + clearBufferedValues(); + } + + /** {@inheritDoc} + */ + @Override + public Transform3D_OLD getTransformMatrix() { + Point3f pivot = getPivotReference(); + AxisAngle4f n = new AxisAngle4f(this.a,this.b,this.c, 0.f); + + Transform3D_OLD m = new Transform3D_OLD(); + m.setRotation(n); + m.setTranslation(new Vector3f(pivot)); + + return m; + } + + /** {@inheritDoc} + */ + @Override + public final void setTranslation(float x, float y, float z) { + setPivot(x,y,z); + } + + /** {@inheritDoc} + */ + @Override + public void translate(float dx, float dy, float dz) { + // Compute the reference point for the plane + // (usefull for translation) + Point3f refPoint = getPivotReference(); + + // a.x + b.y + c.z + d = 0 + // where (x,y,z) is the translation point + setPivot(refPoint.getX()+dx,refPoint.getY()+dy,refPoint.getZ()+dz); + } + + /** {@inheritDoc} + */ + @Override + public final void setTranslation(Point3f position) { + setTranslation(position.getX(),position.getY(),position.getZ()); + } + + /** {@inheritDoc} + */ + @Override + public final Point3f getTranslation() { + return getPivot(); + } + + /** {@inheritDoc} + */ + @Override + public final void translate(Vector3f v) { + translate(v.getX(),v.getY(),v.getZ()); + } + + /** {@inheritDoc} + */ + @Override + public void setScale(float sx, float sy, float sz) { + // + } + + /** {@inheritDoc} + */ + @Override + public void scale(float sx, float sy, float sz) { + // + } + + /** {@inheritDoc} + */ + @Override + public Tuple3f getScale() { + return new Vector3f(1,1,1); + } + + /** {@inheritDoc} + */ + @Override + public final void setRotation(AxisAngle4f quaternion) { + setRotation(quaternion,getPivotReference()); + } + + /** {@inheritDoc} + */ + @Override + public final void setRotation(AxisAngle4f quaternion, Point3f pivot) { + setRotation(quaternion, pivot, 0f, 0f, 1f); + } + + /** {@inheritDoc} + */ + @Override + public final void setRotation(Quaternion quaternion) { + setRotation(quaternion,getPivotReference()); + } + + /** {@inheritDoc} + */ + @Override + public final void setRotation(Quaternion quaternion, Point3f pivot) { + setRotation(quaternion, pivot, 0f, 0f, 1f); + } + + /** + * Set the rotation for the object. + * The normal used as reference is given as parameter. This + * normal is inside the plane equation when the plane + * was not rotated. + * + * @param quaternion is the rotation + * @param pivot is the pivot point + * @param vx is the default plane normal when the plane is not rotated. + * @param vy is the default plane normal when the plane is not rotated. + * @param vz is the default plane normal when the plane is not rotated. + */ + public void setRotation(Quaternion quaternion, Point3f pivot, float vx, float vy, float vz) { + // Update the plane equation according + // to the desired normal (computed from + // the identity vector and the rotations). + Matrix4f m = new Matrix4f(); + m.set(quaternion); + Vector3f v = new Vector3f(vx,vy,vz); + m.transform(v); + this.a = v.getX(); + this.b = v.getY(); + this.c = v.getZ(); + + // a.x + b.y + c.z + d = 0 + // where (x,y,z) is the translation point + this.d = - (this.a*pivot.getX() + + this.b*pivot.getY() + + this.c*pivot.getZ()); + + clearBufferedValues(); + } + + /** + * Set the rotation for the object. + * The normal used as reference is given as parameter. This + * normal is inside the plane equation when the plane + * was not rotated. + * + * @param quaternion is the rotation + * @param pivot is the pivot point + * @param vx is the default plane normal when the plane is not rotated. + * @param vy is the default plane normal when the plane is not rotated. + * @param vz is the default plane normal when the plane is not rotated. + */ + public void setRotation(AxisAngle4f quaternion, Point3f pivot, float vx, float vy, float vz) { + // Update the plane equation according + // to the desired normal (computed from + // the identity vector and the rotations). + Matrix4f m = new Matrix4f(); + m.set(quaternion); + Vector3f v = new Vector3f(vx,vy,vz); + m.transform(v); + this.a = v.getX(); + this.b = v.getY(); + this.c = v.getZ(); + + // a.x + b.y + c.z + d = 0 + // where (x,y,z) is the translation point + this.d = - (this.a*pivot.getX() + + this.b*pivot.getY() + + this.c*pivot.getZ()); + + clearBufferedValues(); + } + + /** {@inheritDoc} + */ + @Override + public final void rotate(AxisAngle4f quaternion) { + rotate(quaternion,null); + } + + /** {@inheritDoc} + */ + @Override + public void rotate(AxisAngle4f quaternion, Point3f pivot) { + Point3f currentPivot = getPivotReference(); + + // Update the plane equation according + // to the desired normal (computed from + // the identity vector and the rotations). + Matrix4f m = new Matrix4f(); + m.set(quaternion); + Vector3f v = new Vector3f(this.a,this.b,this.c); + m.transform(v); + this.a = v.getX(); + this.b = v.getY(); + this.c = v.getZ(); + + if ((currentPivot==pivot)||(pivot==null)) { + // a.x + b.y + c.z + d = 0 + // where (x,y,z) is the translation point + this.d = - (this.a*currentPivot.getX() + + this.b*currentPivot.getY() + + this.c*currentPivot.getZ()); + } + else { + // Compute the new point + Point3f nP = new Point3f( + currentPivot.getX() - pivot.getX(), + currentPivot.getY() - pivot.getY(), + currentPivot.getZ() - pivot.getZ()); + m.transform(nP); + nP.setX(nP.getX() + pivot.getX()); + nP.setY(nP.getY() + pivot.getY()); + nP.setZ(nP.getZ() + pivot.getZ()); + + // a.x + b.y + c.z + d = 0 + // where (x,y,z) is the translation point + this.d = - (this.a*nP.getX() + + this.b*nP.getY() + + this.c*nP.getZ()); + } + + clearBufferedValues(); + } + + /** {@inheritDoc} + */ + @Override + public final void rotate(Quaternion quaternion) { + rotate(quaternion,null); + } + + /** {@inheritDoc} + */ + @Override + public void rotate(Quaternion quaternion, Point3f pivot) { + Point3f currentPivot = getPivotReference(); + + // Update the plane equation according + // to the desired normal (computed from + // the identity vector and the rotations). + Matrix4f m = new Matrix4f(); + m.set(quaternion); + Vector3f v = new Vector3f(this.a,this.b,this.c); + m.transform(v); + this.a = v.getX(); + this.b = v.getY(); + this.c = v.getZ(); + + if ((currentPivot==pivot)||(pivot==null)) { + // a.x + b.y + c.z + d = 0 + // where (x,y,z) is the translation point + this.d = - (this.a*currentPivot.getX() + + this.b*currentPivot.getY() + + this.c*currentPivot.getZ()); + } + else { + // Compute the new point + Point3f nP = new Point3f( + currentPivot.getX() - pivot.getX(), + currentPivot.getY() - pivot.getY(), + currentPivot.getZ() - pivot.getZ()); + m.transform(nP); + nP.setX(nP.getX() + pivot.getX()); + nP.setY(nP.getY() + pivot.getY()); + nP.setZ(nP.getZ() + pivot.getZ()); + + // a.x + b.y + c.z + d = 0 + // where (x,y,z) is the translation point + this.d = - (this.a*nP.getX() + + this.b*nP.getY() + + this.c*nP.getZ()); + } + + clearBufferedValues(); + } + + /** {@inheritDoc} + */ + @Override + public final AxisAngle4f getAxisAngle() { + return new AxisAngle4f(this.a,this.b,this.c, 0.f); + } + + /** {@inheritDoc} + */ + @Override + public void setPivot(float x, float y, float z) { + // a.x + b.y + c.z + d = 0 + // where (x,y,z) is the translation point + this.d = - (this.a*x + this.b*y + this.c*z); + + clearBufferedValues(); + } + + /** {@inheritDoc} + */ + @Override + public final void setPivot(Point3f point) { + setPivot(point.getX(),point.getY(),point.getZ()); + } + + /** {@inheritDoc} + */ + @Override + public final Point3f getPivot() { + return new Point3f(getPivotReference()); + } + + /** Replies the pivot point around which the rotation must be done. + * + * @return a reference on the buffered pivot point. + */ + protected Point3f getPivotReference() { + if (this.cachedPivot==null) { + this.cachedPivot = MathUtil.projectsPointOnPlaneIn3d(new Point3f(), this); + } + return this.cachedPivot; + } + + /** Replies if this plane has valid normal. + * + * @return true if the plane equation is valid, otherwise false + */ + public boolean isValid() { + return ((!Double.isNaN(this.a))&& + (!Double.isNaN(this.b))&& + (!Double.isNaN(this.c))&& + ((this.a!=0)||(this.b!=0)||(this.c!=0))); + } + + /** + * Exception thowned when the equation of the plane + * is invalid to realize a computation. + * + * @author $Author: sgalland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ + public final class InvalidPlaneEquationException extends RuntimeException { + + private static final long serialVersionUID = 4358255085523562003L; + + /** + * + */ + public InvalidPlaneEquationException() { + // + } + + } + +} \ No newline at end of file diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/PlaneClassifier.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/PlaneClassifier.java new file mode 100644 index 000000000..eb42a1c7e --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/PlaneClassifier.java @@ -0,0 +1,135 @@ +/* + * $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.geometry3d.continuous; + + +/** + * This interface describes an object that permits to classify intersection + * between objects and planes. + * + * @author $Author: cbohrhauer$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public interface PlaneClassifier { + + /** + * Classifies a plane with respect to the plane. + * + * @param otherPlane is the plane to classify + * @return the classification + */ + public PlanarClassificationType classifies(Plane otherPlane); + + /** + * Classifies a point with respect to the plane. + * + * @param vec + * @return the classification + */ + public PlanarClassificationType classifies(Tuple3f vec); + + /** + * Classifies a point with respect to the plane. + * + * @param x + * @param y + * @param z + * @return the classification + */ + public PlanarClassificationType classifies(float x, float y, float z); + + /** + * Classifies a box with respect to the plane. + * + * @param lx + * @param ly + * @param lz + * @param ux + * @param uy + * @param uz + * @return the classification + */ + public PlanarClassificationType classifies(float lx, float ly, float lz, float ux, float uy, float uz); + + /** + * Classifies a sphere with respect to the plane. + * + * @param x + * @param y + * @param z + * @param radius + * @return the classification + */ + public PlanarClassificationType classifies(float x, float y, float z, float radius); + + /** + * Replies if the given plane is intersecting the plane. + * + * @param otherPlane is the plane to classify + * @return true if intersection, otherwise false + */ + public boolean intersects(Plane otherPlane); + + /** + * Replies if the given point is intersecting the plane. + * + * @param vec + * @return true if intersection, otherwise false + */ + public boolean intersects(Tuple3f vec); + + /** + * Replies if the given point is intersecting the plane. + * + * @param x + * @param y + * @param z + * @return true if intersection, otherwise false + */ + public boolean intersects(float x, float y, float z); + + /** + * Replies if the given box is intersecting the plane. + * + * @param lx + * @param ly + * @param lz + * @param ux + * @param uy + * @param uz + * @return true if intersection, otherwise false + */ + public boolean intersects(float lx, float ly, float lz, float ux, float uy, float uz); + + /** + * Replies if the given sphere is intersecting the plane. + * + * @param x + * @param y + * @param z + * @param radius + * @return true if intersection, otherwise false + */ + public boolean intersects(float x, float y, float z, float radius); + +} \ No newline at end of file diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Point3f.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Point3f.java new file mode 100644 index 000000000..5116048bf --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Point3f.java @@ -0,0 +1,267 @@ +/* + * $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.continuous; + +import org.arakhne.afc.math.geometry3d.Point3D; +import org.arakhne.afc.math.geometry3d.Tuple3D; +import org.arakhne.afc.math.geometry3d.Vector3D; + +/** 3D Point with 3 floating-point numbers. + * + * @author $Author: galland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class Point3f extends Tuple3f implements Point3D { + + private static final long serialVersionUID = -4821663886493835147L; + + /** + */ + public Point3f() { + // + } + + /** + * @param tuple is the tuple to copy. + */ + public Point3f(Tuple3D tuple) { + super(tuple); + } + + /** + * @param tuple is the tuple to copy. + */ + public Point3f(int[] tuple) { + super(tuple); + } + + /** + * @param tuple is the tuple to copy. + */ + public Point3f(float[] tuple) { + super(tuple); + } + + /** + * @param x + * @param y + * @param z + */ + public Point3f(int x, int y, int z) { + super(x,y,z); + } + + /** + * @param x + * @param y + * @param z + */ + public Point3f(float x, float y, float z) { + super(x,y,z); + } + + /** + * @param x + * @param y + * @param z + */ + public Point3f(double x, double y, double z) { + super((float)x,(float)y,(float)z); + } + + /** + * @param x + * @param y + * @param z + */ + public Point3f(long x, long y, long z) { + super(x,y,z); + } + + /** {@inheritDoc} + */ + @Override + public Point3f clone() { + return (Point3f)super.clone(); + } + + /** + * {@inheritDoc} + */ + @Override + public int distanceSquared(Point3D p1) { + float dx, dy, dz; + dx = this.x-p1.getX(); + dy = this.y-p1.getY(); + dz = this.z-p1.getZ(); + return (int)(dx*dx+dy*dy+dz*dz); + } + + /** + * {@inheritDoc} + */ + @Override + public float getDistanceSquared(Point3D p1) { + float dx, dy, dz; + dx = this.x-p1.getX(); + dy = this.y-p1.getY(); + dz = this.z-p1.getZ(); + return dx*dx+dy*dy+dz*dz; + } + + /** + * {@inheritDoc} + */ + @Override + public int distance(Point3D p1) { + float dx, dy, dz; + dx = this.x-p1.getX(); + dy = this.y-p1.getY(); + dz = this.z-p1.getZ(); + return (int)Math.sqrt(dx*dx+dy*dy+dz*dz); + } + + /** + * {@inheritDoc} + */ + @Override + public float getDistance(Point3D p1) { + float dx, dy, dz; + dx = this.x-p1.getX(); + dy = this.y-p1.getY(); + dz = this.z-p1.getZ(); + return (float)Math.sqrt(dx*dx+dy*dy+dz*dz); + } + + /** + * {@inheritDoc} + */ + @Override + public int distanceL1(Point3D p1) { + return (int)(Math.abs(this.x-p1.getX()) + Math.abs(this.y-p1.getY()) + Math.abs(this.z-p1.getZ())); + } + + /** + * {@inheritDoc} + */ + @Override + public float getDistanceL1(Point3D p1) { + return Math.abs(this.x-p1.getX()) + Math.abs(this.y-p1.getY() + Math.abs(this.z-p1.getZ())); + } + + /** + * {@inheritDoc} + */ + @Override + public int distanceLinf(Point3D p1) { + return (int)Math.max(Math.max( Math.abs(this.x-p1.getX()), Math.abs(this.y-p1.getY())), + Math.abs(this.z-p1.getZ())); + } + + /** + * {@inheritDoc} + */ + @Override + public float getDistanceLinf(Point3D p1) { + return Math.max( Math.max( Math.abs(this.x-p1.getX()), Math.abs(this.y-p1.getY())), + Math.abs(this.z-p1.getZ())); + } + + @Override + public void add(Point3D t1, Vector3D t2) { + this.x = t1.getX() + t2.getX(); + this.y = t1.getY() + t2.getY(); + this.z = t1.getZ() + t2.getZ(); + } + + @Override + public void add(Vector3D t1, Point3D t2) { + this.x = t1.getX() + t2.getX(); + this.y = t1.getY() + t2.getY(); + this.z = t1.getZ() + t2.getZ(); + } + + @Override + public void add(Vector3D t1) { + this.x += t1.getX(); + this.y += t1.getY(); + this.z += t1.getZ(); + } + + @Override + public void scaleAdd(int s, Vector3D t1, Point3D t2) { + this.x = s * t1.getX() + t2.getX(); + this.y = s * t1.getY() + t2.getY(); + this.z = s * t1.getZ() + t2.getZ(); + } + + @Override + public void scaleAdd(float s, Vector3D t1, Point3D t2) { + this.x = s * t1.getX() + t2.getX(); + this.y = s * t1.getY() + t2.getY(); + this.z = s * t1.getZ() + t2.getZ(); + } + + @Override + public void scaleAdd(int s, Point3D t1, Vector3D t2) { + this.x = s * t1.getX() + t2.getX(); + this.y = s * t1.getY() + t2.getY(); + this.z = s * t1.getZ() + t2.getZ(); + } + + @Override + public void scaleAdd(float s, Point3D t1, Vector3D t2) { + this.x = s * t1.getX() + t2.getX(); + this.y = s * t1.getY() + t2.getY(); + this.z = s * t1.getZ() + t2.getZ(); + } + + @Override + public void scaleAdd(int s, Vector3D t1) { + this.x = s * this.x + t1.getX(); + this.y = s * this.y + t1.getY(); + this.z = s * this.z + t1.getZ(); + } + + @Override + public void scaleAdd(float s, Vector3D t1) { + this.x = s * this.x + t1.getX(); + this.y = s * this.y + t1.getY(); + this.z = s * this.z + t1.getZ(); + } + + @Override + public void sub(Point3D t1, Vector3D t2) { + this.x = t1.getX() - t2.getX(); + this.y = t1.getY() - t2.getY(); + this.z = t1.getZ() - t2.getZ(); + } + + @Override + public void sub(Vector3D t1) { + this.x -= t1.getX(); + this.y -= t1.getY(); + this.z -= t1.getZ(); + } + +} \ No newline at end of file diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Quaternion.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Quaternion.java new file mode 100644 index 000000000..8c1ffdbab --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Quaternion.java @@ -0,0 +1,712 @@ +/* + * $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.continuous; + +import java.io.Serializable; + +import org.arakhne.afc.math.Matrix3f; +import org.arakhne.afc.math.Matrix4f; +import org.arakhne.afc.math.geometry3d.Vector3D; + +/** A 4 element unit quaternion represented by single precision floating + * point a,b,c,d coordinates. The quaternion is always normalized. + * + * @author $Author: galland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + * @see "http://en.wikipedia.org/wiki/Quaternion" + */ +public class Quaternion implements Cloneable, Serializable { + + private static final long serialVersionUID = 4494919776986180960L; + + private final static double EPS = 0.000001; + private final static double EPS2 = 1.0e-30; + + /** Create a quaternion from the axis-angle representation. + * + * @param x + * @param y + * @param z + * @param angle + */ + public static Quaternion newAxisAngle(float x, float y, float z, float angle) { + Quaternion q = new Quaternion(); + q.setAxisAngle(x, y, z, angle); + return q; + } + + /** x coordinate. + */ + protected float a; + + /** y coordinate. + */ + protected float b; + + /** z coordinate. + */ + protected float c; + + /** w coordinate. + */ + protected float d; + + /** + */ + public Quaternion() { + this.a = this.b = this.c = this.d = 0; + } + + /** Create a quaternion {@code a + b.i + c.j + d.k}. + * Caution: the parameters are not an axis-angle representation. So, d is + * not the angle, and (a, b, c) is not the rotation axis. + * @param a + * @param b + * @param c + * @param d + * @see If you want to create a quaternion from a axis-angle representatipon, see {@link #newAxisAngle(float, float, float, float)}. + */ + public Quaternion(float a, float b, float c, float d) { + float mag = (float)(1.0/Math.sqrt( a*a + b*b + c*c + d*d )); + this.a = a*mag; + this.b = b*mag; + this.c = c*mag; + this.d = d*mag; + } + + /** + * @param axis + * @param angle + */ + public Quaternion(Vector3D axis, float angle) { + setAxisAngle(axis, angle); + } + + /** {@inheritDoc} + */ + @Override + public Quaternion clone() { + try { + return (Quaternion)super.clone(); + } + catch(CloneNotSupportedException e) { + throw new Error(e); + } + } + + /** Replies the a factor of the quaternion. + * + * @return a + */ + public float getA() { + return this.a; + } + + /** Set the a factor of the quaternion. + * + * @param a + */ + public void setA(float a) { + this.a = a; + } + + /** Replies the b factor of the quaternion. + * + * @return b + */ + public float getB() { + return this.b; + } + + /** Set the b factor of the quaternion. + * + * @param b + */ + public void setB(float b) { + this.b = b; + } + + /** Replies the c factor of the quaternion. + * + * @return c + */ + public float getC() { + return this.c; + } + + /** Set the c factor of the quaternion. + * + * @param c + */ + public void setC(float c) { + this.c = c; + } + + /** Replies the d factor of the quaternion. + * + * @return d + */ + public float getD() { + return this.d; + } + + /** Set the d factor of the quaternion. + * + * @param d + */ + public void setD(float d) { + this.d = d; + } + + /** + * {@inheritDoc} + */ + @Override + public boolean equals(Object t1) { + try { + Quaternion t2 = (Quaternion) t1; + return(this.a == t2.getA() && this.b == t2.getB() && this.c == t2.getC() && this.d == t2.getD()); + } + catch(AssertionError e) { + throw e; + } + catch (Throwable e2) { + return false; + } + } + + /** + * Returns true if the distance between this quaternion + * and quaternion t1 is less than or equal to the epsilon parameter. + * + * @param t1 the quaternion to be compared to this quaternion + * @param epsilon the threshold value + * @return true or false + */ + public boolean epsilonEquals(Quaternion t1, float epsilon) { + float diff; + + diff = this.a - t1.getA(); + if(Float.isNaN(diff)) return false; + if((diff<0?-diff:diff) > epsilon) return false; + + diff = this.b - t1.getB(); + if(Float.isNaN(diff)) return false; + if((diff<0?-diff:diff) > epsilon) return false; + + diff = this.c - t1.getC(); + if(Float.isNaN(diff)) return false; + if((diff<0?-diff:diff) > epsilon) return false; + + diff = this.d - t1.getD(); + 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.a); + bits = 31 * bits + Float.floatToIntBits(this.b); + bits = 31 * bits + Float.floatToIntBits(this.c); + bits = 31 * bits + Float.floatToIntBits(this.d); + return bits ^ (bits >> 32); + } + + /** + * {@inheritDoc} + */ + @Override + public String toString() { + return "(" //$NON-NLS-1$ + +this.a + +";" //$NON-NLS-1$ + +this.b + +";" //$NON-NLS-1$ + +this.c + +";" //$NON-NLS-1$ + +this.d + +")"; //$NON-NLS-1$ + } + + /** + * Sets the value of this quaternion to the conjugate of quaternion q1. + * @param q1 the source vector + */ + public final void conjugate(Quaternion q1) { + this.a = -q1.a; + this.b = -q1.b; + this.c = -q1.c; + this.d = q1.d; + } + + /** + * Sets the value of this quaternion to the conjugate of itself. + */ + public final void conjugate() { + this.a = -this.a; + this.b = -this.b; + this.c = -this.c; + } + + /** + * Sets the value of this quaternion to the quaternion product of + * quaternions q1 and q2 (this = q1 * q2). + * Note that this is safe for aliasing (e.g. this can be q1 or q2). + * @param q1 the first quaternion + * @param q2 the second quaternion + */ + public final void mul(Quaternion q1, Quaternion q2) { + if (this != q1 && this != q2) { + this.d = q1.d*q2.d - q1.a*q2.a - q1.b*q2.b - q1.c*q2.c; + this.a = q1.d*q2.a + q2.d*q1.a + q1.b*q2.c - q1.c*q2.b; + this.b = q1.d*q2.b + q2.d*q1.b - q1.a*q2.c + q1.c*q2.a; + this.c = q1.d*q2.c + q2.d*q1.c + q1.a*q2.b - q1.b*q2.a; + } + else { + float a, b, d; + + d = q1.d*q2.d - q1.a*q2.a - q1.b*q2.b - q1.c*q2.c; + a = q1.d*q2.a + q2.d*q1.a + q1.b*q2.c - q1.c*q2.b; + b = q1.d*q2.b + q2.d*q1.b - q1.a*q2.c + q1.c*q2.a; + this.c = q1.d*q2.c + q2.d*q1.c + q1.a*q2.b - q1.b*q2.a; + this.d = d; + this.a = a; + this.b = b; + } + } + + + /** + * Sets the value of this quaternion to the quaternion product of + * itself and q1 (this = this * q1). + * @param q1 the other quaternion + */ + public final void mul(Quaternion q1) { + float a, b, d; + + d = this.d*q1.d - this.a*q1.a - this.b*q1.b - this.c*q1.c; + a = this.d*q1.a + q1.d*this.a + this.b*q1.c - this.c*q1.b; + b = this.d*q1.b + q1.d*this.b - this.a*q1.c + this.c*q1.a; + this.c = this.d*q1.c + q1.d*this.c + this.a*q1.b - this.b*q1.a; + this.d = d; + this.a = a; + this.b = b; + } + + /** + * Multiplies quaternion q1 by the inverse of quaternion q2 and places + * the value into this quaternion. The value of both argument quaternions + * is preservered (this = q1 * q2^-1). + * @param q1 the first quaternion + * @param q2 the second quaternion + */ + public final void mulInverse(Quaternion q1, Quaternion q2) + { + Quaternion tempQuat = q2.clone(); + + tempQuat.inverse(); + this.mul(q1, tempQuat); + } + + + + /** + * Multiplies this quaternion by the inverse of quaternion q1 and places + * the value into this quaternion. The value of the argument quaternion + * is preserved (this = this * q^-1). + * @param q1 the other quaternion + */ + public final void mulInverse(Quaternion q1) { + Quaternion tempQuat = q1.clone(); + + tempQuat.inverse(); + this.mul(tempQuat); + } + + /** + * Sets the value of this quaternion to quaternion inverse of quaternion q1. + * @param q1 the quaternion to be inverted + */ + public final void inverse(Quaternion q1) { + float norm; + + norm = 1f/(q1.d*q1.d + q1.a*q1.a + q1.b*q1.b + q1.c*q1.c); + this.d = norm*q1.d; + this.a = -norm*q1.a; + this.b = -norm*q1.b; + this.c = -norm*q1.c; + } + + + /** + * Sets the value of this quaternion to the quaternion inverse of itself. + */ + public final void inverse() { + float norm; + + norm = 1f/(this.d*this.d + this.a*this.a + this.b*this.b + this.c*this.c); + this.d *= norm; + this.a *= -norm; + this.b *= -norm; + this.c *= -norm; + } + + /** + * Sets the value of this quaternion to the normalized value + * of quaternion q1. + * @param q1 the quaternion to be normalized. + */ + public final void normalize(Quaternion q1) { + float norm; + + norm = (q1.a*q1.a + q1.b*q1.b + q1.c*q1.c + q1.d*q1.d); + + if (norm > 0f) { + norm = 1f/(float)Math.sqrt(norm); + this.a = norm*q1.a; + this.b = norm*q1.b; + this.c = norm*q1.c; + this.d = norm*q1.d; + } else { + this.a = 0f; + this.b = 0f; + this.c = 0f; + this.d = 0f; + } + } + + + /** + * Normalizes the value of this quaternion in place. + */ + public final void normalize() { + float norm; + + norm = (this.a*this.a + this.b*this.b + this.c*this.c + this.d*this.d); + + if (norm > 0f) { + norm = 1f / (float)Math.sqrt(norm); + this.a *= norm; + this.b *= norm; + this.c *= norm; + this.d *= norm; + } else { + this.a = 0f; + this.b = 0f; + this.c = 0f; + this.d = 0f; + } + } + + /** + * Sets the value of this quaternion to the rotational component of + * the passed matrix. + * @param m1 the Matrix4f + */ + public final void setFromMatrix(Matrix4f m1) { + float dd = 0.25f*(m1.m00 + m1.m11 + m1.m22 + m1.m33); + + if (dd >= 0) { + if (dd >= EPS2) { + this.d = (float) Math.sqrt(dd); + dd = 0.25f/this.d; + this.a = (m1.m21 - m1.m12)*dd; + this.b = (m1.m02 - m1.m20)*dd; + this.c = (m1.m10 - m1.m01)*dd; + return; + } + } + else { + this.d = 0; + this.a = 0; + this.b = 0; + this.c = 1; + return; + } + + this.d = 0; + dd = -0.5f*(m1.m11 + m1.m22); + + if (dd >= 0) { + if (dd >= EPS2) { + this.a = (float) Math.sqrt(dd); + dd = 1.0f/(2.0f*this.a); + this.b = m1.m10*dd; + this.c = m1.m20*dd; + return; + } + } else { + this.a = 0; + this.b = 0; + this.c = 1; + return; + } + + this.a = 0; + dd = 0.5f*(1.0f - m1.m22); + + if (dd >= EPS2) { + this.b = (float) Math.sqrt(dd); + this.c = m1.m21/(2.0f*this.b); + return; + } + + this.b = 0; + this.c = 1; + } + + /** + * Sets the value of this quaternion to the rotational component of + * the passed matrix. + * @param m1 the Matrix3f + */ + public final void setFromMatrix(Matrix3f m1) { + float dd = 0.25f*(m1.m00 + m1.m11 + m1.m22 + 1.0f); + + if (dd >= 0) { + if (dd >= EPS2) { + this.d = (float) Math.sqrt(dd); + dd = 0.25f/this.d; + this.a = (m1.m21 - m1.m12)*dd; + this.b = (m1.m02 - m1.m20)*dd; + this.c = (m1.m10 - m1.m01)*dd; + return; + } + } else { + this.d = 0; + this.a = 0; + this.b = 0; + this.c = 1; + return; + } + + this.d = 0; + dd = -0.5f*(m1.m11 + m1.m22); + if (dd >= 0) { + if (dd >= EPS2) { + this.a = (float) Math.sqrt(dd); + dd = 0.5f/this.a; + this.b = m1.m10*dd; + this.c = m1.m20*dd; + return; + } + } else { + this.a = 0; + this.b = 0; + this.c = 1; + return; + } + + this.a = 0; + dd = 0.5f*(1.0f - m1.m22); + if (dd >= EPS2) { + this.b = (float) Math.sqrt(dd); + this.c = m1.m21/(2.0f*this.b); + return; + } + + this.b = 0; + this.c = 1; + } + + /** Set the quaternion coordinates. + * + * @param x + * @param y + * @param z + * @param w + */ + public void set(float x, float y, float z, float w) { + float mag = (float)(1.0/Math.sqrt( x*x + y*y + z*z + w*w )); + this.a = x*mag; + this.b = y*mag; + this.c = z*mag; + this.d = w*mag; + } + + /** + * Sets the value of this quaternion to the equivalent rotation + * of the Axis-Angle arguments. + * @param axis is the axis of rotation. + * @param angle is the rotation around the axis. + */ + public final void setAxisAngle(Vector3D axis, float angle) { + setAxisAngle(axis.getX(), axis.getY(), axis.getZ(), angle); + } + + /** + * Sets the value of this quaternion to the equivalent rotation + * of the Axis-Angle arguments. + * @param x is the x coordinate of the rotation axis + * @param y is the y coordinate of the rotation axis + * @param z is the z coordinate of the rotation axis + * @param angle is the rotation around the axis. + */ + public final void setAxisAngle(float x, float y, float z, float angle) { + float mag,amag; + // Quat = cos(theta/2) + sin(theta/2)(roation_axis) + amag = (float)Math.sqrt(x*x + y*y + z*z); + if (amag < EPS ) { + this.d = 0.0f; + this.a = 0.0f; + this.b = 0.0f; + this.c = 0.0f; + } + else { + amag = 1.0f/amag; + mag = (float)Math.sin(angle/2.0); + this.d = (float)Math.cos(angle/2.0); + this.a = x*amag*mag; + this.b = y*amag*mag; + this.c = z*amag*mag; + } + } + + /** Replies the rotation axis represented by this quaternion. + * + * @return the rotation axis + * @see #setAxisAngle(Vector3D, float) + * @see #setAxisAngle(float, float, float, float) + * @see #getAngle() + */ + public final Vector3f getAxis() { + float mag = this.a*this.a + this.b*this.b + this.c*this.c; + + if ( mag > EPS ) { + mag = (float)Math.sqrt(mag); + float invMag = 1f/mag; + + return new Vector3f( + this.a*invMag, + this.b*invMag, + this.c*invMag); + } + return new Vector3f(0f, 0f, 1f); + } + + /** Replies the rotation angle represented by this quaternion. + * + * @return the rotation axis + * @see #setAxisAngle(Vector3D, float) + * @see #setAxisAngle(float, float, float, float) + * @see #getAxis() + */ + public final float getAngle() { + float mag = this.a*this.a + this.b*this.b + this.c*this.c; + + if ( mag > EPS ) { + mag = (float)Math.sqrt(mag); + return (2.f*(float)Math.atan2(mag, this.d)); + } + return 0f; + } + + /** + * Performs a great circle interpolation between this quaternion + * and the quaternion parameter and places the result into this + * quaternion. + * @param q1 the other quaternion + * @param alpha the alpha interpolation parameter + */ + public final void interpolate(Quaternion q1, float alpha) { + // From "Advanced Animation and Rendering Techniques" + // by Watt and Watt pg. 364, function as implemented appeared to be + // incorrect. Fails to choose the same quaternion for the double + // covering. Resulting in change of direction for rotations. + // Fixed function to negate the first quaternion in the case that the + // dot product of q1 and this is negative. Second case was not needed. + + double dot,s1,s2,om,sinom; + + dot = this.a*q1.a + this.b*q1.b + this.c*q1.c + this.d*q1.d; + + if ( dot < 0 ) { + // negate quaternion + q1.a = -q1.a; q1.b = -q1.b; q1.c = -q1.c; q1.d = -q1.d; + dot = -dot; + } + + if ( (1.0 - dot) > EPS ) { + om = Math.acos(dot); + sinom = Math.sin(om); + s1 = Math.sin((1.0-alpha)*om)/sinom; + s2 = Math.sin( alpha*om)/sinom; + } else{ + s1 = 1.0 - alpha; + s2 = alpha; + } + + this.d = (float)(s1*this.d + s2*q1.d); + this.a = (float)(s1*this.a + s2*q1.a); + this.b = (float)(s1*this.b + s2*q1.b); + this.c = (float)(s1*this.c + s2*q1.c); + } + + + + /** + * Performs a great circle interpolation between quaternion q1 + * and quaternion q2 and places the result into this quaternion. + * @param q1 the first quaternion + * @param q2 the second quaternion + * @param alpha the alpha interpolation parameter + */ + public final void interpolate(Quaternion q1, Quaternion q2, float alpha) { + // From "Advanced Animation and Rendering Techniques" + // by Watt and Watt pg. 364, function as implemented appeared to be + // incorrect. Fails to choose the same quaternion for the double + // covering. Resulting in change of direction for rotations. + // Fixed function to negate the first quaternion in the case that the + // dot product of q1 and this is negative. Second case was not needed. + + double dot,s1,s2,om,sinom; + + dot = q2.a*q1.a + q2.b*q1.b + q2.c*q1.c + q2.d*q1.d; + + if ( dot < 0 ) { + // negate quaternion + q1.a = -q1.a; q1.b = -q1.b; q1.c = -q1.c; q1.d = -q1.d; + dot = -dot; + } + + if ( (1.0 - dot) > EPS ) { + om = Math.acos(dot); + sinom = Math.sin(om); + s1 = Math.sin((1.0-alpha)*om)/sinom; + s2 = Math.sin( alpha*om)/sinom; + } else{ + s1 = 1.0 - alpha; + s2 = alpha; + } + this.d = (float)(s1*q1.d + s2*q2.d); + this.a = (float)(s1*q1.a + s2*q2.a); + this.b = (float)(s1*q1.b + s2*q2.b); + this.c = (float)(s1*q1.c + s2*q2.c); + } +} \ No newline at end of file diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Shape3f.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Shape3f.java new file mode 100644 index 000000000..5804517d4 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Shape3f.java @@ -0,0 +1,179 @@ +/* + * $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.continuous; + +import org.arakhne.afc.math.geometry3d.Point3D; +import org.arakhne.afc.math.geometry3d.Shape3D; + + +/** 2D shape with floating-point points. + * + * @author $Author: galland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public interface Shape3f extends Shape3D { + + /** {@inheritDoc} + */ + @Override + public Shape3f 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(Point3D 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(Point3D 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) +abs(z1-z2). + * @param p the point + * @return the distance. + */ + public float distanceL1(Point3D 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), abs(z1-z2)]. + * @param p the point + * @return the distance. + */ + public float distanceLinf(Point3D p); + + /** Translate the shape. + * + * @param dx + * @param dy + * @param dz + */ + public void translate(float dx, float dy, float dz); + + /** Replies if the given point is inside this shape. + * + * @param x + * @param y + * @param z + * @return true if the given point is inside this + * shape, otherwise false. + */ + public boolean contains(float x, float y, float z); + +// /** 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 Shape3f createTransformedShape(Transform3D 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 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); + +} \ No newline at end of file diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Sphere.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Sphere.java new file mode 100644 index 000000000..49cd1f65c --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Sphere.java @@ -0,0 +1,936 @@ +/* + * $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.MathUtil; +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.BoundingCircle; +import org.arakhne.afc.math.geometry.continuous.object4d.AxisAngle4f; +import org.arakhne.afc.math.geometry.system.CoordinateSystem3D; +import org.arakhne.afc.sizediterator.SizedIterator; +import org.arakhne.afc.util.ArrayUtil; + +/** + * A Bounding Sphere. + * + * @author $Author: sgalland$ + * @author $Author: ngaud$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class Sphere extends AbstractCombinableBounds3D implements TranslatableBounds3D, AlignedCombinableBounds3D, RotatableBounds3D, OrientedCombinableBounds3D { + + private static final long serialVersionUID = -6184671747906774171L; + + /** + * The vertices that composes this box, it is compiled when it is required not before + */ + private transient Vector3f[] vertices = null; + + /** Coordinate of the center. + */ + EuclidianPoint3D center = new EuclidianPoint3D(); + + /** Radius of the sphere. + */ + float radius; + + private boolean isBoundInit; + + /** Uninitialized bounding object. + */ + public Sphere() { + this.radius = 1; + this.isBoundInit = false; + } + + /** + * @param x is the center of the sphere. + * @param y is the center of the sphere. + * @param z is the center of the sphere. + * @param radius is the radius of the sphere. + */ + public Sphere(float x, float y, float z, float radius) { + this.center.set(x,y,z); + this.radius = Math.abs(radius); + this.isBoundInit = true; + } + + /** + * @param p is the center of the sphere. + * @param radius is the radius of the sphere. + */ + public Sphere(Tuple3f p, float radius) { + this.center.set(p); + this.radius = Math.abs(radius); + this.isBoundInit = true; + } + + /** + * @param bboxList are the bounding objects used to initialize this bounding sphere + */ + public Sphere(Collection bboxList) { + combineBounds(false,bboxList); + this.isBoundInit = true; + } + + /** + * @param bboxList are the bounding objects used to initialize this bounding sphere + */ + public Sphere(CombinableBounds3D... bboxList) { + combineBounds(false,Arrays.asList(bboxList)); + this.isBoundInit = true; + } + + /** + * {@inheritDoc} + */ + @Override + public final BoundingPrimitiveType3D getBoundType() { + return BoundingPrimitiveType3D.SPHERE; + } + + /** + * {@inheritDoc} + */ + @Override + public Sphere clone() { + Sphere clone = (Sphere)super.clone(); + clone.center = (EuclidianPoint3D)this.center.clone(); + return clone; + } + + /** {@inheritDoc} + */ + @Override + public boolean equals(Object o) { + if (!isInit()) return false; + if (this==o) return true; + if (o instanceof Sphere) { + Sphere s = (Sphere)o; + return (this.center.equals(s.center) && + this.radius==s.radius); + } + return false; + } + + /** + * {@inheritDoc} + */ + @Override + public void reset() { + if (this.isBoundInit) { + this.isBoundInit = false; + this.center.set(0,0,0); + this.radius = 0; + this.vertices = null; + } + } + + /** + * {@inheritDoc} + */ + @Override + public String toString() { + StringBuilder s = new StringBuilder(); + s.append("BoundingSphere["); //$NON-NLS-1$ + s.append(this.center.getA()); + s.append(','); + s.append(this.center.getB()); + s.append(','); + s.append(this.center.getC()); + s.append("]R"); //$NON-NLS-1$ + s.append(this.radius); + + return s.toString(); + } + + /** Replies the radius of this sphere. + * + * @return the radius of this sphere. + */ + public float getRadius() { + return this.radius; + } + + /** Set the radius of this sphere. + *

+ * If the given radius is negative, the sphere radius is set to zero. + * + * @param r is the radius of this sphere. + */ + public void setRadius(float r) { + this.isBoundInit = true; + this.radius = Math.max(0,r); + this.vertices = null; + } + + /** + * {@inheritDoc} + */ + @Override + public boolean isEmpty() { + return this.radius==0; + } + + /** Set the bounds with the given sphere. + *

+ * If the given radius is negative, the sphere radius is set to zero. + * + * @param p + * @param radius + */ + public void set(Tuple3f p, float radius) { + this.isBoundInit = true; + this.center.set(p); + this.radius = Math.max(0, radius); + this.vertices = null; + } + + /** + * {@inheritDoc} + */ + @Override + public void getLowerUpper(Point3f lower, Point3f upper) { + assert(lower!=null); + assert(upper!=null); + lower.set( + this.center.getA()-this.radius, + this.center.getB()-this.radius, + this.center.getC()-this.radius); + upper.set( + this.center.getA()+this.radius, + this.center.getB()+this.radius, + this.center.getC()+this.radius); + } + + /** + * {@inheritDoc} + */ + @Override + public EuclidianPoint3D getLower() { + return new EuclidianPoint3D( + this.center.getA()-this.radius, + this.center.getB()-this.radius, + this.center.getC()-this.radius); + } + + /** + * {@inheritDoc} + */ + @Override + public EuclidianPoint3D getUpper() { + return new EuclidianPoint3D( + this.center.getA()+this.radius, + this.center.getB()+this.radius, + this.center.getC()+this.radius); + } + + /** + * {@inheritDoc} + */ + @Override + public EuclidianPoint3D getCenter() { + return this.center; + } + + private void ensureVertices() { + if(this.vertices == null) { + this.vertices = new Vector3f[8]; + float r = getRadius(); + + this.vertices[0] = new Vector3f( r, r, r); + this.vertices[1] = new Vector3f(-r, r, r); + this.vertices[2] = new Vector3f(-r,-r, r); + this.vertices[3] = new Vector3f( r,-r, r); + this.vertices[4] = new Vector3f( r,-r,-r); + this.vertices[5] = new Vector3f( r, r,-r); + this.vertices[6] = new Vector3f(-r, r,-r); + this.vertices[7] = new Vector3f(-r,-r,-r); + } + } + + /** + * {@inheritDoc} + */ + @Override + public SizedIterator getLocalOrientedBoundVertices() { + ensureVertices(); + return ArrayUtil.sizedIterator(this.vertices); + } + + /** + * {@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 SizedIterator getGlobalOrientedBoundVertices() { + return new LocalToGlobalVertexIterator(getCenter(), getLocalOrientedBoundVertices()); + } + + /** + * {@inheritDoc} + */ + @Override + public EuclidianPoint3D getGlobalVertexAt(int index) { + ensureVertices(); + EuclidianPoint3D p = new EuclidianPoint3D(getCenter()); + p.add(this.vertices[index]); + return p; + } + + /** + * {@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(this.radius, this.radius, this.radius); + } + + /** + * {@inheritDoc} + */ + @Override + public float[] getOrientedBoundExtents() { + return new float[] { + this.radius, + this.radius, + this.radius + }; + } + + /** + * {@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 this.radius; + } + + /** + * {@inheritDoc} + */ + @Override + public float getSExtent() { + return this.radius; + } + + /** + * {@inheritDoc} + */ + @Override + public float getTExtent() { + return this.radius; + } + + /** + * Gets the center of this bounding box + * + * @param centerPoint is the point to set with the coordinates of the sphere's center. + */ + public void getCenter(Point3f centerPoint) { + centerPoint.set(this.center); + } + + /** + * {@inheritDoc} + */ + @Override + public float getSizeX() { + if (!this.isBoundInit) return Float.NaN; + return this.radius * 2.f; + } + + /** + * {@inheritDoc} + */ + @Override + public float getSizeY() { + if (!this.isBoundInit) return Float.NaN; + return this.radius * 2.f; + } + + /** + * {@inheritDoc} + */ + @Override + public float getSizeZ() { + if (!this.isBoundInit) return Float.NaN; + return this.radius * 2.f; + } + + /** + * {@inheritDoc} + */ + @Override + public Vector3f getSize() { + if (!this.isBoundInit) return null; + float s = this.radius*2; + return new Vector3f(s,s,s); + } + + /** Add the point into the bounds. + * + * @param ptX + * @param ptY + * @param ptZ + */ + public void combine(float ptX, float ptY, float ptZ) { + if (!this.isBoundInit) { + this.isBoundInit = true; + this.center.set(ptX, ptY, ptZ); + this.radius = 0.f; + } + else { + float d = MathUtil.distancePointPoint(ptX,ptY,ptZ,this.center.getA(),this.center.getB(),this.center.getC()); + if (d>this.radius) { + d = (d + this.radius)/2.f; + Vector3f v = new Vector3f(this.center); + v.setX(v.getX() - ptX); + v.setY(v.getY() - ptY); + v.setZ(v.getZ() - ptZ); + v.normalize(); + v.scale(d); + this.center.set(ptX,ptY,ptZ); + this.center.add(v); + this.radius = d; + } + this.vertices = null; + } + } + + /** Add the point into the bounds. + */ + @Override + protected void combinePoints(boolean isAlreadyInit, Collection pointList) { + if (pointList!=null && !pointList.isEmpty()) { + if (!isAlreadyInit) { + float minx, miny, minz; + float maxx, maxy, maxz; + + minx = miny = minz = Float.POSITIVE_INFINITY; + maxx = maxy = maxz = Float.NEGATIVE_INFINITY; + + for(Tuple3f point : pointList) { + if (point!=null) { + if (point.getX()maxx) maxx = point.getX(); + if (point.getY()>maxy) maxy = point.getY(); + if (point.getZ()>maxz) maxz = point.getZ(); + } + } + + this.center.set( + (minx+maxx) / 2.f, + (miny+maxy) / 2.f, + (minz+maxz) / 2.f); + this.radius = 0; + float distance; + for(Tuple3f point : pointList) { + distance = MathUtil.distancePointPoint(this.center.getA(),this.center.getB(),this.center.getC(),point.getX(),point.getY(),point.getZ()); + if(distance>this.radius) + this.radius = distance; + } + + this.vertices = null; + this.isBoundInit = true; + } + else { + + for(Tuple3f point : pointList) { + combine(point.getX(),point.getY(),point.getZ()); + } + + } + } + } + + /** Add the bounds into the bounds. + */ + @Override + protected void combineBounds(boolean isAlreadyInit, Collection bounds) { + if (bounds!=null && bounds.size()>0) { + Point3f tmpCenter = new Point3f(this.center); + float sphereRadius = this.radius; + + if (!isAlreadyInit) { + // Compute the center of the set of spheres + float minx, miny, minz; + float maxx, maxy, maxz; + + minx = miny = minz = Float.POSITIVE_INFINITY; + maxx = maxy = maxz = Float.NEGATIVE_INFINITY; + + for(Bounds3D b : bounds) { + if (b.isInit()) { + if (b.getMinX()maxx) maxx = b.getMaxX(); + if (b.getMaxY()>maxy) maxy = b.getMaxY(); + if (b.getMaxZ()>maxz) maxz = b.getMaxZ(); + } + } + + tmpCenter.setX((minx+maxx) / 2.f); + tmpCenter.setY((miny+maxy) / 2.f); + tmpCenter.setZ((minz+maxz) / 2.f); + + sphereRadius = 0; + } + + // Update the radius + float distanceMax, r2; + float d1, d2, d3; + Point3f bCenter; + Vector3f v = new Vector3f(); + + for(Bounds3D b : bounds) { + if (b.isInit()) { + // We assume that b could be viewed as a sphere, named bs + // bs is a sphere with the same center as b and which encloses b + bCenter = b.getCenter(); + + // Compute the distance from the current center to the farest point of b + distanceMax = b.distanceMax(tmpCenter); + + // v is the vector from the b's center to the current center + v.sub(tmpCenter, bCenter); + + // r2 is the radius of bs + r2 = distanceMax - v.length(); + + //Compute diameter candidate + d1 = 2.f*sphereRadius; + d2 = 2.f*r2; + d3 = distanceMax + sphereRadius; + + if (d2>d3 && d2>d1) { + // d2 is the higher value: + // b is enclosing the current sphere + tmpCenter.set(bCenter); + sphereRadius = r2; + } + else if (d3>d2 && d3>d1) { + // d3 is the higher value: + // b is not enclosing the current sphere and + // the current sphere is not enclosing b + v.scale(sphereRadius / (sphereRadius+r2)); + tmpCenter.add(bCenter,v); + sphereRadius = d3 / 2.f; + + } + // ELSE + // d1 is the higher value: + // the current sphere is enclosing b. + // Nothing to change. + } + } + + this.center.set(tmpCenter); + this.radius = sphereRadius; + this.vertices = null; + this.isBoundInit = true; + } + } + + /** {@inheritDoc} + */ + @Override + public float distance(Point3f p) { + if (!isInit()) return Float.NaN; + float d = MathUtil.distance(this.center, p); + return (dthis.radius+r) return IntersectionType.OUTSIDE; + if ((d+r)<=this.radius) return IntersectionType.INSIDE; + if ((d+this.radius)<=r) return IntersectionType.ENCLOSING; + return IntersectionType.SPANNING; + } + + /** + * {@inheritDoc} + */ + @Override + public boolean intersects(Point3f c, float r) { + if (!isInit()) return false; + return c.distance(this.center) <= (this.radius+r); + } + + /** + * {@inheritDoc} + */ + @Override + public IntersectionType classifies(Point3f l, Point3f u) { + assert(l.getX()<=u.getX()); + assert(l.getY()<=u.getY()); + assert(l.getZ()<=u.getZ()); + if (!isInit()) return IntersectionType.OUTSIDE; + return IntersectionUtil.classifiesSolidSphereSolidAlignedBox( + this.center, this.radius, l, u).invert(); + } + + /** + * {@inheritDoc} + */ + @Override + public boolean intersects(Point3f l, Point3f u) { + if (!isInit()) return false; + return IntersectionUtil.intersectsSolidSphereSolidAlignedBox( + this.center, this.radius, l, u); + } + + /** + * {@inheritDoc} + */ + @Override + public IntersectionType classifies(Plane plane) { + if (!isInit()) return IntersectionType.OUTSIDE; + return (plane.classifies( + this.center.getA(), this.center.getB(), this.center.getC(), this.radius) == PlanarClassificationType.COINCIDENT) + ? IntersectionType.SPANNING : IntersectionType.OUTSIDE; + } + + /** + * {@inheritDoc} + */ + @Override + public boolean intersects(Plane plane) { + if (!isInit()) return false; + return plane.intersects( + this.center.getA(), this.center.getB(), this.center.getC(), this.radius); + } + + /** + * {@inheritDoc} + */ + @Override + public PlanarClassificationType classifiesAgainst(Plane plane) { + if (!isInit()) return null; + return (plane.classifies( + this.center.getA(), this.center.getB(), this.center.getC(), this.radius)); + } + + /** + * {@inheritDoc} + */ + @Override + public IntersectionType classifies(Point3f obbCenter, Vector3f[] obbAxis, float[] obbExtent) { + if (!isInit()) return IntersectionType.OUTSIDE; + return IntersectionUtil.classifiesSolidSphereOrientedBox( + this.center, this.radius, + obbCenter, obbAxis, obbExtent).invert(); + } + + /** + * {@inheritDoc} + */ + @Override + public boolean intersects(Point3f obbCenter, Vector3f[] obbAxis, float[] obbExtent) { + if (!isInit()) return false; + return IntersectionUtil.intersectsSolidSphereOrientedBox( + this.center, this.radius, + obbCenter, obbAxis, obbExtent); + } + + //------------------------------------- + // RotatableBounds3D + //------------------------------------- + + /** + * {@inheritDoc} + */ + @Override + public void rotate(AxisAngle4f rotation) { + // + } + + /** + * {@inheritDoc} + */ + @Override + public void rotate(Quaternion rotation) { + // + } + + /** + * {@inheritDoc} + */ + @Override + public void setRotation(AxisAngle4f rotation) { + // + } + + /** + * {@inheritDoc} + */ + @Override + public void setRotation(Quaternion rotation) { + // + } + + @Override + public float area() { + return (float) (4*Math.pow(this.radius,3)*Math.PI/3); + } + +} diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Transform3D.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Transform3D.java new file mode 100644 index 000000000..c0ed336ed --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Transform3D.java @@ -0,0 +1,447 @@ +/* + * $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.geometry3d.continuous; + +import org.arakhne.afc.math.Matrix4f; +import org.arakhne.afc.math.geometry3d.Tuple3D; + + +/** A 3D transformation. + * Is represented internally as a 4x4 floating point matrix. The + * mathematical representation is row major, as in traditional + * matrix mathematics. + *

+ * The transformation matrix is: + * 


+ * | r11 | r12 | r13 | Tx |
+ * | r21 | r22 | r23 | Ty |
+ * | r31 | r32 | r33 | Tz |
+ * | 0   | 0   | 0   | 1  |
+ *
+ * + * @author $Author: galland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class Transform3D extends Matrix4f { + + private static final long serialVersionUID = -8427812783666663224L; + + /** This is the identifity transformation. + */ + public static final Transform3D IDENTITY = new Transform3D(); + + /** + * Constructs a new Transform3D object and sets it to the identity transformation. + */ + public Transform3D() { + setIdentity(); + } + + /** + * 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 + */ + public Transform3D(float m00, float m01, float m02, float m03, float m10, float m11, float m12, float m13, float m20, float m21, float m22, float m23) { + super(m00, m01, m02, m03, m10, m11, m12, m13, m20, m21, m22, m23, 0f, 0f, 0f, 1f); + } + + /** + * Constructs a new Transform3D object and initializes it from the + * specified transform. + * + * @param t + */ + public Transform3D(Transform3D t) { + super(t); + } + + /** + * @param m + */ + public Transform3D(Matrix4f m) { + super(m); + } + + @Override + public Transform3D clone() { + return (Transform3D)super.clone(); + } + + /** Set the position. + *

+ * This function changes only the elements of + * the matrix related to the translation. + * The scaling and the shearing are not changed. + *

+ * After a call to this function, the matrix will + * contains (? means any value): + * 

+	 *          [   ?    ?    x   ]
+	 *          [   ?    ?    y   ]
+	 *          [   ?    ?    z   ]
+	 *          [   ?    ?    ?   ]
+	 *
+ * + * @param x + * @param y + * @param z + * @see #makeTranslationMatrix(float, float, float) + */ + public void setTranslation(float x, float y, float z) { + this.m03 = x; + this.m13 = y; + this.m23 = z; + } + + /** Set the position. + *

+ * This function changes only the elements of + * the matrix related to the translation. + * 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   ]
+	 *          [   ?    ?    t.z   ]
+	 *          [   ?    ?    ?     ]
+	 *
+ * + * @param t + * @see #makeTranslationMatrix(float, float, float) + */ + public void setTranslation(Tuple3D t) { + this.m03 = t.getX(); + this.m13 = t.getY(); + this.m23 = t.getZ(); + } + + /** Translate the position. + *

+ * This function is equivalent to: + * 

+	 * this = this *  [   0    0    0    dx   ]
+	 *                [   0    0    0    dy   ]
+	 *                [   0    0    0    dz   ]
+	 *                [   0    0    0    1    ]
+	 *
+ * + * @param dx + * @param dy + * @param dz + */ + public void translate(float dx, float dy, float dz) { + this.m03 += dx; + this.m13 += dy; + this.m23 += dz; + } + + /** Translate the position. + *

+ * This function is equivalent to: + * 

+	 * this = this *  [   0    0    0    t.x   ]
+	 *                [   0    0    0    t.y   ]
+	 *                [   0    0    0    t.z   ]
+	 *                [   0    0    0    1     ]
+	 *
+ * + * @param t + */ + public void translate(Vector3f t) { + this.m03 += t.getX(); + this.m13 += t.getY(); + this.m23 += t.getZ(); + } + + /** Replies the X translation. + * + * @return the amount + */ + public float getTranslationX() { + return this.m03; + } + + /** Replies the Y translation. + * + * @return the amount + */ + public float getTranslationY() { + return this.m13; + } + + /** Replies the Z translation. + * + * @return the amount + */ + public float getTranslationZ() { + return this.m23; + } + + /** Replies the translation. + * + * @return the amount + */ + public Vector3f getTranslation() { + return new Vector3f(this.m03, this.m13, this.m23); + } + + /** + * Replies the rotation for the object. + * + * @return the amount + */ + public Quaternion getRotation() { + Quaternion q = new Quaternion(); + q.setFromMatrix(this); + return q; + } + + /** + * Set the rotation for the object but do not change the translation. + *

+ * This function changes only the elements of + * the matrix related to the rotation. + * The translation is not changed. + *

+ * After a call to this function, the matrix will + * contains (? means any value, and r is the translation + * of the quaternion as a 3x3 matrix): + * 

+	 *          [   r   r   r   ?   ]
+	 *          [   r   r   r   ?   ]
+	 *          [   r   r   r   ?   ]
+	 *          [   ?   ?   ?   ?   ]
+	 *
+ * + * @param rotation + * @see #makeRotationMatrix(Quaternion) + */ + public void setRotation(Quaternion rotation) { + this.m00 = (1.0f - 2.0f*rotation.getB()*rotation.getB() - 2.0f*rotation.getC()*rotation.getC()); + this.m10 = (2.0f*(rotation.getA()*rotation.getB() + rotation.getD()*rotation.getC())); + this.m20 = (2.0f*(rotation.getA()*rotation.getC() - rotation.getD()*rotation.getB())); + + this.m01 = (2.0f*(rotation.getA()*rotation.getB() - rotation.getD()*rotation.getC())); + this.m11 = (1.0f - 2.0f*rotation.getA()*rotation.getA() - 2.0f*rotation.getC()*rotation.getC()); + this.m21 = (2.0f*(rotation.getB()*rotation.getC() + rotation.getD()*rotation.getA())); + + this.m02 = (2.0f*(rotation.getA()*rotation.getC() + rotation.getD()*rotation.getB())); + this.m12 = (2.0f*(rotation.getB()*rotation.getC() - rotation.getD()*rotation.getA())); + this.m22 = (1.0f - 2.0f*rotation.getA()*rotation.getA() - 2.0f*rotation.getB()*rotation.getB()); + } + + /** + * Rotate the object. + *

+ * This function is equivalent to (where r is the translation + * of the quaternion as a 3x3 matrix): + * 

+	 * this = this *  [   r    r     r     0   ]
+	 *                [   r    r     r     0   ]
+	 *                [   r    r     r     0   ]
+	 *                [   0    0     0     1   ]
+	 *
+ * + * @param rotation + */ + public void rotate(Quaternion rotation) { + Transform3D m = new Transform3D(); + m.makeRotationMatrix(rotation); + mul(m); + } + + /** + * Sets the value of this matrix to a rotation matrix, and no translation. + *

+ * This function changes all the elements of + * the matrix, including the translation. + *

+ * After a call to this function, the matrix will + * contains (? means any value, and r a value from + * the quaternion): + * 

+	 *          [   r  r  r  0   ]
+	 *          [   r  r  r  0   ]
+	 *          [   r  r  r  0   ]
+	 *          [   0  0  0  1   ]
+	 *
+ * + * @param rotation + * @see #setRotation(Quaternion) + */ + public final void makeRotationMatrix(Quaternion rotation) { + this.m00 = (1.0f - 2.0f*rotation.getB()*rotation.getB() - 2.0f*rotation.getC()*rotation.getC()); + this.m10 = (2.0f*(rotation.getA()*rotation.getB() + rotation.getD()*rotation.getC())); + this.m20 = (2.0f*(rotation.getA()*rotation.getC() - rotation.getD()*rotation.getB())); + + this.m01 = (2.0f*(rotation.getA()*rotation.getB() - rotation.getD()*rotation.getC())); + this.m11 = (1.0f - 2.0f*rotation.getA()*rotation.getA() - 2.0f*rotation.getC()*rotation.getC()); + this.m21 = (2.0f*(rotation.getB()*rotation.getC() + rotation.getD()*rotation.getA())); + + this.m02 = (2.0f*(rotation.getA()*rotation.getC() + rotation.getD()*rotation.getB())); + this.m12 = (2.0f*(rotation.getB()*rotation.getC() - rotation.getD()*rotation.getA())); + this.m22 = (1.0f - 2.0f*rotation.getA()*rotation.getA() - 2.0f*rotation.getB()*rotation.getB()); + + this.m03 = (float) 0.0; + this.m13 = (float) 0.0; + this.m23 = (float) 0.0; + + this.m30 = (float) 0.0; + this.m31 = (float) 0.0; + this.m32 = (float) 0.0; + this.m33 = (float) 1.0; + } + + /** + * 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    0    dx   ]
+	 *          [   0    1    0    dy   ]
+	 *          [   0    0    1    dz   ]
+	 *          [   0    0    0    1    ]
+	 *
+ * + * @param dx is the position to put in the matrix. + * @param dy is the position to put in the matrix. + * @param dz is the position to put in the matrix. + * @see #setTranslation(float, float, float) + * @see #setTranslation(Tuple3D) + */ + public final void makeTranslationMatrix(float dx, float dy, float dz) { + this.m00 = 1f; + this.m01 = 0f; + this.m02 = 0f; + this.m03 = dx; + + this.m10 = 0f; + this.m11 = 1f; + this.m12 = 0f; + this.m13 = dy; + + this.m20 = 0f; + this.m21 = 0f; + this.m22 = 1f; + this.m23 = dz; + + this.m30 = 0f; + this.m31 = 0f; + this.m32 = 0f; + this.m33 = 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(Tuple3D t) { + float x, y, z; + x = this.m00 * t.getX() + this.m01 * t.getY() + this.m02 * t.getZ() + this.m03; + y = this.m10 * t.getX() + this.m11 * t.getY() + this.m12 * t.getZ() + this.m13; + z = this.m20 * t.getX() + this.m21 * t.getY() + this.m22 * t.getZ() + this.m23; + t.set(x, y, z); + } + + /** + * Multiply this matrix by the tuple t and and place the result into the + * tuple "result" (result = this*t). + * + * @param t + * the tuple to be multiplied by this matrix + * @param result + * the tuple into which the product is placed + */ + public void transform(Tuple3D t, Tuple3D result) { + result.set( + this.m00 * t.getX() + this.m01 * t.getY() + this.m02 * t.getZ() + this.m03, + this.m10 * t.getX() + this.m11 * t.getY() + this.m12 * t.getZ() + this.m13, + this.m20 * t.getX() + this.m21 * t.getY() + this.m22 * t.getZ() + this.m23); + } + + /** + * 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 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 + */ + 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) { + set(m00, m01, m02, m03, m10, m11, m12, m13, m20, m21, m22, m23, 0f, 0f, 0f, 1f); + } + +} diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Triangle3f.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Triangle3f.java new file mode 100644 index 000000000..d55782863 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Triangle3f.java @@ -0,0 +1,1017 @@ +/* + * $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.io.Serializable; +import java.lang.ref.SoftReference; + +import org.arakhne.afc.math.MathUtil; +import org.arakhne.afc.math.geometry.IntersectionUtil; +import org.arakhne.afc.math.geometry.continuous.intersection.Triangle3f; +import org.arakhne.afc.math.geometry.continuous.object4d.AxisAngle4f; +import org.arakhne.afc.math.geometry.continuous.transform.Transformable3D; +import org.arakhne.afc.math.geometry.system.CoordinateSystem3D; +import org.arakhne.afc.math.geometry2d.continuous.Point2f; +import org.arakhne.afc.math.transform.Transform3D_OLD; + + +/** + * A triangle in space. It is defined by three points. + *

+ * A triangle is transformable. So it has a position given + * by its first point, an orientation given its normal + * and no scale factor. + *

+ * Additionnaly a triangle may have a pivot point + * around which rotations will be apply. By default + * the pivot point is the first point of the triangle. + * + * @author $Author: olamotte$ + * @author $Author: sgalland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + * @since 3.0 + */ +public class Triangle3f implements Cloneable, Serializable, Transformable3D { + + private static final long serialVersionUID = 4801325947764161253L; + + private final Point3f p1; + private final Point3f p2; + private final Point3f p3; + + private SoftReference normal = null; + private Point3f pivot = null; + private SoftReference orientation = null; + + /** + * Construct a triangle 3D. + * This constructor does not copy the given points. + * The triangle's points will be references to the + * given points. + * + * @param p1 + * @param p2 + * @param p3 + */ + public Triangle3f(Point3f p1, Point3f p2, Point3f p3) { + this(p1, p2, p3, false); + } + + /** + * Construct a triangle 3D. + * This constructor does not copy the given points. + * The triangle's points will be references to the + * given points. + * + * @param p1 + * @param p2 + * @param p3 + * @param copyPoints indicates if the given points may be copied + * or referenced by this triangle. If true points + * will be copied, false points will be referenced. + */ + public Triangle3f(Point3f p1, Point3f p2, Point3f p3, boolean copyPoints) { + if (copyPoints) { + this.p1 = new Point3f(p1); + this.p2 = new Point3f(p2); + this.p3 = new Point3f(p3); + } + else { + this.p1 = p1; + this.p2 = p2; + this.p3 = p3; + } + } + + /** + * Construct a triangle 3D. + * + * @param p1x is the x coordinate of the first point. + * @param p1y is the y coordinate of the first point. + * @param p1z is the z coordinate of the first point. + * @param p2x is the x coordinate of the first point. + * @param p2y is the y coordinate of the first point. + * @param p2z is the z coordinate of the first point. + * @param p3x is the x coordinate of the first point. + * @param p3y is the y coordinate of the first point. + * @param p3z is the z coordinate of the first point. + */ + public Triangle3f( + float p1x, float p1y, float p1z, + float p2x, float p2y, float p2z, + float p3x, float p3y, float p3z) { + this.p1 = new Point3f(p1x, p1y, p1z); + this.p2 = new Point3f(p2x, p2y, p2z); + this.p3 = new Point3f(p3x, p3y, p3z); + } + + /** + * Checks if a point is inside this triangle. + * + * @param point is the the point + * @return true if the point is in the triangle , otherwise false. + */ + public boolean contains(Point3f point) { + return IntersectionUtil.intersectsPointTriangle( + point.getX(), point.getY(), point.getZ(), + this.p1.getX(), this.p1.getY(), this.p1.getZ(), + this.p2.getX(), this.p2.getY(), this.p2.getZ(), + this.p3.getX(), this.p3.getY(), this.p3.getZ(), + true, 0f); + } + + /** + * Checks if the projection of a point on the triangle's plane is inside the triangle. + * + * @param point is the the point + * @return true if the point is in the triangle , otherwise false. + */ + public boolean containsProjectionOf(Point3f point) { + + //TODO project point on the triangle's plane + return IntersectionUtil.intersectsPointTriangle( + point.getX(), point.getY(), point.getZ(), + this.p1.getX(), this.p1.getY(), this.p1.getZ(), + this.p2.getX(), this.p2.getY(), this.p2.getZ(), + this.p3.getX(), this.p3.getY(), this.p3.getZ(), + true, 0f); + } + + /** Replies the normal to this triangle face. + * + * @return the normal. + */ + public Vector3f getNormal() { + Vector3f v = null; + if (this.normal!=null) { + v = this.normal.get(); + } + if (v==null) { + v = new Vector3f(); + if (CoordinateSystem3D.getDefaultCoordinateSystem().isLeftHanded()) { + MathUtil.crossProductLeftHand( + this.p2.getX() - this.p1.getX(), + this.p2.getY() - this.p1.getY(), + this.p2.getZ() - this.p1.getZ(), + this.p3.getX() - this.p1.getX(), + this.p3.getY() - this.p1.getY(), + this.p3.getZ() - this.p1.getZ(), + v); + } + else { + MathUtil.crossProductRightHand( + this.p2.getX() - this.p1.getX(), + this.p2.getY() - this.p1.getY(), + this.p2.getZ() - this.p1.getZ(), + this.p3.getX() - this.p1.getX(), + this.p3.getY() - this.p1.getY(), + this.p3.getZ() - this.p1.getZ(), + v); + } + v.normalize(); + this.normal = new SoftReference<>(v); + } + return v; + } + + /** Replies the plane on which this triangle is coplanar. + * + * @return the coplanar plane to this triangle + */ + public Plane getPlane() { + Vector3f norm = getNormal(); + assert(norm!=null); + if (norm.getY()==0. && norm.getZ()==0.) + return new YZPlane(this.p1.getX()); + if (norm.getX()==0. && norm.getZ()==0.) + return new XZPlane(this.p1.getY()); + if (norm.getX()==0. && norm.getY()==0.) + return new XYPlane(this.p1.getZ()); + return new Plane4f(this.p1, this.p2, this.p3); + } + + /** + * Replies the first point of the triangle. + * + * @return the first point of the triangle. + */ + public Point3f getPoint1() { + return this.p1; + } + + /** + * Replies the second point of the triangle. + * + * @return the second point of the triangle. + */ + public Point3f getPoint2() { + return this.p2; + } + + /** + * Replies the third point of the triangle. + * + * @return the third point of the triangle. + */ + public Point3f getPoint3() { + return this.p3; + } + + @Override + public String toString() { + StringBuilder buffer = new StringBuilder(); + buffer.append("["); //$NON-NLS-1$ + buffer.append(this.p1.getX()); + buffer.append(";"); //$NON-NLS-1$ + buffer.append(this.p1.getY()); + buffer.append(";"); //$NON-NLS-1$ + buffer.append(this.p1.getZ()); + buffer.append("]-["); //$NON-NLS-1$ + buffer.append(this.p2.getX()); + buffer.append(";"); //$NON-NLS-1$ + buffer.append(this.p2.getY()); + buffer.append(";"); //$NON-NLS-1$ + buffer.append(this.p2.getZ()); + buffer.append("]-["); //$NON-NLS-1$ + buffer.append(this.p3.getX()); + buffer.append(";"); //$NON-NLS-1$ + buffer.append(this.p3.getY()); + buffer.append(";"); //$NON-NLS-1$ + buffer.append(this.p3.getZ()); + buffer.append("]"); //$NON-NLS-1$ + return buffer.toString(); + } + + /** + * Replies the distance from the given point to the triangle. + *

+ * If the point is on the same plane as the triangle, the + * distance between the point and the nearest segment + * is computed. + * + * @param x is the x-coordinate of the point. + * @param y is the y-coordinate of the point. + * @param z is the z-coordinate of the point. + * @return the distance from the triangle to the point. + */ + public float distanceTo(float x, float y, float z) { + if (IntersectionUtil.intersectsPointTriangle( + x, y, z, + this.p1.getX(), this.p1.getY(), this.p1.getZ(), + this.p2.getX(), this.p2.getY(), this.p2.getZ(), + this.p3.getX(), this.p3.getY(), this.p3.getZ(), + false)) { + Vector3f n = getNormal(); + float d = -(n.getX() * this.p1.getX() + n.getY() * this.p1.getY() + n.getZ() * this.p1.getZ()); + return n.getX() * x + n.getY() * y + n.getZ() * z + d; + } + return MathUtil.min( + MathUtil.distancePointSegment( + x, y, z, + this.p1.getX(), this.p1.getY(), this.p1.getZ(), + this.p2.getX(), this.p2.getY(), this.p2.getZ()), + MathUtil.distancePointSegment( + x, y, z, + this.p1.getX(), this.p1.getY(), this.p1.getZ(), + this.p3.getX(), this.p3.getY(), this.p3.getZ()), + MathUtil.distancePointSegment( + x, y, z, + this.p2.getX(), this.p2.getY(), this.p2.getZ(), + this.p3.getX(), this.p3.getY(), this.p3.getZ())); + } + + /** + * Replies the distance from the given point to the plane. + *

+ * If the point is on the same plane as the triangle, the + * distance between the point and the nearest segment + * is computed. + * + * @param v is the point. + * @return the distance from the triangle to the point. + */ + public final float distanceTo(Tuple3f v) { + assert(v!=null); + return distanceTo(v.getX(), v.getY(), v.getZ()); + } + + /** + * {@inheritDoc} + */ + @Override + public final AxisAngle4f getAxisAngle() { + AxisAngle4f orient = null; + if (this.orientation!=null) { + orient = this.orientation.get(); + } + if (orient==null) { + Vector3f norm = getNormal(); + assert(norm!=null); + CoordinateSystem3D cs = CoordinateSystem3D.getDefaultCoordinateSystem(); + assert(cs!=null); + Vector3f up = cs.getUpVector(); + assert(up!=null); + Vector3f axis = new Vector3f(); + if (CoordinateSystem3D.getDefaultCoordinateSystem().isLeftHanded()) { + MathUtil.crossProductLeftHand( + up.getX(), up.getY(), up.getZ(), + norm.getX(), norm.getY(), norm.getZ(), + axis); + } + else { + MathUtil.crossProductRightHand( + up.getX(), up.getY(), up.getZ(), + norm.getX(), norm.getY(), norm.getZ(), + axis); + } + axis.normalize(); + orient = new AxisAngle4f(axis, + MathUtil.signedAngle( + up.getX(), up.getY(), up.getZ(), + norm.getX(), norm.getY(), norm.getZ())); + this.orientation = new SoftReference<>(orient); + } + return orient; + } + + /** + * {@inheritDoc} + */ + @Override + public Point3f getPivot() { + return this.pivot==null ? this.p1 : this.pivot; + } + + /** + * {@inheritDoc} + */ + @Override + public final Tuple3f getScale() { + return new Vector3f(1., 1., 1.); + } + + /** + * {@inheritDoc} + */ + @Override + public final Transform3D_OLD getTransformMatrix() { + Transform3D_OLD tr = new Transform3D_OLD(); + tr.setTranslation(this.p1); + tr.setRotation(getAxisAngle()); + return tr; + } + + /** + * {@inheritDoc} + */ + @Override + public Point3f getTranslation() { + return this.p1; + } + + /** + * {@inheritDoc} + */ + @Override + public final void rotate(AxisAngle4f quaternion) { + rotate(quaternion, getPivot()); + } + + /** + * {@inheritDoc} + */ + @Override + public final void rotate(Quaternion quaternion) { + rotate(quaternion, getPivot()); + } + + /** + * {@inheritDoc} + */ + @Override + public final void rotate(AxisAngle4f quaternion, Point3f pivot) { + assert(pivot!=null); + assert(quaternion!=null); + + Transform3D_OLD tr = new Transform3D_OLD(); + tr.setRotation(quaternion); + Vector3f v = new Vector3f(); + + v.sub(this.p1, pivot); + tr.transform(v); + this.p1.add(pivot, v); + + v.sub(this.p2, pivot); + tr.transform(v); + this.p2.add(pivot, v); + + v.sub(this.p3, pivot); + tr.transform(v); + this.p3.add(pivot, v); + + this.normal = null; + this.orientation = null; + } + + /** + * {@inheritDoc} + */ + @Override + public final void rotate(Quaternion quaternion, Point3f pivot) { + assert(pivot!=null); + assert(quaternion!=null); + + Transform3D_OLD tr = new Transform3D_OLD(); + tr.setRotation(quaternion); + Vector3f v = new Vector3f(); + + v.sub(this.p1, pivot); + tr.transform(v); + this.p1.add(pivot, v); + + v.sub(this.p2, pivot); + tr.transform(v); + this.p2.add(pivot, v); + + v.sub(this.p3, pivot); + tr.transform(v); + this.p3.add(pivot, v); + + this.normal = null; + this.orientation = null; + } + + /** + * {@inheritDoc} + */ + @Override + public final void scale(float sx, float sy, float sz) { + // + } + + /** + * {@inheritDoc} + */ + @Override + public final void setIdentityTransform() { + setTransform(new Transform3D_OLD()); + } + + /** + * {@inheritDoc} + */ + @Override + public final void setPivot(float x, float y, float z) { + this.pivot = new Point3f(x, y, z); + } + + /** + * {@inheritDoc} + */ + @Override + public final void setPivot(Point3f point) { + if (this.pivot==null) + this.pivot = new Point3f(point); + else + this.pivot.set(point); + } + + /** + * {@inheritDoc} + */ + @Override + public final void setRotation(Quaternion quaternion) { + setRotation(quaternion, getPivot()); + } + + /** + * {@inheritDoc} + */ + @Override + public final void setRotation(AxisAngle4f quaternion) { + setRotation(quaternion, getPivot()); + } + + /** + * {@inheritDoc} + */ + @Override + public final void setRotation(Quaternion quaternion, Point3f pivot) { + assert(pivot!=null); + assert(quaternion!=null); + + Transform3D_OLD tr = new Transform3D_OLD(); + tr.setRotation(quaternion); + + Vector3f norm = getNormal(); + assert(norm!=null); + + Vector3f newNorm = new Vector3f(0, 0, 1); + tr.transform(newNorm); + + Vector3f rotAxis = new Vector3f(); + rotAxis.cross(norm, newNorm); + + float angle = MathUtil.signedAngle(norm.getX(), norm.getY(), norm.getZ(), newNorm.getX(), newNorm.getY(), newNorm.getZ()); + + rotate(new AxisAngle4f(rotAxis, angle)); + } + + /** + * {@inheritDoc} + */ + @Override + public final void setRotation(AxisAngle4f quaternion, Point3f pivot) { + assert(pivot!=null); + assert(quaternion!=null); + + CoordinateSystem3D cs = CoordinateSystem3D.getDefaultCoordinateSystem(); + + Transform3D_OLD tr = new Transform3D_OLD(); + tr.setRotation(quaternion); + + Vector3f norm = getNormal(); + assert(norm!=null); + + Vector3f newNorm = new Vector3f(); + cs.getUpVector(newNorm); + tr.transform(newNorm); + + Vector3f rotAxis = new Vector3f(); + if (cs.isLeftHanded()) + MathUtil.crossProductLeftHand( + norm.getX(), norm.getY(), norm.getZ(), + newNorm.getX(), newNorm.getY(), newNorm.getZ(), + rotAxis); + else + MathUtil.crossProductRightHand( + norm.getX(), norm.getY(), norm.getZ(), + newNorm.getX(), newNorm.getY(), newNorm.getZ(), + rotAxis); + + float angle = MathUtil.signedAngle(norm.getX(), norm.getY(), norm.getZ(), newNorm.getX(), newNorm.getY(), newNorm.getZ()); + + rotate(new AxisAngle4f(rotAxis, angle)); + } + + /** + * {@inheritDoc} + */ + @Override + public final void setScale(float sx, float sy, float sz) { + // + } + + /** + * {@inheritDoc} + */ + @Override + public final void setTransform(Transform3D_OLD trans) { + Quaternion q = new Quaternion(); + trans.get(q); + setRotation(q, getPivot()); + setTranslation(trans.m30, trans.m31, trans.m32); + } + + /** + * {@inheritDoc} + */ + @Override + public void setTranslation(float x, float y, float z) { + float vx, vy, vz; + + vx = this.p2.getX() - this.p1.getX(); + vy = this.p2.getY() - this.p1.getY(); + vz = this.p2.getZ() - this.p1.getZ(); + this.p2.set(x+vx, y+vy, z+vz); + + vx = this.p3.getX() - this.p1.getX(); + vy = this.p3.getY() - this.p1.getY(); + vz = this.p3.getZ() - this.p1.getZ(); + this.p3.set(x+vx, y+vy, z+vz); + + this.p1.set(x, y, z); + } + + /** + * {@inheritDoc} + */ + @Override + public final void setTranslation(Point3f position) { + setTranslation(position.getX(), position.getY(), position.getZ()); + } + + /** + * {@inheritDoc} + */ + @Override + public final void transform(Transform3D_OLD trans) { + Quaternion q = new Quaternion(); + trans.get(q); + rotate(q, getPivot()); + translate(trans.m30, trans.m31, trans.m32); + } + + /** + * {@inheritDoc} + */ + @Override + public void translate(float dx, float dy, float dz) { + this.p1.setX(this.p1.getX() + dx); + this.p1.setY(this.p1.getY() + dy); + this.p1.setZ(this.p1.getZ() + dz); + this.p2.setX(this.p2.getX() + dx); + this.p2.setY(this.p2.getY() + dy); + this.p2.setZ(this.p2.getZ() + dz); + this.p3.setX(this.p3.getX() + dx); + this.p3.setY(this.p3.getY() + dy); + this.p3.setZ(this.p3.getZ() + dz); + } + + /** + * {@inheritDoc} + */ + @Override + public void translate(Vector3f v) { + assert(v!=null); + this.p1.add(v); + this.p2.add(v); + this.p3.add(v); + } + + /** + * Replies the height of the 2D point when projected + * on this facet. + *

+ * Height depends on the current coordinate system. + * + * @param x is the x-coordinate of the 2D point. + * @param y is the x-coordinate of the 2D point. + * @return the height of the 2D point when projected on the facet. + * @see CoordinateSystem3D#getDefaultCoordinateSystem() + */ + public float interpolateHeight(float x, float y) { + return interpolateHeight(x, y, CoordinateSystem3D.getDefaultCoordinateSystem()); + } + + /** + * Replies the height of the 2D point when projected + * on this facet. + * + * @param x is the x-coordinate of the 2D point. + * @param y is the x-coordinate of the 2D point. + * @param system is the coordinate system to use. + * @return the height of the 2D point when projected on the facet. + */ + public float interpolateHeight(float x, float y, CoordinateSystem3D system) { + assert(system!=null); + int idx = system.getHeightCoordinateIndex(); + assert(idx==1 || idx==2); + Point3f p1 = getPoint1(); + assert(p1!=null); + Vector3f v = getNormal(); + assert(v!=null); + + if (idx==1 && v.getY()==0.) + return p1.getY(); + if (idx==2 && v.getZ()==0.) + return p1.getZ(); + + float d = -(v.getX() * p1.getX() + v.getY() * p1.getY() + v.getZ() * p1.getZ()); + + if (idx==2) + return -(v.getX() * x + v.getY() * y + d) / v.getZ(); + + return -(v.getX() * x + v.getZ() * y + d) / v.getY(); + } + + /** + * Replies the height of the 2D point when projected + * on this facet. + *

+ * Height depends on the current coordinate system. + * + * @param point is the 2D point. + * @return the height of the 2D point when projected on the facet. + * @see CoordinateSystem3D#getDefaultCoordinateSystem() + */ + public final float interpolateHeight(Point2f point) { + return interpolateHeight(point.getX(), point.getY()); + } + + /** + * Replies the height of the 2D point when projected + * on this facet. + * + * @param point is the 2D point. + * @param system is the coordinate system to use. + * @return the height of the 2D point when projected on the facet. + */ + public float interpolateHeight(Point2f point, CoordinateSystem3D system) { + return interpolateHeight(point.getX(), point.getY(), system); + } + + /** + * Replies the 3D point of the 2D point when projected + * on this facet. + * + * @param x is the x-coordinate of the 2D point. + * @param y is the x-coordinate of the 2D point. + * @return the 3D point which is corresponding to the 3D point on the facet. + * @see CoordinateSystem3D#getDefaultCoordinateSystem() + */ + public Point3f interpolatePoint(float x, float y) { + return interpolatePoint(x,y,CoordinateSystem3D.getDefaultCoordinateSystem()); + } + + /** + * Replies the 3D point of the 2D point when projected + * on this facet. + * + * @param x is the x-coordinate of the 2D point. + * @param y is the x-coordinate of the 2D point. + * @param system is the coordinate system to use. + * @return the 3D point which is corresponding to the 3D point on the facet. + */ + public Point3f interpolatePoint(float x, float y, CoordinateSystem3D system) { + assert(system!=null); + int idx = system.getHeightCoordinateIndex(); + assert(idx==1 || idx==2); + Point3f p1 = getPoint1(); + assert(p1!=null); + Vector3f v = getNormal(); + assert(v!=null); + + if (idx==1 && v.getY()==0.) + return new Point3f(x, p1.getY(), y); + if (idx==2 && v.getZ()==0.) + return new Point3f(x, y, p1.getZ()); + + float d = -(v.getX() * p1.getX() + v.getY() * p1.getY() + v.getZ() * p1.getZ()); + + if (idx==2) + return new Point3f(x, y, -(v.getX() * x + v.getY() * y + d) / v.getZ()); + + return new Point3f(x, -(v.getX() * x + v.getZ() * y + d) / v.getY(), y); + } + + /** + * Replies the 3D point of the 2D point when projected + * on this facet. + * + * @param point is the the point + * @return the 3D point which is corresponding to the 3D point on the facet. + * @see CoordinateSystem3D#getDefaultCoordinateSystem() + */ + public final Point3f interpolatePoint(Point2f point) { + return interpolatePoint(point.getX(), point.getY(), CoordinateSystem3D.getDefaultCoordinateSystem()); + } + + /** + * Replies the 3D point of the 2D point when projected + * on this facet. + * + * @param point is the 2D point. + * @param system is the coordinate system to use. + * @return the 3D point which is corresponding to the 3D point on the facet. + */ + public Point3f interpolatePoint(Point2f point, CoordinateSystem3D system) { + return interpolatePoint(point.getX(), point.getY(), system); + } + + /** Replies if a point is inside a triangle. + */ + public static boolean pointInsideTriangle(int i0, int i1, float[] v, float[] u1, float[] u2, float[] u3) { + // is T1 completly inside T2? + // check if V0 is inside tri(U0,U1,U2) + float a = u2[i1] - u1[i1]; + float b = -(u2[i0] - u1[i0]); + float c = -a * u1[i0] - b * u1[i1]; + float d0 = a * v[i0] + b * v[i1] + c; + + a = u3[i1] - u2[i1]; + b = -(u3[i0] - u2[i0]); + c = -a * u2[i0] - b * u2[i1]; + float d1 = a * v[i0] + b * v[i1] + c; + + a = u1[i1] - u2[i1]; + b = -(u1[i0] - u3[i0]); + c = -a * u3[i0] - b * u3[i1]; + float d2 = a * v[i0] + b * v[i1] + c; + + return ((d0*d1>0.)&&(d0*d2>0.0)); + } + + /** Replies how a segment is connected to a triangle. + * + * @return 0 when not connected, + * 1 when the first segment's vertex was connected, + * 2 when the second segment's vertex was connected, + * 3 when both segment vertices were connected. + */ + public static int getSegmentConnection(int i0, int i1, float[] s1, float[] s2, float[] u1, float[] u2, float[] u3) { + // Test if the segment is connected to at least one point of the triangle + int con = 0; + + if (((s1[i0]==u1[i0])&&(s1[i1]==u1[i1]))|| + ((s1[i0]==u2[i0])&&(s1[i1]==u2[i1]))|| + ((s1[i0]==u3[i0])&&(s1[i1]==u3[i1]))) con |= 1; + + if (((s2[i0]==u1[i0])&&(s2[i1]==u1[i1]))|| + ((s2[i0]==u2[i0])&&(s2[i1]==u2[i1]))|| + ((s2[i0]==u3[i0])&&(s2[i1]==u3[i1]))) con |= 2; + + return con; + } + + /** Replies if two coplanar triangles intersect. + * Triangles intersect even if they are connected by two of their + * edges. + *

+ * Triangle/triangle intersection test routine, + * by Tomas Moller, 1997. + * See article "A Fast Triangle-Triangle Intersection Test", + * Journal of Graphics Tools, 2(2), 1997. + * + * @param v1 + * @param v2 + * @param v3 + * @param u1 + * @param u2 + * @param u3 + * @return true if the two triangles are intersecting. + * @see Triangle3f#overlapsCoplanarTriangle(Tuple3f, Tuple3f, Tuple3f, Tuple3f, Tuple3f, Tuple3f) + */ + public static boolean intersectsCoplanarTriangle(float v1x, float v1y, float v1z, float v2x, float v2y, float v2z, float v3x, float v3y, float v3z, + float u1x, float u1y, float u1z, float u2x, float u2y, float u2z, float u3x, float u3y, float u3z) { + + int i0, i1; + + // first project onto an axis-aligned plane, that maximizes the area + // of the triangles, compute indices: i0,i1. + { + float nx = v1y * (v2z - v3z) + v2y * (v3z - v1z) + v3y * (v1z - v2z); + float ny = v1z * (v2x - v3x) + v2z * (v3x - v1x) + v3z * (v1x - v2x); + float nz = v1x * (v2y - v3y) + v2x * (v3y - v1y) + v3x * (v1y - v2y); + + nx = (nx<0) ? -nx : nx; + ny = (ny<0) ? -ny : ny; + nz = (nz<0) ? -nz : nz; + + if(nx>ny) { + if(nx>nz) { + // nx is greatest + i0 = 1; + i1 = 2; + } + else { + // nz is greatest + i0 = 0; + i1 = 1; + } + } + else { /* nx<=ny */ + if(nz>ny) { + // nz is greatest + i0 = 0; + i1 = 1; + } + else { + // ny is greatest + i0 = 0; + i1 = 2; + } + } + } + + float[] tv1 = new float[] {v1x,v1y,v1z}; + float[] tv2 = new float[] {v2x,v2y,v2z}; + float[] tv3 = new float[] {v3x,v3y,v3z}; + float[] tu1 = new float[] {u1x,u1y,u1z}; + float[] tu2 = new float[] {u2x,u2y,u2z}; + float[] tu3 = new float[] {u3x,u3y,u3z}; + + // test all edges of triangle 1 against the edges of triangle 2 + if (intersectsCoplanarTriangle(i0,i1,0,tv1,tv2,tu1,tu2,tu3)) return true; + if (intersectsCoplanarTriangle(i0,i1,0,tv2,tv3,tu1,tu2,tu3)) return true; + if (intersectsCoplanarTriangle(i0,i1,0,tv3,tv1,tu1,tu2,tu3)) return true; + + // finally, test if tri1 is totally contained in tri2 or vice versa + if (Triangle3f.pointInsideTriangle(i0,i1,tv1,tu1,tu2,tu3)) return true; //TODO : créer un isInsideUtil sur le modèle de classifie et intersect + if (Triangle3f.pointInsideTriangle(i0,i1,tu1,tv1,tv2,tv3)) return true; + + return false; + } + + /** Replies if coplanar segment-triangle intersect. + */ + private static boolean intersectsCoplanarTriangle(int i0, int i1, int con, float[] s1, float[] s2, float[] u1, float[] u2, float[] u3) {//TODO : virer les tableaux + float Ax,Ay; + + Ax = s2[i0] - s1[i0]; + Ay = s2[i1] - s1[i1]; + + // test edge U0,U1 against V0,V1 + if (intersectEdgeEdge(i0, i1, con, Ax, Ay, s1, u1, u2)) return true; + // test edge U1,U2 against V0,V1 + if (intersectEdgeEdge(i0, i1, con, Ax, Ay, s1, u2, u3)) return true; + // test edge U2,U1 against V0,V1 + if (intersectEdgeEdge(i0, i1, con, Ax, Ay, s1, u3, u1)) return true; + + return false; + } + + /** This edge to edge test is based on Franlin Antonio's gem: + * "Faster Line Segment Intersection", in Graphics Gems III, + * pp. 199-202. + */ + private static boolean intersectEdgeEdge(int i0, int i1, int con, float Ax, float Ay, float[] v, float[] u1, float[] u2) {//TODO : meme chose. + // [v,b] is the segment that contains the point v + // [c,d] is the segment [u1,u2] + + // A is the vector (v,b) + // B is the vector (d,c) + // C is the vector (c,v) + + float Bx = u1[i0] - u2[i0]; + float By = u1[i1] - u2[i1]; + float Cx = v[i0] - u1[i0]; + float Cy = v[i1] - u1[i1]; + + // + + float f = Ay * Bx - Ax * By; + float d = By * Cx - Bx * Cy; // Line equation: V+d*A + + boolean up = false; + boolean down = false; + + if (f>0) { + switch(con) { + case 1: // First point must be ignored + down = (d>0); + up = (d<=f); + break; + case 2: // Second point must be ignored + down = (d>=0); + up = (d0); + up = (d=0); + up = (d<=f); + } + } + else if (f<0) { + switch(con) { + case 1: // First point must be ignored + down = (d>=f); + up = (d<0); + break; + case 2: // Second point must be ignored + down = (d>f); + up = (d<=0); + break; + case 3: // First and Second points must be ignored + down = (d>f); + up = (d<0); + break; + default: + down = (d>=f); + up = (d<=0); + } + } + + if (up&&down) { + float e = Ax * Cy - Ay * Cx; + if (f>=0) return ((e>=0)&&(e<=f)); + return ((e>=f)&&(e<=0)); + } + + return false; + } + +} diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Tuple3f.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Tuple3f.java new file mode 100644 index 000000000..f46863aff --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Tuple3f.java @@ -0,0 +1,710 @@ +/* + * $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.continuous; + +import org.arakhne.afc.math.geometry3d.Tuple3D; + +/** 3D tuple with 3 floating-point numbers. + * + * @param is the implementation type of the tuple. + * @author $Author: galland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class Tuple3f> implements Tuple3D { + + private static final long serialVersionUID = -2153633162767463917L; + + /** x coordinate. + */ + protected float x; + + /** y coordinate. + */ + protected float y; + + /** z coordinate. + */ + protected float z; + + /** + */ + public Tuple3f() { + this.x = this.y = this.z = 0; + } + + /** + * @param tuple is the tuple to copy. + */ + public Tuple3f(Tuple3D tuple) { + this.x = tuple.getX(); + this.y = tuple.getY(); + this.z = tuple.getZ(); + } + + /** + * @param tuple is the tuple to copy. + */ + public Tuple3f(int[] tuple) { + this.x = tuple[0]; + this.y = tuple[1]; + this.z = tuple[2]; + } + + /** + * @param tuple is the tuple to copy. + */ + public Tuple3f(float[] tuple) { + this.x = tuple[0]; + this.y = tuple[1]; + this.z = tuple[2]; + } + + /** + * @param x + * @param y + * @param z + */ + public Tuple3f(int x, int y, int z) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * @param x + * @param y + * @param z + */ + public Tuple3f(float x, float y, float z) { + this.x = x; + this.y = y; + this.z = z; + } + + /** {@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); + this.z = Math.abs(this.z); + } + + /** + * {@inheritDoc} + */ + @Override + public void absolute(T t) { + t.set(Math.abs(this.x), Math.abs(this.y), Math.abs(this.z)); + } + + /** + * {@inheritDoc} + */ + @Override + public void add(int x, int y, int z) { + this.x += x; + this.y += y; + this.z += z; + } + + /** + * {@inheritDoc} + */ + @Override + public void add(float x, float y, float z) { + this.x += x; + this.y += y; + this.z += z; + } + + /** + * {@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 addZ(int z) { + this.z += z; + } + + /** + * {@inheritDoc} + */ + @Override + public void addZ(float z) { + this.z += z; + } + + /** + * {@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, this.z); + } + + /** + * {@inheritDoc} + */ + @Override + public void get(int[] t) { + t[0] = (int)this.x; + t[1] = (int)this.y; + t[2] = (int)this.z; + } + + /** + * {@inheritDoc} + */ + @Override + public void get(float[] t) { + t[0] = this.x; + t[1] = this.y; + t[2] = this.z; + } + + /** + * {@inheritDoc} + */ + @Override + public void negate(T t1) { + this.x = -t1.getX(); + this.y = -t1.getY(); + this.z = -t1.getZ(); + } + + /** + * {@inheritDoc} + */ + @Override + public void negate() { + this.x = -this.x; + this.y = -this.y; + this.z = -this.z; + } + + /** + * {@inheritDoc} + */ + @Override + public void scale(int s, T t1) { + this.x = s * t1.getX(); + this.y = s * t1.getY(); + this.z = s * t1.getZ(); + } + + /** + * {@inheritDoc} + */ + @Override + public void scale(float s, T t1) { + this.x = (s * t1.getX()); + this.y = (s * t1.getY()); + this.z = (s * t1.getZ()); + } + + /** + * {@inheritDoc} + */ + @Override + public void scale(int s) { + this.x = s * this.x; + this.y = s * this.y; + this.z = s * this.z; + } + + /** + * {@inheritDoc} + */ + @Override + public void scale(float s) { + this.x = (s * this.x); + this.y = (s * this.y); + this.z = (s * this.z); + } + + /** + * {@inheritDoc} + */ + @Override + public void set(Tuple3D t1) { + this.x = t1.getX(); + this.y = t1.getY(); + this.z = t1.getZ(); + } + + /** + * {@inheritDoc} + */ + @Override + public void set(int x, int y, int z) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * {@inheritDoc} + */ + @Override + public void set(float x, float y, float z) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * {@inheritDoc} + */ + @Override + public void set(int[] t) { + this.x = t[0]; + this.y = t[1]; + this.z = t[2]; + } + + /** + * {@inheritDoc} + */ + @Override + public void set(float[] t) { + this.x = t[0]; + this.y = t[1]; + this.z = t[2]; + } + + /** + * {@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 float getZ() { + return this.z; + } + + /** + * {@inheritDoc} + */ + @Override + public int z() { + return (int)this.z; + } + + /** + * {@inheritDoc} + */ + @Override + public void setZ(int z) { + this.z = z; + } + + /** + * {@inheritDoc} + */ + @Override + public void setZ(float z) { + this.z = z; + } + + /** + * {@inheritDoc} + */ + @Override + public void sub(int x, int y, int z) { + this.x -= x; + this.y -= y; + this.z -= z; + } + + /** + * {@inheritDoc} + */ + @Override + public void subX(int x) { + this.x -= x; + } + + /** + * {@inheritDoc} + */ + @Override + public void subY(int y) { + this.y -= y; + } + + /** + * {@inheritDoc} + */ + @Override + public void subZ(int z) { + this.z -= z; + } + + /** + * {@inheritDoc} + */ + @Override + public void sub(float x, float y, float z) { + this.x -= x; + this.y -= y; + this.z -= z; + } + + /** + * {@inheritDoc} + */ + @Override + public void subX(float x) { + this.x -= x; + } + + /** + * {@inheritDoc} + */ + @Override + public void subY(float y) { + this.y -= y; + } + + /** + * {@inheritDoc} + */ + @Override + public void subZ(float z) { + this.z -= z; + } + + /** + * {@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()); + this.z = ((1f-alpha)*t1.getZ() + alpha*t2.getZ()); + } + + /** + * {@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()); + this.z = ((1f-alpha)*this.z + alpha*t1.getZ()); + } + + /** + * {@inheritDoc} + */ + @Override + public boolean equals(Tuple3D t1) { + try { + return(this.x == t1.getX() && this.y == t1.getY() && this.z == t1.getZ()); + } + 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() && this.z == t2.getZ()); + } + 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; + + diff = this.z - t1.getZ(); + 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); + bits = 31 * bits + Float.floatToIntBits(this.z); + return bits ^ (bits >> 32); + } + + /** + * {@inheritDoc} + */ + @Override + public String toString() { + return "(" //$NON-NLS-1$ + +this.x + +";" //$NON-NLS-1$ + +this.y + +";" //$NON-NLS-1$ + +this.z + +")"; //$NON-NLS-1$ + } + +} \ No newline at end of file diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Tuple3fComparator.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Tuple3fComparator.java new file mode 100644 index 000000000..60a3341c6 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Tuple3fComparator.java @@ -0,0 +1,56 @@ +/* + * $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.continuous; + +import java.util.Comparator; + +/** + * Comparator of Tuple2f. + * + * @author $Author: galland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class Tuple3fComparator implements Comparator> { + + /** + */ + public Tuple3fComparator() { + // + } + + /** + * {@inheritDoc} + */ + @Override + public int compare(Tuple3f o1, Tuple3f 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; + cmp = Float.compare(o1.getY(), o2.getY()); + if (cmp!=0) return cmp; + return Float.compare(o1.getZ(), o2.getZ()); + } + +} \ No newline at end of file diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/UnmodifiablePoint3f.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/UnmodifiablePoint3f.java new file mode 100644 index 000000000..7613d349c --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/UnmodifiablePoint3f.java @@ -0,0 +1,115 @@ +/* + * $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.geometry3d.continuous; + +import org.arakhne.afc.math.geometry3d.Tuple3D; + +/** This class implements a Point3f that cannot be modified by + * the setters. + * + * @author $Author: galland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class UnmodifiablePoint3f extends Point3f { + + private static final long serialVersionUID = -2156483626057722906L; + + /** + */ + public UnmodifiablePoint3f() { + super(); + } + + /** + * @param x + * @param y + * @param z + */ + public UnmodifiablePoint3f(float x, float y, float z) { + super(x, y, z); + } + + /** + * {@inheritDoc} + */ + @Override + public UnmodifiablePoint3f clone() { + return (UnmodifiablePoint3f)super.clone(); + } + + @Override + public void set(float x, float y, float z) { + // + } + + @Override + public void set(float[] t) { + // + } + + @Override + public void set(int x, int y, int z) { + // + } + + @Override + public void set(int[] t) { + // + } + + @Override + public void set(Tuple3D 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) { + // + } + + @Override + public void setZ(float z) { + // + } + + @Override + public void setZ(int z) { + // + } + +} diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Vector3f.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Vector3f.java new file mode 100644 index 000000000..bca20c130 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/Vector3f.java @@ -0,0 +1,320 @@ +/* + * $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.continuous; + +import org.arakhne.afc.math.Matrix3f; +import org.arakhne.afc.math.geometry3d.Point3D; +import org.arakhne.afc.math.geometry3d.Tuple3D; +import org.arakhne.afc.math.geometry3d.Vector3D; + +/** 3D Vector with 3 floating-point values. + * + * @author $Author: galland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class Vector3f extends Tuple3f implements Vector3D { + + private static final long serialVersionUID = -1222875298451525734L; + + /** + */ + public Vector3f() { + // + } + + /** + * @param tuple is the tuple to copy. + */ + public Vector3f(Tuple3D tuple) { + super(tuple); + } + + /** + * @param tuple is the tuple to copy. + */ + public Vector3f(int[] tuple) { + super(tuple); + } + + /** + * @param tuple is the tuple to copy. + */ + public Vector3f(float[] tuple) { + super(tuple); + } + + /** + * @param x + * @param y + * @param z + */ + public Vector3f(int x, int y, int z) { + super(x,y,z); + } + + /** + * @param x + * @param y + * @param z + */ + public Vector3f(float x, float y, float z) { + super(x,y,z); + } + + /** + * @param x + * @param y + * @param z + */ + public Vector3f(double x, double y, double z) { + super((float)x,(float)y,(float)z); + } + + /** + * @param x + * @param y + * @param z + */ + public Vector3f(long x, long y, long z) { + super(x,y,z); + } + + /** {@inheritDoc} + */ + @Override + public Vector3f clone() { + return (Vector3f)super.clone(); + } + + /** + * {@inheritDoc} + */ + @Override + public float angle(Vector3D 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(Vector3D v1) { + return (this.x*v1.getX() + this.y*v1.getY() + this.z*v1.getZ()); + } + + /** + * Multiply this vector, transposed, by the given matrix and replies the resulting vector. + * + * @param m + * @return transpose(this * m) + */ + public final Vector3f mul(Matrix3f m) { + Vector3f r = new Vector3f(); + r.x = this.getX() * m.m00 + this.getY() * m.m01 + this.getZ() * m.m02; + r.y = this.getX() * m.m10 + this.getY() * m.m11 + this.getZ() * m.m12; + r.z = this.getX() * m.m20 + this.getY() * m.m21 + this.getZ() * m.m22; + return r; + } + + @Override + public Vector3D cross(Vector3D v1) { + return crossLeftHand(v1); + } + + @Override + public void cross(Vector3D v1, Vector3D v2) { + crossLeftHand(v1, v2); + } + + @Override + public Vector3D crossLeftHand(Vector3D v1) { + float x = v1.getY()*getZ() - v1.getZ()*getY(); + float y = v1.getZ()*getX() - v1.getX()*getZ(); + float z = v1.getX()*getY() - v1.getY()*getX(); + return new Vector3f(x,y,z); + } + + @Override + public void crossLeftHand(Vector3D v1, Vector3D v2) { + float x = v2.getY()*v1.getZ() - v2.getZ()*v1.getY(); + float y = v2.getZ()*v1.getX() - v2.getX()*v1.getZ(); + float z = v2.getX()*v1.getY() - v2.getY()*v1.getX(); + set(x,y,z); + } + + @Override + public Vector3D crossRightHand(Vector3D v1) { + float x = getY()*v1.getZ() - getZ()*v1.getY(); + float y = getZ()*v1.getX() - getX()*v1.getZ(); + float z = getX()*v1.getY() - getY()*v1.getX(); + return new Vector3f(x,y,z); + } + + @Override + public void crossRightHand(Vector3D v1, Vector3D v2) { + float x = v1.getY()*v2.getZ() - v1.getZ()*v2.getY(); + float y = v1.getZ()*v2.getX() - v1.getX()*v2.getZ(); + float z = v1.getX()*v2.getY() - v1.getY()*v2.getX(); + set(x,y,z); + } + + /** + * {@inheritDoc} + */ + @Override + public float length() { + return (float) Math.sqrt(this.x*this.x + this.y*this.y + this.z*this.z); + } + + /** + * {@inheritDoc} + */ + @Override + public float lengthSquared() { + return (this.x*this.x + this.y*this.y + this.z*this.z); + } + + /** + * {@inheritDoc} + */ + @Override + public void normalize(Vector3D v1) { + float norm = 1f / v1.length(); + this.x = (int)(v1.getX()*norm); + this.y = (int)(v1.getY()*norm); + this.z = (int)(v1.getZ()*norm); + } + + /** + * {@inheritDoc} + */ + @Override + public void normalize() { + float norm; + norm = (float)(1./Math.sqrt(this.x*this.x + this.y*this.y + this.z*this.z)); + this.x *= norm; + this.y *= norm; + this.z *= norm; + } + + /** + * {@inheritDoc} + */ + @Override + public void turnVector(Vector3D axis, float angle) { + Transform3D mat = new Transform3D(); + mat.setRotation(new Quaternion(axis, angle)); + mat.transform(this); + } + + /** + * {@inheritDoc} + */ + @Override + public void add(Vector3D t1, Vector3D t2) { + this.x = t1.getX() + t2.getX(); + this.y = t1.getY() + t2.getY(); + this.z = t1.getZ() + t2.getZ(); + } + + /** + * {@inheritDoc} + */ + @Override + public void add(Vector3D t1) { + this.x += t1.getX(); + this.y += t1.getY(); + this.z += t1.getZ(); + } + + /** + * {@inheritDoc} + */ + @Override + public void scaleAdd(int s, Vector3D t1, Vector3D t2) { + this.x = s * t1.getX() + t2.getX(); + this.y = s * t1.getY() + t2.getY(); + this.z = s * t1.getZ() + t2.getZ(); + } + + /** + * {@inheritDoc} + */ + @Override + public void scaleAdd(float s, Vector3D t1, Vector3D t2) { + this.x = s * t1.getX() + t2.getX(); + this.y = s * t1.getY() + t2.getY(); + this.z = s * t1.getZ() + t2.getZ(); + } + + /** + * {@inheritDoc} + */ + @Override + public void scaleAdd(int s, Vector3D t1) { + this.x = s * this.x + t1.getX(); + this.y = s * this.y + t1.getY(); + this.z = s * this.z + t1.getZ(); + } + + /** + * {@inheritDoc} + */ + @Override + public void scaleAdd(float s, Vector3D t1) { + this.x = s * this.x + t1.getX(); + this.y = s * this.y + t1.getY(); + this.z = s * this.z + t1.getZ(); + } + + /** + * {@inheritDoc} + */ + @Override + public void sub(Vector3D t1, Vector3D t2) { + this.x = t1.getX() - t2.getX(); + this.y = t1.getY() - t2.getY(); + this.z = t1.getZ() - t2.getZ(); + } + + @Override + public void sub(Point3D t1, Point3D t2) { + this.x = t1.getX() - t2.getX(); + this.y = t1.getY() - t2.getY(); + this.z = t1.getZ() - t2.getZ(); + } + + /** + * {@inheritDoc} + */ + @Override + public void sub(Vector3D t1) { + this.x -= t1.getX(); + this.y -= t1.getY(); + this.z -= t1.getZ(); + } + +} \ No newline at end of file diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/XYPlane.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/XYPlane.java new file mode 100644 index 000000000..c4efb67c6 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/XYPlane.java @@ -0,0 +1,238 @@ +/* + * $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.transform.Transform3D_OLD; + +/** This class represents a 3D plane which is colinear to the X and Y axis. + * + * @author $Author: cbohrhauer$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class XYPlane extends AbstractOrthoPlane { + + private static final long serialVersionUID = 6857600518749058720L; + + /** + * Coordinate of the plane. + */ + public float z; + + /** + * @param z is the coordinate of the plane. + */ + public XYPlane(float z) { + this.z = z; + } + + /** + * @param p is a point on the plane. + */ + public XYPlane(Tuple3f p) { + this.z = p.getZ(); + } + + /** Replies a clone of this plane. + * + * @return a clone. + */ + @Override + public XYPlane clone() { + return (XYPlane)super.clone(); + } + + /** {@inheritDoc} + */ + @Override + public void set(Plane plane) { + this.z = plane.getEquationComponentC(); + normalize(); + } + + /** {@inheritDoc} + */ + @Override + public void setIdentityTransform() { + this.z = 0; + } + + + /** {@inheritDoc} + */ + @Override + public void setTransform(Transform3D_OLD trans) { + this.z = trans.m23; + } + + /** {@inheritDoc} + */ + @Override + public void transform(Transform3D_OLD trans) { + this.z += trans.m23; + } + + /** {@inheritDoc} + */ + @Override + public Transform3D_OLD getTransformMatrix() { + Transform3D_OLD m = new Transform3D_OLD(); + m.m23 = this.z; + return m; + } + + /** {@inheritDoc} + */ + @Override + public void setTranslation(float x, float y, float z) { + this.z = z; + } + + /** {@inheritDoc} + */ + @Override + public void translate(float dx, float dy, float dz) { + this.z += dz; + } + + /** {@inheritDoc} + */ + @Override + public void setTranslation(Point3f position) { + this.z = position.getZ(); + } + + /** {@inheritDoc} + */ + @Override + public Point3f getTranslation() { + return new Point3f(0.,0.,this.z); + } + + /** {@inheritDoc} + */ + @Override + public void translate(Vector3f v) { + this.z += v.getZ(); + } + + /** {@inheritDoc} + */ + @Override + public Point3f getPivot() { + return new Point3f(0.,0.,this.z); + } + + /** {@inheritDoc} + */ + @Override + public Vector3f getNormal() { + return new Vector3f(0,0,this.isPositive ? 1 : -1); + } + + /** {@inheritDoc} + */ + @Override + public float getEquationComponentA() { + return 0; + } + + /** {@inheritDoc} + */ + @Override + public float getEquationComponentB() { + return 0; + } + + /** {@inheritDoc} + */ + @Override + public float getEquationComponentC() { + return this.isPositive ? 1 : -1; + } + + /** {@inheritDoc} + */ + @Override + public final float getEquationComponentD() { + return this.isPositive ? -this.z : this.z; + } + + /** {@inheritDoc} + */ + @Override + public float distanceTo(float px, float py, float pz) { + float d = pz - this.z; + return this.isPositive ? d : -d; + } + + /** + * Classifies a box 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. + * + * @param lx {@inheritDoc} + * @param ly {@inheritDoc} + * @param lz {@inheritDoc} + * @param ux {@inheritDoc} + * @param uy {@inheritDoc} + * @param uz {@inheritDoc} + * @return {@inheritDoc} + */ + @Override + public PlanarClassificationType classifies(float lx, float ly, float lz, float ux, float uy, float uz) { + float cuz = uz; + float clz = lz; + if (cuzthis.z) t = PlanarClassificationType.IN_FRONT_OF; + else t = PlanarClassificationType.COINCIDENT; + return this.isPositive ? t : PlanarClassificationType.invert(t); + } + + /** + * {@inheritDoc} + */ + @Override + public boolean intersects(float lx, float ly, float lz, float ux, float uy, float uz) { + float cuz = uz; + float clz = lz; + if (cuz=this.z)&&(clz<=this.z); + } + +} \ No newline at end of file diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/XZPlane.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/XZPlane.java new file mode 100644 index 000000000..5c2db8692 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/XZPlane.java @@ -0,0 +1,213 @@ +/* + * $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.transform.Transform3D_OLD; + +/** This class represents a 3D plane which is colinear to the X and Z axis. + * + * @author $Author: cbohrhauer$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class XZPlane extends AbstractOrthoPlane { + + private static final long serialVersionUID = -2378546863694735717L; + + /** Is the coordinate of the plane. + */ + public float y; + + /** + * @param y is the coordinate of the plane + */ + public XZPlane(float y) { + this.y = y; + } + + /** + * @param p is a point on the plane. + */ + public XZPlane(Tuple3f p) { + this.y = p.getY(); + } + + /** Replies a clone of this plane. + * + * @return a clone. + */ + @Override + public XZPlane clone() { + return (XZPlane)super.clone(); + } + + /** {@inheritDoc} + */ + @Override + public void set(Plane plane) { + this.y = plane.getEquationComponentB(); + normalize(); + } + + /** {@inheritDoc} + */ + @Override + public void setIdentityTransform() { + this.y = 0; + } + + /** {@inheritDoc} + */ + @Override + public void setTransform(Transform3D_OLD trans) { + this.y = trans.m13; + } + + /** {@inheritDoc} + */ + @Override + public void transform(Transform3D_OLD trans) { + this.y += trans.m13; + } + + /** {@inheritDoc} + */ + @Override + public Transform3D_OLD getTransformMatrix() { + Transform3D_OLD m = new Transform3D_OLD(); + m.m13 = this.y; + return m; + } + + /** {@inheritDoc} + */ + @Override + public void setTranslation(float x, float y, float z) { + this.y = y; + } + + /** {@inheritDoc} + */ + @Override + public void translate(float dx, float dy, float dz) { + this.y += dy; + } + + /** {@inheritDoc} + */ + @Override + public void setTranslation(Point3f position) { + this.y = position.getY(); + } + + /** {@inheritDoc} + */ + @Override + public Point3f getTranslation() { + return new Point3f(0,this.y, 0); + } + + /** {@inheritDoc} + */ + @Override + public void translate(Vector3f v) { + this.y += v.getY(); + } + + /** {@inheritDoc} + */ + @Override + public Point3f getPivot() { + return new Point3f(0.,this.y, 0.); + } + + /** {@inheritDoc} + */ + @Override + public Vector3f getNormal() { + return new Vector3f(0,this.isPositive ? 1 : -1, 0); + } + + /** {@inheritDoc} + */ + @Override + public float getEquationComponentA() { + return 0; + } + + /** {@inheritDoc} + */ + @Override + public float getEquationComponentB() { + return this.isPositive ? 1 : -1; + } + + /** {@inheritDoc} + */ + @Override + public float getEquationComponentC() { + return 0; + } + + /** {@inheritDoc} + */ + @Override + public final float getEquationComponentD() { + return this.isPositive ? -this.y : this.y; + } + + /** {@inheritDoc} + */ + @Override + public float distanceTo(float px, float py, float pz) { + float d = py - this.y; + return this.isPositive ? d : -d; + } + + /** + * Classifies a box 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 lx, float ly, float lz, float ux, float uy, float uz) { + float cuy = uy; + float cly = ly; + if (uythis.y) t = PlanarClassificationType.IN_FRONT_OF; + else t = PlanarClassificationType.COINCIDENT; + return this.isPositive ? t : PlanarClassificationType.invert(t); + } + +} \ No newline at end of file diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/YZPlane.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/YZPlane.java new file mode 100644 index 000000000..3900c5548 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/YZPlane.java @@ -0,0 +1,213 @@ +/* + * $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.transform.Transform3D_OLD; + +/** This class represents a 3D plane which is colinear to the Y and Z axis. + * + * @author $Author: sgalland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class YZPlane extends AbstractOrthoPlane { + + private static final long serialVersionUID = -1609935828387479933L; + + /** Coordinate of the plane. + */ + public float x; + + /** + * @param x is the coordinate of the plane. + */ + public YZPlane(float x) { + this.x = x; + } + + /** + * @param p is a point on the plane. + */ + public YZPlane(Tuple3f p) { + this.x = p.getX(); + } + + /** Replies a clone of this plane. + * + * @return a clone. + */ + @Override + public YZPlane clone() { + return (YZPlane)super.clone(); + } + + /** {@inheritDoc} + */ + @Override + public void set(Plane plane) { + this.x = plane.getEquationComponentA(); + normalize(); + } + + /** {@inheritDoc} + */ + @Override + public void setIdentityTransform() { + this.x = 0; + } + + /** {@inheritDoc} + */ + @Override + public void setTransform(Transform3D_OLD trans) { + this.x = trans.m03; + } + + /** {@inheritDoc} + */ + @Override + public void transform(Transform3D_OLD trans) { + this.x += trans.m03; + } + + /** {@inheritDoc} + */ + @Override + public Transform3D_OLD getTransformMatrix() { + Transform3D_OLD m = new Transform3D_OLD(); + m.m03 = this.x; + return m; + } + + /** {@inheritDoc} + */ + @Override + public void setTranslation(float x, float y, float z) { + this.x = x; + } + + /** {@inheritDoc} + */ + @Override + public void translate(float dx, float dy, float dz) { + this.x += dx; + } + + /** {@inheritDoc} + */ + @Override + public void setTranslation(Point3f position) { + this.x = position.getX(); + } + + /** {@inheritDoc} + */ + @Override + public Point3f getTranslation() { + return new Point3f(this.x,0.,0.); + } + + /** {@inheritDoc} + */ + @Override + public void translate(Vector3f v) { + this.x += v.getX(); + } + + /** {@inheritDoc} + */ + @Override + public Point3f getPivot() { + return new Point3f(this.x, 0., 0.); + } + + /** {@inheritDoc} + */ + @Override + public Vector3f getNormal() { + return new Vector3f(this.isPositive ? 1 : -1, 0, 0); + } + + /** {@inheritDoc} + */ + @Override + public float getEquationComponentA() { + return this.isPositive ? 1 : -1; + } + + /** {@inheritDoc} + */ + @Override + public float getEquationComponentB() { + return 0; + } + + /** {@inheritDoc} + */ + @Override + public float getEquationComponentC() { + return 0; + } + + /** {@inheritDoc} + */ + @Override + public final float getEquationComponentD() { + return this.isPositive ? -this.x : this.x; + } + + /** {@inheritDoc} + */ + @Override + public float distanceTo(float px, float py, float pz) { + float d = px - this.x; + return this.isPositive ? d : -d; + } + + /** + * Classifies a box 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 lx, float ly, float lz, float ux, float uy, float uz) { + float cux = ux; + float clx = lx; + if (cuxthis.x) t = PlanarClassificationType.IN_FRONT_OF; + else t = PlanarClassificationType.COINCIDENT; + return this.isPositive ? t : PlanarClassificationType.invert(t); + } + +} \ No newline at end of file diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/ConvexHull.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/ConvexHull.java new file mode 100644 index 000000000..a86ae707a --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/ConvexHull.java @@ -0,0 +1,156 @@ +/* + * $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.convexhull; + +import java.util.Collection; + +import org.arakhne.afc.math.geometry3d.continuous.Point3f; + + +/** This class permits to create convex hull from a + * set of points. + * + * @author $Author: sgalland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class ConvexHull { + + /** + * Select the points that corresponds to the convex envelop + * of the set of points. + *

+ * The divide-and-conquer algorithm is an algorithm for + * computing the convex hull of a set of points in two + * or more dimensions. + *

    + * + *
  1. Divide the points into two equal sized sets L + * and R such that all points of L are + * to the left of the most leftmost points in R.
    + * Recursively find the convex hull of L (shown in + * light blue) and R (shown in purple).
    2. + * + *
  2. To merge in 3D need to construct a cylinder of triangles + * connecting the left hull and the right hull.
    3. + * + *
  3. One edge of the cylinder AB is just the lower + * common tangent. The upper common tangent can be found in + * linear time by scanning around the left hull in a clockwise + * direction and around the right hull in an anti-clockwise + * direction. The two tangents divide each hull into two pieces. + * The edges belonging to one of thse pieces must be deleted.
    4. + * + *
  4. We next need to find a triangle ABC belonging to the + * cylinder which has AB as one of its edges. The third vertex + * of the triangle (C) must belong to either the left hull or the + * right hull. (In this case it belongs to the right hull.) + * Consequently, either AC is an edge of the left hull or BC is + * an edge of the right hull. (In this case BC is an edge of the + * right hull.) Hence, when considering possible candidates for the + * third vertex it is only necessary to consider candidates that + * are adjacent to A in the left hull or adjacent to B in the + * right hull.
    + * If we use a data structure for the hulls that allow us to find + * an adjacent vertex in constant time then the search for vertex + * C takes time proportional to the number of candidate vertices + * checked.
    5. + * + *
  5. After finding triangle ABC we now have a new edge AC that + * joins the left hull and the right hull. We can find triangle + * ACD just as we did ABC. Continuing in this way, we can find all + * the triangles belonging to the cylinder, ending when we get back + * to AB.
    + * The total time to find the cylinder is O(n) so the algorithm + * takes O(n log n) time.
    6. + * + *
+ * + * @param pointList is the set of points from which the + * convex hull must be generated. + * @return the convex hull + */ + public static Point3f[] computeConvexHullWithDivideAndConquerAlgorithm(Point3f... pointList) { + ConvexHullAlgorithm algo = new DivideAndConquerAlgorithm(); + return algo.computeConvexHull(pointList); + } + + /** + * Select the points that corresponds to the convex envelop + * of the set of points. + *

+ * The divide-and-conquer algorithm is an algorithm for + * computing the convex hull of a set of points in two + * or more dimensions. + *

    + * + *
  1. Divide the points into two equal sized sets L + * and R such that all points of L are + * to the left of the most leftmost points in R.
    + * Recursively find the convex hull of L (shown in + * light blue) and R (shown in purple).
    2. + * + *
  2. To merge in 3D need to construct a cylinder of triangles + * connecting the left hull and the right hull.
    3. + * + *
  3. One edge of the cylinder AB is just the lower + * common tangent. The upper common tangent can be found in + * linear time by scanning around the left hull in a clockwise + * direction and around the right hull in an anti-clockwise + * direction. The two tangents divide each hull into two pieces. + * The edges belonging to one of thse pieces must be deleted.
    4. + * + *
  4. We next need to find a triangle ABC belonging to the + * cylinder which has AB as one of its edges. The third vertex + * of the triangle (C) must belong to either the left hull or the + * right hull. (In this case it belongs to the right hull.) + * Consequently, either AC is an edge of the left hull or BC is + * an edge of the right hull. (In this case BC is an edge of the + * right hull.) Hence, when considering possible candidates for the + * third vertex it is only necessary to consider candidates that + * are adjacent to A in the left hull or adjacent to B in the + * right hull.
    + * If we use a data structure for the hulls that allow us to find + * an adjacent vertex in constant time then the search for vertex + * C takes time proportional to the number of candidate vertices + * checked.
    5. + * + *
  5. After finding triangle ABC we now have a new edge AC that + * joins the left hull and the right hull. We can find triangle + * ACD just as we did ABC. Continuing in this way, we can find all + * the triangles belonging to the cylinder, ending when we get back + * to AB.
    + * The total time to find the cylinder is O(n) so the algorithm + * takes O(n log n) time.
    6. + * + *
+ * + * @param pointList is the set of points from which the + * convex hull must be generated. + * @return the convex hull + */ + public static Collection> computeConvexHullTrianglesWithDivideAndConquerAlgorithm(Point3f... pointList) { + ConvexHullAlgorithm algo = new DivideAndConquerAlgorithm(); + return algo.computeConvexHullTriangles(pointList); + } + +} \ No newline at end of file diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/ConvexHullAlgorithm.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/ConvexHullAlgorithm.java new file mode 100644 index 000000000..b9d17b61b --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/ConvexHullAlgorithm.java @@ -0,0 +1,56 @@ +/* + * $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.convexhull; + +import java.util.Collection; + +import org.arakhne.afc.math.geometry3d.continuous.Point3f; + +/** This interface describes a convex hull computation algorithm. + * + * @author $Author: sgalland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public interface ConvexHullAlgorithm { + + /** + * Select the points that corresponds to the convex envelop + * of the set of points. + * + * @param pointList is the set of points from which the + * convex hull must be generated. + * @return the convex hull + */ + public Point3f[] computeConvexHull(Point3f... pointList); + + /** + * Select the triangles that corresponds to the convex envelop + * of the set of points. + * + * @param pointList is the set of points from which the + * convex hull must be generated. + * @return the convex hull + */ + public Collection> computeConvexHullTriangles(Point3f... pointList); + +} \ No newline at end of file diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/DivideAndConquerAlgorithm.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/DivideAndConquerAlgorithm.java new file mode 100644 index 000000000..43f02f572 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/DivideAndConquerAlgorithm.java @@ -0,0 +1,768 @@ +/* + * $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.convexhull; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Collection; +import java.util.Collections; +import java.util.Comparator; +import java.util.ConcurrentModificationException; +import java.util.HashSet; +import java.util.Iterator; +import java.util.List; +import java.util.Map; +import java.util.Map.Entry; +import java.util.Set; +import java.util.TreeMap; + +import org.arakhne.afc.math.MathUtil; +import org.arakhne.afc.math.geometry3d.continuous.PlanarClassificationType; +import org.arakhne.afc.math.geometry3d.continuous.Plane4f; +import org.arakhne.afc.math.geometry3d.continuous.Point3f; +import org.arakhne.afc.math.geometry3d.continuous.Triangle3f; +import org.arakhne.afc.math.geometry3d.continuous.Tuple3f; + + +/** This class permits to create convex hull from a + * set of points with the divide and conquer algorithm. + * + * @author $Author: cbohrhauer$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class DivideAndConquerAlgorithm implements ConvexHullAlgorithm { + + private Point3f[] pointList = null; + private final Collection> convexHull = new ArrayList>(); + private Map indexes = new TreeMap(); + + /** + * Select the points that corresponds to the convex envelop + * of the set of points. + *

+ * The divide-and-conquer algorithm is an algorithm for + * computing the convex hull of a set of points in two + * or more dimensions. + *

    + * + *
  1. Divide the points into two equal sized sets L + * and R such that all points of L are + * to the left of the most leftmost points in R.
    + * Recursively find the convex hull of L (shown in + * light blue) and R (shown in purple).
    2. + * + *
  2. To merge in 3D need to construct a cylinder of triangles + * connecting the left hull and the right hull.
    3. + * + *
  3. One edge of the cylinder AB is just the lower + * common tangent. The upper common tangent can be found in + * linear time by scanning around the left hull in a clockwise + * direction and around the right hull in an anti-clockwise + * direction. The two tangents divide each hull into two pieces. + * The edges belonging to one of thse pieces must be deleted.
    4. + * + *
  4. We next need to find a triangle ABC belonging to the + * cylinder which has AB as one of its edges. The third vertex + * of the triangle (C) must belong to either the left hull or the + * right hull. (In this case it belongs to the right hull.) + * Consequently, either AC is an edge of the left hull or BC is + * an edge of the right hull. (In this case BC is an edge of the + * right hull.) Hence, when considering possible candidates for the + * third vertex it is only necessary to consider candidates that + * are adjacent to A in the left hull or adjacent to B in the + * right hull.
    + * If we use a data structure for the hulls that allow us to find + * an adjacent vertex in constant time then the search for vertex + * C takes time proportional to the number of candidate vertices + * checked.
    5. + * + *
  5. After finding triangle ABC we now have a new edge AC that + * joins the left hull and the right hull. We can find triangle + * ACD just as we did ABC. Continuing in this way, we can find all + * the triangles belonging to the cylinder, ending when we get back + * to AB.
    + * The total time to find the cylinder is O(n) so the algorithm + * takes O(n log n) time.
    6. + * + *
+ * + * @param listOfPoint is the set of points from which the + * convex hull must be generated. + * @return the convex hull + * @throws ConcurrentModificationException + */ + @Override + public Point3f[] computeConvexHull(Point3f... listOfPoint) { + + // Compute the convex hull + Collection> hull = computeConvexHullTriangles(listOfPoint); + + // Rebuild the list of points + Set lConvexHull = new HashSet(); + for (HullObject hullObject : hull) { + Tuple3f[] pts = hullObject.getObjectPoints(listOfPoint); + for (Tuple3f tuple3d : pts) { + lConvexHull.add(tuple3d); + } + } + hull.clear(); + + Point3f[] tab = new Point3f[lConvexHull.size()]; + lConvexHull.toArray(tab); + lConvexHull.clear(); + + return tab; + } + + /** + * Select the points that corresponds to the convex envelop + * of the set of points. + *

+ * The divide-and-conquer algorithm is an algorithm for + * computing the convex hull of a set of points in two + * or more dimensions. + *

    + * + *
  1. Divide the points into two equal sized sets L + * and R such that all points of L are + * to the left of the most leftmost points in R.
    + * Recursively find the convex hull of L (shown in + * light blue) and R (shown in purple).
    2. + * + *
  2. To merge in 3D need to construct a cylinder of triangles + * connecting the left hull and the right hull.
    3. + * + *
  3. One edge of the cylinder AB is just the lower + * common tangent. The upper common tangent can be found in + * linear time by scanning around the left hull in a clockwise + * direction and around the right hull in an anti-clockwise + * direction. The two tangents divide each hull into two pieces. + * The edges belonging to one of thse pieces must be deleted.
    4. + * + *
  4. We next need to find a triangle ABC belonging to the + * cylinder which has AB as one of its edges. The third vertex + * of the triangle (C) must belong to either the left hull or the + * right hull. (In this case it belongs to the right hull.) + * Consequently, either AC is an edge of the left hull or BC is + * an edge of the right hull. (In this case BC is an edge of the + * right hull.) Hence, when considering possible candidates for the + * third vertex it is only necessary to consider candidates that + * are adjacent to A in the left hull or adjacent to B in the + * right hull.
    + * If we use a data structure for the hulls that allow us to find + * an adjacent vertex in constant time then the search for vertex + * C takes time proportional to the number of candidate vertices + * checked.
    5. + * + *
  5. After finding triangle ABC we now have a new edge AC that + * joins the left hull and the right hull. We can find triangle + * ACD just as we did ABC. Continuing in this way, we can find all + * the triangles belonging to the cylinder, ending when we get back + * to AB.
    + * The total time to find the cylinder is O(n) so the algorithm + * takes O(n log n) time.
    6. + * + *
+ * + * @param listOfPoints is the set of points from which the + * convex hull must be generated. + * @return the convex hull + */ + @Override + public Collection> computeConvexHullTriangles(Point3f... listOfPoints) { + + if (this.pointList!=null) + throw new ConcurrentModificationException(); + + this.pointList = listOfPoints; + this.convexHull.clear(); + + // Trivial case + { + if (createFacesForBasicCases(0, 0, listOfPoints.length-1)) + return this.convexHull; + } + + // Sort the points + Arrays.sort(listOfPoints, new Comparator() { + @Override + public int compare(Point3f o1, Point3f o2) { + int c = Float.compare(o1.getX(),o2.getX()); + if (c!=0) return c; + c = Float.compare(o1.getY(),o2.getY()); + if (c!=0) return c; + return Float.compare(o1.getZ(),o2.getZ()); + } + }); + + // Compute the convex hull + divideAndConquer(1, 0, listOfPoints.length-1); + + return this.convexHull; + } + + /** Divide and Conquer Algorithm. + */ + private Map divideAndConquer(int creationLevel, final int startIndex, final int endIndex) { + // Trivial case: convex hull is a canonical 3D entity + if (createFacesForBasicCases(creationLevel, startIndex, endIndex)) { + if (creationLevel>1) return consumeLastInsertedIndexes(); + return null; + } + + // + // STEP 1: Compute two convex hulls that respectively contains an half + // of the points + // + + // Recurse on the two groups to find the convex hulls + List leftIndexes, rightIndexes; + + { + final int midIndex = (startIndex+endIndex) / 2; + Map leftMap = divideAndConquer(creationLevel+1, startIndex, midIndex); + Map rightMap = divideAndConquer(creationLevel+1, midIndex+1, endIndex); + + if ((leftMap==null)||(leftMap.isEmpty())) { + if (creationLevel>1) return rightMap; + return null; + } + if ((rightMap==null)||(rightMap.isEmpty())) { + if (creationLevel>1) return leftMap; + return null; + } + + // Fill the index reference count + leftIndexes = new ArrayList(); + rightIndexes = new ArrayList(); + + for (Entry p : leftMap.entrySet()) { + incrementIndexReference(p.getKey(),p.getValue().intValue()); + leftIndexes.add(p.getKey()); + } + Collections.sort(leftIndexes); + + for (Entry p : rightMap.entrySet()) { + incrementIndexReference(p.getKey(),p.getValue().intValue()); + rightIndexes.add(p.getKey()); + } + Collections.sort(rightIndexes); + + } + + // + // STEP 2: Find the edge which is tangent to the two + // convex hulls. + // + + // Indicate if the solution could only be an edge + final boolean solutionIsEdge = (leftIndexes.size()==1)&&(rightIndexes.size()==1); + + int leftCandidate; + int rightCandidate; + + if (!solutionIsEdge) { + int[] t = computeCandidates(leftIndexes, rightIndexes); + leftCandidate = t[0]; + rightCandidate = t[1]; + } + else { + leftCandidate = leftIndexes.get(leftIndexes.size()-1); + rightCandidate = rightIndexes.get(0); + } + + // Save the tangent as an edge, because no face could be computed + if (solutionIsEdge) { + addIntoResult(new HullEdge3D(this.pointList,leftCandidate,rightCandidate,creationLevel)); + } + else { + createHullCylinder( + leftIndexes, rightIndexes, leftCandidate, rightCandidate, creationLevel); + } + + System.gc(); + + return consumeLastInsertedIndexes(); + } + + /** Compute a plane that permits to detect candidate points. + */ + private void computeCandidatePlanes(Plane4f tangentPlane, Point3f leftCandidatePoint, Point3f rightCandidatePoint, boolean yOrder) { + float dx, dy, dz; + + if (yOrder) { + dy = 0; + dz = 1; + } + else { + dy = 1; + dz = 0; + } + + Plane4f tangentCandidate = new Plane4f( + leftCandidatePoint.getX(),leftCandidatePoint.getY(),leftCandidatePoint.getZ(), + rightCandidatePoint.getX(),rightCandidatePoint.getY(),rightCandidatePoint.getZ(), + leftCandidatePoint.getX(),leftCandidatePoint.getY()+dy,leftCandidatePoint.getZ()+dz); + boolean valid = tangentCandidate.isValid(); + + if (!valid) { + if (yOrder) { + dx = 1; + dz = 0; + } + else { + dx = 0; + dz = 1; + } + tangentCandidate = new Plane4f( + leftCandidatePoint.getX(),leftCandidatePoint.getY(),leftCandidatePoint.getZ(), + rightCandidatePoint.getX(),rightCandidatePoint.getY(),rightCandidatePoint.getZ(), + leftCandidatePoint.getX()+dx,leftCandidatePoint.getY(),leftCandidatePoint.getZ()+dz); + valid = tangentCandidate.isValid(); + } + + if (valid) { + if (yOrder) { + dx = 0; + dy = 1; + dz = 0; + } + else { + dx = 0; + dy = 0; + dz = 1; + } + + float dot = MathUtil.dotProduct(dx, dy, dz, tangentCandidate.getEquationComponentA(), tangentCandidate.getEquationComponentB(), tangentCandidate.getEquationComponentC()); + + if (dot==0) { + // Because the two vectors are perpendicular, + // compute the determinant of the vectors to + // determine on which side the vector lies. + // + // The determinant is equals to 1 or -1 because + // the vector are normalized + dot = MathUtil.determinant(tangentCandidate.getEquationComponentA(), tangentCandidate.getEquationComponentB(), tangentCandidate.getEquationComponentC(), dx, dy, dz); + } + + if (dot<0) { + tangentCandidate.negate(); + } + tangentPlane.set(tangentCandidate); + } + } + + /** Compute the left and right candidates. + * + * @param pointList is the list of the points + * @param leftIndexes is the list of point's indexes for the left part + * @param rightIndexes is the list of point's indexes for the right part + * @return an array contains the left candidate and the right candidate + */ + private int[] computeCandidates(List leftIndexes, List rightIndexes) { + int leftCandidate = leftIndexes.get(leftIndexes.size()-1); + int rightCandidate = rightIndexes.get(0); + + Point3f leftCandidatePoint = this.pointList[leftCandidate]; + Point3f rightCandidatePoint = this.pointList[rightCandidate]; + + boolean allCoincident; + boolean yOrder = true; + boolean changed; // has the tangent changed? + Plane4f tangentCandidate = new Plane4f(); + computeCandidatePlanes(tangentCandidate, leftCandidatePoint, rightCandidatePoint, yOrder); + + do { + allCoincident = true; + + // Search for a point which is in the other side + // of the tangent plane + int lastCandidate = leftCandidate; + for(int idxIdx=leftIndexes.size()-1; idxIdx>=0; --idxIdx) { + int candidate = leftIndexes.get(idxIdx); + if (candidate!=leftCandidate) { + PlanarClassificationType classification = tangentCandidate.classifies(this.pointList[candidate]); + switch(classification) { + case BEHIND: + leftCandidate = candidate; + leftCandidatePoint = this.pointList[leftCandidate]; + allCoincident = false; + computeCandidatePlanes(tangentCandidate, leftCandidatePoint, rightCandidatePoint, yOrder); + break; + case COINCIDENT: + break; + default: + // Ignore point + allCoincident = false; + } + } + } + changed = (lastCandidate!=leftCandidate); + + // Search for a point which is in the other side + // of the tangent plane + lastCandidate = rightCandidate; + for(int idxIdx=0; idxIdx leftIndexes, List rightIndexes, int leftCandidate, int rightCandidate, int creationLevel) { + final int minIndex = leftIndexes.get(0); + final int maxIndex = rightIndexes.get(rightIndexes.size()-1); + // + // STEP 4: find faces that connect + // the two small hulls + // + int l = leftCandidate; + int r = rightCandidate; + + do { + int cand = (l==leftIndexes.get(0)) ? 1 : 0; + int idx = (cand(this.pointList,l,idx,r,creationLevel)); + addIntoResult(new HullTriangle3D(this.pointList,l,r,idx,creationLevel)); + + if (candtrue if a default case was handled, otherwhise false. + */ + private boolean createFacesForBasicCases(final int creationLevel, int startIndex, int endIndex) { + int pointCount = endIndex - startIndex + 1; + if (pointCount<=4) { + final int i0 = startIndex; + final int i1 = startIndex+1; + final int i2 = startIndex+2; + final int i3 = startIndex+3; + switch(pointCount) { + case 4: + addIntoResult(new HullTriangle3D(this.pointList,i1,i0,i2,creationLevel)); + addIntoResult(new HullTriangle3D(this.pointList,i1,i2,i3,creationLevel)); + addIntoResult(new HullTriangle3D(this.pointList,i0,i3,i2,creationLevel)); + addIntoResult(new HullTriangle3D(this.pointList,i0,i1,i3,creationLevel)); + return true; + case 3: + addIntoResult(new HullTriangle3D(this.pointList,i0,i1,i2,creationLevel)); + addIntoResult(new HullTriangle3D(this.pointList,i0,i2,i1,creationLevel)); + return true; + case 2: + addIntoResult(new HullEdge3D(this.pointList,i0,i1,creationLevel)); + return true; + case 1: + addIntoResult(new HullVertex3D(this.pointList,i0,creationLevel)); + return true; + } + } + return false; + } + + /** Mark as deleted faces that can see a point from the other hull. + * This function also remove all the 3D edges. + */ + private void deleteFaces(final int creationLevel, final int startIndex, final int endIndex) { + //int deletionLevel = creationLevel+1; + boolean remove; + Iterator> iterator = this.convexHull.iterator(); + while (iterator.hasNext()) { + remove = false; + HullObject obj = iterator.next(); + int insideCount = obj.indexesInRange(startIndex,endIndex); + if (obj instanceof HullTriangle3D) { + // Test if the object has one point outside the index range + // It means that the object was generated for another + // hull part + if (insideCount<=1) { + HullTriangle3D classifier = (HullTriangle3D)obj; + for (int j=startIndex; j<=endIndex; ++j) { + PlanarClassificationType classification = classifier.classifies(this.pointList[j]); + if (classification==PlanarClassificationType.IN_FRONT_OF) { + remove = true; + break; // Exit from the for internal loop + } + } + } + } + else if (insideCount==0) { + /*if (obj.getCreationLevel()>deletionLevel)*/ + // Remove the edges that are too old + remove = true; + } + + // Do the removal action + if (remove) { + iterator.remove(); + int[] cindexes = obj.getObjectIndexes(); + for (int i : cindexes) { + decrementIndexReference(i); + } + } + } + } + + /** Add an artefact into the result list. + */ + private void addIntoResult(HullObject obj) { + this.convexHull.add(obj); + for(int p : obj.getObjectIndexes()) { + incrementIndexReference(p, 1); + } + } + + /** Increment the reference on an point's index. + */ + private void incrementIndexReference(int pointIndex, int inc) { + DInteger nb = this.indexes.get(pointIndex); + if (nb==null) { + nb = new DInteger(); + this.indexes.put(pointIndex, nb); + } + nb.value += inc; + } + + /** Decrement the reference on an point's index. + */ + private void decrementIndexReference(int pointIndex) { + DInteger nb = this.indexes.get(pointIndex); + if (nb==null) return; + nb.value --; + if (nb.value<=0) + this.indexes.remove(pointIndex); + } + + /** Extract the last inserted point indexes. + */ + private Map consumeLastInsertedIndexes() { + Map old = this.indexes; + this.indexes = new TreeMap(); + return old; + } + + /** Replies if the specified triangle overlaps another existing triangle. + *

+ * A triangle overlaps another one when they are coplanar and and intersection + * existing between their edges. + */ + private boolean triangleOverlaps(int minIndex, int maxIndex, int p1, int p2, int p3) { + final HullTriangle3D triangle = new HullTriangle3D(this.pointList,p1,p2,p3, -1); + + for (HullObject object : this.convexHull) { + if (object instanceof HullTriangle3D) { + HullTriangle3D t = (HullTriangle3D)object; + if (t.hasSamePoints(triangle)) return true; + + if ((t.a>=minIndex)&&(t.a<=maxIndex)&& + (t.b>=minIndex)&&(t.b<=maxIndex)&& + (t.c>=minIndex)&&(t.c<=maxIndex)) { + if ((t.isCoplanar(triangle))&& + (Triangle3f.overlapsCoplanarTriangle( + this.pointList[triangle.a], + this.pointList[triangle.b], + this.pointList[triangle.c], + this.pointList[t.a], + this.pointList[t.b], + this.pointList[t.c]))) + return true; + } + } + } + + return false; + } + + /** + * @author $Author: sgalland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ + private class DInteger extends Number implements Comparable { + + private static final long serialVersionUID = 5407180795619273240L; + + /** value if this integer. + */ + public int value; + + /** + */ + public DInteger() { + this.value = 0; + } + + /** {@inheritDoc} + */ + @Override + public int compareTo(Integer o) { + return o.compareTo(this.value); + } + + /** {@inheritDoc} + */ + @Override + public float floatValue() { + return this.value; + } + + /** {@inheritDoc} + */ + @Override + public int intValue() { + return this.value; + } + + /** {@inheritDoc} + */ + @Override + public long longValue() { + return this.value; + } + + /** {@inheritDoc} + */ + @Override + public String toString() { + return Integer.toString(this.value); + } + + @Override + public double doubleValue() { + return this.value; + } + } + +} \ No newline at end of file diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/HullEdge3D.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/HullEdge3D.java new file mode 100644 index 000000000..c61cd0c45 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/HullEdge3D.java @@ -0,0 +1,112 @@ +/* + * $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.convexhull; + +import org.arakhne.afc.math.geometry3d.continuous.Point3f; + +/** This class represents a 3D edge used inside + * Convex Hull Computation Algorithms. + * + * @see ConvexHull + * @param is the type of the points. + * @author $Author: sgalland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class HullEdge3D implements HullObject { + + /** Index of the first point of this segment. + */ + public final int a; + + /** Index of the last point of this segment. + */ + public final int b; + + private final int creationLevel; + + /** + * @param points is the list of points + * @param a is the index of the first point + * @param b is the index of the last point. + * @param creationLevel is the level inside the creation algorithm. + */ + public HullEdge3D(T[] points, int a, int b, int creationLevel) { + this.a = a; + this.b = b; + this.creationLevel = creationLevel; + } + + /** {@inheritDoc} + */ + @Override + public int getCreationLevel() { + return this.creationLevel; + } + + /** {@inheritDoc} + * + * @return {@inheritDoc} + */ + @Override + public String toString() { + return "[Edge3D, "+this.a+","+this.b+"]\n"; //$NON-NLS-1$//$NON-NLS-2$ //$NON-NLS-3$ + } + + /** {@inheritDoc} + */ + @Override + public Point3f[] getObjectPoints(T[] points) { + return new Point3f[] { + points[this.a], + points[this.b], + }; + } + + /** {@inheritDoc} + */ + @Override + public int[] getObjectIndexes() { + return new int[] { + this.a, + this.b, + }; + } + + /** {@inheritDoc} + */ + @Override + public int indexesInRange(int min, int max) { + int ci = min; + int ca = max; + if (ci>ca) { + int t = ci; + ci = ca; + ca = t; + } + int c = 0; + if ((this.a>=ci)&&(this.a<=ca)) ++c; + if ((this.b>=ci)&&(this.b<=ca)) ++c; + return c; + } + +} \ No newline at end of file diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/HullObject.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/HullObject.java new file mode 100644 index 000000000..e250b0978 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/HullObject.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.geometry3d.continuous.convexhull; + +import org.arakhne.afc.math.geometry3d.continuous.Point3f; + +/** This class represents a 3D object used inside + * Convex Hull Computation Algorithms. + * + * @see ConvexHull + * @param is the type of the points. + * @author $Author: sgalland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public interface HullObject { + + /** Replies the points that composed the Hull Object. + * + * @param points is the list of real coordinates of the points. + * @return the list of the points of this object. + */ + public Point3f[] getObjectPoints(T[] points); + + /** Replies the point indexes that composed the Hull Object. + * + * @return the point indexes that composed the Hull Object. + */ + public int[] getObjectIndexes(); + + /** Replies the creation level of thos object. + * + * @return the creation level of thos object. + */ + public int getCreationLevel(); + + /** Replies if all the indexes of this object are + * inside the specified range. + * + * @param min is the minimal value to test. + * @param max is the maximal value to test. + * @return the count of indexes inside the specified range. + */ + public int indexesInRange(int min, int max); + +} \ No newline at end of file diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/HullTriangle3D.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/HullTriangle3D.java new file mode 100644 index 000000000..95241d7f3 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/HullTriangle3D.java @@ -0,0 +1,624 @@ +/* + * $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.convexhull; + +import org.arakhne.afc.math.MathUtil; +import org.arakhne.afc.math.geometry3d.continuous.PlanarClassificationType; +import org.arakhne.afc.math.geometry3d.continuous.Plane; +import org.arakhne.afc.math.geometry3d.continuous.Plane4f; +import org.arakhne.afc.math.geometry3d.continuous.PlaneClassifier; +import org.arakhne.afc.math.geometry3d.continuous.Point3f; +import org.arakhne.afc.math.geometry3d.continuous.Tuple3f; +import org.arakhne.afc.math.geometry3d.continuous.Vector3f; + +/** This class represents a 3D triangle used inside + * Convex Hull Computation Algorithms. + * + * @see ConvexHull + * @param is the type of the points. + * @author $Author: sgalland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class HullTriangle3D implements HullObject, PlaneClassifier { + + /** Index of the first point of the triangle. + */ + public final int a; + + /** Index of the second point of the triangle. + */ + public final int b; + + /** Index of the third point of the triangle. + */ + public final int c; + + /** normal to the triangle facet. + */ + public final float nx; + /** normal to the triangle facet. + */ + public final float ny; + /** normal to the triangle facet. + */ + public final float nz; + /** normal to the triangle facet. + */ + public final float nw; + + /** Level inside the creation algorithm. + */ + private final int creationLevel; + + /** + * @param points is the list of points + * @param a is the index of the first point in the list of points. + * @param b is the index of the second point in the list of points. + * @param c is the index of the third point in the list of points. + * @param creationLevel is the level in the creation algorithm. + */ + public HullTriangle3D(T[] points, int a, int b, int c, int creationLevel) { + this.a = a; + this.b = b; + this.c = c; + this.creationLevel = creationLevel; + + T pa = points[this.a]; + T pb = points[this.b]; + T pc = points[this.c]; + + float lnx = pa.getY() * (pb.getZ() - pc.getZ()) + pb.getY() * (pc.getZ() - pa.getZ()) + pc.getY() * (pa.getZ() - pb.getZ()); + float lny = pa.getZ() * (pb.getX() - pc.getX()) + pb.getZ() * (pc.getX() - pa.getX()) + pc.getZ() * (pa.getX() - pb.getX()); + float lnz = pa.getX() * (pb.getY() - pc.getY()) + pb.getX() * (pc.getY() - pa.getY()) + pc.getX() * (pa.getY() - pb.getY()); + float d = - (lnx * pa.getX() + lny * pa.getY() + lnz * pa.getZ()); + float t = (float) Math.sqrt(lnx*lnx+lny*lny+lnz*lnz); + this.nx = lnx / t; + this.ny = lny / t; + this.nz = lnz / t; + this.nw = d / t; + } + + /** {@inheritDoc} + */ + @Override + public int getCreationLevel() { + return this.creationLevel; + } + + /** {@inheritDoc} + * + * @return {@inheritDoc} + */ + @Override + public String toString() { + return "[Triangle3D, "+this.a+","+this.b+","+this.c+"]\n"; //$NON-NLS-1$//$NON-NLS-2$ //$NON-NLS-3$ //$NON-NLS-4$ + } + + /** Replies the normal to the triangle. + * + * @return the normal to the facet. + * @see #nx + * @see #ny + * @see #nz + */ + public Vector3f getNormal() { + return new Vector3f(this.nx,this.ny,this.nz); + } + + /** + * 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 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. + * + * @param x is the coordinate of the point + * @param y is the coordinate of the point + * @param z is the coordinate of the point + * @return the distance from the given point to the triangle. + */ + public float distanceTo(float x, float y, float z) { + return this.nx * x + this.ny * y + this.nz * z + this.nw; + } + + /** + * 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. + * + * @param vec is the point to classify + * @return the classification of the point against the plane. + */ + @Override + public PlanarClassificationType classifies(Tuple3f vec) { + PlanarClassificationType lc; + int cmp = MathUtil.epsilonDistanceSign(distanceTo(vec)); + if (cmp < 0) + lc = PlanarClassificationType.BEHIND; + else if (cmp > 0) + lc = PlanarClassificationType.IN_FRONT_OF; + else + lc = PlanarClassificationType.COINCIDENT; + return lc; + } + + /** {@inheritDoc} + */ + @Override + public boolean intersects(Tuple3f vec) { + return MathUtil.epsilonDistanceSign(distanceTo(vec))==0; + } + + /** + * 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. + * + * @param x is the coordinate of the point to classify. + * @param y is the coordinate of the point to classify. + * @param z is the coordinate of the point to classify. + * @return the classification of the point against the plane. + */ + @Override + public PlanarClassificationType classifies(float x, float y, float z) { + PlanarClassificationType lc; + int cmp = MathUtil.epsilonDistanceSign(distanceTo(x,y,z)); + if (cmp < 0) + lc = PlanarClassificationType.BEHIND; + else if (cmp > 0) + lc = PlanarClassificationType.IN_FRONT_OF; + else + lc = PlanarClassificationType.COINCIDENT; + return lc; + } + + /** + * 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. + * + * @param x is the coordinate of the point to classify. + * @param y is the coordinate of the point to classify. + * @param z is the coordinate of the point to classify. + * @return the classification of the point against the plane. + */ + @Override + public boolean intersects(float x, float y, float z) { + return MathUtil.epsilonDistanceSign(distanceTo(x,y,z))==0; + } + + /** + * Classifies a box 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. + * + * @param lx is the coordinate of the lower point of the box to classify. + * @param ly is the coordinate of the lower point of the box to classify. + * @param lz is the coordinate of the lower point of the box to classify. + * @param ux is the coordinate of the upper point of the box to classify. + * @param uy is the coordinate of the upper point of the box to classify. + * @param uz is the coordinate of the upper point of the box to classify. + * @return the classification of the box against the plane. + */ + @Override + public PlanarClassificationType classifies(float lx, float ly, float lz, float ux, float uy, float uz) { + float[] d = new float[] { + distanceTo(lx,ly,lz), + distanceTo(ux,ly,lz), + distanceTo(ux,uy,lz), + distanceTo(lx,uy,lz), + distanceTo(lx,ly,uz), + distanceTo(ux,ly,uz), + distanceTo(ux,uy,uz), + distanceTo(lx,uy,uz) + }; + + float sign = 0; + for(int i=0,j=1; i<7; ++i,++j) { + sign = Math.signum(d[i]); + if ((sign==0)||(sign!=Math.signum(d[j]))) { + // Signs are different, it means that + // the box intersects the plan + return PlanarClassificationType.COINCIDENT; + } + } + + return (sign<0) ? PlanarClassificationType.BEHIND : PlanarClassificationType.IN_FRONT_OF; + } + + /** {@inheritDoc} + */ + @Override + public boolean intersects(float lx, float ly, float lz, float ux, float uy, float uz) { + float[] d = new float[] { + distanceTo(lx,ly,lz), + distanceTo(ux,ly,lz), + distanceTo(ux,uy,lz), + distanceTo(lx,uy,lz), + distanceTo(lx,ly,uz), + distanceTo(ux,ly,uz), + distanceTo(ux,uy,uz), + distanceTo(lx,uy,uz) + }; + + float sign = 0; + for(int i=0,j=1; i<7; ++i,++j) { + sign = Math.signum(d[i]); + if ((sign==0)||(sign!=Math.signum(d[j]))) { + // Signs are different, it means that + // the box intersects the plan + return true; + } + } + + return false; + } + + /** + * 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. + * + * @param x is the coordinate of the center of the sphere to classify. + * @param y is the coordinate of the center of the sphere to classify. + * @param z is the coordinate of the center of the sphere to classify. + * @param radius is the radius of the sphere to classify. + * @return the classification of the sphere against the plane. + */ + @Override + public PlanarClassificationType classifies(float x, float y, float z, float radius) { + float d = Math.abs(distanceTo(x,y,z)); + int cmp = MathUtil.epsilonDistanceSign(d-radius); + if (cmp==0) return PlanarClassificationType.COINCIDENT; + if (d>radius) return PlanarClassificationType.IN_FRONT_OF; + return PlanarClassificationType.BEHIND; + } + + /** {@inheritDoc} + */ + @Override + public boolean intersects(float x, float y, float z, float radius) { + float d = Math.abs(distanceTo(x,y,z)); + return MathUtil.epsilonDistanceSign(d-radius)==0; + } + + /** {@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 + if ((distance==Float.NaN)|| + (distance==0)) + return PlanarClassificationType.COINCIDENT; + + if (distance>0) return PlanarClassificationType.IN_FRONT_OF; + return PlanarClassificationType.BEHIND; + } + + /** {@inheritDoc} + */ + @Override + public boolean intersects(Plane otherPlane) { + float distance = distanceTo(otherPlane); + // The distance could not be computed + // the planes intersect. + // Planes intersect also when the distance + // is null + return ((distance==Float.NaN)||(distance==0)); + } + + /** + * 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 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. + * + * @param v is the point + * @return the distance from the given point to this plane. + */ + public final float distanceTo(Tuple3f v) { + return distanceTo(v.getX(), v.getY(), v.getZ()); + } + + /** + * Compute the distance between two colinear planes. + * + * @param p is a plane + * @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 Float#NaN} if the planes are not colinear. + */ + public float distanceTo(Plane p) { + // Compute the normales + Vector3f oNormal = p.getNormal(); + oNormal.normalize(); + Vector3f mNormal = new Vector3f(this.nx,this.ny,this.nz); + mNormal.normalize(); + + float dotProduct = oNormal.dot(mNormal); + + if (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 Point3f[] getObjectPoints(T[] points) { + return new Point3f[] { + points[this.a], + points[this.b], + points[this.c] + }; + } + + /** {@inheritDoc} + */ + @Override + public int[] getObjectIndexes() { + return new int[] { + this.a, + this.b, + this.c, + }; + } + + /** Replies if this triangle is equals to the specified object. + */ + @Override + public boolean equals(Object o) { + if (o instanceof HullTriangle3D) { + int la = this.a; + int lb = this.b; + int lc = this.c; + int d = ((HullTriangle3D)o).a; + int e = ((HullTriangle3D)o).b; + int f = ((HullTriangle3D)o).c; + + if ((lbtrue if this triangle and the given one are equal, + * otherwise false + */ + public boolean hasSamePoints(HullTriangle3D triangle) { + int la = this.a; + int lb = this.b; + int lc = this.c; + int d = triangle.a; + int e = triangle.b; + int f = triangle.c; + + if (la>lb) { + int t = la; + la = lb; + lb = t; + } + if (la>lc) { + int t = la; + la = lc; + lc = t; + } + if (lb>lc) { + int t = lb; + lb = lc; + lc = t; + } + + if (d>e) { + int t = d; + d = e; + e = t; + } + if (d>f) { + int t = d; + d = f; + f = t; + } + if (e>f) { + int t = e; + e = f; + f = t; + } + + return ((la==d)&&(lb==e)&&(lc==f)); + } + + /** Replies if this triangle is co-planar to the specified triangle. + * + * @param o is another triangle + * @return true if this triangle and the given one are coplanar, + * otherwise false + */ + public boolean isCoplanar(HullTriangle3D o) { + Vector3f n1 = getNormal(); + Vector3f n2 = o.getNormal(); + return ((MathUtil.epsilonColinear(n1, n2))&& + (MathUtil.epsilonEqualsDistance(this.nw, o.nw))); + } + + /** Replies the plane of the triangle. + * + * @return the plane of this triangle. + */ + public Plane getPlane() { + return new Plane4f(this.nx,this.ny,this.nz,this.nw); + } + + /** {@inheritDoc} + */ + @Override + public int indexesInRange(int min, int max) { + int mi = min; + int ma = max; + if (mi>ma) { + int t = mi; + mi = ma; + ma = t; + } + int lc = 0; + if ((this.a>=mi)&&(this.a<=ma)) ++lc; + if ((this.b>=mi)&&(this.b<=ma)) ++lc; + if ((this.c>=mi)&&(this.c<=ma)) ++lc; + return lc; + } + +} \ No newline at end of file diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/HullVertex3D.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/HullVertex3D.java new file mode 100644 index 000000000..369a9ae06 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/convexhull/HullVertex3D.java @@ -0,0 +1,102 @@ +/* + * $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.convexhull; + +import org.arakhne.afc.math.geometry3d.continuous.Point3f; + +/** This class represents a 3D vertex used inside + * Convex Hull Computation Algorithms. + * + * @see ConvexHull + * @param is the type of the points. + * @author $Author: sgalland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class HullVertex3D implements HullObject { + + /** Index of the point. + */ + public final int index; + + private final int creationLevel; + + /** + * @param points is the list of points. + * @param index is the index of this vertex. + * @param creationLevel is the level in the creation level. + */ + public HullVertex3D(T[] points, int index, int creationLevel) { + this.index = index; + this.creationLevel = creationLevel; + } + + /** {@inheritDoc} + */ + @Override + public int getCreationLevel() { + return this.creationLevel; + } + + /** + * {@inheritDoc} + * + * @return {@inheritDoc} + */ + @Override + public String toString() { + return "[Vertex3D, "+this.index+"]\n"; //$NON-NLS-1$//$NON-NLS-2$ + } + + /** {@inheritDoc} + */ + @Override + public Point3f[] getObjectPoints(T[] points) { + return new Point3f[] { + points[this.index], + }; + } + + /** {@inheritDoc} + */ + @Override + public int[] getObjectIndexes() { + return new int[] { + this.index, + }; + } + + /** {@inheritDoc} + */ + @Override + public int indexesInRange(int min, int max) { + int i = min; + int a = max; + if (i>a) { + int t = i; + i = a; + a = t; + } + return ((this.index>=i)&&(this.index<=a)) ? 1 : 0; + } + +} \ No newline at end of file diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/AbstractSpline.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/AbstractSpline.java new file mode 100644 index 000000000..378679fa1 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/AbstractSpline.java @@ -0,0 +1,49 @@ +/* + * $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.spline; + +/** This an abstract implementation of a spline. + * + * @author $Author: nvieval$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public abstract class AbstractSpline implements Spline { + + private float precisionFactor = 0.1f; + + /** + * {@inheritDoc} + */ + @Override + public void setPrecision(float p) { + this.precisionFactor = p; + } + + /** + * {@inheritDoc} + */ + @Override + public float getPrecision() { + return this.precisionFactor; + } +} diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/BSpline.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/BSpline.java new file mode 100644 index 000000000..1144773d2 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/BSpline.java @@ -0,0 +1,212 @@ +/* + * $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.spline; + +import java.util.ArrayList; +import java.util.List; + +import org.arakhne.afc.math.Matrix4f; +import org.arakhne.afc.math.geometry3d.continuous.Point3f; + +/** + * this class compute the B spline clalculation. + * + * @author $Author: nvieval$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class BSpline extends AbstractSpline{ + + /** the B spline matrix. */ + private final Matrix4f Bmatrix = new Matrix4f(); + + /** + * constructor. + */ + public BSpline() { + this.setBMatrix(); + } + + /** + * compute the spline with differents configuration. + * 2 points, 3 points, 4 points, ....., N points + * @param points + * @return {@inheritDoc} + */ + @Override + public List compute(List points) { + ArrayList globalPoints = new ArrayList(); + ArrayList intermediatePoints = new ArrayList(); + for (int i = 0; i < (points.size() - 1); i++) { + if (i == 0) { + if (points.size() > 2) { // fist part + // use the first point two time for catch spline on this point + intermediatePoints = this.splineEval(points.get(i), points.get(i), points.get(i + 1), points.get(i + 2)); + } else { + // just 2 points ! a line + intermediatePoints = this.splineEval(points.get(i), points.get(i), points.get(i + 1), points.get(i + 1)); + } + } else { + if (i == (points.size() - 2)) { + intermediatePoints = this.splineEval(points.get(i - 1), points.get(i), points.get(i + 1), points.get(i + 1)); + } else { + intermediatePoints = this.splineEval(points.get(i - 1), points.get(i), points.get(i + 1), points.get(i + 2)); + + } + } + globalPoints.addAll(intermediatePoints); + intermediatePoints.clear(); + } + return globalPoints; + } + + /** + * Calculate the spline between point 2 and point 3.
+ * the point 1 and the point 4 are used for the catmullrom method. + * + * @param pt1 + * the first point + * @param pt2 + * the second point + * @param pt3 + * the third point + * @param pt4 + * the fourth point + * @return the array list of points + */ + private ArrayList splineEval(Point3f pt1, Point3f pt2, Point3f pt3, Point3f pt4) { + ArrayList newPoints = new ArrayList(); + double segmentLength = pt2.distance(pt3); + double nbIntermediatePoints = segmentLength / this.getPrecision(); + + for (double j = 0D; j < 1D; j += 1 / nbIntermediatePoints) { + newPoints.add(this.computeIntermediatePoint(j, pt1, pt2, pt3, pt4)); + } + return newPoints; + } + + /** + * Calculate the intermediate point with t and the geometric matrix. + * xi(t) = 1/6[(1-t)^3xi + (3t^3-6t^2+4)xi+1 + (-3t^3+3t^2+3t+1)xi+2 + t^3xi+3] + * yi(t) = 1/6[(1-t)^3yi + (3t^3-6t^2+4)yi+1 + (-3t^3+3t^2+3t+1)yi+2 + t^3yi+3] + * + * @param t + * the place the point + * @param pt1 + * the first point + * @param pt2 + * the second point + * @param pt3 + * the third point + * @param pt4 + * the fourth point + * @return the intermediate point + */ + private Point3f computeIntermediatePoint(double t, Point3f pt1, Point3f pt2, Point3f pt3, Point3f pt4) { + // math for point + // Q(t) = (x(t), y(t) ) = T.C = T.M.G + // T : t³ + t² + t + 1 + // M : base matrix + // G : geometric matrix + double xValue = 0D, yValue = 0D, zValue = 0D; + double t3 = Math.pow(t, 3); + double t2 = Math.pow(t, 2); + + Matrix4f pointsMatrix = new Matrix4f(this.setGeometricMatrix(pt1, pt2, pt3, pt4)); // generate the geometric matrix + + Matrix4f inter = new Matrix4f(); // generate the intermediate matrix + inter.mul(this.Bmatrix, pointsMatrix); + + xValue = (inter.getM00() * t3) + (inter.getM10() * t2) + (inter.getM20() * t) + inter.getM30(); + yValue = (inter.getM01() * t3) + (inter.getM11() * t2) + (inter.getM21() * t) + inter.getM31(); + zValue = (inter.getM02() * t3) + (inter.getM12() * t2) + (inter.getM22() * t) + inter.getM32(); + + return new Point3f(xValue, yValue, zValue); + } + + /** + * Set the catmullrom matrix for polyspline.
+ * |-1 3 -3 1 |
+ * | 3 -6 3 0 | * 1/6
+ * |-3 0 3 0 |
+ * | 1 4 1 0 | + */ + private void setBMatrix() { + + this.Bmatrix.setM00(-1f / 6f); + this.Bmatrix.setM01(3f / 6f); + this.Bmatrix.setM02(-3f / 6f); + this.Bmatrix.setM03(1f / 6f); + + this.Bmatrix.setM10(3f / 6f); + this.Bmatrix.setM11(-6f / 6f); + this.Bmatrix.setM12(3f / 6f); + this.Bmatrix.setM13(0f); + + this.Bmatrix.setM20(-3f / 6f); + this.Bmatrix.setM21(0f); + this.Bmatrix.setM22(3f / 6f); + this.Bmatrix.setM23(0f); + + this.Bmatrix.setM30(1f / 6f); + this.Bmatrix.setM31(4f / 6f); + this.Bmatrix.setM32(1f / 6f); + this.Bmatrix.setM33(0f); + } + + /** + * Set matrix of geometric contsraint + * + * @param pt1 + * the first point to define the matrix + * @param pt2 + * the second point to define the matrix + * @param pt3 + * the third point to define the matrix + * @param pt4 + * the fourth point to define the matrix + * @return the geometric matrix + */ + private Matrix4f setGeometricMatrix(Point3f pt1, Point3f pt2, Point3f pt3, Point3f pt4) { + + Matrix4f pointsMatrix = new Matrix4f(); + + pointsMatrix.setM00(pt1.getX()); + pointsMatrix.setM01(pt1.getY()); + pointsMatrix.setM02(pt1.getZ()); + pointsMatrix.setM03(0); + pointsMatrix.setM10(pt2.getX()); + pointsMatrix.setM11(pt2.getY()); + pointsMatrix.setM12(pt2.getZ()); + pointsMatrix.setM13(0); + pointsMatrix.setM20(pt3.getX()); + pointsMatrix.setM21(pt3.getY()); + pointsMatrix.setM22(pt3.getZ()); + pointsMatrix.setM23(0); + pointsMatrix.setM30(pt4.getX()); + pointsMatrix.setM31(pt4.getY()); + pointsMatrix.setM32(pt4.getZ()); + pointsMatrix.setM33(0); + + return pointsMatrix; + } +} diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/BezierSpline.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/BezierSpline.java new file mode 100644 index 000000000..4ef7448b4 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/BezierSpline.java @@ -0,0 +1,127 @@ +/* + * $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.spline; + +import java.util.ArrayList; +import java.util.List; + +import org.arakhne.afc.math.geometry3d.continuous.Point3f; + +/** + * this class calculate a bezier curve. + * + * @author $Author: nvieval$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class BezierSpline extends AbstractSpline{ + + /** number of points in the result spline. */ + private final int n; + + /** + * default constructor; + */ + public BezierSpline() { + super(); + this.n = (int) (1 / this.getPrecision()); + } + + /** + * constructor. + * @param nbPoints the number of out points + */ + public BezierSpline(int nbPoints) { + this.n = nbPoints; + } + + /** + * compute the points of bezier curve from control points. + * @param points the control points + * @return the spline points + */ + @Override + public List compute(List points) { + ArrayList globalPoints = new ArrayList(); + for(int i=0;i points) { + float xbezier, ybezier, zbezier; + float[] xjp=new float[nbControlPoint]; + float[] yjp=new float[nbControlPoint]; + float[] zjp=new float[nbControlPoint]; + + for(int i=0;i=1) { + float[] xj=new float[nbControlPoint-j]; + float[] yj=new float[nbControlPoint-j]; + float[] zj=new float[nbControlPoint-j]; + + for(int i=0;i<(nbControlPoint-j);i++) { + xj[i]=interpol(t,xjp[i],xjp[i+1]); + yj[i]=interpol(t,yjp[i],yjp[i+1]); + zj[i]=interpol(t,zjp[i],zjp[i+1]); + } + for(int i=0;i<(nbControlPoint-j);i++) { + xjp[i]=xj[i]; + yjp[i]=yj[i]; + zjp[i]=zj[i]; + } + } + } + xbezier=xjp[0]; + ybezier=yjp[0]; + zbezier=zjp[0]; + + return new Point3f(xbezier, ybezier, zbezier); + } + + /** + * interpole position between a and b with t + * @param t t + * @param a a + * @param b b + * @return value + */ + public float interpol(float t, float a, float b) { + return a*(1-t)+b*t; + } +} diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/CatMullRomSpline.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/CatMullRomSpline.java new file mode 100644 index 000000000..40f3c93a5 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/CatMullRomSpline.java @@ -0,0 +1,251 @@ +/* + * $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.spline; + +import java.util.ArrayList; +import java.util.List; + +import org.arakhne.afc.math.Matrix4f; +import org.arakhne.afc.math.geometry3d.continuous.Point3f; + +/** + * This class compute the cattmulrom spline. + * + * @author $Author: nvieval$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class CatMullRomSpline extends AbstractSpline{ + + /** the catmullrom matrix. */ + private final Matrix4f catMullRomMatrix = new Matrix4f(); + + private int discretizeLevel = 100; + + /** + * constructor. + */ + public CatMullRomSpline() { + this(100); + } + + /** + * @param discretizeLvl + */ + public CatMullRomSpline(int discretizeLvl) { + this.setCatmullRomMatrix(); + setDiscretizationLevel(discretizeLvl); + } + + + + /** Replies the level of discretization of the spline. + * + * @return the level of discretization of the spline. + */ + public int getDiscretizationLevel() { + return this.discretizeLevel; + } + + /** Set the level of discretization of the spline. + * + * @param level is the level of discretization of the spline. + */ + public void setDiscretizationLevel(int level) { + this.discretizeLevel = level; + setPrecision(1.f/level); + } + + /** + * {@inheritDoc} + */ + @Override + public void setPrecision(float precision) { + super.setPrecision(precision); + this.discretizeLevel = (int)(1./precision); + } + + /** + * compute the spline with differents configuration. + * 2 points, 3 points, 4 points, ....., N points + * @param points + * @return {@inheritDoc} + */ + @Override + public List compute(List points) { + ArrayList globalPoints = new ArrayList(); + ArrayList intermediatePoints = new ArrayList(); + for (int i = 0; i < (points.size() - 1); i++) { + if (i == 0) { + if (points.size() > 2) { // fist part + // use the first point two time for catch spline on this point + intermediatePoints = this.splineEval(points.get(i), points.get(i), points.get(i + 1), points.get(i + 2)); + } else { + // just 2 points ! a line + intermediatePoints = this.splineEval(points.get(i), points.get(i), points.get(i + 1), points.get(i + 1)); + } + } else { + if (i == (points.size() - 2)) { + intermediatePoints = this.splineEval(points.get(i - 1), points.get(i), points.get(i + 1), points.get(i + 1)); + } else { + intermediatePoints = this.splineEval(points.get(i - 1), points.get(i), points.get(i + 1), points.get(i + 2)); + + } + } + globalPoints.addAll(intermediatePoints); + intermediatePoints.clear(); + } + return globalPoints; + } + + /** + * Calculate the spline between point 2 and point 3.
+ * the point 1 and the point 4 are used for the catmullrom method. + * + * @param pt1 + * the first point + * @param pt2 + * the second point + * @param pt3 + * the third point + * @param pt4 + * the fourth point + * @return the array list of points + */ + public ArrayList splineEval(Point3f pt1, Point3f pt2, Point3f pt3, Point3f pt4) { + ArrayList newPoints = new ArrayList(); + + float step = 1.f / this.discretizeLevel; + + for (float j = 0f; j < 1f; j += step) { + newPoints.add(this.computeIntermediatePoint(j, pt1, pt2, pt3, pt4)); + } + + return newPoints; + } + + /** + * Calculate the intermediate point with t and the geometric matrix. + * xi(t) = 1/6[(1-t)^3xi + (3t^3-6t^2+4)xi+1 + (-3t^3+3t^2+3t+1)xi+2 + t^3xi+3] + * yi(t) = 1/6[(1-t)^3yi + (3t^3-6t^2+4)yi+1 + (-3t^3+3t^2+3t+1)yi+2 + t^3yi+3] + * + * @param t + * the place the point + * @param pt1 + * the first point + * @param pt2 + * the second point + * @param pt3 + * the third point + * @param pt4 + * the fourth point + * @return the intermediate point + */ + private Point3f computeIntermediatePoint(float t, Point3f pt1, Point3f pt2, Point3f pt3, Point3f pt4) { + // math for point + // Q(t) = (x(t), y(t) ) = T.C = T.M.G + // T : t³ + t² + t + 1 + // M : base matrix + // G : geometric matrix + float xValue = 0f, yValue = 0f, zValue = 0f; + float t3 = (float) Math.pow(t, 3); + float t2 = (float) Math.pow(t, 2); + + Matrix4f pointsMatrix = new Matrix4f(this.setGeometricMatrix(pt1, pt2, pt3, pt4)); // generate the geometric matrix + + Matrix4f inter = new Matrix4f(); // generate the intermediate matrix + inter.mul(this.catMullRomMatrix, pointsMatrix); + + xValue = (inter.getM00() * t3) + (inter.getM10() * t2) + (inter.getM20() * t) + inter.getM30(); + yValue = (inter.getM01() * t3) + (inter.getM11() * t2) + (inter.getM21() * t) + inter.getM31(); + zValue = (inter.getM02() * t3) + (inter.getM12() * t2) + (inter.getM22() * t) + inter.getM32(); + + return new Point3f(xValue, yValue, zValue); + } + + /** + * Set the catmullrom matrix for polyspline.
+ * |-1 3 -3 1 |
+ * | 1 -5 4 -1 | * 1/2
+ * |-1 0 1 0 |
+ * | 0 1 0 0 | + */ + private void setCatmullRomMatrix() { + + this.catMullRomMatrix.setM00(-0.5f); + this.catMullRomMatrix.setM01(1.5f); + this.catMullRomMatrix.setM02(-1.5f); + this.catMullRomMatrix.setM03(0.5f); + + this.catMullRomMatrix.setM10(1f); + this.catMullRomMatrix.setM11(-2.5f); + this.catMullRomMatrix.setM12(2f); + this.catMullRomMatrix.setM13(-0.5f); + + this.catMullRomMatrix.setM20(-0.5f); + this.catMullRomMatrix.setM21(0); + this.catMullRomMatrix.setM22(0.5f); + this.catMullRomMatrix.setM23(0); + + this.catMullRomMatrix.setM30(0); + this.catMullRomMatrix.setM31(1f); + this.catMullRomMatrix.setM32(0); + this.catMullRomMatrix.setM33(0); + } + + /** + * Set matrix of geometric contsraint + * + * @param pt1 + * the first point to define the matrix + * @param pt2 + * the second point to define the matrix + * @param pt3 + * the third point to define the matrix + * @param pt4 + * the fourth point to define the matrix + * @return the geometric matrix + */ + private Matrix4f setGeometricMatrix(Point3f pt1, Point3f pt2, Point3f pt3, Point3f pt4) { + + Matrix4f pointsMatrix = new Matrix4f(); + + pointsMatrix.setM00(pt1.getX()); + pointsMatrix.setM01(pt1.getY()); + pointsMatrix.setM02(pt1.getZ()); + pointsMatrix.setM03(0); + pointsMatrix.setM10(pt2.getX()); + pointsMatrix.setM11(pt2.getY()); + pointsMatrix.setM12(pt2.getZ()); + pointsMatrix.setM13(0); + pointsMatrix.setM20(pt3.getX()); + pointsMatrix.setM21(pt3.getY()); + pointsMatrix.setM22(pt3.getZ()); + pointsMatrix.setM23(0); + pointsMatrix.setM30(pt4.getX()); + pointsMatrix.setM31(pt4.getY()); + pointsMatrix.setM32(pt4.getZ()); + pointsMatrix.setM33(0); + + return pointsMatrix; + } +} diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/CoxDeBourSpline.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/CoxDeBourSpline.java new file mode 100644 index 000000000..d3689513e --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/CoxDeBourSpline.java @@ -0,0 +1,180 @@ +/* + * $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.spline; + +import java.util.ArrayList; +import java.util.List; + +import org.arakhne.afc.math.geometry3d.continuous.Point3f; + +/** + * This class implements the Cox De Boor algorithm. + * + * @author $Author: nvieval$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class CoxDeBourSpline extends AbstractSpline{ + + /** the degree of spline. */ + private int k; + + /** + * default constructor. + */ + public CoxDeBourSpline() { + this(3); + } + + /** + * constructor. + * @param degree degree of spline + */ + public CoxDeBourSpline(int degree) { + this.k = degree; + } + + /** + * default computation. The weight of every points are + * set by program at start. + * @param points the control points + * @return the spline points + */ + @Override + public List compute(List points) { + int size = points.size() + this.k; + ArrayList weight = new ArrayList(); // generate weight + for(int i=0; i<= size; i++) { + weight.add(new Float(i)); // just a iteratif number + } + + float t; + ArrayList globalPoints = new ArrayList(); + + for(int r = this.k; r < points.size(); r++) { + for(t=weight.get(r); t<=weight.get(r+1); t+= this.getPrecision()) { + globalPoints.add(computeCoxDeBoor(r, t, points, weight)); // compute cox de boor + } + } + return globalPoints; + } + + /** + * the custom computation. You need to define a weight for every + * control point. + * @param points the control points + * @param weight the weight + * @return the points of the spline + */ + public List compute(List points, List weight) { + float temp = 0; + /* In a default case the weight are 0,1,2,3,4,5,... + * But in a custom case the user send for example : 4, 1, 1, 4 + * you need to complete with the same length that points and additionate + * values extends => for 6 points 0,4,5,6,10,10 + */ + int add = ((points.size() - weight.size()) + this.k) / 2; + ArrayList weightT = new ArrayList(); + for(int i = 0; i < add; i++) { + weightT.add(new Float(0)); + } + for(int i = 0; i < weight.size(); i++) { + temp = temp + weight.get(i).floatValue(); + weightT.add(new Float(temp)); + } + while(weightT.size() < (points.size() + this.k)) { + weightT.add(new Float(temp)); + } + // end of weight computation + + ArrayList globalPoints = new ArrayList(); + float t; + for(int r = this.k; r < points.size(); r++) { + for(t=weightT.get(r); t<=weightT.get(r+1); t+= this.getPrecision()) { + globalPoints.add(computeCoxDeBoor(r, t , points, weightT)); + } + } + return globalPoints; + } + + /** + * start the cox de boor algorithm. + * @param r r value of the algorithm + * @param t t value + * @param points the control points + * @param weight the weight + * @return + */ + private Point3f computeCoxDeBoor(int r, float t, List points, List weight) { + Point3f[][] Pt; + + Pt = new Point3f[this.k+1][r+1]; + for(int i = r-this.k; i<=r; i++) { + Pt[0][i] = points.get(i); + } + + for(int j = 1; j <= this.k; j++) { + for(int i = r-this.k+j; i <= r; i++) { + Pt[j][i] = this.computePointCalcul(i, j, t, Pt, weight); + } + } + return Pt[this.k][r]; + } + + /** + * compute point. + * @param i + * @param j + * @param t + * @param Pt + * @param weight + * @return + */ + private Point3f computePointCalcul(int i, int j, float t, Point3f[][] Pt, List weight){ + Point3f res = new Point3f(); + float a = t - weight.get(i).intValue(); + float b = weight.get(i - j + this.k + 1).intValue() - t; + float c = weight.get(i - j + this.k + 1).intValue() - weight.get(i).intValue(); + + res.setX(((Pt[j-1][i].getX() *a) + (Pt[j-1][i-1].getX() * b)) / c); + res.setY(((Pt[j-1][i].getY() *a) + (Pt[j-1][i-1].getY() * b)) / c); + res.setZ(((Pt[j-1][i].getZ() *a) + (Pt[j-1][i-1].getZ() * b)) / c); + + return res; + } + + /** + * set the degree of curve. + * @param k the nex degree + */ + public void setDegree(int k) { + this.k = k; + } + + /** + * get the degree. + * @return degree + */ + public int getDegree() { + return this.k; + } +} diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/CubicBezierSpline.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/CubicBezierSpline.java new file mode 100644 index 000000000..3b6dfac48 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/CubicBezierSpline.java @@ -0,0 +1,106 @@ +/* + * $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.spline; + +import java.util.ArrayList; +import java.util.List; + +import org.arakhne.afc.math.geometry3d.continuous.Point3f; + +/** + * this class calculate a cubic bezier spline. + * + * @author $Author: nvieval$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class CubicBezierSpline extends AbstractSpline{ + + /** + * copnstructor. + */ + public CubicBezierSpline() { + // default & empty + } + + /** + * compute the spline with differents configuration. + * 2 points, 3 points, 4 points, ....., N points + * @param points + * @return {@inheritDoc} + */ + @Override + public List compute(List points) { + ArrayList globalPoints = new ArrayList(); + ArrayList intermediatePoints = new ArrayList(); + for (int i = 0; i < (points.size() - 1); i++) { + if (i == 0) { + if (points.size() > 2) { // fist part + // use the first point two time for catch spline on this point + intermediatePoints = this.splineEval(points.get(i), points.get(i), points.get(i + 1), points.get(i + 2)); + } else { + // just 2 points ! a line + intermediatePoints = this.splineEval(points.get(i), points.get(i), points.get(i + 1), points.get(i + 1)); + } + } else { + if (i == (points.size() - 2)) { + intermediatePoints = this.splineEval(points.get(i - 1), points.get(i), points.get(i + 1), points.get(i + 1)); + } else { + intermediatePoints = this.splineEval(points.get(i - 1), points.get(i), points.get(i + 1), points.get(i + 2)); + + } + } + globalPoints.addAll(intermediatePoints); + intermediatePoints.clear(); + } + return globalPoints; + } + + /** + * evaluation of spline between 4 points + * Pt = p1 * (1-t)³ + p2 * 3t(1-t)² p3 * 3t²(1-t) p4* t³ + * + * @param p1 the first point + * @param p2 the second point + * @param p3 the third point + * @param p4 the fourth point + * @return the points of spline between this points + */ + private ArrayList splineEval( Point3f p1, Point3f p2, Point3f p3, Point3f p4) { + ArrayList res = new ArrayList(); + double xp, yp, zp; + double t3, t2, t1, t0; + for (double t=0 ; t<=1 ; t+=this.getPrecision()){ + t3 = (1-t)*(1-t)*(1-t); + t2 = 3*t*(1-t)*(1-t); + t1 = 3*t*(1-t)*t; + t0 = t*t*t; + + xp = (t3*p1.getX() + t2*p2.getX() + t1*p3.getX() + t0*p4.getX()); + yp = (t3*p1.getY() + t2*p2.getY() + t1*p3.getY() + t0*p4.getY()); + zp = (t3*p1.getZ() + t2*p2.getZ() + t1*p3.getZ() + t0*p4.getZ()); + res.add(new Point3f(xp, yp, zp)); + } + return res; + } + +} diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/HermiteSpline.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/HermiteSpline.java new file mode 100644 index 000000000..8305ef2c1 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/HermiteSpline.java @@ -0,0 +1,229 @@ +/* + * $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.spline; + +import java.util.ArrayList; +import java.util.List; + +import org.arakhne.afc.math.Matrix4f; +import org.arakhne.afc.math.geometry3d.continuous.Point3f; +import org.arakhne.afc.math.geometry3d.continuous.Vector3f; + +/** + * This class compute an Hermite curve. + * The main calculation is operate by the method + * compute(points, tangentes) + * The simple method compute(points) is not usable + * because Hermite need tengantes for computation + * + * @author $Author: nvieval$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class HermiteSpline extends AbstractSpline{ + /** the hermite matrix */ + private final Matrix4f hermiteMatrix = new Matrix4f(); + + /** + * constructor. + */ + public HermiteSpline() { + this.setHermiteMatrix(); + } + + /** + * WARNING : don't use this function use the other. + * @param points the points + * @return points + */ + @Override + public List compute(List points) { + return points; + } + + /** + * compute the Hermite algorithm. The points array and the tengantes array + * need the same length. + * @param points the control points + * @param tangentes the tengantes on points + * @return the points of the spline + */ + public List compute(List points, List tangentes) { + ArrayList globalPoints = new ArrayList(); + Point3f p1, p2 = new Point3f(); + Vector3f t1, t2 = new Vector3f(); + for (int i = 0; i < (points.size() - 1); i++) { + p1 = points.get(i); + if(i > 0) { + p2 = points.get(i - 1); + } + t1 = tangentes.get(i); + if(i > 0) { + t2 = tangentes.get(i - 1); + } + if (i == 0) { + ArrayList intermediatePoints = new ArrayList(); + intermediatePoints = this.computeSplineSegment(p1, p1, t1, t1); + globalPoints.addAll(intermediatePoints); + } else { + if (i == (points.size() - 2)) { + ArrayList intermediatePoints = new ArrayList(); + intermediatePoints = this.computeSplineSegment(p2, p1, t2, t1); + globalPoints.addAll(intermediatePoints); + } else { + ArrayList intermediatePoints = new ArrayList(); + intermediatePoints = this.computeSplineSegment(p2, p1, t2, t1); + globalPoints.addAll(intermediatePoints); + } + } + } + return globalPoints; + } + + /** + * Calculate the spline between point 1 and point 2.
+ * the tengantes 1 and 2 are use too. + * + * @param pt1 + * the first point + * @param pt2 + * the second point + * @param t1 + * the first tengante + * @param t2 + * the second tengante + * @return the array list of points + */ + private ArrayList computeSplineSegment(Point3f pt1, Point3f pt2, Vector3f t1, Vector3f t2) { + ArrayList newPoints = new ArrayList(); + double segmentLength = pt1.distance(pt2); + double nbIntermediatePoints = segmentLength / this.getPrecision(); + + for (double j = 0D; j < 1D; j += 1 / nbIntermediatePoints) { + newPoints.add(this.computeIntermediatePoint(j, pt1, pt2, t1, t2)); + } + return newPoints; + } + + /** + * Calculate the intermediate point with t and the geometric matrix. + * @param t + * the place the point + * @param pt1 + * the first point + * @param pt2 + * the second point + * @param pt3 + * the third point + * @param pt4 + * the fourth point + * @return the intermediate point + */ + private Point3f computeIntermediatePoint(double t, Point3f pt1, Point3f pt2, Vector3f ta1, Vector3f ta2) { + // math for point + // Q(t) = (x(t), y(t) ) = T.C = T.M.G + // T : t³ + t² + t + 1 + // M : base matrix + // G : geometric matrix + double xValue = 0D, yValue = 0D, zValue = 0D; + double t3 = Math.pow(t, 3); + double t2 = Math.pow(t, 2); + + Matrix4f pointsMatrix = new Matrix4f(this.setGeometricMatrix(pt1, pt2, ta1, ta2)); // generate the geometric matrix + + Matrix4f inter = new Matrix4f(); // generate the intermediate matrix + inter.mul(this.hermiteMatrix, pointsMatrix); + + xValue = (inter.getM00() * t3) + (inter.getM10() * t2) + (inter.getM20() * t) + inter.getM30(); + yValue = (inter.getM01() * t3) + (inter.getM11() * t2) + (inter.getM21() * t) + inter.getM31(); + zValue = (inter.getM02() * t3) + (inter.getM12() * t2) + (inter.getM22() * t) + inter.getM32(); + + return new Point3f(xValue, yValue, zValue); + } + + /** + * Set the Hermite matrix for polyspline.
+ * | 2 -2 1 1 |
+ * |-3 3 -2 -1 |
+ * | 0 0 1 0 |
+ * | 1 0 0 0 | + */ + private void setHermiteMatrix() { + + this.hermiteMatrix.setM00(2f); + this.hermiteMatrix.setM01(-2f); + this.hermiteMatrix.setM02(1f); + this.hermiteMatrix.setM03(1f); + + this.hermiteMatrix.setM10(-3f); + this.hermiteMatrix.setM11(3f); + this.hermiteMatrix.setM12(-2f); + this.hermiteMatrix.setM13(-1f); + + this.hermiteMatrix.setM20(0f); + this.hermiteMatrix.setM21(0f); + this.hermiteMatrix.setM22(1f); + this.hermiteMatrix.setM23(0f); + + this.hermiteMatrix.setM30(1f); + this.hermiteMatrix.setM31(0f); + this.hermiteMatrix.setM32(0f); + this.hermiteMatrix.setM33(0f); + } + + /** + * Set matrix of geometric contsraint + * + * @param pt1 + * the first point to define the matrix + * @param pt2 + * the second point to define the matrix + * @param t1 + * the first tengante to define the matrix + * @param t2 + * the second tengante to define the matrix + * @return the geometric matrix + */ + private Matrix4f setGeometricMatrix(Point3f pt1, Point3f pt2, Vector3f t1, Vector3f t2) { + + Matrix4f pointsMatrix = new Matrix4f(); + + pointsMatrix.setM00(pt1.getX()); + pointsMatrix.setM01(pt1.getY()); + pointsMatrix.setM02(pt1.getZ()); + pointsMatrix.setM03(0); + pointsMatrix.setM10(pt2.getX()); + pointsMatrix.setM11(pt2.getY()); + pointsMatrix.setM12(pt2.getZ()); + pointsMatrix.setM13(0); + pointsMatrix.setM20(t1.getX()); + pointsMatrix.setM21(t1.getY()); + pointsMatrix.setM22(t1.getZ()); + pointsMatrix.setM23(0); + pointsMatrix.setM30(t2.getX()); + pointsMatrix.setM31(t2.getY()); + pointsMatrix.setM32(t2.getZ()); + pointsMatrix.setM33(0); + + return pointsMatrix; + } +} diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/LagrangeSpline.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/LagrangeSpline.java new file mode 100644 index 000000000..74a4074eb --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/continuous/spline/LagrangeSpline.java @@ -0,0 +1,85 @@ +/* + * $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.spline; + +import java.util.ArrayList; +import java.util.List; + +import org.arakhne.afc.math.geometry3d.continuous.Point3f; + +/** + * This class implements the Lagrange algorithm. + * This Lagrange interpolation is based on the Aitken algorithm: + * Pij = Pij-1 (ti+j - t) / (ti+j - ti) + Pi+1j-1 (t - ti) / (ti+j - ti) , + * j =[1, n] i = [0, n-j] + * + * @author $Author: sgalland$ + * @author $Author: nvieval$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class LagrangeSpline extends AbstractSpline{ + + @Override + public List compute(List points){ + int N = points.size(); // Knots count + + int totalSize = Math.round(points.size()/getPrecision()); + + List res = new ArrayList(totalSize); + + float[] ti = new float[N]; + + for (int i=0; i compute(List points); + + /** + * set the precision factor. + * @param p the factor + */ + public void setPrecision(float p); + + /** + * get the precision factor. + * @return the factor + */ + public float getPrecision(); + +} diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/discrete/Point3i.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/discrete/Point3i.java new file mode 100644 index 000000000..1c915af09 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/discrete/Point3i.java @@ -0,0 +1,255 @@ +/* + * $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.geometry3d.discrete; + +import org.arakhne.afc.math.MathUtil; +import org.arakhne.afc.math.geometry3d.Point3D; +import org.arakhne.afc.math.geometry3d.Tuple3D; +import org.arakhne.afc.math.geometry3d.Vector3D; + +/** 3D Point with 3 integers. + * + * @author $Author: galland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class Point3i extends Tuple3i implements Point3D { + + private static final long serialVersionUID = -1506750779625667216L; + + /** + */ + public Point3i() { + // + } + + /** + * @param tuple is the tuple to copy. + */ + public Point3i(Tuple3D tuple) { + super(tuple); + } + + /** + * @param tuple is the tuple to copy. + */ + public Point3i(int[] tuple) { + super(tuple); + } + + /** + * @param tuple is the tuple to copy. + */ + public Point3i(float[] tuple) { + super(tuple); + } + + /** + * @param x + * @param y + * @param z + */ + public Point3i(int x, int y, int z) { + super(x,y,z); + } + + /** + * @param x + * @param y + * @param z + */ + public Point3i(float x, float y, float z) { + super(x,y,z); + } + + /** + * @param x + * @param y + * @param z + */ + public Point3i(double x, double y, double z) { + super((float)x,(float)y,(float)z); + } + + /** + * @param x + * @param y + * @param z + */ + public Point3i(long x, long y, long z) { + super(x,y,z); + } + + /** {@inheritDoc} + */ + @Override + public Point3i clone() { + return (Point3i)super.clone(); + } + + /** + * {@inheritDoc} + */ + @Override + public int distanceSquared(Point3D p1) { + float dx, dy, dz; + dx = this.x-p1.getX(); + dy = this.y-p1.getY(); + dz = this.z-p1.getZ(); + return (int)(dx*dx+dy*dy+dz*dz); + } + + @Override + public float getDistanceSquared(Point3D p1) { + float dx, dy, dz; + dx = this.x-p1.getX(); + dy = this.y-p1.getY(); + dz = this.z-p1.getZ(); + return (dx*dx+dy*dy+dz*dz); + } + + /** + * {@inheritDoc} + */ + @Override + public int distance(Point3D p1) { + float dx, dy, dz; + dx = this.x-p1.getX(); + dy = this.y-p1.getY(); + dz = this.z-p1.getZ(); + return (int)Math.sqrt(dx*dx+dy*dy+dz*dz); + } + + @Override + public float getDistance(Point3D p1) { + float dx, dy, dz; + dx = this.x-p1.getX(); + dy = this.y-p1.getY(); + dz = this.z-p1.getZ(); + return (float)Math.sqrt(dx*dx+dy*dy+dz*dz); + } + + /** + * {@inheritDoc} + */ + @Override + public int distanceL1(Point3D p1) { + return (int)(Math.abs(this.x-p1.getX()) + Math.abs(this.y-p1.getY()) + Math.abs(this.z-p1.getZ())); + } + + @Override + public float getDistanceL1(Point3D p1) { + return Math.abs(this.x-p1.getX()) + Math.abs(this.y-p1.getY()) + Math.abs(this.z-p1.getZ()); + } + + /** + * {@inheritDoc} + */ + @Override + public int distanceLinf(Point3D p1) { + return (int)(MathUtil.max( Math.abs(this.x-p1.getX()), Math.abs(this.y-p1.getY()), Math.abs(this.z-p1.getZ()))); + } + + @Override + public float getDistanceLinf(Point3D p1) { + return (MathUtil.max( Math.abs(this.x-p1.getX()), Math.abs(this.y-p1.getY()), Math.abs(this.z-p1.getZ()))); + } + + @Override + public void add(Point3D t1, Vector3D t2) { + this.x = (int)(t1.getX() + t2.getX()); + this.y = (int)(t1.getY() + t2.getY()); + this.z = (int)(t1.getZ() + t2.getZ()); + } + + @Override + public void add(Vector3D t1, Point3D t2) { + this.x = (int)(t1.getX() + t2.getX()); + this.y = (int)(t1.getY() + t2.getY()); + this.z = (int)(t1.getZ() + t2.getZ()); + } + + @Override + public void add(Vector3D t1) { + this.x = (int)(this.x + t1.getX()); + this.y = (int)(this.y + t1.getY()); + this.z = (int)(this.z + t1.getZ()); + } + + @Override + public void scaleAdd(int s, Vector3D t1, Point3D t2) { + this.x = (int)(s * t1.getX() + t2.getX()); + this.y = (int)(s * t1.getY() + t2.getY()); + this.z = (int)(s * t1.getZ() + t2.getZ()); + } + + @Override + public void scaleAdd(float s, Vector3D t1, Point3D t2) { + this.x = (int)(s * t1.getX() + t2.getX()); + this.y = (int)(s * t1.getY() + t2.getY()); + this.z = (int)(s * t1.getZ() + t2.getZ()); + } + + @Override + public void scaleAdd(int s, Point3D t1, Vector3D t2) { + this.x = (int)(s * t1.getX() + t2.getX()); + this.y = (int)(s * t1.getY() + t2.getY()); + this.z = (int)(s * t1.getZ() + t2.getZ()); + } + + @Override + public void scaleAdd(float s, Point3D t1, Vector3D t2) { + this.x = (int)(s * t1.getX() + t2.getX()); + this.y = (int)(s * t1.getY() + t2.getY()); + this.z = (int)(s * t1.getZ() + t2.getZ()); + } + + @Override + public void scaleAdd(int s, Vector3D t1) { + this.x = (int)(s * this.x + t1.getX()); + this.y = (int)(s * this.y + t1.getY()); + this.z = (int)(s * this.z + t1.getZ()); + } + + @Override + public void scaleAdd(float s, Vector3D t1) { + this.x = (int)(s * this.x + t1.getX()); + this.y = (int)(s * this.y + t1.getY()); + this.z = (int)(s * this.z + t1.getZ()); + } + + @Override + public void sub(Point3D t1, Vector3D t2) { + this.x = (int)(t1.getX() - t1.getX()); + this.y = (int)(t1.getY() - t1.getY()); + this.z = (int)(t1.getZ() - t1.getZ()); + } + + @Override + public void sub(Vector3D t1) { + this.x = (int)(this.x - t1.getX()); + this.y = (int)(this.y - t1.getY()); + this.z = (int)(this.z - t1.getZ()); + } + +} \ No newline at end of file diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/discrete/Tuple3i.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/discrete/Tuple3i.java new file mode 100644 index 000000000..c16f2af79 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/discrete/Tuple3i.java @@ -0,0 +1,728 @@ +/* + * $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.geometry3d.discrete; + +import org.arakhne.afc.math.geometry3d.Tuple3D; + +/** 3D tuple with 3 integers. + * + * @param is the implementation type of the tuple. + * @author $Author: galland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class Tuple3i> implements Tuple3D { + + private static final long serialVersionUID = 358537735186816489L; + + /** x coordinate. + */ + protected int x; + + /** y coordinate. + */ + protected int y; + + /** z coordinate. + */ + protected int z; + + /** + */ + public Tuple3i() { + this.x = this.y = this.z = 0; + } + + /** + * @param tuple is the tuple to copy. + */ + public Tuple3i(Tuple3i tuple) { + this.x = tuple.x; + this.y = tuple.y; + this.z = tuple.z; + } + + /** + * @param tuple is the tuple to copy. + */ + public Tuple3i(Tuple3D tuple) { + this.x = (int)tuple.getX(); + this.y = (int)tuple.getY(); + this.z = (int)tuple.getZ(); + } + + /** + * @param tuple is the tuple to copy. + */ + public Tuple3i(int[] tuple) { + this.x = tuple[0]; + this.y = tuple[1]; + this.z = tuple[2]; + } + + /** + * @param tuple is the tuple to copy. + */ + public Tuple3i(float[] tuple) { + this.x = (int)tuple[0]; + this.y = (int)tuple[1]; + this.z = (int)tuple[2]; + } + + /** + * @param x + * @param y + * @param z + */ + public Tuple3i(int x, int y, int z) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * @param x + * @param y + * @param z + */ + public Tuple3i(float x, float y, float z) { + this.x = (int)x; + this.y = (int)y; + this.z = (int)z; + } + + /** {@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); + this.z = Math.abs(this.z); + } + + /** + * {@inheritDoc} + */ + @Override + public void absolute(T t) { + t.set(Math.abs(this.x), Math.abs(this.y), Math.abs(this.z)); + } + + /** + * {@inheritDoc} + */ + @Override + public void add(int x, int y, int z) { + this.x += x; + this.y += y; + this.z += z; + } + + /** + * {@inheritDoc} + */ + @Override + public void add(float x, float y, float z) { + this.x += x; + this.y += y; + this.z += z; + } + + /** + * {@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 addZ(int z) { + this.z += z; + } + + /** + * {@inheritDoc} + */ + @Override + public void addZ(float z) { + this.z += z; + } + + /** + * {@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; + if (this.z < min) this.z = min; + else if (this.z > max) this.z = 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; + if (this.z < min) this.z = 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; + if (this.z > max) this.z = 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); + if (this.z < min) t.setZ(min); + else if (this.z > max) t.setZ(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); + if (this.z < min) t.setZ(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); + if (this.z > max) t.setZ(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, this.z); + } + + /** + * {@inheritDoc} + */ + @Override + public void get(int[] t) { + t[0] = this.x; + t[1] = this.y; + t[2] = this.z; + } + + /** + * {@inheritDoc} + */ + @Override + public void get(float[] t) { + t[0] = this.x; + t[1] = this.y; + t[2] = this.z; + } + + /** + * {@inheritDoc} + */ + @Override + public void negate(T t1) { + this.x = -t1.x(); + this.y = -t1.y(); + this.z = -t1.z(); + } + + /** + * {@inheritDoc} + */ + @Override + public void negate() { + this.x = -this.x; + this.y = -this.y; + this.z = -this.z; + } + + /** + * {@inheritDoc} + */ + @Override + public void scale(int s, T t1) { + this.x = (int)(s * t1.getX()); + this.y = (int)(s * t1.getY()); + this.z = (int)(s * t1.getZ()); + } + + /** + * {@inheritDoc} + */ + @Override + public void scale(float s, T t1) { + this.x = (int)(s * t1.getX()); + this.y = (int)(s * t1.getY()); + this.z = (int)(s * t1.getZ()); + } + + /** + * {@inheritDoc} + */ + @Override + public void scale(int s) { + this.x = s * this.x; + this.y = s * this.y; + this.z = s * this.z; + } + + /** + * {@inheritDoc} + */ + @Override + public void scale(float s) { + this.x = (int)(s * this.x); + this.y = (int)(s * this.y); + this.z = (int)(s * this.z); + } + + /** + * {@inheritDoc} + */ + @Override + public void set(Tuple3D t1) { + this.x = t1.x(); + this.y = t1.y(); + this.z = t1.z(); + } + + /** + * {@inheritDoc} + */ + @Override + public void set(int x, int y, int z) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * {@inheritDoc} + */ + @Override + public void set(float x, float y, float z) { + this.x = (int)x; + this.y = (int)y; + this.z = (int)z; + } + + /** + * {@inheritDoc} + */ + @Override + public void set(int[] t) { + this.x = t[0]; + this.y = t[1]; + this.z = t[2]; + } + + /** + * {@inheritDoc} + */ + @Override + public void set(float[] t) { + this.x = (int)t[0]; + this.y = (int)t[1]; + this.z = (int)t[2]; + } + + /** + * {@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 float getZ() { + return this.z; + } + + /** + * {@inheritDoc} + */ + @Override + public int z() { + return this.z; + } + + /** + * {@inheritDoc} + */ + @Override + public void setZ(int z) { + this.z = z; + } + + /** + * {@inheritDoc} + */ + @Override + public void setZ(float z) { + this.z = (int)z; + } + + /** + * {@inheritDoc} + */ + @Override + public void sub(int x, int y, int z) { + this.x -= x; + this.y -= y; + this.z -= z; + } + + /** + * {@inheritDoc} + */ + @Override + public void subX(int x) { + this.x -= x; + } + + /** + * {@inheritDoc} + */ + @Override + public void subY(int y) { + this.y -= y; + } + + /** + * {@inheritDoc} + */ + @Override + public void subZ(int z) { + this.z -= z; + } + + /** + * {@inheritDoc} + */ + @Override + public void sub(float x, float y, float z) { + this.x -= x; + this.y -= y; + this.z -= z; + } + + /** + * {@inheritDoc} + */ + @Override + public void subX(float x) { + this.x -= x; + } + + /** + * {@inheritDoc} + */ + @Override + public void subY(float y) { + this.y -= y; + } + + /** + * {@inheritDoc} + */ + @Override + public void subZ(float z) { + this.z -= z; + } + + /** + * {@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()); + this.z = (int)((1f-alpha)*t1.getZ() + alpha*t2.getZ()); + } + + /** + * {@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()); + this.z = (int)((1f-alpha)*this.z + alpha*t1.getZ()); + } + + /** + * {@inheritDoc} + */ + @Override + public boolean equals(Tuple3D t1) { + try { + return(this.x == t1.x() && this.y == t1.y() && this.z == t1.z()); + } + 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() && this.z == t2.z()); + } + 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; + + diff = this.z - t1.getZ(); + 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; + bits = 31 * bits + this.z; + return bits ^ (bits >> 32); + } + + /** + * {@inheritDoc} + */ + @Override + public String toString() { + return "(" //$NON-NLS-1$ + +this.x + +";" //$NON-NLS-1$ + +this.y + +";" //$NON-NLS-1$ + +this.z + +")"; //$NON-NLS-1$ + } + +} \ No newline at end of file diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/discrete/Tuple3iComparator.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/discrete/Tuple3iComparator.java new file mode 100644 index 000000000..2b4100883 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/discrete/Tuple3iComparator.java @@ -0,0 +1,57 @@ +/* + * $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.geometry3d.discrete; + +import java.util.Comparator; + +/** + * Comparator of Tuple3i. + * + * @author $Author: galland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class Tuple3iComparator implements Comparator> { + + /** + */ + public Tuple3iComparator() { + // + } + + /** + * {@inheritDoc} + */ + @Override + public int compare(Tuple3i o1, Tuple3i 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; + cmp = o1.y() - o2.y(); + if (cmp!=0) return cmp; + return o1.z() - o2.z(); + } + +} \ No newline at end of file diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/discrete/UnmodifiablePoint3i.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/discrete/UnmodifiablePoint3i.java new file mode 100644 index 000000000..0ae120aca --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/discrete/UnmodifiablePoint3i.java @@ -0,0 +1,115 @@ +/* + * $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.geometry3d.discrete; + +import org.arakhne.afc.math.geometry3d.Tuple3D; + +/** This class implements a Point3i that cannot be modified by + * the setters. + * + * @author $Author: galland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class UnmodifiablePoint3i extends Point3i { + + private static final long serialVersionUID = -2749011435787339613L; + + /** + */ + public UnmodifiablePoint3i() { + super(); + } + + /** + * @param x + * @param y + * @param z + */ + public UnmodifiablePoint3i(float x, float y, float z) { + super(x, y, z); + } + + /** + * {@inheritDoc} + */ + @Override + public UnmodifiablePoint3i clone() { + return (UnmodifiablePoint3i)super.clone(); + } + + @Override + public void set(float x, float y, float z) { + // + } + + @Override + public void set(float[] t) { + // + } + + @Override + public void set(int x, int y, int z) { + // + } + + @Override + public void set(int[] t) { + // + } + + @Override + public void set(Tuple3D 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) { + // + } + + @Override + public void setZ(float z) { + // + } + + @Override + public void setZ(int z) { + // + } + +} diff --git a/core/math/src/main/java/org/arakhne/afc/math/geometry3d/discrete/Vector3i.java b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/discrete/Vector3i.java new file mode 100644 index 000000000..056a54f09 --- /dev/null +++ b/core/math/src/main/java/org/arakhne/afc/math/geometry3d/discrete/Vector3i.java @@ -0,0 +1,283 @@ +/* + * $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.geometry3d.discrete; + +import org.arakhne.afc.math.geometry3d.Point3D; +import org.arakhne.afc.math.geometry3d.Tuple3D; +import org.arakhne.afc.math.geometry3d.Vector3D; +import org.arakhne.afc.math.geometry3d.continuous.Quaternion; +import org.arakhne.afc.math.geometry3d.continuous.Transform3D; +import org.arakhne.afc.math.geometry3d.continuous.Vector3f; + +/** 3D Vector with 3 integers. + * + * @author $Author: galland$ + * @version $FullVersion$ + * @mavengroupid $GroupId$ + * @mavenartifactid $ArtifactId$ + */ +public class Vector3i extends Tuple3i implements Vector3D { + + private static final long serialVersionUID = 1997599488590527335L; + + /** + */ + public Vector3i() { + // + } + + /** + * @param tuple is the tuple to copy. + */ + public Vector3i(Tuple3D tuple) { + super(tuple); + } + + /** + * @param tuple is the tuple to copy. + */ + public Vector3i(int[] tuple) { + super(tuple); + } + + /** + * @param tuple is the tuple to copy. + */ + public Vector3i(float[] tuple) { + super(tuple); + } + + /** + * @param x + * @param y + * @param z + */ + public Vector3i(int x, int y, int z) { + super(x,y,z); + } + + /** + * @param x + * @param y + * @param z + */ + public Vector3i(float x, float y, float z) { + super(x,y,z); + } + + /** + * @param x + * @param y + * @param z + */ + public Vector3i(double x, double y, double z) { + super((float)x,(float)y,(float)z); + } + + /** + * @param x + * @param y + * @param z + */ + public Vector3i(long x, long y, long z) { + super(x,y,z); + } + + /** {@inheritDoc} + */ + @Override + public Vector3i clone() { + return (Vector3i)super.clone(); + } + + /** + * {@inheritDoc} + */ + @Override + public float angle(Vector3D 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(Vector3D v1) { + return (this.x*v1.getX() + this.y*v1.getY() + this.z*v1.getZ()); + } + + /** + * {@inheritDoc} + */ + @Override + public float length() { + return (float) Math.sqrt(this.x*this.x + this.y*this.y + this.z*this.z); + } + + /** + * {@inheritDoc} + */ + @Override + public float lengthSquared() { + return (this.x*this.x + this.y*this.y + this.z*this.z); + } + + /** + * {@inheritDoc} + */ + @Override + public void normalize(Vector3D v1) { + float norm; + norm = (float) (1./Math.sqrt(v1.getX()*v1.getX() + v1.getY()*v1.getY() + v1.getZ()*v1.getZ())); + this.x = (int)(v1.getX()*norm); + this.y = (int)(v1.getY()*norm); + this.z = (int)(v1.getZ()*norm); + } + + /** + * {@inheritDoc} + */ + @Override + public void normalize() { + float norm; + norm = (float)(1./Math.sqrt(this.x*this.x + this.y*this.y + this.z*this.z)); + this.x *= norm; + this.y *= norm; + this.z *= norm; + } + + @Override + public void add(Vector3D t1, Vector3D t2) { + this.x = (int)(t1.getX() + t2.getX()); + this.y = (int)(t1.getY() + t2.getY()); + this.z = (int)(t1.getZ() + t2.getZ()); + } + + @Override + public void add(Vector3D t1) { + this.x = (int)(this.x + t1.getX()); + this.y = (int)(this.y + t1.getY()); + this.z = (int)(this.z + t1.getZ()); + } + + @Override + public void scaleAdd(int s, Vector3D t1, Vector3D t2) { + this.x = (int)(s * t1.getX() + t2.getX()); + this.y = (int)(s * t1.getY() + t2.getY()); + this.z = (int)(s * t1.getZ() + t2.getZ()); + } + + @Override + public void scaleAdd(float s, Vector3D t1, Vector3D t2) { + this.x = (int)(s * t1.getX() + t2.getX()); + this.y = (int)(s * t1.getY() + t2.getY()); + this.z = (int)(s * t1.getZ() + t2.getZ()); + } + + @Override + public void scaleAdd(int s, Vector3D t1) { + this.x = (int)(s * this.x + t1.getX()); + this.y = (int)(s * this.y + t1.getY()); + this.z = (int)(s * this.z + t1.getZ()); + } + + @Override + public void scaleAdd(float s, Vector3D t1) { + this.x = (int)(s * this.x + t1.getX()); + this.y = (int)(s * this.y + t1.getY()); + this.z = (int)(s * this.z + t1.getZ()); + } + + @Override + public void sub(Vector3D t1, Vector3D t2) { + this.x = (int)(t1.getX() - t2.getX()); + this.y = (int)(t1.getY() - t2.getY()); + this.z = (int)(t1.getZ() - t2.getZ()); + } + + @Override + public void sub(Point3D t1, Point3D t2) { + this.x = (int)(t1.getX() - t2.getX()); + this.y = (int)(t1.getY() - t2.getY()); + this.z = (int)(t1.getZ() - t2.getZ()); + } + + @Override + public void sub(Vector3D t1) { + this.x = (int)(this.x - t1.getX()); + this.y = (int)(this.y - t1.getY()); + this.z = (int)(this.z - t1.getZ()); + } + + @Override + public Vector3D cross(Vector3D v1) { + return crossLeftHand(v1); + } + + @Override + public void cross(Vector3D v1, Vector3D v2) { + crossLeftHand(v1, v2); + } + + @Override + public Vector3D crossLeftHand(Vector3D v1) { + float x = v1.getY()*getZ() - v1.getZ()*getY(); + float y = v1.getZ()*getX() - v1.getX()*getZ(); + float z = v1.getX()*getY() - v1.getY()*getX(); + return new Vector3i(x,y,z); + } + + @Override + public void crossLeftHand(Vector3D v1, Vector3D v2) { + float x = v2.getY()*v1.getZ() - v2.getZ()*v1.getY(); + float y = v2.getZ()*v1.getX() - v2.getX()*v1.getZ(); + float z = v2.getX()*v1.getY() - v2.getY()*v1.getX(); + set(x,y,z); + } + + @Override + public Vector3D crossRightHand(Vector3D v1) { + float x = getY()*v1.getZ() - getZ()*v1.getY(); + float y = getZ()*v1.getX() - getX()*v1.getZ(); + float z = getX()*v1.getY() - getY()*v1.getX(); + return new Vector3f(x,y,z); + } + + @Override + public void crossRightHand(Vector3D v1, Vector3D v2) { + float x = v1.getY()*v2.getZ() - v1.getZ()*v2.getY(); + float y = v1.getZ()*v2.getX() - v1.getX()*v2.getZ(); + float z = v1.getX()*v2.getY() - v1.getY()*v2.getX(); + set(x,y,z); + } + + @Override + public void turnVector(Vector3D axis, float angle) { + Transform3D mat = new Transform3D(); + mat.setRotation(new Quaternion(axis, angle)); + mat.transform(this); + } + +} \ No newline at end of file