From a70a8d489528d1d29946b6e1525e153bf2858e32 Mon Sep 17 00:00:00 2001 From: Robert Griffin Date: Sun, 13 Oct 2024 22:50:52 -0500 Subject: [PATCH] started moving a bunch of packages to allow extracting the whole-body control core significantly overhauled and set up custom pid parameters for the whole body controller core moved a lot of the filtered yo variables into a common project that can be moved to commons moved a bunch of the polynomial classes out after getting things solidified, but need to fix the linear systems tests cleaned up a lot more of the yo variable types and moved their packages got more code moved and most of the linear algebra tests fixed removed unused code set up wbcc for extraction started refactoring some of the code to prep for the commons move set up the euclid update repo, including javadoc and pulling over the tests moved the mecano packages around set up a robotics tools project to become its own repo set up the ihmc math library more properly, and worked to add some javadoc on some of the core classes did a bunch of testing and javadoc, and cleaned up polynomial3D updated teh location of all the code finished the api tests for the euclid frame tools to account for the new geometry tools, and fixed a bug in porting the pose reference frame test --- .../ihmc/robotics/EuclidCoreMissingTools.java | 1530 ----------------- .../robotics/EuclidGeometryMissingTools.java | 118 -- .../EuclidGeometryPolygonMissingTools.java | 46 - .../geometry/shapes/FramePlane3d.java | 313 ---- .../robotics/math/QuaternionCalculus.java | 243 --- .../referenceFrames/Pose2dReferenceFrame.java | 80 - .../referenceFrames/PoseReferenceFrame.java | 274 --- .../TranslationReferenceFrame.java | 38 - .../euclid/referenceFrame/FramePlane3D.java | 175 ++ .../interfaces/FixedFramePlane3DBasics.java | 103 ++ .../interfaces/FramePlane3DBasics.java | 147 ++ .../interfaces/FramePlane3DReadOnly.java | 300 ++++ .../tools/EuclidFrameShapeTools.java | 280 ++- .../referenceFrame/Pose2DReferenceFrame.java | 183 ++ .../referenceFrame/PoseReferenceFrame.java | 335 ++++ .../TranslationReferenceFrame.java | 98 ++ .../tools/EuclidFrameFactories.java | 7 +- .../tools/EuclidFrameTools.java | 503 +++++- .../tools/EuclidGeometryPolygonTools.java | 3 + .../geometry/tools/EuclidGeometryTools.java | 1438 +++++++++++++--- .../us/ihmc/euclid/QuaternionCalculus.java | 500 ++++++ .../us/ihmc/euclid/tools/EuclidCoreTools.java | 337 +++- .../ihmc/euclid/QuaternionCalculusTest.java | 331 ++++ .../geometry/tools/ConvexPolygon2DTest.java | 35 + .../tools/EuclidGeometryToolsTest.java | 106 ++ .../referenceFrame/FramePlane3DTest.java | 120 ++ .../Pose2DReferenceFrameTest.java | 164 ++ .../PoseReferenceFrameTest.java | 164 ++ .../TranslationReferenceFrameTest.java | 103 ++ .../tools/EuclidFrameShapeToolsTest.java | 193 +++ .../tools/EuclidFrameToolsTest.java | 7 + .../euclid/tools/EuclidCoreToolsTest.java | 457 +++++ 32 files changed, 5816 insertions(+), 2915 deletions(-) delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/EuclidCoreMissingTools.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/EuclidGeometryMissingTools.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/EuclidGeometryPolygonMissingTools.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/geometry/shapes/FramePlane3d.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/QuaternionCalculus.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/referenceFrames/Pose2dReferenceFrame.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/referenceFrames/PoseReferenceFrame.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/referenceFrames/TranslationReferenceFrame.java create mode 100644 src/frame-shape/java/us/ihmc/euclid/referenceFrame/FramePlane3D.java create mode 100644 src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FixedFramePlane3DBasics.java create mode 100644 src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FramePlane3DBasics.java create mode 100644 src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FramePlane3DReadOnly.java create mode 100644 src/frame/java/us/ihmc/euclid/referenceFrame/Pose2DReferenceFrame.java create mode 100644 src/frame/java/us/ihmc/euclid/referenceFrame/PoseReferenceFrame.java create mode 100644 src/frame/java/us/ihmc/euclid/referenceFrame/TranslationReferenceFrame.java create mode 100644 src/main/java/us/ihmc/euclid/QuaternionCalculus.java create mode 100644 src/test/java/us/ihmc/euclid/QuaternionCalculusTest.java create mode 100644 src/test/java/us/ihmc/euclid/geometry/tools/ConvexPolygon2DTest.java create mode 100644 src/test/java/us/ihmc/euclid/referenceFrame/FramePlane3DTest.java create mode 100644 src/test/java/us/ihmc/euclid/referenceFrame/Pose2DReferenceFrameTest.java create mode 100644 src/test/java/us/ihmc/euclid/referenceFrame/PoseReferenceFrameTest.java create mode 100644 src/test/java/us/ihmc/euclid/referenceFrame/TranslationReferenceFrameTest.java diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/EuclidCoreMissingTools.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/EuclidCoreMissingTools.java deleted file mode 100644 index 0505d8ff3..000000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/EuclidCoreMissingTools.java +++ /dev/null @@ -1,1530 +0,0 @@ -package us.ihmc.robotics; - -import static us.ihmc.euclid.geometry.tools.EuclidGeometryTools.ONE_MILLIONTH; -import static us.ihmc.euclid.geometry.tools.EuclidGeometryTools.ONE_TRILLIONTH; -import static us.ihmc.euclid.tools.EuclidCoreRandomTools.nextDouble; -import static us.ihmc.euclid.tools.EuclidCoreRandomTools.nextMatrix3D; -import static us.ihmc.euclid.tools.EuclidCoreTools.normSquared; - -import java.lang.reflect.Field; -import java.util.Random; - -import org.ejml.data.DMatrixRMaj; - -import us.ihmc.commons.MathTools; -import us.ihmc.euclid.Axis3D; -import us.ihmc.euclid.geometry.tools.EuclidGeometryTools; -import us.ihmc.euclid.matrix.Matrix3D; -import us.ihmc.euclid.matrix.RotationMatrix; -import us.ihmc.euclid.matrix.interfaces.CommonMatrix3DBasics; -import us.ihmc.euclid.matrix.interfaces.Matrix3DReadOnly; -import us.ihmc.euclid.orientation.interfaces.Orientation3DBasics; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.exceptions.ReferenceFrameMismatchException; -import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameTuple3DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.ReferenceFrameHolder; -import us.ihmc.euclid.referenceFrame.tools.EuclidFrameFactories; -import us.ihmc.euclid.tools.EuclidCoreIOTools; -import us.ihmc.euclid.tools.EuclidCoreTools; -import us.ihmc.euclid.tools.TupleTools; -import us.ihmc.euclid.transform.interfaces.RigidBodyTransformReadOnly; -import us.ihmc.euclid.tuple2D.Point2D; -import us.ihmc.euclid.tuple2D.interfaces.Point2DBasics; -import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly; -import us.ihmc.euclid.tuple2D.interfaces.Tuple2DBasics; -import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Point3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Tuple3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.UnitVector3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; -import us.ihmc.euclid.tuple4D.interfaces.QuaternionBasics; -import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; -public class EuclidCoreMissingTools -{ - public static final String DEGREE_SYMBOL = "\u00B0"; - - public static void transform(Matrix3DReadOnly matrix, double xOriginal, double yOriginal, double zOriginal, Tuple3DBasics tupleTransformed) - { - double x = matrix.getM00() * xOriginal + matrix.getM01() * yOriginal + matrix.getM02() * zOriginal; - double y = matrix.getM10() * xOriginal + matrix.getM11() * yOriginal + matrix.getM12() * zOriginal; - double z = matrix.getM20() * xOriginal + matrix.getM21() * yOriginal + matrix.getM22() * zOriginal; - tupleTransformed.set(x, y, z); - } - - public static void floorToGivenPrecision(Tuple3DBasics tuple3d, double precision) - { - tuple3d.setX(MathTools.floorToPrecision(tuple3d.getX(), precision)); - tuple3d.setY(MathTools.floorToPrecision(tuple3d.getY(), precision)); - tuple3d.setZ(MathTools.floorToPrecision(tuple3d.getZ(), precision)); - } - - public static void roundToGivenPrecision(Tuple3DBasics tuple3d, double precision) - { - tuple3d.setX(MathTools.roundToPrecision(tuple3d.getX(), precision)); - tuple3d.setY(MathTools.roundToPrecision(tuple3d.getY(), precision)); - tuple3d.setZ(MathTools.roundToPrecision(tuple3d.getZ(), precision)); - } - - public static void floorToGivenPrecision(Tuple2DBasics tuple2d, double precision) - { - tuple2d.setX(MathTools.floorToPrecision(tuple2d.getX(), precision)); - tuple2d.setY(MathTools.floorToPrecision(tuple2d.getY(), precision)); - } - - public static void roundToGivenPrecision(Tuple2DBasics tuple2d, double precision) - { - tuple2d.setX(MathTools.roundToPrecision(tuple2d.getX(), precision)); - tuple2d.setY(MathTools.roundToPrecision(tuple2d.getY(), precision)); - } - - public static boolean isFinite(Tuple3DBasics tuple) - { - return Double.isFinite(tuple.getX()) && Double.isFinite(tuple.getY()) && Double.isFinite(tuple.getZ()); - } - - /** - * Tests if the point 2D is located on the infinitely long line 2D. - *

- * The test is performed by computing the distance between the point and the line, if that distance - * is below {@link EuclidGeometryTools#IS_POINT_ON_LINE_EPS} this method returns {@code true}. - *

- * - * @param lineSegmentStart the first endpoint of the line segment. Not modified. - * @param lineSegmentEnd the second endpoint of the line segment. Not modified. - * @return {@code true} if the query is considered to be lying on the line, {@code false} otherwise. - */ - public static boolean isPoint2DOnLineSegment2D(double pointX, double pointY, Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd) - { - return EuclidGeometryTools.distanceSquaredFromPoint2DToLineSegment2D(pointX, - pointY, - lineSegmentStart.getX(), - lineSegmentStart.getY(), - lineSegmentEnd.getX(), - lineSegmentEnd.getY()) < EuclidGeometryTools.IS_POINT_ON_LINE_EPS; - } - - /** - * Projects the provided {@code rotation} onto {@code axis} such that the original rotation can be - * decomposed into a rotation around {@code axis} and one around an orthogonal axis. - *

- * rotation = orthogonalRotation * result - *

- * - * @param rotation is the original rotation to be projected onto {@code axis} - * @param axis is the desired rotation axis of the result. - * @param result will be modified to contain the component of {@code rotation} that is around - * {@code axis} - */ - public static void projectRotationOnAxis(QuaternionReadOnly rotation, Vector3DReadOnly axis, QuaternionBasics result) - { - double dotProduct = rotation.getX() * axis.getX() + rotation.getY() * axis.getY() + rotation.getZ() * axis.getZ(); - - double scale = dotProduct / axis.normSquared(); - double projectedX = scale * axis.getX(); - double projectedY = scale * axis.getY(); - double projectedZ = scale * axis.getZ(); - - result.set(projectedX, projectedY, projectedZ, rotation.getS()); - result.normalize(); - } - - public static boolean intersectionBetweenTwoLine2Ds(Point2DReadOnly firstPointOnLine1, - Point2DReadOnly secondPointOnLine1, - Point2DReadOnly firstPointOnLine2, - Point2DReadOnly secondPointOnLine2, - Point2DBasics intersectionToPack) - { - double pointOnLine1x = firstPointOnLine1.getX(); - double pointOnLine1y = firstPointOnLine1.getY(); - double lineDirection1x = secondPointOnLine1.getX() - firstPointOnLine1.getX(); - double lineDirection1y = secondPointOnLine1.getY() - firstPointOnLine1.getY(); - double pointOnLine2x = firstPointOnLine2.getX(); - double pointOnLine2y = firstPointOnLine2.getY(); - double lineDirection2x = secondPointOnLine2.getX() - firstPointOnLine2.getX(); - double lineDirection2y = secondPointOnLine2.getY() - firstPointOnLine2.getY(); - return EuclidGeometryTools.intersectionBetweenTwoLine2Ds(pointOnLine1x, - pointOnLine1y, - lineDirection1x, - lineDirection1y, - pointOnLine2x, - pointOnLine2y, - lineDirection2x, - lineDirection2y, - intersectionToPack); - } - - /** - * Computes the intersection between two infinitely long 2D lines each defined by a 2D point and a - * 2D direction and returns a percentage {@code alpha} along the first line such that the - * intersection coordinates can be computed as follows:
- * {@code intersection = pointOnLine1 + alpha * lineDirection1} - *

- * Edge cases: - *

- *

- * - * @param startPoint1x x-coordinate of a point located on the first line. - * @param startPoint1y y-coordinate of a point located on the first line. - * @param segmentTravel1x x-component of the first line direction. - * @param segmentTravel1y y-component of the first line direction. - * @param startPoint2x x-coordinate of a point located on the second line. - * @param startPoint2y y-coordinate of a point located on the second line. - * @param segmentTravel2x x-component of the second line direction. - * @param segmentTravel2y y-component of the second line direction. - * @return {@code alpha} the percentage along the first line of the intersection location. This - * method returns {@link Double#NaN} if the lines do not intersect. - */ - public static double percentageOfIntersectionBetweenTwoLine2DsInfCase(double startPoint1x, - double startPoint1y, - double segmentTravel1x, - double segmentTravel1y, - double startPoint2x, - double startPoint2y, - double segmentTravel2x, - double segmentTravel2y) - { - // We solve for x the problem of the form: A * x = b - // A * x = b - // / segmentTravel1x -segmentTravel2x \ / alpha \ / startPoint2x - startPoint1x \ - // | | * | | = | | - // \ segmentTravel1y -segmentTravel2y / \ beta / \ startPoint2y - startPoint1y / - // Here, only alpha or beta is needed. - - double determinant = -segmentTravel1x * segmentTravel2y + segmentTravel1y * segmentTravel2x; - - double dx = startPoint2x - startPoint1x; - double dy = startPoint2y - startPoint1y; - - if (Math.abs(determinant) < EuclidGeometryTools.ONE_TRILLIONTH) - { // The lines are parallel - // Check if they are collinear - double cross = dx * segmentTravel1y - dy * segmentTravel1x; - if (Math.abs(cross) < EuclidGeometryTools.ONE_TRILLIONTH) - { - /* - * The two lines are collinear. There's an infinite number of intersection. Let's set the result to - * infinity, i.e. alpha = infinity so it can be handled. - */ - return Double.POSITIVE_INFINITY; - } - else - { - return Double.NaN; - } - } - else - { - double oneOverDeterminant = 1.0 / determinant; - double AInverse00 = oneOverDeterminant * -segmentTravel2y; - double AInverse01 = oneOverDeterminant * segmentTravel2x; - - double alpha = AInverse00 * dx + AInverse01 * dy; - - return alpha; - } - } - - /** - * This method implements the same operation as - * {@link EuclidGeometryTools#orientation3DFromZUpToVector3D(Vector3DReadOnly, Orientation3DBasics)} - * except that it does not rely on {@code Math#acos(double)} making it faster. - * - * @param vector the vector that is rotated with respect to {@code zUp}. Not modified. - * @param rotationToPack the minimum rotation from {@code zUp} to the given {@code vector}. - * Modified. - * @see EuclidGeometryTools#orientation3DFromZUpToVector3D(Vector3DReadOnly, Orientation3DBasics) - */ - public static void rotationMatrix3DFromZUpToVector3D(Vector3DReadOnly vector, CommonMatrix3DBasics rotationToPack) - { - rotationMatrix3DFromFirstToSecondVector3D(Axis3D.Z, vector, rotationToPack); - } - - /** - * This method implements the same operation as - * {@link EuclidGeometryTools#orientation3DFromFirstToSecondVector3D(Vector3DReadOnly, Vector3DReadOnly, Orientation3DBasics)} - * except that it does not rely on {@code Math#acos(double)} making it faster. - * - * @param firstVector the first vector. Not modified. - * @param secondVector the second vector that is rotated with respect to the first vector. Not - * modified. - * @param rotationToPack the minimum rotation from {@code firstVector} to the {@code secondVector}. - * Modified. - * @see EuclidGeometryTools#orientation3DFromFirstToSecondVector3D(Vector3DReadOnly, - * Vector3DReadOnly, Orientation3DBasics) - */ - public static void rotationMatrix3DFromFirstToSecondVector3D(Vector3DReadOnly firstVector, - Vector3DReadOnly secondVector, - CommonMatrix3DBasics rotationToPack) - { - rotationMatrix3DFromFirstToSecondVector3D(firstVector.getX(), - firstVector.getY(), - firstVector.getZ(), - secondVector.getX(), - secondVector.getY(), - secondVector.getZ(), - rotationToPack); - } - - /** - * This method implements the same operation as - * {@link EuclidGeometryTools#orientation3DFromFirstToSecondVector3D(double, double, double, double, double, double, Orientation3DBasics)} - * except that it does not rely on {@code Math#acos(double)} making it faster. - * - * @param firstVectorX x-component of the first vector. - * @param firstVectorY y-component of the first vector. - * @param firstVectorZ z-component of the first vector. - * @param secondVectorX x-component of the second vector that is rotated with respect to the first - * vector. - * @param secondVectorY y-component of the second vector that is rotated with respect to the first - * vector. - * @param secondVectorZ z-component of the second vector that is rotated with respect to the first - * vector. - * @param rotationToPack the minimum rotation from {@code firstVector} to the {@code secondVector}. - * Modified. - * @see EuclidGeometryTools#orientation3DFromFirstToSecondVector3D(double, double, double, double, - * double, double, Orientation3DBasics) - */ - public static void rotationMatrix3DFromFirstToSecondVector3D(double firstVectorX, - double firstVectorY, - double firstVectorZ, - double secondVectorX, - double secondVectorY, - double secondVectorZ, - CommonMatrix3DBasics rotationToPack) - { - double firstVectorLengthInv = 1.0 / Math.sqrt(normSquared(firstVectorX, firstVectorY, firstVectorZ)); - double secondVectorLengthInv = 1.0 / Math.sqrt(normSquared(secondVectorX, secondVectorY, secondVectorZ)); - firstVectorX *= firstVectorLengthInv; - firstVectorY *= firstVectorLengthInv; - firstVectorZ *= firstVectorLengthInv; - secondVectorX *= secondVectorLengthInv; - secondVectorY *= secondVectorLengthInv; - secondVectorZ *= secondVectorLengthInv; - - double rotationAxisX = firstVectorY * secondVectorZ - firstVectorZ * secondVectorY; - double rotationAxisY = firstVectorZ * secondVectorX - firstVectorX * secondVectorZ; - double rotationAxisZ = firstVectorX * secondVectorY - firstVectorY * secondVectorX; - double sinAngle = Math.sqrt(normSquared(rotationAxisX, rotationAxisY, rotationAxisZ)); - - boolean normalsAreParallel = sinAngle < EuclidGeometryTools.ONE_TEN_MILLIONTH; - - if (normalsAreParallel) - { - rotationToPack.setIdentity(); - return; - } - - rotationAxisX /= sinAngle; - rotationAxisY /= sinAngle; - rotationAxisZ /= sinAngle; - - double cosAngle = firstVectorX * secondVectorX + firstVectorY * secondVectorY + firstVectorZ * secondVectorZ; - - if (cosAngle > 1.0) - cosAngle = 1.0; - else if (cosAngle < -1.0) - cosAngle = -1.0; - - double t = 1.0 - cosAngle; - - double xz = rotationAxisX * rotationAxisZ; - double xy = rotationAxisX * rotationAxisY; - double yz = rotationAxisY * rotationAxisZ; - - double m00 = t * rotationAxisX * rotationAxisX + cosAngle; - double m01 = t * xy - sinAngle * rotationAxisZ; - double m02 = t * xz + sinAngle * rotationAxisY; - double m10 = t * xy + sinAngle * rotationAxisZ; - double m11 = t * rotationAxisY * rotationAxisY + cosAngle; - double m12 = t * yz - sinAngle * rotationAxisX; - double m20 = t * xz - sinAngle * rotationAxisY; - double m21 = t * yz + sinAngle * rotationAxisX; - double m22 = t * rotationAxisZ * rotationAxisZ + cosAngle; - - if (rotationToPack instanceof RotationMatrix) - ((RotationMatrix) rotationToPack).setUnsafe(m00, m01, m02, m10, m11, m12, m20, m21, m22); - else - rotationToPack.set(m00, m01, m02, m10, m11, m12, m20, m21, m22); - } -} - - /** - * Given two 2D line segments with finite length, this methods computes two points P ∈ - * lineSegment1 and Q ∈ lineSegment2 such that the distance || P - Q || is the minimum distance - * between the two 2D line segments. Useful - * link. - * - * @param lineSegmentStart1 the first endpoint of the first line segment. Not - * modified. - * @param lineSegmentEnd1 the second endpoint of the first line segment. Not - * modified. - * @param lineSegmentStart2 the first endpoint of the second line segment. Not - * modified. - * @param lineSegmentEnd2 the second endpoint of the second line segment. Not - * modified. - * @param closestPointOnLineSegment1ToPack the 2D coordinates of the point P are packed in this 2D - * point. Modified. Can be {@code null}. - * @param closestPointOnLineSegment2ToPack the 2D coordinates of the point Q are packed in this 2D - * point. Modified. Can be {@code null}. - * @return the minimum distance between the two line segments. - */ - public static double closestPoint2DsBetweenTwoLineSegment2Ds(Point2DReadOnly lineSegmentStart1, - Point2DReadOnly lineSegmentEnd1, - Point2DReadOnly lineSegmentStart2, - Point2DReadOnly lineSegmentEnd2, - Point2DBasics closestPointOnLineSegment1ToPack, - Point2DBasics closestPointOnLineSegment2ToPack) - { - // Switching to the notation used in http://geomalgorithms.com/a07-_distance.html. - // The line1 is defined by (P0, u) and the line2 by (Q0, v). - Point2DReadOnly P0 = lineSegmentStart1; - double ux = lineSegmentEnd1.getX() - lineSegmentStart1.getX(); - double uy = lineSegmentEnd1.getY() - lineSegmentStart1.getY(); - Point2DReadOnly Q0 = lineSegmentStart2; - double vx = lineSegmentEnd2.getX() - lineSegmentStart2.getX(); - double vy = lineSegmentEnd2.getY() - lineSegmentStart2.getY(); - - Point2DBasics Psc = closestPointOnLineSegment1ToPack; - Point2DBasics Qtc = closestPointOnLineSegment2ToPack; - - double w0X = P0.getX() - Q0.getX(); - double w0Y = P0.getY() - Q0.getY(); - - double a = ux * ux + uy * uy; - double b = ux * vx + uy * vy; - double c = vx * vx + vy * vy; - double d = ux * w0X + uy * w0Y; - double e = vx * w0X + vy * w0Y; - - double delta = a * c - b * b; - - double sc, sNumerator, sDenominator = delta; - double tc, tNumerator, tDenominator = delta; - - // check to see if the lines are parallel - if (delta <= ONE_MILLIONTH) - { - /* - * The lines are parallel, there's an infinite number of pairs, but for one chosen point on one of - * the lines, there's only one closest point to it on the other line. So let's choose arbitrarily a - * point on the lineSegment1 and calculate the point that is closest to it on the lineSegment2. - */ - sNumerator = 0.0; - sDenominator = 1.0; - tNumerator = e; - tDenominator = c; - } - else - { - sNumerator = b * e - c * d; - tNumerator = a * e - b * d; - - if (sNumerator < 0.0) - { - sNumerator = 0.0; - tNumerator = e; - tDenominator = c; - } - else if (sNumerator > sDenominator) - { - sNumerator = sDenominator; - tNumerator = e + b; - tDenominator = c; - } - } - - if (tNumerator < 0.0) - { - tNumerator = 0.0; - sNumerator = -d; - if (sNumerator < 0.0) - sNumerator = 0.0; - else if (sNumerator > a) - sNumerator = a; - sDenominator = a; - } - else if (tNumerator > tDenominator) - { - tNumerator = tDenominator; - sNumerator = -d + b; - if (sNumerator < 0.0) - sNumerator = 0.0; - else if (sNumerator > a) - sNumerator = a; - sDenominator = a; - } - - sc = Math.abs(sNumerator) < ONE_MILLIONTH ? 0.0 : sNumerator / sDenominator; - tc = Math.abs(tNumerator) < ONE_MILLIONTH ? 0.0 : tNumerator / tDenominator; - - double PscX = sc * ux + P0.getX(); - double PscY = sc * uy + P0.getY(); - - double QtcX = tc * vx + Q0.getX(); - double QtcY = tc * vy + Q0.getY(); - - if (Psc != null) - Psc.set(PscX, PscY); - if (Qtc != null) - Qtc.set(QtcX, QtcY); - - double dx = PscX - QtcX; - double dy = PscY - QtcY; - return Math.sqrt(normSquared(dx, dy)); - } - - /** - * This methods computes the minimum distance between the two 2D line segments with finite length. - * Useful link. - * - * @param lineSegmentStart1 the first endpoint of the first line segment. Not modified. - * @param lineSegmentEnd1 the second endpoint of the first line segment. Not modified. - * @param lineSegmentStart2 the first endpoint of the second line segment. Not modified. - * @param lineSegmentEnd2 the second endpoint of the second line segment. Not modified. - * @return the minimum distance between the two line segments. - */ - public static double distanceBetweenTwoLineSegment2Ds(Point2DReadOnly lineSegmentStart1, - Point2DReadOnly lineSegmentEnd1, - Point2DReadOnly lineSegmentStart2, - Point2DReadOnly lineSegmentEnd2) - { - return closestPoint2DsBetweenTwoLineSegment2Ds(lineSegmentStart1, lineSegmentEnd1, lineSegmentStart2, lineSegmentEnd2, null, null); - } - - public static double distanceSquaredFromPoint2DToLineSegment2D(double pointX, - double pointY, - double lineSegmentStartX, - double lineSegmentStartY, - double lineSegmentEndX, - double lineSegmentEndY, - Point2D intersectionPointToPack) - { - double percentage = EuclidGeometryTools.percentageAlongLineSegment2D(pointX, - pointY, - lineSegmentStartX, - lineSegmentStartY, - lineSegmentEndX, - lineSegmentEndY); - - if (percentage > 1.0) - percentage = 1.0; - else if (percentage < 0.0) - percentage = 0.0; - - double projectionX = (1.0 - percentage) * lineSegmentStartX + percentage * lineSegmentEndX; - double projectionY = (1.0 - percentage) * lineSegmentStartY + percentage * lineSegmentEndY; - - if (intersectionPointToPack != null) - intersectionPointToPack.set(projectionX, projectionY); - - double dx = projectionX - pointX; - double dy = projectionY - pointY; - return dx * dx + dy * dy; - } - - public static boolean epsilonEquals(RigidBodyTransformReadOnly a, RigidBodyTransformReadOnly b, double rotationEpsilon, double translationEpsilon) - { - return a.getRotation().geometricallyEquals(b.getRotation(), rotationEpsilon) - && a.getTranslation().geometricallyEquals(b.getTranslation(), translationEpsilon); - } - - /** - * Calculates the 3D part of the given {@code input} that is parallel to the given - * {@code normalAxis} and stores the result in {@code inputNormalPartToPack}. - *

- * The result has the following properties: - *

- * where: - * - *

- * - * @param input the tuple to extract the normal part of. Not modified. - * @param normalAxis the normal vector. It is normalized internally if needed. Not - * modified. - * @param inputNormalPartToPack the tuple used to store the normal part of the input. Modified. - */ - public static void extractNormalPart(Tuple3DReadOnly input, Vector3DReadOnly normalAxis, Tuple3DBasics inputNormalPartToPack) - { - double normalX = normalAxis.getX(); - double normalY = normalAxis.getY(); - double normalZ = normalAxis.getZ(); - double normalLengthSquared = EuclidCoreTools.normSquared(normalX, normalY, normalZ); - - double dot = TupleTools.dot(normalAxis, input) / normalLengthSquared; - inputNormalPartToPack.setAndScale(dot, normalAxis); - } - - /** - * Calculates the 3D part of the given {@code input} that is parallel to the given - * {@code normalAxis} and stores the result in {@code inputNormalPartToPack}. - *

- * The result has the following properties: - *

- * where: - * - *

- * - * @param input the tuple to extract the normal part of. Not modified. - * @param normalAxis the normal vector. It is normalized internally if needed. Not - * modified. - * @param inputNormalPartToPack the tuple used to store the normal part of the input. Modified. - * @throws ReferenceFrameMismatchException if the arguments are not expressed in the same reference - * frame. - */ - public static void extractNormalPart(FrameTuple3DReadOnly input, FrameVector3DReadOnly normalAxis, FixedFrameTuple3DBasics inputNormalPartToPack) - { - input.checkReferenceFrameMatch(normalAxis, inputNormalPartToPack); - extractNormalPart((Tuple3DReadOnly) input, (Vector3DReadOnly) normalAxis, (Tuple3DBasics) inputNormalPartToPack); - } - - /** - * Calculates the 3D part of the given {@code input} that is parallel to the given - * {@code normalAxis} and stores the result in {@code inputNormalPartToPack}. - *

- * The result has the following properties: - *

- * where: - * - *

- * - * @param input the tuple to extract the normal part of. Not modified. - * @param normalAxis the normal vector. It is normalized internally if needed. Not - * modified. - * @param inputNormalPartToPack the tuple used to store the normal part of the input. Modified. - * @throws ReferenceFrameMismatchException if {@code input} and {@code normalAxis} are not expressed - * in the same reference frame. - */ - public static void extractNormalPart(FrameTuple3DReadOnly input, FrameVector3DReadOnly normalAxis, FrameTuple3DBasics inputNormalPartToPack) - { - input.checkReferenceFrameMatch(normalAxis); - inputNormalPartToPack.setReferenceFrame(input.getReferenceFrame()); - extractNormalPart((Tuple3DReadOnly) input, (Vector3DReadOnly) normalAxis, (Tuple3DBasics) inputNormalPartToPack); - } - - /** - * Calculates the 3D part of the given {@code input} that is orthogonal to the given - * {@code normalAxis} and stores the result in {@code inputTangentialPartToPack}. - *

- * The result has the following properties: - *

- * where: - * - *

- * - * @param input the tuple to extract the tangential part of. Not modified. - * @param normalAxis the normal vector. It is normalized internally if needed. Not - * modified. - * @param inputTangentialPartToPack the tuple used to store the tangential part of the input. - * Modified. - */ - public static void extractTangentialPart(Tuple3DReadOnly input, Vector3DReadOnly normalAxis, Tuple3DBasics inputTagentialPartToPack) - { - double normalX = normalAxis.getX(); - double normalY = normalAxis.getY(); - double normalZ = normalAxis.getZ(); - double normalLengthSquared = EuclidCoreTools.normSquared(normalX, normalY, normalZ); - - double dot = TupleTools.dot(normalX, normalY, normalZ, input) / normalLengthSquared; - double normalPartX = dot * normalX; - double normalPartY = dot * normalY; - double normalPartZ = dot * normalZ; - - inputTagentialPartToPack.set(input); - inputTagentialPartToPack.sub(normalPartX, normalPartY, normalPartZ); - } - - /** - * Calculates the 3D part of the given {@code input} that is orthogonal to the given - * {@code normalAxis} and stores the result in {@code inputTangentialPartToPack}. - *

- * The result has the following properties: - *

- * where: - * - *

- * - * @param input the tuple to extract the tangential part of. Not modified. - * @param normalAxis the normal vector. It is normalized internally if needed. Not - * modified. - * @param inputTangentialPartToPack the tuple used to store the tangential part of the input. - * Modified. - * @throws ReferenceFrameMismatchException if the arguments are not expressed in the same reference - * frame. - */ - public static void extractTangentialPart(FrameTuple3DReadOnly input, FrameVector3DReadOnly normalAxis, FixedFrameTuple3DBasics inputTangentialPartToPack) - { - input.checkReferenceFrameMatch(normalAxis, inputTangentialPartToPack); - extractTangentialPart((Tuple3DReadOnly) input, (Vector3DReadOnly) normalAxis, (Tuple3DBasics) inputTangentialPartToPack); - } - - /** - * Calculates the 3D part of the given {@code input} that is orthogonal to the given - * {@code normalAxis} and stores the result in {@code inputTangentialPartToPack}. - *

- * The result has the following properties: - *

- * where: - * - *

- * - * @param input the tuple to extract the tangential part of. Not modified. - * @param normalAxis the normal vector. It is normalized internally if needed. Not - * modified. - * @param inputTangentialPartToPack the tuple used to store the tangential part of the input. - * Modified. - * @throws ReferenceFrameMismatchException if {@code input} and {@code normalAxis} are not expressed - * in the same reference frame. - */ - public static void extractTangentialPart(FrameTuple3DReadOnly input, FrameVector3DReadOnly normalAxis, FrameTuple3DBasics inputTangentialPartToPack) - { - input.checkReferenceFrameMatch(normalAxis); - inputTangentialPartToPack.setReferenceFrame(input.getReferenceFrame()); - extractTangentialPart((Tuple3DReadOnly) input, (Vector3DReadOnly) normalAxis, (Tuple3DBasics) inputTangentialPartToPack); - } - - /** - * Modifies {@code tupleToModify} such that its normal part is equal to the normal part of - * {@code input}. - *

- * This method performs the following calculation: x = x - (x.n)n + (y.n)n, where: - *

- *

- * - * @param input the tuple containing the normal part used to update {@code tupleToModify}. - * Not modified. - * @param normalAxis the normal vector. It is normalized internally if needed. Not modified. - * @param tupleToModify the tuple to modify the normal part of. Modified. - */ - public static void setNormalPart(Tuple3DReadOnly input, Vector3DReadOnly normalAxis, Tuple3DBasics tupleToModify) - { - double normalX = normalAxis.getX(); - double normalY = normalAxis.getY(); - double normalZ = normalAxis.getZ(); - double normalLengthSquared = EuclidCoreTools.normSquared(normalX, normalY, normalZ); - double dot = (TupleTools.dot(normalAxis, input) - TupleTools.dot(normalAxis, tupleToModify)) / normalLengthSquared; - tupleToModify.scaleAdd(dot, normalAxis, tupleToModify); - } - - public static int intersectionBetweenLineSegment2DAndCylinder3D(double circleRadius, - Point2DReadOnly circlePosition, - Point2DReadOnly startPoint, - Point2DReadOnly endPoint, - Point2DBasics firstIntersectionToPack, - Point2DBasics secondIntersectionToPack) - { - return intersectionBetweenLine2DAndCircle(circleRadius, - circlePosition.getX(), - circlePosition.getY(), - startPoint.getX(), - startPoint.getY(), - false, - endPoint.getX(), - endPoint.getY(), - false, - firstIntersectionToPack, - secondIntersectionToPack); - } - - public static int intersectionBetweenRay2DAndCircle(double circleRadius, - Point2DReadOnly circlePosition, - Point2DReadOnly startPoint, - Point2DReadOnly pointOnRay, - Point2DBasics firstIntersectionToPack, - Point2DBasics secondIntersectionToPack) - { - return intersectionBetweenLine2DAndCircle(circleRadius, - circlePosition.getX(), - circlePosition.getY(), - startPoint.getX(), - startPoint.getY(), - false, - pointOnRay.getX(), - pointOnRay.getY(), - true, - firstIntersectionToPack, - secondIntersectionToPack); - } - - public static int intersectionBetweenRay2DAndCircle(double circleRadius, - Point2DReadOnly circlePosition, - Point2DReadOnly startPoint, - Vector2DReadOnly direction, - Point2DBasics firstIntersectionToPack, - Point2DBasics secondIntersectionToPack) - { - return intersectionBetweenLine2DAndCircle(circleRadius, - circlePosition.getX(), - circlePosition.getY(), - startPoint.getX(), - startPoint.getY(), - false, - startPoint.getX() + direction.getX(), - startPoint.getY() + direction.getY(), - true, - firstIntersectionToPack, - secondIntersectionToPack); - } - - public static int intersectionBetweenLine2DAndCircle(double circleRadius, - Point2DReadOnly circlePosition, - Point2DReadOnly pointOnLine, - Vector2DReadOnly direction, - Point2DBasics firstIntersectionToPack, - Point2DBasics secondIntersectionToPack) - { - return intersectionBetweenLine2DAndCircle(circleRadius, - circlePosition.getX(), - circlePosition.getY(), - pointOnLine.getX(), - pointOnLine.getY(), - true, - pointOnLine.getX() + direction.getX(), - pointOnLine.getY() + direction.getY(), - true, - firstIntersectionToPack, - secondIntersectionToPack); - } - - private static int intersectionBetweenLine2DAndCircle(double circleRadius, - double circlePositionX, - double circlePositionY, - double startX, - double startY, - boolean canIntersectionOccurBeforeStart, - double endX, - double endY, - boolean canIntersectionOccurAfterEnd, - Point2DBasics firstIntersectionToPack, - Point2DBasics secondIntersectionToPack) - { - if (circleRadius < 0.0) - throw new IllegalArgumentException("The circle radius has to be positive."); - - if (firstIntersectionToPack != null) - firstIntersectionToPack.setToNaN(); - if (secondIntersectionToPack != null) - secondIntersectionToPack.setToNaN(); - - if (circleRadius == 0.0) - return 0; - - double radiusSquared = circleRadius * circleRadius; - - double dx = endX - startX; - double dy = endY - startY; - - double dIntersection1 = Double.NaN; - double dIntersection2 = Double.NaN; - - // Compute possible intersections with the circle - // - double deltaPX = startX - circlePositionX; - double deltaPY = startY - circlePositionY; - - double A = EuclidCoreTools.normSquared(dx, dy); - double B = 2.0 * (dx * deltaPX + dy * deltaPY); - double C = EuclidCoreTools.normSquared(deltaPX, deltaPY) - radiusSquared; - - double delta = EuclidCoreTools.squareRoot(B * B - 4 * A * C); - - if (Double.isFinite(delta)) - { - double oneOverTwoA = 0.5 / A; - double dCircle1 = -(B + delta) * oneOverTwoA; - double dCircle2 = -(B - delta) * oneOverTwoA; - - double intersection1X = dCircle1 * dx + startX; - double intersection1Y = dCircle1 * dy + startY; - - if (Math.abs(EuclidGeometryTools.percentageAlongLine2D(intersection1X, intersection1Y, circlePositionX, circlePositionY, 1.0, 0.0)) > circleRadius - - ONE_TRILLIONTH) - dCircle1 = Double.NaN; - - if (Double.isFinite(dCircle1)) - { - if (Double.isNaN(dIntersection1) || Math.abs(dCircle1 - dIntersection1) < ONE_TRILLIONTH) - { - dIntersection1 = dCircle1; - } - else if (dCircle1 < dIntersection1) - { - dIntersection2 = dIntersection1; - dIntersection1 = dCircle1; - } - else - { - dIntersection2 = dCircle1; - } - } - - double intersection2X = dCircle2 * dx + startX; - double intersection2Y = dCircle2 * dy + startY; - - if (Math.abs(EuclidGeometryTools.percentageAlongLine2D(intersection2X, intersection2Y, circlePositionX, circlePositionY, 1.0, 0.0)) > circleRadius - - ONE_TRILLIONTH) - dCircle2 = Double.NaN; - else if (Math.abs(dCircle1 - dCircle2) < ONE_TRILLIONTH) - dCircle2 = Double.NaN; - - if (Double.isFinite(dCircle2)) - { - if (Double.isNaN(dIntersection1)) - { - dIntersection1 = dCircle2; - } - else if (dCircle2 < dIntersection1) - { - dIntersection2 = dIntersection1; - dIntersection1 = dCircle2; - } - else - { - dIntersection2 = dCircle2; - } - } - - } - - if (!canIntersectionOccurBeforeStart) - { - if (dIntersection2 < 0.0) - dIntersection2 = Double.NaN; - - if (dIntersection1 < 0.0) - { - dIntersection1 = dIntersection2; - dIntersection2 = Double.NaN; - } - } - - if (!canIntersectionOccurAfterEnd) - { - if (dIntersection2 > 1.0) - dIntersection2 = Double.NaN; - - if (dIntersection1 > 1.0) - { - dIntersection1 = dIntersection2; - dIntersection2 = Double.NaN; - } - } - - if (Double.isNaN(dIntersection1)) - return 0; - - if (firstIntersectionToPack != null) - { - firstIntersectionToPack.set(dx, dy); - firstIntersectionToPack.scale(dIntersection1); - firstIntersectionToPack.add(startX, startY); - } - - if (Double.isNaN(dIntersection2)) - return 1; - - if (secondIntersectionToPack != null) - { - secondIntersectionToPack.set(dx, dy); - secondIntersectionToPack.scale(dIntersection2); - secondIntersectionToPack.add(startX, startY); - } - - return 2; - } - - /** - * Computes the angle in radians from the first 3D vector to the second 3D vector. The computed - * angle is in the range [0; pi]. - * - * @param firstVector the first vector. Not modified. - * @param secondVector the second vector. Not modified. - * @return the angle in radians from the first vector to the second vector. - */ - public static double angleFromFirstToSecondVector3D(Vector3DReadOnly firstVector, Vector3DReadOnly secondVector) - { - return angleFromFirstToSecondVector3D(firstVector.getX(), - firstVector.getY(), - firstVector.getZ(), - firstVector instanceof UnitVector3DReadOnly, - secondVector.getX(), - secondVector.getY(), - secondVector.getZ(), - secondVector instanceof UnitVector3DReadOnly); - } - - private static double angleFromFirstToSecondVector3D(double firstVectorX, - double firstVectorY, - double firstVectorZ, - boolean isFirstVectorUnitary, - double secondVectorX, - double secondVectorY, - double secondVectorZ, - boolean isSecondVectorUnitary) - { - double firstVectorLength = isFirstVectorUnitary ? 1.0 : EuclidCoreTools.norm(firstVectorX, firstVectorY, firstVectorZ); - double secondVectorLength = isSecondVectorUnitary ? 1.0 : EuclidCoreTools.norm(secondVectorX, secondVectorY, secondVectorZ); - - double dotProduct = firstVectorX * secondVectorX + firstVectorY * secondVectorY + firstVectorZ * secondVectorZ; - dotProduct /= firstVectorLength * secondVectorLength; - - if (dotProduct > 1.0) - dotProduct = 1.0; - else if (dotProduct < -1.0) - dotProduct = -1.0; - - return EuclidCoreTools.acos(dotProduct); - } - - /** - * Computes the coordinates of the intersection between a plane and an infinitely long line. - * Useful link . - *

- * Edge cases: - *

- *

- * - * @param intersectionToPack point in which the coordinates of the intersection are stored. - * @return {@code true} if the method succeeds, {@code false} otherwise. - */ - public static boolean intersectionBetweenLine3DAndPlane3D(double pointOnPlaneX, - double pointOnPlaneY, - double pointOnPlaneZ, - double planeNormalX, - double planeNormalY, - double planeNormalZ, - double pointOnLineX, - double pointOnLineY, - double pointOnLineZ, - double lineDirectionX, - double lineDirectionY, - double lineDirectionZ, - Point3DBasics intersectionToPack) - { - // Switching to the notation used in https://en.wikipedia.org/wiki/Line%E2%80%93plane_intersection - // Note: the algorithm is independent from the magnitudes of planeNormal and lineDirection - - // Let's compute the value of the coefficient d = ( (p0 - l0).n ) / ( l.n ) - double numerator, denominator; - numerator = (pointOnPlaneX - pointOnLineX) * planeNormalX; - numerator += (pointOnPlaneY - pointOnLineY) * planeNormalY; - numerator += (pointOnPlaneZ - pointOnLineZ) * planeNormalZ; - denominator = planeNormalX * lineDirectionX + planeNormalY * lineDirectionY + planeNormalZ * lineDirectionZ; - - // Check if the line is parallel to the plane - if (Math.abs(denominator) < ONE_TRILLIONTH) - { - return false; - } - else - { - double d = numerator / denominator; - - intersectionToPack.setX(d * lineDirectionX + pointOnLineX); - intersectionToPack.setY(d * lineDirectionY + pointOnLineY); - intersectionToPack.setZ(d * lineDirectionZ + pointOnLineZ); - return true; - } - } - - public static boolean intersectionBetweenRay2DAndLine2D(Point2DReadOnly rayOrigin, - Vector2DReadOnly rayDirection, - Point2DReadOnly lineOrigin, - Vector2DReadOnly lineDirection, - Point2DBasics intersectionToPack) - { - return intersectionBetweenRay2DAndLine2D(rayOrigin.getX(), - rayOrigin.getY(), - rayDirection.getX(), - rayDirection.getY(), - lineOrigin.getX(), - lineOrigin.getY(), - lineDirection.getX(), - lineDirection.getY(), - intersectionToPack); - } - - public static boolean intersectionBetweenRay2DAndLine2D(Point2DReadOnly rayOrigin, - Vector2DReadOnly rayDirection, - Point2DReadOnly linePoint1, - Point2DReadOnly linePoint2, - Point2DBasics intersectionToPack) - { - return intersectionBetweenRay2DAndLine2D(rayOrigin.getX(), - rayOrigin.getY(), - rayDirection.getX(), - rayDirection.getY(), - linePoint1.getX(), - linePoint1.getY(), - linePoint2.getX() - linePoint1.getX(), - linePoint2.getY() - linePoint1.getY(), - intersectionToPack); - } - - /** - * Just a more thorough version - */ - public static boolean intersectionBetweenRay2DAndLine2D(double rayOriginX, - double rayOriginY, - double rayDirectionX, - double rayDirectionY, - double lineOriginX, - double lineOriginY, - double lineDirectionX, - double lineDirectionY, - Point2DBasics intersectionToPack) - { - double start1x = rayOriginX; - double start1y = rayOriginY; - double end1x = rayOriginX + rayDirectionX; - double end1y = rayOriginY + rayDirectionY; - double start2x = lineOriginX; - double start2y = lineOriginY; - double end2x = lineOriginX + lineDirectionX; - double end2y = lineOriginY + lineDirectionY; - return intersectionBetweenTwoLine2DsImpl(start1x, start1y, false, end1x, end1y, true, start2x, start2y, true, end2x, end2y, true, intersectionToPack); - } - - /** - * This is only included here because it's a private method in the euclid class - */ - private static boolean intersectionBetweenTwoLine2DsImpl(double start1x, - double start1y, - boolean canIntersectionOccurBeforeStart1, - double end1x, - double end1y, - boolean canIntersectionOccurBeforeEnd1, - double start2x, - double start2y, - boolean canIntersectionOccurBeforeStart2, - double end2x, - double end2y, - boolean canIntersectionOccurBeforeEnd2, - Point2DBasics intersectionToPack) - { - double epsilon = EuclidGeometryTools.ONE_TEN_MILLIONTH; - - double direction1x = end1x - start1x; - double direction1y = end1y - start1y; - double direction2x = end2x - start2x; - double direction2y = end2y - start2y; - - double determinant = -direction1x * direction2y + direction1y * direction2x; - - double zeroish = 0.0 - epsilon; - - if (Math.abs(determinant) < epsilon) - { // The lines are parallel - // Check if they are collinear - double dx = start2x - start1x; - double dy = start2y - start1y; - double cross = dx * direction1y - dy * direction1x; - - if (Math.abs(cross) < epsilon) - { - if (canIntersectionOccurBeforeStart1 && canIntersectionOccurBeforeEnd1) - { // (start1, end1) represents a line - if (canIntersectionOccurBeforeStart2 && canIntersectionOccurBeforeEnd2) - { // (start2, end2) represents a line - if (intersectionToPack != null) - intersectionToPack.set(start1x, start1y); - return true; - } - - if (intersectionToPack != null) - intersectionToPack.set(start2x, start2y); - return true; - } - - if (canIntersectionOccurBeforeStart2 && canIntersectionOccurBeforeEnd2) - { // (start2, end2) represents a line - if (intersectionToPack != null) - intersectionToPack.set(start1x, start1y); - return true; - } - - // Let's find the first endpoint that is inside the other line segment and return it. - double direction1LengthSquare = EuclidCoreTools.normSquared(direction1x, direction1y); - double dot; - - // Check if start2 is inside (start1, end1) - dx = start2x - start1x; - dy = start2y - start1y; - dot = dx * direction1x + dy * direction1y; - - if ((canIntersectionOccurBeforeStart1 || zeroish < dot) && (canIntersectionOccurBeforeEnd1 || dot < direction1LengthSquare + epsilon)) - { - if (intersectionToPack != null) - intersectionToPack.set(start2x, start2y); - return true; - } - - // Check if end2 is inside (start1, end1) - dx = end2x - start1x; - dy = end2y - start1y; - dot = dx * direction1x + dy * direction1y; - - if ((canIntersectionOccurBeforeStart1 || zeroish < dot) && (canIntersectionOccurBeforeEnd1 || dot < direction1LengthSquare + epsilon)) - { - if (intersectionToPack != null) - intersectionToPack.set(end2x, end2y); - return true; - } - - double direction2LengthSquare = EuclidCoreTools.normSquared(direction2x, direction2y); - - // Check if start1 is inside (start2, end2) - dx = start1x - start2x; - dy = start1y - start2y; - dot = dx * direction2x + dy * direction2y; - - if ((canIntersectionOccurBeforeStart2 || zeroish < dot) && (canIntersectionOccurBeforeEnd2 || dot < direction2LengthSquare + epsilon)) - { - if (intersectionToPack != null) - intersectionToPack.set(start1x, start1y); - return true; - } - - // Check if end1 is inside (start2, end2) - dx = end1x - start2x; - dy = end1y - start2y; - dot = dx * direction2x + dy * direction2y; - - if ((canIntersectionOccurBeforeStart2 || zeroish < dot) && (canIntersectionOccurBeforeEnd2 || dot < direction2LengthSquare + epsilon)) - { - if (intersectionToPack != null) - intersectionToPack.set(end1x, end1y); - return true; - } - - // (start1, end1) and (start2, end2) represent ray and/or line segment and they are collinear but do not overlap. - if (intersectionToPack != null) - intersectionToPack.setToNaN(); - return false; - } - // The lines are parallel but are not collinear, they do not intersect. - else - { - if (intersectionToPack != null) - intersectionToPack.setToNaN(); - return false; - } - } - - double dx = start2x - start1x; - double dy = start2y - start1y; - - double oneOverDeterminant = 1.0 / determinant; - double AInverse00 = -direction2y; - double AInverse01 = direction2x; - double AInverse10 = -direction1y; - double AInverse11 = direction1x; - - double alpha = oneOverDeterminant * (AInverse00 * dx + AInverse01 * dy); - double beta = oneOverDeterminant * (AInverse10 * dx + AInverse11 * dy); - - double oneish = 1.0 + epsilon; - - boolean areIntersecting = (canIntersectionOccurBeforeStart1 || zeroish < alpha) && (canIntersectionOccurBeforeEnd1 || alpha < oneish); - if (areIntersecting) - areIntersecting = (canIntersectionOccurBeforeStart2 || zeroish < beta) && (canIntersectionOccurBeforeEnd2 || beta < oneish); - - if (intersectionToPack != null) - { - if (areIntersecting) - { - intersectionToPack.set(start1x + alpha * direction1x, start1y + alpha * direction1y); - } - else - { - intersectionToPack.setToNaN(); - } - } - - return areIntersecting; - } - - /** - * Calculate the angular velocity by differentiating orientation. - * - * @param qStart the initial orientation at time t. - * @param qEnd the final orientation at time t + duration. - * @param duration the time interval between the 2 orientations. - * @param angularVelocityToPack the angular velocity. - */ - public static void differentiateOrientation(QuaternionReadOnly qStart, QuaternionReadOnly qEnd, double duration, Vector3DBasics angularVelocityToPack) - { - double q1x = qStart.getX(); - double q1y = qStart.getY(); - double q1z = qStart.getZ(); - double q1s = qStart.getS(); - - double q2x = qEnd.getX(); - double q2y = qEnd.getY(); - double q2z = qEnd.getZ(); - double q2s = qEnd.getS(); - - double diffx = q1s * q2x - q1x * q2s - q1y * q2z + q1z * q2y; - double diffy = q1s * q2y + q1x * q2z - q1y * q2s - q1z * q2x; - double diffz = q1s * q2z - q1x * q2y + q1y * q2x - q1z * q2s; - double diffs = q1s * q2s + q1x * q2x + q1y * q2y + q1z * q2z; - - if (diffs < 0.0) - { - diffx = -diffx; - diffy = -diffy; - diffz = -diffz; - diffs = -diffs; - } - - double sinHalfTheta = EuclidCoreTools.norm(diffx, diffy, diffz); - - double angle; - if (EuclidCoreTools.epsilonEquals(1.0, diffs, 1.0e-12)) - angle = 2.0 * sinHalfTheta / diffs; - else - angle = 2.0 * EuclidCoreTools.atan2(sinHalfTheta, diffs); - angularVelocityToPack.set(diffx, diffy, diffz); - angularVelocityToPack.scale(angle / (sinHalfTheta * duration)); - } - - // *** NOTE ***: The 4x4 output matrix produced by this method assumes a Quaternion component ordering of: - // Quat = [ Qs - // Qx - // Qy - // Qz ] - public static DMatrixRMaj quaternionDotToOmegaTransform(QuaternionReadOnly rotatingFrameQuaternion) - { - double qs = rotatingFrameQuaternion.getS(); - double qx = rotatingFrameQuaternion.getX(); - double qy = rotatingFrameQuaternion.getY(); - double qz = rotatingFrameQuaternion.getZ(); - - DMatrixRMaj E = new DMatrixRMaj(4,4); - - E.set(0,0, qs); E.set(0,1, qx); E.set(0,2, qy); E.set(0,3, qz); - E.set(1,0,-qx); E.set(1,1, qs); E.set(1,2, qz); E.set(1,3,-qy); - E.set(2,0,-qy); E.set(2,1,-qz); E.set(2,2, qs); E.set(2,3, qx); - E.set(3,0,-qz); E.set(3,1, qy); E.set(3,2,-qx); E.set(3,3, qs); - - return E; - } - - /** - * Sets the yaw pitch roll but the doubles are given in degrees. - */ - public static void setYawPitchRollDegrees(Orientation3DBasics orientation3DBasics, double yaw, double pitch, double roll) - { - orientation3DBasics.setYawPitchRoll(Math.toRadians(yaw), Math.toRadians(pitch), Math.toRadians(roll)); - } - - /** - * Get the orientation as yaw pitch roll String but they are in degrees. - * Says yaw-pitch-roll. - */ - public static String getYawPitchRollStringDegrees(Orientation3DBasics orientation3DBasics) - { - // Degree symbol placed at the end so you don't have to remove it when copy and pasting - return EuclidCoreIOTools.getYawPitchRollString(EuclidCoreIOTools.DEFAULT_FORMAT, - Math.toDegrees(orientation3DBasics.getYaw()), - Math.toDegrees(orientation3DBasics.getPitch()), - Math.toDegrees(orientation3DBasics.getRoll())) + DEGREE_SYMBOL; - } - - /** - * Get the orientation as yaw pitch roll String but they are in degrees. - * Doesn't say yaw-pitch-roll. - */ - public static String getYawPitchRollValuesStringDegrees(Orientation3DBasics orientation3DBasics) - { - // Degree symbol placed at the end so you don't have to remove it when copy and pasting - return EuclidCoreIOTools.getStringOf("(", ")", ", ", - EuclidCoreIOTools.DEFAULT_FORMAT, - Math.toDegrees(orientation3DBasics.getYaw()), - Math.toDegrees(orientation3DBasics.getPitch()), - Math.toDegrees(orientation3DBasics.getRoll())) + DEGREE_SYMBOL; - } - - /** - * Generates a random positive definite matrix. - *

- * {@code matrix}ij ∈ [-1.0; 1.0]. - *

- *

- * The approach used here generates a random 3D matrix with values in [-1.0, 1.0], and then performs A * AT which is guaranteed to result in a - * symmetric positive semi-definite matrix. We then add diagonal terms to make the matrix positive definite, and finally scale the matrix by a random double - * that upper bounds the absolute values of the positive definite matrix elements to 1.0. - *

- * - * @param random the random generator to use. - * @return the random positive definite matrix. - */ - public static Matrix3D nextPositiveDefiniteMatrix3D(Random random) - { - return nextPositiveDefiniteMatrix3D(random, 1.0); - } - - /** - * Generates a random positive definite matrix. - *

- * {@code matrix}ij ∈ [-minMaxValue, minMaxValue] - *

- *

- * The approach used here generates a random 3D matrix with values in [{@code -minMaxValue}, {@code minMaxValue}], and then performs A * AT, - * which is guaranteed to result in a symmetric positive semi-definite matrix. We then add diagonal terms to make the matrix positive definite, and finally - * scale the matrix by a random double that upper bounds the absolute values of the positive definite matrix elements to {@code minMaxValue}. - *

- * - * @param random the random generator to use. - * @param minMaxValue the maximum value for each element. - * @return the random positive definite matrix. - * @throws RuntimeException if {@code minMaxValue < 0}. - */ - public static Matrix3D nextPositiveDefiniteMatrix3D(Random random, double minMaxValue) - { - Matrix3D matrix3D = nextMatrix3D(random, minMaxValue); - matrix3D.multiplyTransposeOther(matrix3D); - - double diagonalDominanceScalar = Math.abs(minMaxValue); - matrix3D.addM00(diagonalDominanceScalar); - matrix3D.addM11(diagonalDominanceScalar); - matrix3D.addM22(diagonalDominanceScalar); - - double scalarToShrinkMatrixWithinBounds = nextDouble(random, 0.0, minMaxValue / matrix3D.maxAbsElement()); - matrix3D.scale(scalarToShrinkMatrixWithinBounds); - return matrix3D; - } - - /** - * Remove when this issue is fixed: - * https://github.com/ihmcrobotics/euclid/issues/57 - */ - private static final Field referenceFrameHasBeenRemoved; - static - { - try - { - referenceFrameHasBeenRemoved = ReferenceFrame.class.getDeclaredField("hasBeenRemoved"); - referenceFrameHasBeenRemoved.setAccessible(true); - } - catch (NoSuchFieldException e) - { - throw new RuntimeException(e); - } - } - - public static boolean hasBeenRemoved(ReferenceFrame referenceFrame) - { - try - { - return referenceFrameHasBeenRemoved.getBoolean(referenceFrame); - } - catch (IllegalAccessException e) - { - throw new RuntimeException(e); - } - } - - /** - * Remove when this issue is fixed: - * https://github.com/ihmcrobotics/euclid/issues/57 - */ - private static final Field referenceFrameName; - static - { - try - { - referenceFrameName = ReferenceFrame.class.getDeclaredField("frameName"); - referenceFrameName.setAccessible(true); - } - catch (NoSuchFieldException e) - { - throw new RuntimeException(e); - } - } - - public static String frameName(ReferenceFrame referenceFrame) - { - try - { - return referenceFrameName.get(referenceFrame).toString(); - } - catch (IllegalAccessException e) - { - throw new RuntimeException(e); - } - } - - public static FrameVector3DReadOnly newLinkedFrameVector3DReadOnly(ReferenceFrameHolder referenceFrameHolder, DMatrixRMaj source) - { - return newLinkedFrameVector3DReadOnly(referenceFrameHolder, 0, source); - } - - public static FrameVector3DReadOnly newLinkedFrameVector3DReadOnly(ReferenceFrameHolder referenceFrameHolder, int startIndex, DMatrixRMaj source) - { - int xIndex = startIndex; - int yIndex = startIndex + 1; - int zIndex = startIndex + 2; - return EuclidFrameFactories.newLinkedFrameVector3DReadOnly(referenceFrameHolder, - () -> source.get(xIndex, 0), - () -> source.get(yIndex, 0), - () -> source.get(zIndex, 0)); - } -} \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/EuclidGeometryMissingTools.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/EuclidGeometryMissingTools.java deleted file mode 100644 index b56e4c0a2..000000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/EuclidGeometryMissingTools.java +++ /dev/null @@ -1,118 +0,0 @@ -package us.ihmc.robotics; - -import us.ihmc.euclid.geometry.BoundingBox2D; -import us.ihmc.euclid.geometry.BoundingBox3D; -import us.ihmc.euclid.geometry.interfaces.BoundingBox2DReadOnly; -import us.ihmc.euclid.geometry.interfaces.BoundingBox3DReadOnly; -import us.ihmc.euclid.geometry.tools.EuclidGeometryTools; -import us.ihmc.euclid.referenceFrame.exceptions.ReferenceFrameMismatchException; -import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; - -public class EuclidGeometryMissingTools -{ - public static double getZOnPlane(Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal, double pointX, double pointY) - { - // The three components of the plane origin - double x0 = pointOnPlane.getX(); - double y0 = pointOnPlane.getY(); - double z0 = pointOnPlane.getZ(); - // The three components of the plane normal - double a = planeNormal.getX(); - double b = planeNormal.getY(); - double c = planeNormal.getZ(); - - // Given the plane equation: a*x + b*y + c*z + d = 0, with d = -(a*x0 + b*y0 + c*z0), we find z: - double z = a / c * (x0 - pointX) + b / c * (y0 - pointY) + z0; - return z; - } - - /** - * Returns the minimum XY distance between a 3D point and an infinitely long 3D line defined by two - * points. - *

- * Edge cases: - *

- *

- *

- * WARNING: the 3D arguments are projected onto the XY-plane to perform the actual computation in - * 2D. - *

- * - * @param point the 3D point is projected onto the xy-plane. It's projection is used to - * compute the distance from the line. Not modified. - * @param firstPointOnLine the projection of this 3D onto the xy-plane refers to the first point on - * the 2D line. Not modified. - * @param secondPointOnLine the projection of this 3D onto the xy-plane refers to the second point - * one the 2D line. Not modified. - * @return the minimum distance between the 2D point and the 2D line. - * @throws ReferenceFrameMismatchException if the arguments are not expressed in the same reference - * frame. - */ - public static double distanceXYFromPoint3DToLine3D(FramePoint3DReadOnly point, FramePoint3DReadOnly firstPointOnLine, FramePoint3DReadOnly secondPointOnLine) - { - point.checkReferenceFrameMatch(firstPointOnLine); - point.checkReferenceFrameMatch(secondPointOnLine); - - double pointOnLineX = firstPointOnLine.getX(); - double pointOnLineY = firstPointOnLine.getY(); - double lineDirectionX = secondPointOnLine.getX() - firstPointOnLine.getX(); - double lineDirectionY = secondPointOnLine.getY() - firstPointOnLine.getY(); - return EuclidGeometryTools.distanceFromPoint2DToLine2D(point.getX(), point.getY(), pointOnLineX, pointOnLineY, lineDirectionX, lineDirectionY); - } - - /** - * Finds the intersection of two bounding boxes defined by a bounding box Allocates a new - * BoundingBox2D. TODO: Check, Unit test, move where BoundingBox union is - * - * @param a - * @param b - * @return the intersection bounding box, or null if no intersection - */ - public static BoundingBox2D computeIntersectionOfTwoBoundingBoxes(BoundingBox2DReadOnly a, BoundingBox2DReadOnly b) - { - double maxX = Math.min(a.getMaxX(), b.getMaxX()); - double maxY = Math.min(a.getMaxY(), b.getMaxY()); - double minX = Math.max(a.getMinX(), b.getMinX()); - double minY = Math.max(a.getMinY(), b.getMinY()); - - if ((maxX <= minX) || (maxY <= minY)) - return null; - - return new BoundingBox2D(minX, minY, maxX, maxY); - } - - /** - * Finds the intersection of two bounding boxes defined by a bounding box Allocates a new boundingBox3D. - * - * @param a - * @param b - * @return the intersection bounding box, or null if no intersection - */ - public static BoundingBox3D computeIntersectionOfTwoBoundingBoxes(BoundingBox3DReadOnly a, BoundingBox3DReadOnly b) - { - double maxX = Math.min(a.getMaxX(), b.getMaxX()); - double maxY = Math.min(a.getMaxY(), b.getMaxY()); - double maxZ = Math.min(a.getMaxZ(), b.getMaxZ()); - - double minX = Math.max(a.getMinX(), b.getMinX()); - double minY = Math.max(a.getMinY(), b.getMinY()); - double minZ = Math.max(a.getMinZ(), b.getMinZ()); - - if ((maxX <= minX) || (maxY <= minY) || (maxZ <= minZ)) - return null; - - return new BoundingBox3D(minX, minY, minZ, maxX, maxY, maxZ); - } - - public static double computeBoundingBoxVolume3D(BoundingBox3DReadOnly boundingBox) - { - return Math.abs(boundingBox.getMaxX() - boundingBox.getMinX()) - * Math.abs(boundingBox.getMaxY() - boundingBox.getMinY()) - * Math.abs(boundingBox.getMaxZ() - boundingBox.getMinZ()); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/EuclidGeometryPolygonMissingTools.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/EuclidGeometryPolygonMissingTools.java deleted file mode 100644 index 24908a4c0..000000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/EuclidGeometryPolygonMissingTools.java +++ /dev/null @@ -1,46 +0,0 @@ -package us.ihmc.robotics; - -import us.ihmc.euclid.geometry.tools.EuclidGeometryPolygonTools; -import us.ihmc.euclid.geometry.tools.EuclidGeometryTools; -import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly; - -import java.util.List; - -public class EuclidGeometryPolygonMissingTools -{ - public static boolean doLineSegment2DAndConvexPolygon2DIntersect(Point2DReadOnly lineSegmentStart, - Point2DReadOnly lineSegmentEnd, - List convexPolygon2D, - int numberOfVertices) - { - checkNumberOfVertices(convexPolygon2D, numberOfVertices); - - if (numberOfVertices == 0) - return false; - - if (numberOfVertices == 1) - return EuclidGeometryTools.isPoint2DOnLineSegment2D(convexPolygon2D.get(0), lineSegmentStart, lineSegmentEnd); - - if (numberOfVertices == 2) - return EuclidGeometryTools.doLineSegment2DsIntersect(convexPolygon2D.get(0), convexPolygon2D.get(1), lineSegmentStart, lineSegmentEnd); - - - for (int edgeIndex = 0; edgeIndex < numberOfVertices; edgeIndex++) - { - Point2DReadOnly edgeStart = convexPolygon2D.get(edgeIndex); - Point2DReadOnly edgeEnd = convexPolygon2D.get(EuclidGeometryPolygonTools.next(edgeIndex, numberOfVertices)); - - if (EuclidGeometryTools.doLineSegment2DsIntersect(edgeStart, edgeEnd, lineSegmentStart, lineSegmentEnd)) - return true; - - } - - return false; - } - - private static void checkNumberOfVertices(List convexPolygon2D, int numberOfVertices) - { - if (numberOfVertices < 0 || numberOfVertices > convexPolygon2D.size()) - throw new IllegalArgumentException("Illegal numberOfVertices: " + numberOfVertices + ", expected a value in ] 0, " + convexPolygon2D.size() + "]."); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/geometry/shapes/FramePlane3d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/geometry/shapes/FramePlane3d.java deleted file mode 100644 index 992976848..000000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/geometry/shapes/FramePlane3d.java +++ /dev/null @@ -1,313 +0,0 @@ -package us.ihmc.robotics.geometry.shapes; - -import us.ihmc.euclid.Axis3D; -import us.ihmc.euclid.geometry.Plane3D; -import us.ihmc.euclid.geometry.interfaces.Line3DReadOnly; -import us.ihmc.euclid.referenceFrame.FrameLine3D; -import us.ihmc.euclid.referenceFrame.FramePoint2D; -import us.ihmc.euclid.referenceFrame.FramePoint3D; -import us.ihmc.euclid.referenceFrame.FrameVector3D; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.ReferenceFrameHolder; -import us.ihmc.euclid.transform.RigidBodyTransform; -import us.ihmc.euclid.tuple3D.Point3D; -import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.euclid.tuple3D.interfaces.Point3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; - -public class FramePlane3d implements ReferenceFrameHolder -{ - private ReferenceFrame referenceFrame; - private final Plane3D plane3d; - - private final RigidBodyTransform temporaryTransformToDesiredFrame = new RigidBodyTransform(); - private final Vector3D temporaryVector = new Vector3D(); - private final Point3D temporaryPoint = new Point3D(); - - public FramePlane3d() - { - this(ReferenceFrame.getWorldFrame()); - } - - public FramePlane3d(ReferenceFrame referenceFrame, Plane3D plane3d) - { - this.referenceFrame = referenceFrame; - this.plane3d = plane3d; - } - - public FramePlane3d(ReferenceFrame referenceFrame) - { - this.referenceFrame = referenceFrame; - this.plane3d = new Plane3D(); - } - - public FramePlane3d(FramePlane3d framePlane3d) - { - this.referenceFrame = framePlane3d.referenceFrame; - this.plane3d = new Plane3D(framePlane3d.plane3d); - } - - public FramePlane3d(FrameVector3D normal, FramePoint3D point) - { - normal.checkReferenceFrameMatch(point); - this.referenceFrame = normal.getReferenceFrame(); - this.plane3d = new Plane3D(point, normal); - } - - public FramePlane3d(ReferenceFrame referenceFrame, Point3DReadOnly point, Vector3DReadOnly normal) - { - this.referenceFrame = referenceFrame; - this.plane3d = new Plane3D(point, normal); - } - - @Override - public ReferenceFrame getReferenceFrame() - { - return referenceFrame; - } - - public void getNormal(FrameVector3D normalToPack) - { - checkReferenceFrameMatch(normalToPack.getReferenceFrame()); - temporaryVector.set(this.plane3d.getNormal()); - normalToPack.set(temporaryVector); - } - - public FrameVector3D getNormalCopy() - { - FrameVector3D returnVector = new FrameVector3D(this.referenceFrame); - getNormal(returnVector); - return returnVector; - } - - public Vector3DBasics getNormal() - { - return plane3d.getNormal(); - } - - public void setNormal(double x, double y, double z) - { - plane3d.getNormal().set(x, y, z); - } - - public void setNormal(Vector3DReadOnly normal) - { - plane3d.getNormal().set(normal); - } - - public void getPoint(FramePoint3D pointToPack) - { - checkReferenceFrameMatch(pointToPack.getReferenceFrame()); - temporaryPoint.set(this.plane3d.getPoint()); - pointToPack.set(temporaryPoint); - } - - public FramePoint3D getPointCopy() - { - FramePoint3D pointToReturn = new FramePoint3D(this.getReferenceFrame()); - this.getPoint(pointToReturn); - return pointToReturn; - } - - public Point3DBasics getPoint() - { - return plane3d.getPoint(); - } - - public void setPoint(double x, double y, double z) - { - plane3d.getPoint().set(x, y, z); - } - - public void setPoint(Point3DReadOnly point) - { - plane3d.getPoint().set(point); - } - - public void setPoints(FramePoint3D pointA, FramePoint3D pointB, FramePoint3D pointC) - { - pointA.checkReferenceFrameMatch(referenceFrame); - pointB.checkReferenceFrameMatch(referenceFrame); - pointC.checkReferenceFrameMatch(referenceFrame); - - plane3d.set(pointA, pointB, pointC); - } - - public void changeFrame(ReferenceFrame desiredFrame) - { - if (desiredFrame != referenceFrame) - { - referenceFrame.getTransformToDesiredFrame(temporaryTransformToDesiredFrame, desiredFrame); - plane3d.applyTransform(temporaryTransformToDesiredFrame); - referenceFrame = desiredFrame; - } - - // otherwise: in the right frame already, so do nothing - } - - public boolean isOnOrAbove(FramePoint3D pointToTest) - { - checkReferenceFrameMatch(pointToTest); - - return plane3d.isOnOrAbove(pointToTest); - } - - public boolean isOnOrAbove(Point3DReadOnly pointToTest) - { - return plane3d.isOnOrAbove(pointToTest); - } - - public boolean isOnOrAbove(Point3DReadOnly pointToTest, double epsilon) - { - return plane3d.isOnOrAbove(pointToTest, epsilon); - } - - public boolean isOnOrBelow(FramePoint3D pointToTest) - { - checkReferenceFrameMatch(pointToTest); - - return plane3d.isOnOrBelow(pointToTest); - } - - /** - * Tests if the two planes are parallel by testing if their normals are collinear. - * The latter is done given a tolerance on the angle between the two normal axes in the range ]0; pi/2[. - * - *

- * Edge cases: - *

- *

- * - * @param otherPlane the other plane to do the test with. Not modified. - * @param angleEpsilon tolerance on the angle in radians. - * @return {@code true} if the two planes are parallel, {@code false} otherwise. - */ - public boolean isParallel(FramePlane3d otherPlane, double angleEpsilon) - { - checkReferenceFrameMatch(otherPlane); - return plane3d.isParallel(otherPlane.plane3d, angleEpsilon); - } - - /** - * Tests if this plane and the given plane are coincident: - * - *

- * Edge cases: - *

- *

- * - * @param otherPlane the other plane to do the test with. Not modified. - * @param angleEpsilon tolerance on the angle in radians to determine if the plane normals are collinear. - * @param distanceEpsilon tolerance on the distance to determine if {@code otherPlane.point} belongs to this plane. - * @return {@code true} if the two planes are coincident, {@code false} otherwise. - */ - public boolean isCoincident(FramePlane3d otherPlane, double angleEpsilon, double distanceEpsilon) - { - checkReferenceFrameMatch(otherPlane); - return plane3d.isCoincident(otherPlane.plane3d, angleEpsilon, distanceEpsilon); - } - - public FramePoint3D orthogonalProjectionCopy(FramePoint3D point) - { - checkReferenceFrameMatch(point); - FramePoint3D returnPoint = new FramePoint3D(point); - orthogonalProjection(returnPoint); - - return returnPoint; - } - - public void orthogonalProjection(FramePoint3D point) - { - checkReferenceFrameMatch(point); - plane3d.orthogonalProjection(point); - } - - public double getZOnPlane(FramePoint2D xyPoint) - { - checkReferenceFrameMatch(xyPoint.getReferenceFrame()); - return plane3d.getZOnPlane(xyPoint.getX(), xyPoint.getY()); - } - - public double distance(FramePoint3D point) - { - checkReferenceFrameMatch(point); - return plane3d.distance(point); - } - - public boolean epsilonEquals(FramePlane3d plane, double epsilon) - { - checkReferenceFrameMatch(plane.getReferenceFrame()); - - return ((referenceFrame == plane.getReferenceFrame()) && (plane.plane3d.epsilonEquals(this.plane3d, epsilon))); - } - - public FramePlane3d applyTransformCopy(RigidBodyTransform transformation) - { - FramePlane3d returnPlane = new FramePlane3d(this); - returnPlane.applyTransform(transformation); - - return returnPlane; - } - - public void applyTransform(RigidBodyTransform transformation) - { - plane3d.applyTransform(transformation); - } - - public void setIncludingFrame(ReferenceFrame referenceFrame, double pointX, double pointY, double pointZ, double normalX, double normalY, double normalZ) - { - this.referenceFrame = referenceFrame; - plane3d.getPoint().set(pointX, pointY, pointZ); - plane3d.getNormal().set(normalX, normalY, normalZ); - } - - public void getIntersectionWithLine(FramePoint3D pointToPack, FrameLine3D line) - { - checkReferenceFrameMatch(line.getReferenceFrame()); - checkReferenceFrameMatch(pointToPack.getReferenceFrame()); - - Point3D intersectionToPack = new Point3D(); - plane3d.intersectionWith(intersectionToPack, line.getPoint(), line.getDirection()); - pointToPack.set(intersectionToPack); - } - - public void getIntersectionWithLine(Point3D pointToPack, Line3DReadOnly line) - { - plane3d.intersectionWith(pointToPack, line.getPoint(), line.getDirection()); - } - - public void setToZero() - { - getPoint().setToZero(); - getNormal().set(Axis3D.Z); - } - - public void setToZero(ReferenceFrame referenceFrame) - { - setReferenceFrame(referenceFrame); - setToZero(); - } - - public void setReferenceFrame(ReferenceFrame referenceFrame) - { - this.referenceFrame = referenceFrame; - } - - @Override - public String toString() - { - StringBuilder builder = new StringBuilder(); - builder.append("ReferenceFrame = " + referenceFrame + ", " + plane3d.toString()); - - return builder.toString(); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/QuaternionCalculus.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/QuaternionCalculus.java deleted file mode 100644 index 8f3af913c..000000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/QuaternionCalculus.java +++ /dev/null @@ -1,243 +0,0 @@ -package us.ihmc.robotics.math; - -import us.ihmc.euclid.axisAngle.AxisAngle; -import us.ihmc.euclid.rotationConversion.QuaternionConversion; -import us.ihmc.euclid.tools.QuaternionTools; -import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; -import us.ihmc.euclid.tuple4D.Quaternion; -import us.ihmc.euclid.tuple4D.Vector4D; -import us.ihmc.euclid.tuple4D.interfaces.QuaternionBasics; -import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; -import us.ihmc.euclid.tuple4D.interfaces.Vector4DBasics; -import us.ihmc.euclid.tuple4D.interfaces.Vector4DReadOnly; -import us.ihmc.commons.MathTools; - -public class QuaternionCalculus -{ - public QuaternionCalculus() - { - } - - private final AxisAngle axisAngleForLog = new AxisAngle(); - - /** - * This computes: resultToPack = log(q) - * @param q is a unit quaternion and describes a orientation. - * @param resultToPack is used to store the result and is a pure quaternion (w = 0.0). - */ - public void log(QuaternionReadOnly q, Vector4DBasics resultToPack) - { - axisAngleForLog.set(q); - resultToPack.set(axisAngleForLog.getX(), axisAngleForLog.getY(), axisAngleForLog.getZ(), 0.0); - resultToPack.scale(axisAngleForLog.getAngle()); - } - - /** - * This computes: resultToPack = log(q) - * @param q is a unit quaternion and describes a orientation. - * @param resultToPack is used to store the result. - */ - public void log(QuaternionReadOnly q, Vector3DBasics resultToPack) - { - axisAngleForLog.set(q); - resultToPack.set(axisAngleForLog.getX(), axisAngleForLog.getY(), axisAngleForLog.getZ()); - resultToPack.scale(axisAngleForLog.getAngle()); - } - - public void exp(Vector3DReadOnly rotationVector, QuaternionBasics quaternionToPack) - { - QuaternionConversion.convertRotationVectorToQuaternion(rotationVector, quaternionToPack); - } - - /** - * Interpolation from q0 to q1 for a given alpha = [0, 1] using SLERP. - * Computes: resultToPack = q0 (q0^-1 q1)^alpha. - */ - public void interpolate(double alpha, QuaternionReadOnly q0, QuaternionReadOnly q1, QuaternionBasics qInterpolatedToPack) - { - interpolate(alpha, q0, q1, qInterpolatedToPack, true); - } - - private final Quaternion tempQ1ForInterpolation = new Quaternion(); - - public void interpolate(double alpha, QuaternionReadOnly q0, QuaternionReadOnly q1, QuaternionBasics qInterpolatedToPack, boolean preventExtraSpin) - { - tempQ1ForInterpolation.set(q1); - - if (preventExtraSpin && q0.dot(tempQ1ForInterpolation) < 0.0) - { - tempQ1ForInterpolation.negate(); - } - - computeQuaternionDifference(q0, tempQ1ForInterpolation, qInterpolatedToPack); - pow(qInterpolatedToPack, alpha, qInterpolatedToPack); - qInterpolatedToPack.multiply(q0, qInterpolatedToPack); - } - - private final AxisAngle axisAngleForPow = new AxisAngle(); - - /** - * This computes: resultToPack = q^power. - * @param q is a unit quaternion and describes a orientation. - * @param resultToPack is used to store the result. - */ - public void pow(QuaternionReadOnly q, double power, QuaternionBasics resultToPack) - { - axisAngleForPow.set(q); - axisAngleForPow.setAngle(power * axisAngleForPow.getAngle()); - resultToPack.set(axisAngleForPow); - } - - private final Quaternion qConj = new Quaternion(); - - public void computeAngularVelocityInBodyFixedFrame(QuaternionReadOnly q, Vector4DReadOnly qDot, Vector3DBasics angularVelocityToPack) - { - qConj.setAndConjugate(q); - multiply(qConj, qDot, angularVelocityToPack); - angularVelocityToPack.scale(2.0); - } - - public void computeAngularVelocityInWorldFrame(QuaternionReadOnly q, Vector4DReadOnly qDot, Vector3DBasics angularVelocityToPack) - { - qConj.setAndConjugate(q); - multiply(qDot, qConj, angularVelocityToPack); - angularVelocityToPack.scale(2.0); - } - - public void computeQDotInWorldFrame(QuaternionReadOnly q, Vector3DReadOnly angularVelocityInWorld, Vector4DBasics qDotToPack) - { - multiply(angularVelocityInWorld, q, qDotToPack); - qDotToPack.scale(0.5); - } - - public void computeQDotInBodyFixedFrame(QuaternionReadOnly q, Vector3DReadOnly angularVelocityInBody, Vector4DBasics qDotToPack) - { - multiply(q, angularVelocityInBody, qDotToPack); - qDotToPack.scale(0.5); - } - - private final Vector4D intermediateQDot = new Vector4D(); - private final Vector4D qDotConj = new Vector4D(); - private final Vector3D intermediateAngularAcceleration = new Vector3D(); - private final Vector3D intermediateAngularVelocity = new Vector3D(); - private final Vector4D intermediateQDDot = new Vector4D(); - - public void computeQDDotInWorldFrame(QuaternionReadOnly q, Vector4DReadOnly qDot, Vector3DReadOnly angularAcceleration, Vector4DBasics qDDotToPack) - { - computeAngularVelocityInWorldFrame(q, qDot, intermediateAngularVelocity); - computeQDDotInWorldFrame(q, qDot, intermediateAngularVelocity, angularAcceleration, qDDotToPack); - } - - public void computeQDDotInWorldFrame(QuaternionReadOnly q, Vector3DReadOnly angularVelocityInWorld, Vector3DReadOnly angularAccelerationInWorld, Vector4DBasics qDDotToPack) - { - computeQDotInWorldFrame(q, angularVelocityInWorld, intermediateQDot); - computeQDDotInWorldFrame(q, intermediateQDot, angularVelocityInWorld, angularAccelerationInWorld, qDDotToPack); - } - - public void computeQDDotInWorldFrame(QuaternionReadOnly q, Vector4DReadOnly qDot, Vector3DReadOnly angularVelocity, Vector3DReadOnly angularAcceleration, Vector4DBasics qDDotToPack) - { - multiply(angularAcceleration, q, intermediateQDDot); - multiply(angularVelocity, qDot, qDDotToPack); - qDDotToPack.add(intermediateQDDot); - qDDotToPack.scale(0.5); - } - - public void computeAngularAccelerationInWorldFrame(QuaternionReadOnly q, Vector4DReadOnly qDDot, Vector3DReadOnly angularVelocityInWorld, Vector3DBasics angularAccelerationToPack) - { - computeQDotInWorldFrame(q, angularVelocityInWorld, intermediateQDot); - computeAngularAcceleration(q, intermediateQDot, qDDot, angularAccelerationToPack); - } - - public void computeAngularAcceleration(QuaternionReadOnly q, Vector4DReadOnly qDot, Vector4DReadOnly qDDot, Vector3DBasics angularAccelerationToPack) - { - qConj.setAndConjugate(q); - qDotConj.set(-qDot.getX(), -qDot.getY(), -qDot.getZ(), qDot.getS()); - multiply(qDot, qDotConj, intermediateAngularAcceleration); - multiply(qDDot, qConj, angularAccelerationToPack); - angularAccelerationToPack.add(intermediateAngularAcceleration); - angularAccelerationToPack.scale(2.0); - } - - public void computeQDotByFiniteDifferenceCentral(QuaternionReadOnly qPrevious, QuaternionReadOnly qNext, double dt, Vector4DBasics qDotToPack) - { - qDotToPack.set(qNext); - qDotToPack.sub(qPrevious); - qDotToPack.scale(0.5 / dt); - } - - public void computeQDDotByFiniteDifferenceCentral(QuaternionReadOnly qPrevious, QuaternionReadOnly q, QuaternionReadOnly qNext, double dt, Vector4DBasics qDDotToPack) - { - qDDotToPack.set(qNext); - qDDotToPack.sub(q); - qDDotToPack.sub(q); - qDDotToPack.add(qPrevious); - qDDotToPack.scale(1.0 / MathTools.square(dt)); - } - - private final Quaternion qInv = new Quaternion(); - - /** - * This computes the product: resultToPack = (q0^-1 q1) - */ - public void computeQuaternionDifference(QuaternionReadOnly q0, QuaternionReadOnly q1, QuaternionBasics resultToPack) - { - resultToPack.setAndConjugate(q0); - resultToPack.multiply(q1); - } - - private final Vector4D pureQuatForMultiply = new Vector4D(); - - public void multiply(QuaternionReadOnly q, Vector3DReadOnly v, Vector4DBasics resultToPack) - { - setAsPureQuaternion(v, resultToPack); - QuaternionTools.multiply(q, resultToPack, resultToPack); - } - - public void multiply(Vector3DReadOnly v, Vector4DReadOnly q, Vector4DBasics resultToPack) - { - setAsPureQuaternion(v, resultToPack); - QuaternionTools.multiply(resultToPack, q, resultToPack); - } - - public void multiply(Vector3DReadOnly v, QuaternionReadOnly q, Vector4DBasics resultToPack) - { - setAsPureQuaternion(v, resultToPack); - QuaternionTools.multiply(resultToPack, q, resultToPack); - } - - public void multiply(Vector4DReadOnly q1, Vector4DReadOnly q2, Vector3DBasics resultToPack) - { - QuaternionTools.multiply(q1, q2, pureQuatForMultiply); - setVectorFromPureQuaternion(pureQuatForMultiply, resultToPack); - } - - public void multiply(Vector4DReadOnly q1, QuaternionReadOnly q2, Vector3DBasics resultToPack) - { - QuaternionTools.multiply(q1, q2, pureQuatForMultiply); - setVectorFromPureQuaternion(pureQuatForMultiply, resultToPack); - } - - public void multiply(QuaternionReadOnly q1, Vector4DReadOnly q2, Vector3DBasics resultToPack) - { - QuaternionTools.multiply(q1, q2, pureQuatForMultiply); - setVectorFromPureQuaternion(pureQuatForMultiply, resultToPack); - } - - public void inverseMultiply(QuaternionReadOnly q1, QuaternionReadOnly q2, QuaternionBasics resultToPack) - { - qInv.setAndConjugate(q1); - resultToPack.multiply(qInv, q2); - } - - public void setAsPureQuaternion(Vector3DReadOnly vector, Vector4DBasics pureQuaternionToSet) - { - pureQuaternionToSet.set(vector.getX(), vector.getY(), vector.getZ(), 0.0); - } - - public void setVectorFromPureQuaternion(Vector4DReadOnly pureQuaternion, Vector3DBasics vectorToPack) - { - vectorToPack.set(pureQuaternion.getX(), pureQuaternion.getY(), pureQuaternion.getZ()); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/referenceFrames/Pose2dReferenceFrame.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/referenceFrames/Pose2dReferenceFrame.java deleted file mode 100644 index 1ce6a4f86..000000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/referenceFrames/Pose2dReferenceFrame.java +++ /dev/null @@ -1,80 +0,0 @@ -package us.ihmc.robotics.referenceFrames; - -import us.ihmc.euclid.referenceFrame.FrameOrientation2D; -import us.ihmc.euclid.referenceFrame.FramePoint2D; -import us.ihmc.euclid.referenceFrame.FramePose2D; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.transform.RigidBodyTransform; -import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly; - -public class Pose2dReferenceFrame extends ReferenceFrame -{ - private final FramePose2D originPose; - - public Pose2dReferenceFrame(String frameName, ReferenceFrame parentFrame) - { - super(frameName, parentFrame); - - originPose = new FramePose2D(parentFrame); - } - - public Pose2dReferenceFrame(String frameName, FramePose2D pose) - { - this(frameName, pose.getReferenceFrame()); - setPoseAndUpdate(pose); - } - - @Override - protected void updateTransformToParent(RigidBodyTransform transformToParent) - { - originPose.get(transformToParent); - } - - public void setPositionAndUpdate(FramePoint2D framePoint) - { - framePoint.checkReferenceFrameMatch(getParent()); - originPose.getPosition().set(framePoint); - this.update(); - } - - public void setOrientationAndUpdate(FrameOrientation2D frameOrientation) - { - frameOrientation.checkReferenceFrameMatch(getParent()); - originPose.getOrientation().set(frameOrientation); - this.update(); - } - - public void setPoseAndUpdate(Point2DReadOnly position, double orientation) - { - originPose.getPosition().set(position); - originPose.getOrientation().setYaw(orientation); - this.update(); - } - - public void setPoseAndUpdate(FramePose2D pose) - { - originPose.set(pose); - this.update(); - } - - public void setPoseAndUpdate(FramePoint2D position, FrameOrientation2D orientation) - { - position.changeFrame(originPose.getReferenceFrame()); - originPose.getPosition().set(position); - - orientation.changeFrame(originPose.getReferenceFrame()); - originPose.getOrientation().set(orientation); - this.update(); - } - - public FramePose2D getPoseCopy() - { - return new FramePose2D(originPose); - } - - @Override - public String toString() - { - return super.toString() + ", originPose = " + originPose; - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/referenceFrames/PoseReferenceFrame.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/referenceFrames/PoseReferenceFrame.java deleted file mode 100644 index fdb1d879b..000000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/referenceFrames/PoseReferenceFrame.java +++ /dev/null @@ -1,274 +0,0 @@ -package us.ihmc.robotics.referenceFrames; - -import us.ihmc.euclid.geometry.interfaces.Pose3DReadOnly; -import us.ihmc.euclid.matrix.RotationMatrix; -import us.ihmc.euclid.orientation.interfaces.Orientation3DBasics; -import us.ihmc.euclid.orientation.interfaces.Orientation3DReadOnly; -import us.ihmc.euclid.referenceFrame.FramePoint2D; -import us.ihmc.euclid.referenceFrame.FramePoint3D; -import us.ihmc.euclid.referenceFrame.FramePose2D; -import us.ihmc.euclid.referenceFrame.FramePose3D; -import us.ihmc.euclid.referenceFrame.FrameQuaternion; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FrameOrientation3DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FrameOrientation3DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; -import us.ihmc.euclid.transform.RigidBodyTransform; -import us.ihmc.euclid.transform.interfaces.RigidBodyTransformReadOnly; -import us.ihmc.euclid.tuple3D.Point3D; -import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; -import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; - -public class PoseReferenceFrame extends ReferenceFrame -{ - private final FramePose3D originPose; - - public PoseReferenceFrame(String frameName, ReferenceFrame parentFrame) - { - super(frameName, parentFrame, parentFrame.isAStationaryFrame(), false); - - originPose = new FramePose3D(parentFrame); - } - - public PoseReferenceFrame(String frameName, FramePose3DReadOnly pose) - { - this(frameName, pose.getReferenceFrame()); - setPoseAndUpdate(pose); - } - - public boolean containsNaN() - { - return originPose.containsNaN(); - } - - public void setX(double x) - { - originPose.setX(x); - } - - public void setY(double y) - { - originPose.setY(y); - } - - public void setZ(double z) - { - originPose.setZ(z); - } - - public void setPoseAndUpdate(FramePose3DReadOnly pose) - { - originPose.set(pose); - this.update(); - } - - public void setPoseAndUpdate(PoseReferenceFrame poseReferenceFrame) - { - originPose.set(poseReferenceFrame.originPose); - this.update(); - } - - public void setPoseAndUpdate(FramePoint3DReadOnly position, FrameOrientation3DReadOnly orientation) - { - originPose.set(position, orientation); - this.update(); - } - - public void setPoseAndUpdate(Point3DReadOnly position, Orientation3DReadOnly orientation) - { - originPose.set(position, orientation); - this.update(); - } - - public void setPoseAndUpdate(Pose3DReadOnly pose) - { - originPose.set(pose); - this.update(); - } - - public void setPoseAndUpdate(RigidBodyTransformReadOnly transformToParent) - { - originPose.set(transformToParent); - this.update(); - } - - public void setPositionWithoutChecksAndUpdate(Point3DReadOnly position) - { - originPose.getPosition().set(position); - this.update(); - } - - public void setPositionWithoutChecksAndUpdate(double x, double y, double z) - { - originPose.getPosition().set(x, y, z); - this.update(); - } - - public void setPositionAndUpdate(FramePoint3DReadOnly framePoint) - { - framePoint.checkReferenceFrameMatch(getParent()); - originPose.getPosition().set(framePoint); - this.update(); - } - - public void setOrientationAndUpdate(Orientation3DReadOnly orientation3D) - { - originPose.getOrientation().set(orientation3D); - this.update(); - } - - public void setOrientationAndUpdate(double qx, double qy, double qz, double qs) - { - originPose.getOrientation().set(qx, qy, qz, qs); - update(); - } - - public void setOrientationAndUpdate(FrameOrientation3DReadOnly frameOrientation) - { - frameOrientation.checkReferenceFrameMatch(getParent()); - originPose.getOrientation().set(frameOrientation); - this.update(); - } - - public void setXYFromPosition2dAndUpdate(FramePoint2DReadOnly position2d) - { - position2d.checkReferenceFrameMatch(getParent()); - originPose.getPosition().set(position2d); - this.update(); - } - - public void translateAndUpdate(double x, double y, double z) - { - originPose.prependTranslation(x, y, z); - this.update(); - } - - public double getX() - { - return originPose.getX(); - } - - public double getY() - { - return originPose.getY(); - } - - public double getZ() - { - return originPose.getZ(); - } - - public double getYaw() - { - return originPose.getYaw(); - } - - public double getPitch() - { - return originPose.getPitch(); - } - - public double getRoll() - { - return originPose.getRoll(); - } - - public void getPose(Point3D pointToPack, Orientation3DBasics quaternionToPack) - { - originPose.get(pointToPack, quaternionToPack); - } - - /** - * Same as the inherited method getTransformToParent(Transform3D) from ReferenceFrame, it is just for readability. - * @param transformToPack - */ - public void getPose(RigidBodyTransform transformToPack) - { - getTransformToParent(transformToPack); - } - - public void getPoseIncludingFrame(FramePoint3D framePointToPack, FrameOrientation3DBasics frameOrientationToPack) - { - originPose.get(framePointToPack, frameOrientationToPack); - } - - public void getPoseIncludingFrame(FramePose3D framePoseToPack) - { - framePoseToPack.setIncludingFrame(originPose); - } - - public Point3DReadOnly getPosition() - { - return originPose.getPosition(); - } - - public void getPosition(Point3D pointToPack) - { - pointToPack.set(originPose.getPosition()); - } - - public void getPositionIncludingFrame(FramePoint3D framePointToPack) - { - framePointToPack.setIncludingFrame(originPose.getPosition()); - } - - public QuaternionReadOnly getOrientation() - { - return originPose.getOrientation(); - } - - public void getOrientation(Orientation3DBasics quaternionToPack) - { - quaternionToPack.set(originPose.getOrientation()); - } - - public void getOrientation(RotationMatrix matrixToPack) - { - matrixToPack.set(originPose.getOrientation()); - } - - public void getOrientationIncludingFrame(FrameQuaternion frameOrientationToPack) - { - frameOrientationToPack.setIncludingFrame(originPose.getOrientation()); - } - - public void getPose2dIncludingFrame(FramePose2D framePose2dToPack) - { - framePose2dToPack.setIncludingFrame(originPose); - } - - public void getPosition2dIncludingFrame(FramePoint2D framePoint2dToPack) - { - framePoint2dToPack.setIncludingFrame(originPose.getPosition()); - } - - public void interpolate(FramePose3D framePose1, FramePose3D framePose2, double alpha) - { - originPose.interpolate(framePose1, framePose2, alpha); - } - - public void interpolate(PoseReferenceFrame poseReferenceFrame1, PoseReferenceFrame poseReferenceFrame2, double alpha) - { - originPose.interpolate(poseReferenceFrame1.originPose, poseReferenceFrame2.originPose, alpha); - } - - public boolean epsilonEquals(PoseReferenceFrame otherPoseReferenceFrame, double epsilon) - { - return originPose.epsilonEquals(otherPoseReferenceFrame.originPose, epsilon); - } - - @Override - protected void updateTransformToParent(RigidBodyTransform transformToParent) - { - originPose.checkReferenceFrameMatch(getParent()); - originPose.get(transformToParent); - } - - @Override - public String toString() - { - return super.toString() + ", originPose = " + originPose; - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/referenceFrames/TranslationReferenceFrame.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/referenceFrames/TranslationReferenceFrame.java deleted file mode 100644 index a0c8c509d..000000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/referenceFrames/TranslationReferenceFrame.java +++ /dev/null @@ -1,38 +0,0 @@ -package us.ihmc.robotics.referenceFrames; - -import us.ihmc.euclid.referenceFrame.FrameVector3D; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; -import us.ihmc.euclid.transform.RigidBodyTransform; -import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; - -public class TranslationReferenceFrame extends ReferenceFrame -{ - public final FrameVector3D originVector; - - public TranslationReferenceFrame(String frameName, ReferenceFrame parentFrame) - { - super(frameName, parentFrame, false, parentFrame.isZupFrame()); - - originVector = new FrameVector3D(parentFrame); - } - - @Override - protected void updateTransformToParent(RigidBodyTransform transformToParent) - { - transformToParent.setIdentity(); - transformToParent.getTranslation().set(originVector); - } - - public void updateTranslation(FrameTuple3DReadOnly frameVector) - { - originVector.set(frameVector); - this.update(); - } - - public void updateTranslation(Tuple3DReadOnly translation) - { - originVector.set(translation); - this.update(); - } -} diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/FramePlane3D.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/FramePlane3D.java new file mode 100644 index 000000000..87f18d735 --- /dev/null +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/FramePlane3D.java @@ -0,0 +1,175 @@ +package us.ihmc.euclid.referenceFrame; + +import us.ihmc.euclid.Axis3D; +import us.ihmc.euclid.geometry.interfaces.Plane3DBasics; +import us.ihmc.euclid.geometry.interfaces.Plane3DReadOnly; +import us.ihmc.euclid.interfaces.Settable; +import us.ihmc.euclid.referenceFrame.interfaces.*; +import us.ihmc.euclid.referenceFrame.tools.EuclidFrameFactories; +import us.ihmc.euclid.tools.EuclidCoreIOTools; +import us.ihmc.euclid.tools.EuclidHashCodeTools; +import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; + +/** + * {@link FramePlane3D} is a 3D plane expressed in a given reference frame. + *

+ * In addition to representing a {@link Plane3DBasics}, a {@link ReferenceFrame} is associated to a + * {@link FramePlane3D}. This allows, for instance, to enforce, at runtime, that operations on planes + * occur in the same coordinate system. Also, via the method {@link #changeFrame(ReferenceFrame)}, + * one can easily calculates the value of a point in different reference frames. + *

+ *

+ * Because a {@link FramePlane3D} extends {@link Plane3DBasics}, it is compatible with methods only + * requiring {@link Plane3DBasics}. However, these methods do NOT assert that the operation occur in + * the proper coordinate system. Use this feature carefully and always prefer using methods + * requiring {@link FramePlane3D}. + *

+ */ +public class FramePlane3D implements FramePlane3DBasics, Settable +{ + /** The reference frame in which this plane is expressed. */ + private ReferenceFrame referenceFrame; + private final FixedFramePoint3DBasics point = EuclidFrameFactories.newFixedFramePoint3DBasics(this); + private final FixedFrameUnitVector3DBasics normal = EuclidFrameFactories.newFixedFrameUnitVector3DBasics(this, Axis3D.Z); + + /** + * Default constructor that initializes its {@code point} to zero, its {@code normal} to + * {@link Axis3D#Z} and the reference frame to {@code ReferenceFrame.getWorldFrame()}. + */ + public FramePlane3D() + { + setToZero(ReferenceFrame.getWorldFrame()); + } + + /** + * Creates a new plane and initializes both {@link #point} and {@link #normal} to zero and the + * reference frame to the given {@code referenceFrame}. + * + * @param referenceFrame the initial reference frame for this plane. + */ + public FramePlane3D(ReferenceFrame referenceFrame) + { + setToZero(referenceFrame); + } + + /** + * Creates a new plane and initializes both {@link #point} and {@link #normal} to the {@param plane3D} and the + * reference frame to the given {@param referenceFrame}. + * + * @param referenceFrame the initial reference frame for this plane. + * @param plane3D the initial value for this plane. + */ + public FramePlane3D(ReferenceFrame referenceFrame, Plane3DReadOnly plane3D) + { + setIncludingFrame(referenceFrame, plane3D); + } + + /** + * Creates a new plane and initializes both {@link #point} and {@link #normal} to the {@param plane3D} and the + * reference frame to the given {@param plane3D}. + * + * @param framePlane3D the initial value and frame for this plane. + */ + public FramePlane3D(FramePlane3DReadOnly framePlane3D) + { + setIncludingFrame(framePlane3D); + } + + /** + * Creates a new plane and initializes both {@link #point} and {@link #normal} to the {@param point} and {@param normal} frame and value. + * + * @param point the initial point for this plane. + * @param normal the initial normal for this plane. + * @throws us.ihmc.euclid.referenceFrame.exceptions.ReferenceFrameMismatchException if the reference frames of {@param point} and {@param normal} are not the same. + */ + public FramePlane3D(FramePoint3DReadOnly point, FrameVector3DReadOnly normal) + { + setIncludingFrame(point, normal); + } + + /** + * Creates a new plane and initializes both {@link #point} and {@link #normal} to the {@param point} and {@param normal} frame and value. + * + * @param referenceFrame the initial + * @param point the initial point for this plane. + * @param normal the initial normal for this plane. + */ + public FramePlane3D(ReferenceFrame referenceFrame, Point3DReadOnly point, Vector3DReadOnly normal) + { + setIncludingFrame(referenceFrame, point, normal); + } + + /** {@inheritDoc} */ + @Override + public void set(FramePlane3D other) + { + FramePlane3DBasics.super.set(other); + } + + /** {@inheritDoc} */ + @Override + public void setReferenceFrame(ReferenceFrame referenceFrame) + { + this.referenceFrame = referenceFrame; + } + + /** {@inheritDoc} */ + @Override + public FixedFramePoint3DBasics getPoint() + { + return point; + } + + /** {@inheritDoc} */ + @Override + public FixedFrameUnitVector3DBasics getNormal() + { + return normal; + } + + /** {@inheritDoc} */ + @Override + public ReferenceFrame getReferenceFrame() + { + return referenceFrame; + } + + /** + * Tests if the given {@code object}'s class is the same as this, in which case the method returns + * {@link #equals(EuclidFrameGeometry)}, it returns {@code false} otherwise. + * + * @param object the object to compare against this. Not modified. + * @return {@code true} if {@code object} and this are exactly equal, {@code false} otherwise. + */ + @Override + public boolean equals(Object object) + { + if (object instanceof FramePlane3DReadOnly) + return equals((EuclidFrameGeometry) object); + else + return false; + } + + /** + * Provides a {@code String} representation of this frame plane 3D as follows:
+ * Plane 3D: point = (x, y, z), normal = (x, y, z)-worldFrame + * + * @return the {@code String} representing this plane 3D. + */ + @Override + public String toString() + { + return toString(EuclidCoreIOTools.DEFAULT_FORMAT); + } + /** + * Calculates and returns a hash code value from the value of each component of this plane 3D. + * + * @return the hash code value for this plane 3D. + */ + @Override + public int hashCode() + { + return EuclidHashCodeTools.toIntHashCode(point, normal); + } +} diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FixedFramePlane3DBasics.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FixedFramePlane3DBasics.java new file mode 100644 index 000000000..66389088a --- /dev/null +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FixedFramePlane3DBasics.java @@ -0,0 +1,103 @@ +package us.ihmc.euclid.referenceFrame.interfaces; + +import us.ihmc.euclid.geometry.interfaces.Plane3DBasics; +import us.ihmc.euclid.geometry.interfaces.Plane3DReadOnly; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; + +public interface FixedFramePlane3DBasics extends FramePlane3DReadOnly, Plane3DBasics +{ + @Override + FixedFramePoint3DBasics getPoint(); + + @Override + FixedFrameUnitVector3DBasics getNormal(); + + /** + * Redefines this plane with a new point and a new normal. + * + * @param referenceFrame the reference frame in which the given plane is expressed. + * @param pointOnPlaneX the new x-coordinate of the point on this plane. + * @param pointOnPlaneY the new y-coordinate of the point on this plane. + * @param pointOnPlaneZ the new z-coordinate of the point on this plane. + * @param planeNormalX the new x-component of the normal of this plane. + * @param planeNormalY the new y-component of the normal of this plane. + * @param planeNormalZ the new z-component of the normal of this plane. + */ + default void set(ReferenceFrame referenceFrame, double pointOnPlaneX, double pointOnPlaneY, double pointOnPlaneZ, double planeNormalX, double planeNormalY, double planeNormalZ) + { + checkReferenceFrameMatch(referenceFrame); + set(pointOnPlaneX, pointOnPlaneY, pointOnPlaneZ, planeNormalX, planeNormalY, planeNormalZ); + } + + /** + * Sets this plane to be the same as the given plane. + * + * @param other the other plane to copy. Not modified. + */ + default void set(FramePlane3DReadOnly other) + { + checkReferenceFrameMatch(other.getReferenceFrame()); + Plane3DBasics.super.set(other); + } + + /** + * Sets this plane to be the same as the given plane, and then transforms the data to match the current frame {@link #getReferenceFrame()}. + * + * @param other the other plane to copy. Not modified. + */ + default void setMatchingFrame(FramePlane3DReadOnly other) + { + setMatchingFrame(other.getReferenceFrame(), other); + } + + /** + * Sets this plane to be the same as the given plane. + * + * @param referenceFrame the reference frame in which the given plane is expressed. + * @param other the other plane to copy. Not modified. + */ + default void set(ReferenceFrame referenceFrame, Plane3DReadOnly other) + { + checkReferenceFrameMatch(referenceFrame); + Plane3DBasics.super.set(other); + } + + /** + * Sets this plane to be the same as the given plane, and then transforms the data to match the current frame {@link #getReferenceFrame()}. + * + * @param referenceFrame the reference frame in which the given plane is expressed. + * @param other the other plane to copy. Not modified. + */ + default void setMatchingFrame(ReferenceFrame referenceFrame, Plane3DReadOnly other) + { + Plane3DBasics.super.set(other); + referenceFrame.transformFromThisToDesiredFrame(getReferenceFrame(), this); + } + + /** + * Redefines this plane such that it goes through the three given points. + * + * @param firstPointOnPlane first point on this plane. Not modified. + * @param secondPointOnPlane second point on this plane. Not modified. + * @param thirdPointOnPlane second point on this plane. Not modified. + * @return {@code true} if the method succeeded, {@code false} if the method failed to estimate the + * normal. + */ + default boolean set(FramePoint3DReadOnly firstPointOnPlane, FramePoint3DReadOnly secondPointOnPlane, FramePoint3DReadOnly thirdPointOnPlane) + { + checkReferenceFrameMatch(firstPointOnPlane, secondPointOnPlane, thirdPointOnPlane); + return Plane3DBasics.super.set(firstPointOnPlane, secondPointOnPlane, thirdPointOnPlane); + } + + /** + * Redefines this plane with a new point and a new normal. + * + * @param pointOnPlane new point on this plane. Not modified. + * @param planeNormal new normal of this plane. Not modified. + */ + default void set(FramePoint3DReadOnly pointOnPlane, FrameVector3DReadOnly planeNormal) + { + checkReferenceFrameMatch(pointOnPlane, planeNormal); + Plane3DBasics.super.set(pointOnPlane, planeNormal); + } +} diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FramePlane3DBasics.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FramePlane3DBasics.java new file mode 100644 index 000000000..2dd6b4003 --- /dev/null +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FramePlane3DBasics.java @@ -0,0 +1,147 @@ +package us.ihmc.euclid.referenceFrame.interfaces; + +import us.ihmc.euclid.Axis3D; +import us.ihmc.euclid.geometry.interfaces.Plane3DReadOnly; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.exceptions.ReferenceFrameMismatchException; +import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; + +public interface FramePlane3DBasics extends FixedFramePlane3DBasics, FrameChangeable +{ + /** + * Sets the reference frame of this plane 3D without updating or modifying its point or direction. + * + * @param referenceFrame the new reference frame for this frame plane 3D. + */ + @Override + void setReferenceFrame(ReferenceFrame referenceFrame); + + /** + * Sets the point of this plane to zero, its normal to {@link Axis3D#Z}, and sets the current + * reference frame to {@code referenceFrame}. + * + * @param referenceFrame the new reference frame to be associated with this plane 3D. + */ + default void setToZero(ReferenceFrame referenceFrame) + { + setReferenceFrame(referenceFrame); + setToZero(); + } + + /** + * Sets the point and direction parts of this plane 3D to {@link Double#NaN} and sets the current + * reference frame to {@code referenceFrame}. + * + * @param referenceFrame the new reference frame to be associated with this plane 3D. + */ + default void setToNaN(ReferenceFrame referenceFrame) + { + setReferenceFrame(referenceFrame); + setToNaN(); + } + + /** + * Redefines this plane with a new point, a new normal vector, and a new reference frame. + * + * @param referenceFrame the new reference frame for this frame plane. + * @param pointOnPlaneX the new x-coordinate of the point on this plane. + * @param pointOnPlaneY the new y-coordinate of the point on this plane. + * @param pointOnPlaneZ the new z-coordinate of the point on this plane. + * @param planeNormalX the new x-component of the normal of this plane. + * @param planeNormalY the new y-component of the normal of this plane. + * @param planeNormalZ the new z-component of the normal of this plane. + */ + default void setIncludingFrame(ReferenceFrame referenceFrame, + double pointOnPlaneX, + double pointOnPlaneY, + double pointOnPlaneZ, + double planeNormalX, + double planeNormalY, + double planeNormalZ) + { + setReferenceFrame(referenceFrame); + set(pointOnPlaneX, pointOnPlaneY, pointOnPlaneZ, planeNormalX, planeNormalY, planeNormalZ); + } + + /** + * Sets this plane to be the same as the given plane. + * + * @param referenceFrame the new reference frame for this frame plane. + * @param plane3DReadOnly the plane to copy. Not modified. + */ + default void setIncludingFrame(ReferenceFrame referenceFrame, Plane3DReadOnly plane3DReadOnly) + { + setReferenceFrame(referenceFrame); + set(plane3DReadOnly); + } + + /** + * Redefines this plane with a new point, a new normal vector, and a new reference frame. + * + * @param referenceFrame the new reference frame for this frame plane. + * @param pointOnPlane new point on this plane. Not modified. + * @param planeNormal new direction of this plane. Not modified. + */ + default void setIncludingFrame(ReferenceFrame referenceFrame, Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal) + { + setReferenceFrame(referenceFrame); + set(pointOnPlane, planeNormal); + } + + /** + * Redefines this plane such that it intersects the three given points in the given reference frame. + * + * @param referenceFrame the new reference frame for this frame plane. + * @param firstPointOnPlane first point on this plane. Not modified. + * @param secondPointOnPlane second point on this plane. Not modified. + * @param thirdPointOnPlane second point on this plane. Not modified. + */ + default void setIncludingFrame(ReferenceFrame referenceFrame, + Point3DReadOnly firstPointOnPlane, + Point3DReadOnly secondPointOnPlane, + Point3DReadOnly thirdPointOnPlane) + { + setReferenceFrame(referenceFrame); + set(firstPointOnPlane, secondPointOnPlane, thirdPointOnPlane); + } + + /** + * Sets this plane to be the same as the given plane. + * + * @param other the other plane to copy. Not modified. + */ + default void setIncludingFrame(FramePlane3DReadOnly other) + { + setIncludingFrame(other.getReferenceFrame(), other); + } + + /** + * Redefines this plane with a new point, a new direction vector, and a new reference frame. + * + * @param pointOnPlane new point on this plane. Not modified. + * @param normalVector new direction of this plane. Not modified. + * @throws ReferenceFrameMismatchException if {@code pointOnPlane} and {@code normalVector} are not + * expressed in the same reference frame + */ + default void setIncludingFrame(FramePoint3DReadOnly pointOnPlane, FrameVector3DReadOnly normalVector) + { + pointOnPlane.checkReferenceFrameMatch(normalVector); + setIncludingFrame(pointOnPlane.getReferenceFrame(), pointOnPlane, normalVector); + } + + /** + * Redefines this plane such that it intersects the three given points in the given reference frame. + * + * @param firstPointOnPlane first point on this plane. Not modified. + * @param secondPointOnPlane second point on this plane. Not modified. + * @param thirdPointOnPlane second point on this plane. Not modified. + * @throws ReferenceFrameMismatchException if {@code firstPointOnPlane}, {@code secondPointOnPlane}, and {@code thirdPointOnPlane} + * are not expressed in the same reference frame + */ + default void setIncludingFrame(FramePoint3DReadOnly firstPointOnPlane, FramePoint3DReadOnly secondPointOnPlane, FramePoint3DReadOnly thirdPointOnPlane) + { + firstPointOnPlane.checkReferenceFrameMatch(secondPointOnPlane, thirdPointOnPlane); + setIncludingFrame(firstPointOnPlane.getReferenceFrame(), firstPointOnPlane, secondPointOnPlane, thirdPointOnPlane); + } +} diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FramePlane3DReadOnly.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FramePlane3DReadOnly.java new file mode 100644 index 000000000..5453ad95d --- /dev/null +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FramePlane3DReadOnly.java @@ -0,0 +1,300 @@ +package us.ihmc.euclid.referenceFrame.interfaces; + +import us.ihmc.euclid.geometry.interfaces.Plane3DReadOnly; +import us.ihmc.euclid.geometry.tools.EuclidGeometryTools; +import us.ihmc.euclid.interfaces.EuclidGeometry; +import us.ihmc.euclid.referenceFrame.exceptions.ReferenceFrameMismatchException; +import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; + +public interface FramePlane3DReadOnly extends Plane3DReadOnly, EuclidFrameGeometry +{ + @Override + FramePoint3DReadOnly getPoint(); + + @Override + FrameUnitVector3DReadOnly getNormal(); + + /** + * Computes the minimum distance to the given 3D point and this plane. Frame of the point must be in this {@link #getReferenceFrame()}. If not, throws + * {@link ReferenceFrameMismatchException}. + * + * @param point - 3D point to compute the distance from the plane. Not modified. + * @return the minimum distance between the 3D point and this 3D plane. + */ + default double distance(FramePoint3DReadOnly point) + { + checkReferenceFrameMatch(point); + return distance((Point3DReadOnly) point); + } + + /** + * Computes the minimum signed distance to the given 3D point and this plane. Frame of the point must be in this {@link #getReferenceFrame()}. If not, throws + * {@link ReferenceFrameMismatchException}. The returned value is negative when the query is located below thep lane, positive otherwise. + * + * @param point - 3D point to compute the distance from the plane. Not modified. + * @return the minimum distance between the 3D point and this 3D plane. + */ + default double signedDistance(FramePoint3DReadOnly point) + { + checkReferenceFrameMatch(point); + return signedDistance((Point3DReadOnly) point); + } + + /** + * Computes the coordinates of the intersection between this plane and an infinitely long 3D line. + *

+ * Edge cases: + *

+ *

+ * + * @param line the line that may intersect this plane. Not modified. + * @param intersectionToPack point in which the coordinates of the intersection are stored. + * @return {@code true} if the method succeeds, {@code false} otherwise. + */ + default boolean intersectionWith(FrameLine3DReadOnly line, FramePoint3DBasics intersectionToPack) + { + checkReferenceFrameMatch(line, intersectionToPack); + return intersectionWith(intersectionToPack, line.getPoint(), line.getDirection()); + } + + /** + * Computes the coordinates of the intersection between this plane and an infinitely long 3D line. + *

+ * Edge cases: + *

+ *

+ * + * @param pointOnLine a point located on the line. Not modified. + * @param lineDirection the direction of the line. Not modified. + * @param intersectionToPack point in which the coordinates of the intersection are stored. + * @return {@code true} if the method succeeds, {@code false} otherwise. + */ + default boolean intersectionWith(FramePoint3DBasics intersectionToPack, FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection) + { + checkReferenceFrameMatch(intersectionToPack, pointOnLine, lineDirection); + return EuclidGeometryTools.intersectionBetweenLine3DAndPlane3D(getPoint(), getNormal(), pointOnLine, lineDirection, intersectionToPack); + } + + /** + * Tests if this plane and the given plane are coincident: + * + *

+ * Edge cases: + *

+ *

+ * + * @param otherPlane the other plane to do the test with. Not modified. + * @param angleEpsilon tolerance on the angle in radians to determine if the plane normals are + * collinear. + * @param distanceEpsilon tolerance on the distance to determine if {@code otherPlane.point} belongs + * to this plane. + * @return {@code true} if the two planes are coincident, {@code false} otherwise. + * @throws IllegalArgumentException if angleEpsilon ∉ [0; pi/2] + */ + default boolean isCoincident(FramePlane3DReadOnly otherPlane, double angleEpsilon, double distanceEpsilon) + { + checkReferenceFrameMatch(otherPlane); + return EuclidGeometryTools.arePlane3DsCoincident(getPoint(), getNormal(), otherPlane.getPoint(), otherPlane.getNormal(), angleEpsilon, distanceEpsilon); + } + + /** + * Tests if the query point is located strictly on or above this plane. + *

+ * Above is defined as the side of the plane toward which the normal is pointing. + *

+ * + * @param pointToTest the coordinates of the query. Not modified. + * @return {@code true} if the query is strictly on or above this plane, {@code false} otherwise. + */ + default boolean isOnOrAbove(FramePoint3DReadOnly pointToTest) + { + return isOnOrAbove(pointToTest, 0.0); + } + + /** + * Tests if the query point is located on or above this plane given the tolerance {@code epsilon}. + *

+ * Above is defined as the side of the plane toward which the normal is pointing. + *

+ *

+ *

    + *
  1. if {@code epsilon == 0}, the query has to be either exactly on the plane or strictly above + * for this method to return {@code true}. + *
  2. if {@code epsilon > 0}, this method returns {@code true} if the query meets the requirements + * of 1., in addition, this method returns also {@code true} if the query is below the plane at a + * distance less or equal than {@code epsilon}. + *
  3. if {@code epsilon < 0}, this method returns {@code true} only if the query is above the plane + * and at a distance of at least {@code Math.abs(epsilon)}. + *
+ *

+ * + * @param pointToTest the coordinates of the query. Not modified. + * @param epsilon the tolerance to use for the test. + * @return {@code true} if the query is considered to be on or above this plane, {@code false} + * otherwise. + */ + default boolean isOnOrAbove(FramePoint3DReadOnly pointToTest, double epsilon) + { + checkReferenceFrameMatch(pointToTest); + return isOnOrAbove(pointToTest.getX(), pointToTest.getY(), pointToTest.getZ(), epsilon); + } + + /** + * Tests if the query point is located strictly on or below this plane. + *

+ * Below is defined as the side of the plane which the normal is pointing away from. + *

+ * + * @param pointToTest the coordinates of the query. Not modified. + * @return {@code true} if the query is strictly on or below this plane, {@code false} otherwise. + */ + default boolean isOnOrBelow(FramePoint3DReadOnly pointToTest) + { + return isOnOrBelow(pointToTest, 0.0); + } + + /** + * Tests if the query point is located on or below this plane given the tolerance {@code epsilon}. + *

+ * Below is defined as the side of the plane which the normal is pointing away from. + *

+ *

+ *

    + *
  1. if {@code epsilon == 0}, the query has to be either exactly on the plane or strictly below + * for this method to return {@code true}. + *
  2. if {@code epsilon > 0}, this method returns {@code true} if the query meets the requirements + * of 1., in addition, this method returns also {@code true} if the query is above the plane at a + * distance less or equal than {@code epsilon}. + *
  3. if {@code epsilon < 0}, this method returns {@code true} only if the query is below the plane + * and at a distance of at least {@code Math.abs(epsilon)}. + *
+ *

+ * + * @param pointToTest the coordinates of the query. Not modified. + * @param epsilon the tolerance to use for the test. + * @return {@code true} if the query is considered to be on or below this plane, {@code false} + * otherwise. + */ + default boolean isOnOrBelow(FramePoint3DReadOnly pointToTest, double epsilon) + { + checkReferenceFrameMatch(pointToTest); + return isOnOrBelow(pointToTest.getX(), pointToTest.getY(), pointToTest.getZ(), epsilon); + } + + + /** + * Tests if the two planes are parallel by testing if their normals are collinear. The latter is + * done given a tolerance on the angle between the two normal axes in the range ]0; pi/2[. + *

+ * Edge cases: + *

+ *

+ * + * @param otherPlane the other plane to do the test with. Not modified. + * @param angleEpsilon tolerance on the angle in radians. + * @return {@code true} if the two planes are parallel, {@code false} otherwise. + * @throws IllegalArgumentException if angleEpsilon ∉ [0; pi/2] + */ + default boolean isParallel(FramePlane3DReadOnly otherPlane, double angleEpsilon) + { + checkReferenceFrameMatch(otherPlane); + return EuclidGeometryTools.areVector3DsParallel(getNormal(), otherPlane.getNormal(), angleEpsilon); + } + + /** + * Computes the orthogonal projection of the given 3D point on this 3D plane. + * + * @param pointToProject the point to project on this plane. Modified. + * @return whether the method succeeded or not. + */ + default boolean orthogonalProjection(FramePoint3DBasics pointToProject) + { + checkReferenceFrameMatch(pointToProject); + return orthogonalProjection(pointToProject, pointToProject); + } + + /** + * Computes the orthogonal projection of the given 3D point on this 3D plane. + * + * @param pointToProject the point to compute the projection of. Not modified. + * @param projectionToPack point in which the projection of the point onto the plane is stored. + * Modified. + * @return whether the method succeeded or not. + */ + default boolean orthogonalProjection(FramePoint3DReadOnly pointToProject, FramePoint3DBasics projectionToPack) + { + checkReferenceFrameMatch(pointToProject, projectionToPack); + return EuclidGeometryTools.orthogonalProjectionOnPlane3D(pointToProject, getPoint(), getNormal(), projectionToPack); + } + + + /** + * {@inheritDoc} + *

+ * This method will return {@code false} if the two planes are physically the same but either the + * point or vector of each plane is different. For instance, if {@code this.point == other.point} + * and {@code this.normal == - other.normal}, the two planes are physically the same but this method + * returns {@code false}. + *

+ */ + @Override + default boolean epsilonEquals(EuclidFrameGeometry geometry, double epsilon) + { + Plane3DReadOnly.super.epsilonEquals(geometry, epsilon); + if (geometry == this) + return true; + if (geometry == null) + return false; + if (!(geometry instanceof FramePlane3DReadOnly other)) + return false; + if (getReferenceFrame() != other.getReferenceFrame()) + return false; + return other.getNormal().epsilonEquals(getNormal(), epsilon) && other.getPoint().epsilonEquals(getPoint(), epsilon); + } + + /** {@inheritDoc} */ + @Override + default boolean equals(EuclidGeometry geometry) + { + if (geometry == this) + return true; + if (geometry == null) + return false; + if (!(geometry instanceof FramePlane3DReadOnly other)) + return false; + if (getReferenceFrame() != other.getReferenceFrame()) + return false; + return getPoint().equals(other.getPoint()) && getNormal().equals(other.getNormal()); + } + + /** {@inheritDoc} */ + @Override + default boolean geometricallyEquals(EuclidGeometry geometry, double epsilon) + { + if (geometry == this) + return true; + if (geometry == null) + return false; + if (!(geometry instanceof FramePlane3DReadOnly other)) + return false; + if (getReferenceFrame() != other.getReferenceFrame()) + return false; + return isCoincident(other, epsilon, epsilon); + } + +} diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameShapeTools.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameShapeTools.java index 27c60a2e6..eefdf0c27 100644 --- a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameShapeTools.java +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameShapeTools.java @@ -1,16 +1,15 @@ package us.ihmc.euclid.referenceFrame.tools; import us.ihmc.euclid.geometry.interfaces.BoundingBox3DBasics; +import us.ihmc.euclid.geometry.interfaces.Line3DBasics; +import us.ihmc.euclid.geometry.interfaces.Plane3DReadOnly; +import us.ihmc.euclid.geometry.tools.EuclidGeometryTools; import us.ihmc.euclid.matrix.interfaces.RotationMatrixBasics; import us.ihmc.euclid.matrix.interfaces.RotationMatrixReadOnly; +import us.ihmc.euclid.referenceFrame.FrameVector3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FrameBox3DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FrameCapsule3DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FrameCylinder3DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FrameEllipsoid3DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FrameRamp3DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FrameSphere3DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FrameTorus3DReadOnly; +import us.ihmc.euclid.referenceFrame.exceptions.ReferenceFrameMismatchException; +import us.ihmc.euclid.referenceFrame.interfaces.*; import us.ihmc.euclid.referenceFrame.polytope.interfaces.FrameConvexPolytope3DReadOnly; import us.ihmc.euclid.shape.convexPolytope.interfaces.ConvexPolytope3DReadOnly; import us.ihmc.euclid.shape.convexPolytope.interfaces.Vertex3DReadOnly; @@ -47,6 +46,273 @@ private EuclidFrameShapeTools() // Suppresses default constructor, ensuring non-instantiability. } + /** + * Returns the minimum XY distance between a 3D point and an infinitely long 3D line defined by two + * points. + *

+ * Edge cases: + *

+ *

+ *

+ * WARNING: the 3D arguments are projected onto the XY-plane to perform the actual computation in + * 2D. + *

+ * + * @param point the 3D point is projected onto the xy-plane. It's projection is used to + * compute the distance from the line. Not modified. + * @param firstPointOnLine the projection of this 3D onto the xy-plane refers to the first point on + * the 2D line. Not modified. + * @param secondPointOnLine the projection of this 3D onto the xy-plane refers to the second point + * one the 2D line. Not modified. + * @return the minimum distance between the 2D point and the 2D line. + * @throws ReferenceFrameMismatchException if the arguments are not expressed in the same reference + * frame. + */ + public static double distanceXYFromPoint3DToLine3D(FramePoint3DReadOnly point, FramePoint3DReadOnly firstPointOnLine, FramePoint3DReadOnly secondPointOnLine) + { + point.checkReferenceFrameMatch(firstPointOnLine); + point.checkReferenceFrameMatch(secondPointOnLine); + + double pointOnLineX = firstPointOnLine.getX(); + double pointOnLineY = firstPointOnLine.getY(); + double lineDirectionX = secondPointOnLine.getX() - firstPointOnLine.getX(); + double lineDirectionY = secondPointOnLine.getY() - firstPointOnLine.getY(); + return EuclidGeometryTools.distanceFromPoint2DToLine2D(point.getX(), point.getY(), pointOnLineX, pointOnLineY, lineDirectionX, lineDirectionY); + } + + /** + * Test if a given line segment intersects a given plane. + *

+ * Edge cases: + *

+ *

+ * + * @param pointOnPlane a point located on the plane. Not modified. + * @param planeNormal the normal of the plane. Not modified. + * @param lineSegmentStart first endpoint of the line segment. Not modified. + * @param lineSegmentEnd second endpoint of the line segment. Not modified. + * @return {@code true} if an intersection line segment - plane exists, {@code false} otherwise. + * @throws ReferenceFrameMismatchException if the arguments are not expressed in the same reference + * frame. + */ + public static boolean isLineSegmentIntersectingPlane(FramePoint3DReadOnly pointOnPlane, + FrameVector3DReadOnly planeNormal, + FramePoint3DReadOnly lineSegmentStart, + FramePoint3DReadOnly lineSegmentEnd) + { + pointOnPlane.checkReferenceFrameMatch(planeNormal, lineSegmentStart, lineSegmentEnd); + + return EuclidGeometryTools.doesLineSegment3DIntersectPlane3D(pointOnPlane, planeNormal, lineSegmentStart, lineSegmentEnd); + } + + /** + * Computes the minimum distance between a given point and a plane. + * + * @param point the 3D query. Not modified. + * @param pointOnPlane a point located on the plane. Not modified. + * @param planeNormal the normal of the plane. Not modified. + * @return the distance between the point and the plane. + * @throws ReferenceFrameMismatchException if the arguments are not expressed in the same reference + * frame. + */ + public static double distanceFromPointToPlane(FramePoint3DReadOnly point, FramePoint3DReadOnly pointOnPlane, FrameVector3DReadOnly planeNormal) + { + point.checkReferenceFrameMatch(pointOnPlane, planeNormal); + + return EuclidGeometryTools.distanceFromPoint3DToPlane3D(point, pointOnPlane, planeNormal); + } + + /** + * This methods calculates the line of intersection between two planes each defined by a point and a normal. The result is packed in a 3D point located on the intersection line and the 3D direction of the intersection. + * Useful link 1 , useful link 2 . + * Edge cases: + * When the length of either the plane normal is below ONE_TRILLIONTH, this methods fails and returns false. + * When the angle between the two planes is below ONE_MILLIONTH, this methods fails and returns false. + * When there is no intersection, this method returns false and pointOnIntersectionToPack and intersectionDirectionToPack are set to Double. NaN. + * + * Uses + * {@link EuclidGeometryTools#intersectionBetweenTwoPlane3Ds(Point3DReadOnly, Vector3DReadOnly, Point3DReadOnly, Vector3DReadOnly, Point3DBasics, Vector3DBasics)} + * + * @param plane1 first plane of which to compute the intersection + * @param plane2 second plane of which to compute the inersection + * @param intersectionToPack line of intersection between the two planes + * @return success (not parallel) + */ + public static boolean getIntersectionBetweenTwoPlanes(Plane3DReadOnly plane1, Plane3DReadOnly plane2, Line3DBasics intersectionToPack) + { + return EuclidGeometryTools.intersectionBetweenTwoPlane3Ds(plane1.getPoint(), + plane1.getNormal(), + plane2.getPoint(), + plane2.getNormal(), + intersectionToPack.getPoint(), + intersectionToPack.getDirection()); + } + + /** + * Computes the normal of a plane that is defined by three points. + *

+ * Edge cases: + *

+ *

+ *

+ * WARNING: This method generates garbage. + *

+ * + * @param firstPointOnPlane first point on the plane. Not modified. + * @param secondPointOnPlane second point on the plane. Not modified. + * @param thirdPointOnPlane third point on the plane. Not modified. + * @return the plane normal or {@code null} when the normal could not be determined. + * @throws ReferenceFrameMismatchException if the arguments are not expressed in the same reference + * frame. + */ + public static FrameVector3D getPlaneNormalGivenThreePoints(FramePoint3DReadOnly firstPointOnPlane, + FramePoint3DReadOnly secondPointOnPlane, + FramePoint3DReadOnly thirdPointOnPlane) + { + FrameVector3D normal = new FrameVector3D(); + boolean success = getPlaneNormalGivenThreePoints(firstPointOnPlane, secondPointOnPlane, thirdPointOnPlane, normal); + if (!success) + return null; + else + return normal; + } + + /** + * Computes the normal of a plane that is defined by three points. + *

+ * Edge cases: + *

+ *

+ * + * @param firstPointOnPlane first point on the plane. Not modified. + * @param secondPointOnPlane second point on the plane. Not modified. + * @param thirdPointOnPlane third point on the plane. Not modified. + * @param normalToPack the vector in which the result is stored. Modified. + * @return whether the plane normal is properly determined. + * @throws ReferenceFrameMismatchException if the arguments are not expressed in the same reference + * frame, except for {@code normalToPack}. + */ + public static boolean getPlaneNormalGivenThreePoints(FramePoint3DReadOnly firstPointOnPlane, + FramePoint3DReadOnly secondPointOnPlane, + FramePoint3DReadOnly thirdPointOnPlane, + FrameVector3DBasics normalToPack) + { + firstPointOnPlane.checkReferenceFrameMatch(secondPointOnPlane, thirdPointOnPlane); + + normalToPack.setToZero(firstPointOnPlane.getReferenceFrame()); + + return EuclidGeometryTools.normal3DFromThreePoint3Ds(firstPointOnPlane, secondPointOnPlane, thirdPointOnPlane, normalToPack); + } + + /** + * Computes the perpendicular defined by an infinitely long 3D line (defined by two 3D points) and a + * 3D point. To do so, the orthogonal projection of the {@code point} on line is first computed. The + * perpendicular vector is computed as follows: + * {@code perpendicularVector = point - orthogonalProjection}, resulting in a vector going from the + * computed projection to the given {@code point} with a length equal to the distance between the + * point and the line. + *

+ * Edge cases: + *

+ *

+ *

+ * WARNING: This method generates garbage. + *

+ * + * @param point the 3D point towards which the perpendicular vector should be + * pointing at. Not modified. + * @param firstPointOnLine a first point on the line. Not modified. + * @param secondPointOnLine a second point on the line. Not modified. + * @param orthogonalProjectionToPack a 3D point in which the projection of {@code point} onto the + * line is stored. Modified. Can be {@code null}. + * @return the vector perpendicular to the line and pointing to the {@code point}, or {@code null} + * when the method fails. + * @throws ReferenceFrameMismatchException if the arguments are not expressed in the same reference + * frame, except for {@code orthogonalProjectionToPack}. + */ + public static FrameVector3D getPerpendicularVectorFromLineToPoint(FramePoint3DReadOnly point, + FramePoint3DReadOnly firstPointOnLine, + FramePoint3DReadOnly secondPointOnLine, + FramePoint3DBasics orthogonalProjectionToPack) + { + FrameVector3D perpendicularVector = new FrameVector3D(); + + boolean success = getPerpendicularVectorFromLineToPoint(point, firstPointOnLine, secondPointOnLine, orthogonalProjectionToPack, perpendicularVector); + if (!success) + return null; + else + return perpendicularVector; + } + + /** + * Computes the perpendicular defined by an infinitely long 3D line (defined by two 3D points) and a + * 3D point. To do so, the orthogonal projection of the {@code point} on line is first computed. The + * perpendicular vector is computed as follows: + * {@code perpendicularVector = point - orthogonalProjection}, resulting in a vector going from the + * computed projection to the given {@code point} with a length equal to the distance between the + * point and the line. + *

+ * Edge cases: + *

+ *

+ * + * @param point the 3D point towards which the perpendicular vector should be + * pointing at. Not modified. + * @param firstPointOnLine a first point on the line. Not modified. + * @param secondPointOnLine a second point on the line. Not modified. + * @param orthogonalProjectionToPack a 3D point in which the projection of {@code point} onto the + * line is stored. Modified. Can be {@code null}. + * @param perpendicularVectorToPack a 3D vector in which the vector perpendicular to the line and + * pointing to the {@code point} is stored. Modified. Can NOT be + * {@code null}. + * @return {@code true} if the method succeeded, {@code false} otherwise. + * @throws ReferenceFrameMismatchException if the arguments are not expressed in the same reference + * frame, except for {@code orthogonalProjectionToPack} and + * {@code perpendicularVectorToPack}. + */ + public static boolean getPerpendicularVectorFromLineToPoint(FramePoint3DReadOnly point, + FramePoint3DReadOnly firstPointOnLine, + FramePoint3DReadOnly secondPointOnLine, + FramePoint3DBasics orthogonalProjectionToPack, + FrameVector3DBasics perpendicularVectorToPack) + { + point.checkReferenceFrameMatch(firstPointOnLine, secondPointOnLine); + perpendicularVectorToPack.setReferenceFrame(point.getReferenceFrame()); + + if (orthogonalProjectionToPack == null) + { + return EuclidGeometryTools.perpendicularVector3DFromLine3DToPoint3D(point, firstPointOnLine, secondPointOnLine, null, perpendicularVectorToPack); + } + else + { + orthogonalProjectionToPack.setReferenceFrame(point.getReferenceFrame()); + return EuclidGeometryTools.perpendicularVector3DFromLine3DToPoint3D(point, + firstPointOnLine, + secondPointOnLine, + orthogonalProjectionToPack, + perpendicularVectorToPack); + } + } + private static interface BoundingBoxRotationPartCalculator { void computeBoundingBoxZeroRotation(T shape, BoundingBox3DBasics boundingBoxToPack); diff --git a/src/frame/java/us/ihmc/euclid/referenceFrame/Pose2DReferenceFrame.java b/src/frame/java/us/ihmc/euclid/referenceFrame/Pose2DReferenceFrame.java new file mode 100644 index 000000000..cfa40d96e --- /dev/null +++ b/src/frame/java/us/ihmc/euclid/referenceFrame/Pose2DReferenceFrame.java @@ -0,0 +1,183 @@ +package us.ihmc.euclid.referenceFrame; + +import us.ihmc.euclid.geometry.interfaces.Pose2DReadOnly; +import us.ihmc.euclid.orientation.interfaces.Orientation2DReadOnly; +import us.ihmc.euclid.referenceFrame.exceptions.ReferenceFrameMismatchException; +import us.ihmc.euclid.referenceFrame.interfaces.*; +import us.ihmc.euclid.transform.RigidBodyTransform; +import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly; + +/** + * This defines a mutable {@link ReferenceFrame}, where the pose of this frame relative to the parent {@link ReferenceFrame}, which is accessible as + * {@link #getParent()} can be updated via a variety of methods, but exists only in the 2D plane. + */ +public class Pose2DReferenceFrame extends ReferenceFrame +{ + /** + * The 2D pose of this frame relative to its parent. + */ + private final FramePose2D originPose; + + /** + *

+ * Creates a new pose 2D reference frame as a child of the given {@param parentFrame} and initializes the transform to its parent frame. + *

+ *

+ * This new reference frame is defined in the {@param parentFrame} and moves with it. + *

+ *

+ * Its pose with respect to the parent frame can be modified at runtime by changing the transform in the methods outlined in this class, including setting + * the pose {@link #setPoseAndUpdate(FramePose2DReadOnly)}, setting the pose from a reference frame {@link #setPoseAndUpdate(Pose2DReferenceFrame)}, setting + * the translation {@link #setPositionAndUpdate(FramePoint2DReadOnly)}, or setting the orientation + * {@link #setOrientationAndUpdate(FrameOrientation2DReadOnly)}. + *

+ * + * @param frameName the name of the new frame. + * @param parentFrame the parent of the frame. It has to extend a {@link ReferenceFrame}. + */ + public Pose2DReferenceFrame(String frameName, ReferenceFrame parentFrame) + { + super(frameName, parentFrame); + + originPose = new FramePose2D(parentFrame); + } + + public Pose2DReferenceFrame(String frameName, FramePose2DReadOnly poseInParentFrame) + { + this(frameName, poseInParentFrame.getReferenceFrame()); + setPoseAndUpdate(poseInParentFrame); + } + + /** {@inheritDoc} */ + @Override + protected void updateTransformToParent(RigidBodyTransform transformToParent) + { + originPose.get(transformToParent); + } + + /** + * Sets the pose of this reference frame relative to its parent. The frame of the passed in tuple {@param pose} must be defined as + * {@link #getParent()}. If it is not, this method will throw a {@link ReferenceFrameMismatchException}. To avoid performing a frame check, please use + * {@link #setPoseAndUpdate(Pose2DReadOnly)}. + * + * @param pose pose of this frame relative to parent to set. + */ + public void setPoseAndUpdate(FramePose2DReadOnly pose) + { + pose.checkReferenceFrameMatch(getParent()); + setPoseAndUpdate((Pose2DReadOnly) pose); + } + + /** + * Sets the position of this reference frame relative to its parent. The frame of the passed in tuple {@param position} must be defined + * as {@link #getParent()}. If it is not, this method will throw a {@link ReferenceFrameMismatchException}. To avoid performing a frame check, please use + * {@link #setPositionAndUpdate(Point2DReadOnly)}. + * + * @param position position of this frame relative to parent to set. + */ + public void setPositionAndUpdate(FramePoint2DReadOnly position) + { + position.checkReferenceFrameMatch(getParent()); + setPositionAndUpdate((Point2DReadOnly) position); + } + + /** + * Sets the orientation of this reference frame relative to its parent. The frame of the passed in tuple {@param orientation} must be defined + * as {@link #getParent()}. If it is not, this method will throw a {@link ReferenceFrameMismatchException}. To avoid performing a frame check, please use + * {@link #setOrientationAndUpdate(Orientation2DReadOnly)}. + * + * @param orientation orientation of this frame relative to parent to set. + */ + public void setOrientationAndUpdate(FrameOrientation2DReadOnly orientation) + { + orientation.checkReferenceFrameMatch(getParent()); + setOrientationAndUpdate((Orientation2DReadOnly) orientation); + } + + /** + * Sets the pose of this reference frame relative to its parent. The frame of the passed in tuples {@param position} and {@param orientation} must be defined + * as {@link #getParent()}. If it is not, this method will throw a {@link ReferenceFrameMismatchException}. To avoid performing a frame check, please use + * {@link #setPoseAndUpdate(Pose2DReadOnly)} or {@link #setPoseAndUpdate(Point2DReadOnly, Orientation2DReadOnly)}. + * + * @param position position of this frame relative to parent to set. + * @param orientation orientation of this frame relative to parent to set. + */ + public void setPoseAndUpdate(FramePoint2DReadOnly position, FrameOrientation2DReadOnly orientation) + { + position.checkReferenceFrameMatch(getParent()); + orientation.checkReferenceFrameMatch(getParent()); + setPoseAndUpdate((Point2DReadOnly) position, (Orientation2DReadOnly) orientation); + } + + /** + * Sets the pose of this reference frame relative to its parent. The frame of the passed in {@param pose} must be defined as + * {@link #getParent()}. This method does not perform a frame check. For frame safe operations, see {@link #setPoseAndUpdate(FramePose2DReadOnly)}. + * + * @param pose pose of this frame relative to parent to set. + */ + public void setPoseAndUpdate(Pose2DReadOnly pose) + { + setPoseAndUpdate(pose.getPosition(), pose.getOrientation()); + } + + /** + * Sets the pose of this reference frame relative to its parent. The frame of the passed in {@param position} and {@param orientation} must be defined as + * {@link #getParent()}. This method does not perform a frame check. For frame safe operations, see + * {@link #setPoseAndUpdate(FramePoint2DReadOnly, FrameOrientation2DReadOnly)}. + * + * @param position position of this frame relative to parent to set. + * @param orientation orientation of this frame relative to parent to set. + */ + public void setPoseAndUpdate(Point2DReadOnly position, Orientation2DReadOnly orientation) + { + setPoseAndUpdate(position, orientation.getYaw()); + } + + /** + * Sets the pose of this reference frame relative to its parent. The frame of the passed in {@param pose} must be defined as + * {@link #getParent()}. This method does not perform a frame check. For frame safe operations, see + * {@link #setPoseAndUpdate(FramePoint2DReadOnly, FrameOrientation2DReadOnly)}. + * + * @param position position of this frame relative to parent to set. + * @param orientation orientation of this frame relative to parent to set. + */ + public void setPoseAndUpdate(Point2DReadOnly position, double orientation) + { + originPose.getPosition().set(position); + originPose.getOrientation().setYaw(orientation); + this.update(); + } + + /** + * Sets the position of this reference frame relative to its parent. This does not perform a frame check. For frame safe operations, please use + * {@link #setPositionAndUpdate(FramePoint2DReadOnly)}. + * + * @param position position of this frame relative to parent to set. + */ + public void setPositionAndUpdate(Point2DReadOnly position) + { + originPose.getPosition().set(position); + this.update(); + } + + /** + * Sets the orientation of this reference frame relative to its parent. This does not perform a frame check. For frame safe operations, please use + * {@link #setOrientationAndUpdate(FrameOrientation2DReadOnly)}. + * + * @param orientation orientation of this frame relative to parent to set. + */ + public void setOrientationAndUpdate(Orientation2DReadOnly orientation) + { + originPose.getOrientation().set(orientation); + this.update(); + } + + /** + * {@inheritDoc} + */ + @Override + public String toString() + { + return super.toString() + ", originPose = " + originPose; + } +} diff --git a/src/frame/java/us/ihmc/euclid/referenceFrame/PoseReferenceFrame.java b/src/frame/java/us/ihmc/euclid/referenceFrame/PoseReferenceFrame.java new file mode 100644 index 000000000..4019432b3 --- /dev/null +++ b/src/frame/java/us/ihmc/euclid/referenceFrame/PoseReferenceFrame.java @@ -0,0 +1,335 @@ +package us.ihmc.euclid.referenceFrame; + +import us.ihmc.euclid.geometry.interfaces.Pose3DReadOnly; +import us.ihmc.euclid.orientation.interfaces.Orientation3DReadOnly; +import us.ihmc.euclid.referenceFrame.exceptions.ReferenceFrameMismatchException; +import us.ihmc.euclid.referenceFrame.interfaces.FrameOrientation3DReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; +import us.ihmc.euclid.transform.RigidBodyTransform; +import us.ihmc.euclid.transform.interfaces.RigidBodyTransformReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; +import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; + +/** + * This defines a mutable {@link ReferenceFrame}, where the pose of this frame relative to the parent {@link ReferenceFrame}, which is accessible as + * {@link #getParent()} can be updated via a variety of methods. + */ +public class PoseReferenceFrame extends ReferenceFrame +{ + /** + * The pose of this frame relative to its parent. + */ + private final FramePose3D originPose; + + /** + *

+ * Creates a new pose reference frame as a child of the given {@param parentFrame} and initializes the transform to its parent frame. + *

+ *

+ * This new reference frame is defined in the {@param parentFrame} and moves with it. + *

+ *

+ * Its pose with respect to the parent frame can be modified at runtime by changing the transform in the methods outlined in this class, including setting + * the pose {@link #setPoseAndUpdate(FramePose3DReadOnly)}, setting the pose from a reference frame {@link #setPoseAndUpdate(PoseReferenceFrame)}, setting + * the translation {@link #setPositionAndUpdate(FramePoint3DReadOnly)}, setting the orientation {@link #setOrientationAndUpdate(FrameOrientation3DReadOnly)}, + * or setting the transform {@link #setTransformAndUpdate(RigidBodyTransformReadOnly)} + *

+ * + * @param frameName the name of the new frame. + * @param parentFrame the parent of the frame. It has to extend a {@link ReferenceFrame}. + */ + public PoseReferenceFrame(String frameName, ReferenceFrame parentFrame) + { + super(frameName, parentFrame, parentFrame.isAStationaryFrame(), false); + + originPose = new FramePose3D(parentFrame); + } + + /** + *

+ * Creates a new pose reference frame with a defined pose relative to the parent frame, both of which are defined by {@param poseInParentFrame}. + *

+ *

+ * Its pose with respect to the parent frame can be modified at runtime by changing the transform in the methods outlined in this class, including setting + * the pose {@link #setPoseAndUpdate(FramePose3DReadOnly)}, setting the pose from a reference frame {@link #setPoseAndUpdate(PoseReferenceFrame)}, setting + * the translation {@link #setPositionAndUpdate(FramePoint3DReadOnly)}, setting the orientation {@link #setOrientationAndUpdate(FrameOrientation3DReadOnly)}, + * or setting the transform {@link #setTransformAndUpdate(RigidBodyTransformReadOnly)} + *

+ * + * @param frameName the name of the new frame. + * @param poseInParentFrame pose of the frame, with the parent frame provided by {@param poseInParentFrame.getParent()}. + */ + public PoseReferenceFrame(String frameName, FramePose3DReadOnly poseInParentFrame) + { + this(frameName, poseInParentFrame.getReferenceFrame()); + setPoseAndUpdate(poseInParentFrame); + } + + /** + *

+ * Creates a new pose reference frame as a child of the given {@param parentFrame} and initializes the transform to its parent frame. + *

+ *

+ * This new reference frame is defined with a transform relative to the parent {@param parentFrame} as transform {@param transformToParent}. + *

+ *

+ * Its pose with respect to the parent frame can be modified at runtime by changing the transform in the methods outlined in this class, including setting + * the pose {@link #setPoseAndUpdate(FramePose3DReadOnly)}, setting the pose from a reference frame {@link #setPoseAndUpdate(PoseReferenceFrame)}, setting + * the translation {@link #setPositionAndUpdate(FramePoint3DReadOnly)}, setting the orientation {@link #setOrientationAndUpdate(FrameOrientation3DReadOnly)}, + * or setting the transform {@link #setTransformAndUpdate(RigidBodyTransformReadOnly)} + *

+ * + * @param frameName the name of the new frame. + * @param parentFrame the parent of the frame. It has to extend a {@link ReferenceFrame}. + */ + public PoseReferenceFrame(String frameName, ReferenceFrame parentFrame, RigidBodyTransformReadOnly transformToParent) + { + this(frameName, parentFrame); + setTransformAndUpdate(transformToParent); + } + + /** + * Sets the pose of this reference frame relative to its parent to match that of {@param poseReferenceFrame}. The parent frame of the passed in frame + * {@param poseReferenceFrame} must match this frame's. If it does not, this method will throw a {@link ReferenceFrameMismatchException}. + * + * @param poseReferenceFrame frame containing the pose to copy. + */ + public void setPoseAndUpdate(PoseReferenceFrame poseReferenceFrame) + { + getParent().checkReferenceFrameMatch(poseReferenceFrame.getParent()); + setPoseAndUpdate(poseReferenceFrame.originPose); + } + + /** + * Sets the pose of this reference frame relative to its parent. The frame of the passed in tuple {@param pose} must be defined as + * {@link #getParent()}. If it is not, this method will throw a {@link ReferenceFrameMismatchException}. To avoid performing a frame check, please use + * {@link #setPoseAndUpdate(Pose3DReadOnly)} + * + * @param pose pose of this frame relative to parent to set. + */ + public void setPoseAndUpdate(FramePose3DReadOnly pose) + { + getParent().checkReferenceFrameMatch(pose); + setPoseAndUpdate((Pose3DReadOnly) pose); + } + + /** + * Sets the pose of this reference frame relative to its parent. The frame of the passed in tuples {@param position} and {@param orientation} must be defined + * as {@link #getParent()}. If it is not, this method will throw a {@link ReferenceFrameMismatchException}. To avoid performing a frame check, please use + * {@link #setPoseAndUpdate(Pose3DReadOnly)} or {@link #setPoseAndUpdate(Point3DReadOnly, Orientation3DReadOnly)}. + * + * @param position position of this frame relative to parent to set. + * @param orientation orientation of this frame relative to parent to set. + */ + public void setPoseAndUpdate(FramePoint3DReadOnly position, FrameOrientation3DReadOnly orientation) + { + position.checkReferenceFrameMatch(getParent()); + orientation.checkReferenceFrameMatch(getParent()); + setPoseAndUpdate((Point3DReadOnly) position, (Orientation3DReadOnly) orientation); + } + + /** + * Sets the position of this reference frame relative to its parent. The frame of the passed in tuple {@param position} must be defined + * as {@link #getParent()}. If it is not, this method will throw a {@link ReferenceFrameMismatchException}. To avoid performing a frame check, please use + * {@link #setPositionAndUpdate(Point3DReadOnly)}. + * + * @param position position of this frame relative to parent to set. + */ + public void setPositionAndUpdate(FramePoint3DReadOnly position) + { + position.checkReferenceFrameMatch(getParent()); + setPositionAndUpdate((Point3DReadOnly) position); + } + + /** + * Sets the orientation of this reference frame relative to its parent. The frame of the passed in tuple {@param orientation} must be defined + * as {@link #getParent()}. If it is not, this method will throw a {@link ReferenceFrameMismatchException}. To avoid performing a frame check, please use + * {@link #setOrientationAndUpdate(Orientation3DReadOnly)}. + * + * @param orientation orientation of this frame relative to parent to set. + */ + public void setOrientationAndUpdate(FrameOrientation3DReadOnly orientation) + { + orientation.checkReferenceFrameMatch(getParent()); + setOrientationAndUpdate((Orientation3DReadOnly) orientation); + } + + /** + * Sets the pose of this reference frame relative to its parent. The frame of the passed in {@param pose} must be defined as + * {@link #getParent()}. This method does not perform a frame check. For frame safe operations, see {@link #setPoseAndUpdate(FramePose3DReadOnly)}. + * + * @param pose pose of this frame relative to parent to set. + */ + public void setPoseAndUpdate(Pose3DReadOnly pose) + { + setPoseAndUpdate(pose.getPosition(), pose.getOrientation()); + } + + /** + * Sets the pose of this reference frame relative to its parent. The frame of the passed in {@param position} and {@param orientation} must be defined as + * {@link #getParent()}. This method does not perform a frame check. For frame safe operations, see + * {@link #setPoseAndUpdate(FramePoint3DReadOnly, FrameOrientation3DReadOnly)}. + * + * @param position position of this frame relative to parent to set. + * @param orientation orientation of this frame relative to parent to set. + */ + public void setPoseAndUpdate(Point3DReadOnly position, Orientation3DReadOnly orientation) + { + originPose.set(position, orientation); + this.update(); + } + + /** + * Sets the transform of this reference frame relative to its parent to be {@param transform}. + * + * @param transformToParent transform relative to parent to set. + */ + public void setTransformAndUpdate(RigidBodyTransformReadOnly transformToParent) + { + originPose.set(transformToParent); + this.update(); + } + + /** + * Sets the position of this reference frame relative to its parent. This does not perform a frame check. For frame safe operations, please use + * {@link #setPositionAndUpdate(FramePoint3DReadOnly)}. + * + * @param position position of this frame relative to parent to set. + */ + public void setPositionAndUpdate(Point3DReadOnly position) + { + setPositionAndUpdate(position.getX(), position.getY(), position.getZ()); + } + + /** + * Sets the orientation of this reference frame relative to its parent. This does not perform a frame check. For frame safe operations, please use + * {@link #setOrientationAndUpdate(FrameOrientation3DReadOnly)}. + * + * @param orientation orientation of this frame relative to parent to set. + */ + public void setOrientationAndUpdate(Orientation3DReadOnly orientation) + { + originPose.getOrientation().set(orientation); + this.update(); + } + + /** + * Sets the position of this reference frame relative to its parent to the corresponding x, y, and z locations. + * + * @param x x position of this frame relative to parent to set. + * @param y y position of this frame relative to parent to set. + * @param z z position of this frame relative to parent to set. + */ + public void setPositionAndUpdate(double x, double y, double z) + { + originPose.getPosition().set(x, y, z); + this.update(); + } + + /** + * Sets the quaternion coordinations of this reference frame relative to its parent to the corresponding qx, qy, qz, and qs locations. + * + * @param qx quaternion value qx of this frame relative to parent to set. + * @param qy quaternion value qy of this frame relative to parent to set. + * @param qz quaternion value qz of this frame relative to parent to set. + * @param qs quaternion value qs of this frame relative to parent to set. + */ + public void setOrientationAndUpdate(double qx, double qy, double qz, double qs) + { + originPose.getOrientation().set(qx, qy, qz, qs); + update(); + } + + /** + * Prepends a translation to this reference frame, relative to its parent, of a magnitude of x, y, and z. + * + * @param x x translation to prepend relative to the parent. + * @param y y translation to prepend relative to the parent. + * @param z z translation to prepend relative to the parent. + */ + public void prependTranslationAndUpdate(double x, double y, double z) + { + originPose.prependTranslation(x, y, z); + this.update(); + } + + /** + * Returns the x translation of this frame relative to the parent frame. + * + * @return x translation + */ + public double getTranslationX() + { + return getPose().getX(); + } + + /** + * Returns the y translation of this frame relative to the parent frame. + * + * @return y translation + */ + public double getTranslationY() + { + return getPose().getY(); + } + + /** + * Returns the z translation of this frame relative to the parent frame. + * + * @return z translation + */ + public double getTranslationZ() + { + return getPose().getZ(); + } + + /** + * Returns the pose of this frame relative to the parent frame. + * + * @return pose of this frame relative to the parent. + */ + public Pose3DReadOnly getPose() + { + return originPose; + } + + /** + * Returns the translation of this frame relative to the parent frame. + * + * @return translation of this frame relative to the parent. + */ + public Point3DReadOnly getTranslation() + { + return getPose().getPosition(); + } + + /** + * Returns the orientation of this frame relative to the parent frame. + * + * @return orientation of this frame relative to the parent. + */ + public QuaternionReadOnly getOrientation() + { + return getPose().getOrientation(); + } + + /** + * {@inheritDoc} + */ + @Override + protected void updateTransformToParent(RigidBodyTransform transformToParentToPack) + { + originPose.checkReferenceFrameMatch(getParent()); + transformToParentToPack.set(originPose); + } + + /** + * {@inheritDoc} + */ + @Override + public String toString() + { + return super.toString() + ", originPose = " + originPose; + } +} diff --git a/src/frame/java/us/ihmc/euclid/referenceFrame/TranslationReferenceFrame.java b/src/frame/java/us/ihmc/euclid/referenceFrame/TranslationReferenceFrame.java new file mode 100644 index 000000000..9b4051cf7 --- /dev/null +++ b/src/frame/java/us/ihmc/euclid/referenceFrame/TranslationReferenceFrame.java @@ -0,0 +1,98 @@ +package us.ihmc.euclid.referenceFrame; + +import us.ihmc.euclid.referenceFrame.exceptions.ReferenceFrameMismatchException; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; +import us.ihmc.euclid.transform.RigidBodyTransform; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; + +/** + * This defines a mutable {@link ReferenceFrame}, where the translation to the parent {@link ReferenceFrame}, which is accessible as {@link #getParent()} can be + * updated via {@link #setTranslationAndUpdate(Tuple3DReadOnly)} or {@link #setTranslationAndUpdate(FrameTuple3DReadOnly)}. + */ +public class TranslationReferenceFrame extends ReferenceFrame +{ + /** + * The translation of this frame relative to its parent. + */ + public final FrameVector3D translationVector; + + /** + *

+ * Creates a new translation reference frame as a child of the given {@param parentFrame} and initializes the transform to its parent frame. + *

+ *

+ * This new reference frame is defined in the {@param parentFrame} and moves with it. + *

+ *

+ * Its pose with respect to the parent frame can be modified at runtime by changing the transform in the methods + * {@link #setTranslationAndUpdate(FrameTuple3DReadOnly)} and {@link #setTranslationAndUpdate(Tuple3DReadOnly)}. + *

+ * + * @param frameName the name of the new frame. + * @param parentFrame the parent of the frame. It has to extend a {@link ReferenceFrame}. + */ + public TranslationReferenceFrame(String frameName, ReferenceFrame parentFrame) + { + super(frameName, parentFrame, false, parentFrame.isZupFrame()); + + translationVector = new FrameVector3D(parentFrame); + } + + /** + *

+ * Creates a new translation reference frame as a child of the given {@param parentFrame} and initializes the transform to its parent frame. + *

+ *

+ * This new reference frame is defined with a constant translation of {@param translationToParent} relative to {@param parentFrame} and moves with it. + *

+ *

+ * Its pose with respect to the parent frame can be modified at runtime by changing the transform in the methods + * {@link #setTranslationAndUpdate(FrameTuple3DReadOnly)} and {@link #setTranslationAndUpdate(Tuple3DReadOnly)}. + *

+ * + * @param frameName the name of the new frame. + * @param parentFrame the parent of the frame. It has to extend a {@link ReferenceFrame}. + * @param translationToParent the initial transform to the parent frame. + */ + public TranslationReferenceFrame(String frameName, ReferenceFrame parentFrame, FrameVector3DReadOnly translationToParent) + { + this(frameName, parentFrame); + setTranslationAndUpdate(translationToParent); + } + + /** + * {@inheritDoc} + **/ + @Override + protected void updateTransformToParent(RigidBodyTransform transformToParentToPack) + { + transformToParentToPack.setIdentity(); + transformToParentToPack.getTranslation().set(translationVector); + } + + /** + * Sets the translation of this reference frame relative to its parent. The frame of the passed in tuple {@param translation} must be defined as + * {@link #getParent()}. If it is not, this method will throw a {@link ReferenceFrameMismatchException}. To avoid performing a frame check, please use + * {@link #setTranslationAndUpdate(Tuple3DReadOnly)} + * + * @param translation translate relative to parent to set. + */ + public void setTranslationAndUpdate(FrameTuple3DReadOnly translation) + { + translationVector.checkReferenceFrameMatch(translation); + setTranslationAndUpdate((Tuple3DReadOnly) translation); + } + + /** + * Sets the translation of this reference frame relative to its parent. The frame of the passed in tuple {@param translation} is assumed to be the same as + * {@link #getParent()}. This method does not perform a frame check. For frame safe operations, see {@link #setTranslationAndUpdate(FrameTuple3DReadOnly)}. + * + * @param translation translate relative to parent to set. + */ + public void setTranslationAndUpdate(Tuple3DReadOnly translation) + { + translationVector.set(translation); + this.update(); + } +} diff --git a/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameFactories.java b/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameFactories.java index 211fce99c..11474ede5 100644 --- a/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameFactories.java +++ b/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameFactories.java @@ -88,12 +88,7 @@ import us.ihmc.euclid.tuple3D.Point3D; import us.ihmc.euclid.tuple3D.UnitVector3D; import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.euclid.tuple3D.interfaces.Point3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.UnitVector3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.UnitVector3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.*; import us.ihmc.euclid.tuple4D.Quaternion; import us.ihmc.euclid.tuple4D.interfaces.QuaternionBasics; import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; diff --git a/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameTools.java b/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameTools.java index 662f0d9ae..8ec308352 100644 --- a/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameTools.java +++ b/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameTools.java @@ -6,36 +6,27 @@ import us.ihmc.euclid.Axis3D; import us.ihmc.euclid.Location; import us.ihmc.euclid.axisAngle.AxisAngle; +import us.ihmc.euclid.geometry.BoundingBox2D; +import us.ihmc.euclid.geometry.BoundingBox3D; import us.ihmc.euclid.geometry.exceptions.BoundingBoxException; +import us.ihmc.euclid.geometry.interfaces.BoundingBox2DReadOnly; +import us.ihmc.euclid.geometry.interfaces.BoundingBox3DReadOnly; +import us.ihmc.euclid.geometry.interfaces.ConvexPolygon2DReadOnly; import us.ihmc.euclid.geometry.tools.EuclidGeometryTools; +import us.ihmc.euclid.matrix.interfaces.CommonMatrix3DBasics; import us.ihmc.euclid.orientation.interfaces.Orientation3DBasics; -import us.ihmc.euclid.referenceFrame.FramePoint2D; -import us.ihmc.euclid.referenceFrame.FramePoint3D; -import us.ihmc.euclid.referenceFrame.FrameVector2D; -import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.*; import us.ihmc.euclid.referenceFrame.exceptions.ReferenceFrameMismatchException; -import us.ihmc.euclid.referenceFrame.interfaces.EuclidFrameGeometry; -import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameOrientation3DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FixedFramePoint2DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FixedFramePoint3DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameVector2DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameVector3DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FrameOrientation3DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FrameOrientation3DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FrameVector2DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FrameVector2DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.*; +import us.ihmc.euclid.tools.EuclidCoreTools; import us.ihmc.euclid.tuple2D.Point2D; +import us.ihmc.euclid.tuple2D.interfaces.Point2DBasics; +import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly; +import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; import us.ihmc.euclid.tuple3D.Point3D; import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.euclid.tuple3D.interfaces.Point3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.*; +import us.ihmc.euclid.tuple4D.interfaces.Vector4DReadOnly; /** * Extension of the tools provided in {@link EuclidGeometryTools} for frame geometries. @@ -7903,4 +7894,470 @@ public static boolean geometricallyEquals(EuclidFrameGeometry a, EuclidFrameGeom { return (a == b) || (a != null && a.geometricallyEquals(b, epsilon)); } + + /** + * Calculates the 3D part of the given {@code input} that is parallel to the given + * {@code normalAxis} and stores the result in {@code inputNormalPartToPack}. + *

+ * The result has the following properties: + *

    + *
  • x.n = xn.n + *
  • |xn×n| = 0 + *
+ * where: + *
    + *
  • x is {@code input}. + *
  • n is {@code normalAxis}. + *
  • xn is {@code inputNormalPartToPack}. + *
+ *

+ * + * @param input the tuple to extract the normal part of. Not modified. + * @param normalAxis the normal vector. It is normalized internally if needed. Not + * modified. + * @param inputNormalPartToPack the tuple used to store the normal part of the input. Modified. + * @throws ReferenceFrameMismatchException if the arguments are not expressed in the same reference + * frame. + */ + public static void extractNormalPart(FrameTuple3DReadOnly input, FrameVector3DReadOnly normalAxis, FixedFrameTuple3DBasics inputNormalPartToPack) + { + input.checkReferenceFrameMatch(normalAxis, inputNormalPartToPack); + EuclidCoreTools.extractNormalPart(input, normalAxis, inputNormalPartToPack); + } + + /** + * Calculates the 3D part of the given {@code input} that is parallel to the given + * {@code normalAxis} and stores the result in {@code inputNormalPartToPack}. + *

+ * The result has the following properties: + *

    + *
  • x.n = xn.n + *
  • |xn×n| = 0 + *
+ * where: + *
    + *
  • x is {@code input}. + *
  • n is {@code normalAxis}. + *
  • xn is {@code inputNormalPartToPack}. + *
+ *

+ * + * @param input the tuple to extract the normal part of. Not modified. + * @param normalAxis the normal vector. It is normalized internally if needed. Not + * modified. + * @param inputNormalPartToPack the tuple used to store the normal part of the input. Modified. + * @throws ReferenceFrameMismatchException if {@code input} and {@code normalAxis} are not expressed + * in the same reference frame. + */ + public static void extractNormalPart(FrameTuple3DReadOnly input, FrameVector3DReadOnly normalAxis, FrameTuple3DBasics inputNormalPartToPack) + { + input.checkReferenceFrameMatch(normalAxis); + inputNormalPartToPack.setReferenceFrame(input.getReferenceFrame()); + EuclidCoreTools.extractNormalPart((Tuple3DReadOnly) input, (Vector3DReadOnly) normalAxis, (Tuple3DBasics) inputNormalPartToPack); + } + + /** + * Calculates the 3D part of the given {@code input} that is orthogonal to the given + * {@code normalAxis} and stores the result in {@code inputTangentialPartToPack}. + *

+ * The result has the following properties: + *

    + *
  • xt.n = 0 + *
  • |x - (x.n)n| = |xt| + *
+ * where: + *
    + *
  • x is {@code input}. + *
  • n is {@code normalAxis}. + *
  • xt is {@code inputTangentialPartToPack}. + *
+ *

+ * + * @param input the tuple to extract the tangential part of. Not modified. + * @param normalAxis the normal vector. It is normalized internally if needed. Not + * modified. + * @param inputTangentialPartToPack the tuple used to store the tangential part of the input. + * Modified. + * @throws ReferenceFrameMismatchException if the arguments are not expressed in the same reference + * frame. + */ + public static void extractTangentialPart(FrameTuple3DReadOnly input, FrameVector3DReadOnly normalAxis, FixedFrameTuple3DBasics inputTangentialPartToPack) + { + input.checkReferenceFrameMatch(normalAxis, inputTangentialPartToPack); + EuclidCoreTools.extractTangentialPart((Tuple3DReadOnly) input, (Vector3DReadOnly) normalAxis, (Tuple3DBasics) inputTangentialPartToPack); + } + + /** + * Calculates the 3D part of the given {@code input} that is orthogonal to the given + * {@code normalAxis} and stores the result in {@code inputTangentialPartToPack}. + *

+ * The result has the following properties: + *

    + *
  • xt.n = 0 + *
  • |x - (x.n)n| = |xt| + *
+ * where: + *
    + *
  • x is {@code input}. + *
  • n is {@code normalAxis}. + *
  • xt is {@code inputTangentialPartToPack}. + *
+ *

+ * + * @param input the tuple to extract the tangential part of. Not modified. + * @param normalAxis the normal vector. It is normalized internally if needed. Not + * modified. + * @param inputTangentialPartToPack the tuple used to store the tangential part of the input. + * Modified. + * @throws ReferenceFrameMismatchException if {@code input} and {@code normalAxis} are not expressed + * in the same reference frame. + */ + public static void extractTangentialPart(FrameTuple3DReadOnly input, FrameVector3DReadOnly normalAxis, FrameTuple3DBasics inputTangentialPartToPack) + { + input.checkReferenceFrameMatch(normalAxis); + inputTangentialPartToPack.setReferenceFrame(input.getReferenceFrame()); + EuclidCoreTools.extractTangentialPart((Tuple3DReadOnly) input, (Vector3DReadOnly) normalAxis, (Tuple3DBasics) inputTangentialPartToPack); + } + + /** + * This method implements the same operation as + * {@link EuclidGeometryTools#orientation3DFromFirstToSecondVector3D(Vector3DReadOnly, Vector3DReadOnly, Orientation3DBasics)} + * except that it does not rely on {@code Math#acos(double)} making it faster. + * + * @param firstVector the first vector. Not modified. + * @param secondVector the second vector that is rotated with respect to the first vector. Not + * modified. + * @param rotationToPack the minimum rotation from {@code firstVector} to the {@code secondVector}. + * Modified. + * @see EuclidGeometryTools#orientation3DFromFirstToSecondVector3D(Vector3DReadOnly, + * Vector3DReadOnly, Orientation3DBasics) + */ + public static void rotationMatrix3DFromFirstToSecondVector3D(FrameVector3DReadOnly firstVector, + FrameVector3DReadOnly secondVector, + FixedFrameCommonMatrix3DBasics rotationToPack) + { + firstVector.checkReferenceFrameMatch(secondVector, rotationToPack); + EuclidGeometryTools.rotationMatrix3DFromFirstToSecondVector3D(firstVector, secondVector, rotationToPack); + } + + /** + * Computes and packs the intersecting points between a 2d line segment and a circle. Returns the number of found intersections. + *

+ * Edge cases: + *

    + *
  • If the line does not intersect the circle, this methods fails and returns {@code 0}. + *
+ *

+ * + * @param circleRadius radius of the circle in question. + * @param circlePosition position of the center of the circle. + * @param startPoint starting position of the line segment. + * @param endPoint end position of the line segment. + * @param firstIntersectionToPack first possible intersecting point. + * @param secondIntersectionToPack second possible intersecting point. + * @return number of intersections found. Can be {@code 0} if no intersections, {@code 1} or {@code 2}. + */ + public static int intersectionBetweenLineSegment2DAndCylinder3D(double circleRadius, + FramePoint2DReadOnly circlePosition, + FramePoint2DReadOnly startPoint, + FramePoint2DReadOnly endPoint, + FixedFramePoint2DBasics firstIntersectionToPack, + FixedFramePoint2DBasics secondIntersectionToPack) + { + circlePosition.checkReferenceFrameMatch(startPoint, endPoint); + circlePosition.checkReferenceFrameMatch(firstIntersectionToPack, secondIntersectionToPack); + return EuclidGeometryTools.intersectionBetweenLineSegment2DAndCylinder3D(circleRadius, + circlePosition, + startPoint, + endPoint, + firstIntersectionToPack, + secondIntersectionToPack); + } + /** + * Computes and packs the intersecting points between a 2d line and a circle. Returns the number of found intersections. + *

+ * Edge cases: + *

    + *
  • If the line does not intersect the circle, this methods fails and returns {@code 0}. + *
+ *

+ * + * @param circleRadius radius of the circle in question. + * @param circlePosition position of the center of the circle. + * @param pointOnLine point on the line + * @param direction direction of the line. + * @param firstIntersectionToPack first possible intersecting point. + * @param secondIntersectionToPack second possible intersecting point. + * @return number of intersections found. Can be {@code 0} if no intersections, {@code 1} or {@code 2}. + */ + public static int intersectionBetweenLine2DAndCircle(double circleRadius, + FramePoint2DReadOnly circlePosition, + FramePoint2DReadOnly pointOnLine, + FrameVector2DReadOnly direction, + FixedFramePoint2DBasics firstIntersectionToPack, + FixedFramePoint2DBasics secondIntersectionToPack) + { + circlePosition.checkReferenceFrameMatch(pointOnLine, direction); + circlePosition.checkReferenceFrameMatch(firstIntersectionToPack, secondIntersectionToPack); + return EuclidGeometryTools.intersectionBetweenLine2DAndCircle(circleRadius, + circlePosition, + pointOnLine, + direction, + firstIntersectionToPack, + secondIntersectionToPack); + } + + /** + * Computes and packs the intersecting points between a 2d ray and a circle. Returns the number of found intersections. + *

+ * Edge cases: + *

    + *
  • If the line does not intersect the circle, this methods fails and returns {@code 0}. + *
+ *

+ * + * @param circleRadius radius of the circle in question. + * @param circlePosition position of the center of the circle. + * @param startPoint starting point of the line. + * @param direction direction of the line. + * @param firstIntersectionToPack first possible intersecting point. + * @param secondIntersectionToPack second possible intersecting point. + * @return number of intersections found. Can be {@code 0} if no intersections, {@code 1} or {@code 2}. + */ + public static int intersectionBetweenRay2DAndCircle(double circleRadius, + FramePoint2DReadOnly circlePosition, + FramePoint2DReadOnly startPoint, + FrameVector2DReadOnly direction, + FixedFramePoint2DBasics firstIntersectionToPack, + FixedFramePoint2DBasics secondIntersectionToPack) + { + circlePosition.checkReferenceFrameMatch(startPoint, direction); + circlePosition.checkReferenceFrameMatch(firstIntersectionToPack, secondIntersectionToPack); + return EuclidGeometryTools.intersectionBetweenRay2DAndCircle(circleRadius, + circlePosition, + startPoint, + direction, + firstIntersectionToPack, + secondIntersectionToPack); + } + + /** + * Computes and packs the intersecting points between a 2d ray and a circle. Returns the number of found intersections. + *

+ * Edge cases: + *

    + *
  • If the line does not intersect the circle, this methods fails and returns {@code 0}. + *
+ *

+ * + * @param circleRadius radius of the circle in question. + * @param circlePosition position of the center of the circle. + * @param startPoint starting point of the line. + * @param pointOnRay point on the line. + * @param firstIntersectionToPack first possible intersecting point. + * @param secondIntersectionToPack second possible intersecting point. + * @return number of intersections found. Can be {@code 0} if no intersections, {@code 1} or {@code 2}. + */ + public static int intersectionBetweenRay2DAndCircle(double circleRadius, + FramePoint2DReadOnly circlePosition, + FramePoint2DReadOnly startPoint, + FramePoint2DReadOnly pointOnRay, + FixedFramePoint2DBasics firstIntersectionToPack, + FixedFramePoint2DBasics secondIntersectionToPack) + { + circlePosition.checkReferenceFrameMatch(startPoint, pointOnRay); + circlePosition.checkReferenceFrameMatch(firstIntersectionToPack, secondIntersectionToPack); + return EuclidGeometryTools.intersectionBetweenRay2DAndCircle(circleRadius, + circlePosition, + startPoint, + pointOnRay, + firstIntersectionToPack, + secondIntersectionToPack); + } + + /** + * Computes the intersection between an infinitely long 2D line (defined by a 2D point and a 2D + * direction) and a 2D ray segment (defined by the start and direction 2D endpoints). + *

+ * Edge cases: + *

    + *
  • When the line and the ray are parallel but not collinear, they do not intersect. + *
  • When the line and the ray are collinear, they are assumed to intersect at + * {@code rayStart}. + *
  • When the line intersects the ray at the start point, this method returns + * {@code true} and the start point is the intersection. + *
  • When there is no intersection, this method returns {@code false} and + * {@code intersectionToPack} is set to {@link Double#NaN}. + *
+ *

+ * + * @param rayOrigin start point of the ray. + * @param rayDirection direction of the ray. + * @param linePoint1 first point located on the line. + * @param linePoint2 second point located on the line. + * @param intersectionToPack the 2D point in which the result is stored. Can be {@code null}. + * Modified. + * @return {@code true} if the line intersects the line segment, {@code false} otherwise. + */ + public static boolean intersectionBetweenRay2DAndLine2D(FramePoint2DReadOnly rayOrigin, + FrameVector2DReadOnly rayDirection, + FramePoint2DReadOnly linePoint1, + FramePoint2DReadOnly linePoint2, + FixedFramePoint2DBasics intersectionToPack) + { + rayOrigin.checkReferenceFrameMatch(rayDirection, linePoint1); + rayOrigin.checkReferenceFrameMatch(linePoint2, intersectionToPack); + return EuclidGeometryTools.intersectionBetweenRay2DAndLine2D(rayOrigin, rayDirection, linePoint1, linePoint2, intersectionToPack); + } + + /** + * Computes the intersection between an infinitely long 2D line (defined by a 2D point and a 2D + * direction) and a 2D ray segment (defined by the start and direction 2D endpoints). + *

+ * Edge cases: + *

    + *
  • When the line and the ray are parallel but not collinear, they do not intersect. + *
  • When the line and the ray are collinear, they are assumed to intersect at + * {@code rayStart}. + *
  • When the line intersects the ray at the start point, this method returns + * {@code true} and the start point is the intersection. + *
  • When there is no intersection, this method returns {@code false} and + * {@code intersectionToPack} is set to {@link Double#NaN}. + *
+ *

+ * + * @param rayOrigin start point of the ray. + * @param rayDirection direction of the ray. + * @param pointOnLine point located on the line. + * @param lineDirection direction of the line. + * @param intersectionToPack the 2D point in which the result is stored. Can be {@code null}. + * Modified. + * @return {@code true} if the line intersects the line segment, {@code false} otherwise. + */ + public static boolean intersectionBetweenRay2DAndLine2D(FramePoint2DReadOnly rayOrigin, + FrameVector2DReadOnly rayDirection, + FramePoint2DReadOnly pointOnLine, + FrameVector2DReadOnly lineDirection, + FixedFramePoint2DBasics intersectionToPack) + { + rayOrigin.checkReferenceFrameMatch(rayDirection, pointOnLine); + rayOrigin.checkReferenceFrameMatch(lineDirection, intersectionToPack); + return EuclidGeometryTools.intersectionBetweenRay2DAndLine2D(rayOrigin, rayDirection, pointOnLine, lineDirection, intersectionToPack); + } + + /** + * Finds the intersection of two bounding boxes defined by a bounding box. + *

+ * WARNING: generates garbage + *

+ * Allocates a new BoundingBox2D. TODO: Check, Unit test, move where BoundingBox union is + * + * @param a first bounding box to check + * @param b second bounding box to check + * @return the intersection bounding box, or null if no intersection + */ + public static FrameBoundingBox2D computeIntersectionOfTwoBoundingBoxes(FrameBoundingBox2DReadOnly a, FrameBoundingBox2DReadOnly b) + { + a.checkReferenceFrameMatch(b); + BoundingBox2D unframedBox = EuclidGeometryTools.computeIntersectionOfTwoBoundingBoxes(a, b); + if (unframedBox == null) + return null; + return new FrameBoundingBox2D(a.getReferenceFrame(), unframedBox); + } + + /** + * Finds the intersection of two bounding boxes defined by a bounding box. + *

+ * WARNING: generates garbage + *

+ * Allocates a new BoundingBox2D. TODO: Check, Unit test, move where BoundingBox union is + * + * @param a first bounding box to check + * @param b second bounding box to check + * @return the intersection bounding box, or null if no intersection + */ + public static FrameBoundingBox3D computeIntersectionOfTwoBoundingBoxes(FrameBoundingBox3DReadOnly a, FrameBoundingBox3DReadOnly b) + { + a.checkReferenceFrameMatch(b); + BoundingBox3D unframedBox = EuclidGeometryTools.computeIntersectionOfTwoBoundingBoxes(a, b); + if (unframedBox == null) + return null; + return new FrameBoundingBox3D(a.getReferenceFrame(), unframedBox); + } + + /** + * Determines if the polygonToTest is fully inside the convex polygon by some epsilon distance. + * + * @return {@code true} if {@code polygonToTest} is fully contained inside {@code polygon}. {@code false} otherwise. + */ + public static boolean isPolygonInside(FrameConvexPolygon2DReadOnly polygonToTest, double epsilon, FrameConvexPolygon2DReadOnly polygon) + { + polygonToTest.checkReferenceFrameMatch(polygon); + return EuclidGeometryTools.isPolygonInside(polygonToTest, epsilon, polygon); + } + + /** + * Determines if the polygonToTest is fully inside the convex polygon by some epsilon distance. + * + * @return {@code true} if {@code polygonToTest} is fully contained inside {@code polygon}. {@code false} otherwise. + */ + public static boolean isPolygonInside(FrameConvexPolygon2DReadOnly polygonToTest, FrameConvexPolygon2DReadOnly polygon) + { + polygonToTest.checkReferenceFrameMatch(polygon); + return EuclidGeometryTools.isPolygonInside(polygonToTest, polygon); + } + + /** + * This method implements the same operation as + * {@link EuclidGeometryTools#orientation3DFromZUpToVector3D(Vector3DReadOnly, Orientation3DBasics)} + * except that it does not rely on {@code Math#acos(double)} making it faster. + * + * @param vector the vector that is rotated with respect to {@code zUp}. Not modified. + * @param rotationToPack the minimum rotation from {@code zUp} to the given {@code vector}. + * Modified. + * @see EuclidGeometryTools#orientation3DFromZUpToVector3D(Vector3DReadOnly, Orientation3DBasics) + */ + public static void rotationMatrix3DFromZUpToVector3D(FrameVector3DReadOnly vector, FixedFrameCommonMatrix3DBasics rotationToPack) + { + vector.checkReferenceFrameMatch(rotationToPack); + EuclidGeometryTools.rotationMatrix3DFromZUpToVector3D(vector, rotationToPack); + } + + /** + * Compute Intersection-over-Union (IoU) of two 3D bounding boxes. This is the percentage of volume of the two bounding boxes that are intersecting. + *

+ * WARNING: generates garbage. + */ + public static double computeIntersectionOverUnionOfTwoBoundingBoxes(FrameBoundingBox3DReadOnly a, FrameBoundingBox3DReadOnly b) + { + a.checkReferenceFrameMatch(b); + return EuclidGeometryTools.computeIntersectionOverUnionOfTwoBoundingBoxes(a, b); + } + + /** + * Compute Intersection-over-Union (IoU) of two 3D bounding boxes. This is the percentage of volume of the two bounding boxes that are intersecting. + *

+ * WARNING: generates garbage. + */ + public static double computeIntersectionOverSmallerOfTwoBoundingBoxes(FrameBoundingBox3DReadOnly a, FrameBoundingBox3DReadOnly b) + { + a.checkReferenceFrameMatch(b); + return EuclidGeometryTools.computeIntersectionOverSmallerOfTwoBoundingBoxes(a, b); + } + + /** + * Finds the projection of a 3D point onto a 3D plane given in general form. + * Uses: projectedPoint = point - (normal.dot(point) + planeScalar) * (normal) + *

+ * WARNING: generates garbage. + * + * @param plane Coefficients of the general form of plane equation (ax + by + cz + d = 0) as Vector4D + * @param point Point to be projected onto the plane as Point3D + * @return Projected point onto the plane as Point3D + */ + public static FramePoint3D projectPointOntoPlane(FrameVector4DReadOnly plane, FramePoint3DReadOnly point) + { + plane.checkReferenceFrameMatch(point); + return new FramePoint3D(plane.getReferenceFrame(), EuclidGeometryTools.projectPointOntoPlane(plane, point)); + } } diff --git a/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryPolygonTools.java b/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryPolygonTools.java index c1336f74a..3f36f81c9 100644 --- a/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryPolygonTools.java +++ b/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryPolygonTools.java @@ -2870,6 +2870,7 @@ else if (candidateCoord1 == bestCoord1 && bound2.isFirstBetter(candidateCoord2, * @return the wrapped index. * @deprecated Use {@link EuclidCoreTools#wrap(int,int)} instead */ + @Deprecated public static int wrap(int index, int listSize) { return EuclidCoreTools.wrap(index, listSize); @@ -2891,6 +2892,7 @@ public static int wrap(int index, int listSize) * @return the wrapped incremented index. * @deprecated Use {@link EuclidCoreTools#next(int,int)} instead */ + @Deprecated public static int next(int index, int listSize) { return EuclidCoreTools.next(index, listSize); @@ -2912,6 +2914,7 @@ public static int next(int index, int listSize) * @return the wrapped decremented index. * @deprecated Use {@link EuclidCoreTools#previous(int,int)} instead */ + @Deprecated public static int previous(int index, int listSize) { return EuclidCoreTools.previous(index, listSize); diff --git a/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryTools.java b/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryTools.java index 8953a1516..41c7c7b8a 100644 --- a/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryTools.java +++ b/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryTools.java @@ -1,34 +1,35 @@ package us.ihmc.euclid.geometry.tools; -import static us.ihmc.euclid.tools.EuclidCoreTools.normSquared; - -import java.util.ArrayList; -import java.util.Collection; -import java.util.List; - import us.ihmc.euclid.Axis2D; import us.ihmc.euclid.Axis3D; import us.ihmc.euclid.Location; import us.ihmc.euclid.axisAngle.AxisAngle; +import us.ihmc.euclid.geometry.BoundingBox2D; +import us.ihmc.euclid.geometry.BoundingBox3D; import us.ihmc.euclid.geometry.exceptions.BoundingBoxException; +import us.ihmc.euclid.geometry.interfaces.BoundingBox2DReadOnly; +import us.ihmc.euclid.geometry.interfaces.BoundingBox3DReadOnly; +import us.ihmc.euclid.geometry.interfaces.ConvexPolygon2DReadOnly; +import us.ihmc.euclid.matrix.RotationMatrix; +import us.ihmc.euclid.matrix.interfaces.CommonMatrix3DBasics; import us.ihmc.euclid.orientation.interfaces.Orientation3DBasics; import us.ihmc.euclid.orientation.interfaces.Orientation3DReadOnly; import us.ihmc.euclid.tools.EuclidCoreTools; import us.ihmc.euclid.tools.TupleTools; import us.ihmc.euclid.tuple2D.Point2D; import us.ihmc.euclid.tuple2D.Vector2D; -import us.ihmc.euclid.tuple2D.interfaces.Point2DBasics; -import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly; -import us.ihmc.euclid.tuple2D.interfaces.UnitVector2DReadOnly; -import us.ihmc.euclid.tuple2D.interfaces.Vector2DBasics; -import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; +import us.ihmc.euclid.tuple2D.interfaces.*; import us.ihmc.euclid.tuple3D.Point3D; +import us.ihmc.euclid.tuple3D.UnitVector3D; import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.euclid.tuple3D.interfaces.Point3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.UnitVector3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.*; +import us.ihmc.euclid.tuple4D.interfaces.Vector4DReadOnly; + +import java.util.ArrayList; +import java.util.Collection; +import java.util.List; + +import static us.ihmc.euclid.tools.EuclidCoreTools.normSquared; /** * This class provides a large variety of basics geometry operations. @@ -37,15 +38,25 @@ */ public class EuclidGeometryTools { - /** Tolerance used to identify edge cases. */ + /** + * Tolerance used to identify edge cases. + */ public static final double ONE_MILLIONTH = 1.0e-6; - /** Tolerance used to identify edge cases. */ + /** + * Tolerance used to identify edge cases. + */ public static final double ONE_TEN_MILLIONTH = 1.0e-7; - /** Tolerance used to identify edge cases. */ + /** + * Tolerance used to identify edge cases. + */ public static final double ONE_TRILLIONTH = 1.0e-12; - /** Tolerance used to identify edge cases. */ + /** + * Tolerance used to identify edge cases. + */ public static final double IS_POINT_ON_LINE_EPS = 1.0e-8; - /** Constant used to save some computation. */ + /** + * Constant used to save some computation. + */ public static final double HALF_PI = 0.5 * Math.PI; private EuclidGeometryTools() @@ -1589,7 +1600,7 @@ public static double triangleArea(Point3DReadOnly a, Point3DReadOnly b, Point3DR * s = 0.5(a + b + c) * A = √{s(s - a)(s - b)(s - c)} * - * + *

* No specific ordering of the arguments is required. *

* Note that the method {@link #triangleArea(Point2DReadOnly, Point2DReadOnly, Point2DReadOnly)} is @@ -1614,7 +1625,7 @@ public static double triangleAreaHeron1(double lengthA, double lengthB, double l * 

     * A = 0.25 * √{4(a2b2 + a2c2 + b2c2) - (a2 + b2 + c2)2}
     *
- * + *

* No specific ordering of the arguments is required. *

* Note that the method {@link #triangleArea(Point2DReadOnly, Point2DReadOnly, Point2DReadOnly)} is @@ -2613,7 +2624,6 @@ public static double signedDistanceFromPoint3DToPlane3D(double pointX, double planeNormalY = planeFirstTangentZ * planeSecondTangentX - planeFirstTangentX * planeSecondTangentZ; double planeNormalZ = planeFirstTangentX * planeSecondTangentY - planeFirstTangentY * planeSecondTangentX; return signedDistanceFromPoint3DToPlane3D(pointX, pointY, pointZ, pointOnPlaneX, pointOnPlaneY, pointOnPlaneZ, planeNormalX, planeNormalY, planeNormalZ); - } /** @@ -3261,10 +3271,10 @@ public static double dotProduct(Point3DReadOnly start1, Point3DReadOnly end1, Po * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line and the bounding box. It is either equal to - * 0 or 2. + * 0 or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ public static int intersectionBetweenLine2DAndBoundingBox2D(Point2DReadOnly boundingBoxMin, Point2DReadOnly boundingBoxMax, @@ -3315,10 +3325,10 @@ public static int intersectionBetweenLine2DAndBoundingBox2D(Point2DReadOnly boun * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line and the bounding box. It is either equal to - * 0, 1, or 2. + * 0, 1, or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ public static int intersectionBetweenLine2DAndBoundingBox2D(Point2DReadOnly boundingBoxMin, Point2DReadOnly boundingBoxMax, @@ -3390,10 +3400,10 @@ public static int intersectionBetweenLine2DAndBoundingBox2D(Point2DReadOnly boun * @param secondIntersectionToPack the coordinate of the second intersection. Can be * {@code null}. Modified. * @return the number of intersections between the line/line-segment/ray and the bounding box. It is - * either equal to 0, 1, or 2. + * either equal to 0, 1, or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ private static int intersectionBetweenLine2DAndBoundingBox2DImpl(Point2DReadOnly boundingBoxMin, Point2DReadOnly boundingBoxMax, @@ -3573,7 +3583,7 @@ private static int intersectionBetweenLine2DAndBoundingBox2DImpl(Point2DReadOnly // if we get here we have a line-segment or a ray colinear with one of the box surfaces and we need to check for edge cases if (canIntersectionOccurAfterEnd) {// we have a ray and need to test for the edge-case that can only occur with ray at this point - // check if ray origin lies on bounding box surface and ray or line are colinear with that bounding box surface + // check if ray origin lies on bounding box surface and ray or line are colinear with that bounding box surface boolean isRayOriginOnBox = false; if (isColinearX) @@ -3613,7 +3623,7 @@ else if (isColinearY) } else {// we have a line-segment and need to test for the edge-case that can only occur with a line-segment at this point - // check if start- and/or end- point lies on bounding box surface and the line-segment is colinear with that bounding box surface + // check if start- and/or end- point lies on bounding box surface and the line-segment is colinear with that bounding box surface boolean isLineSegmentStartOnBox = false; boolean isLineSegmentEndOnBox = false; double deltaXEndmin = boundingBoxMin.getX() - endX; @@ -3632,7 +3642,6 @@ else if (isColinearY) { // line-segment end point also lies on x surface of box isLineSegmentEndOnBox = true; } - } else if (isColinearY) {// check if start point of ray lies on bounding box surface @@ -3644,7 +3653,6 @@ else if (isColinearY) if (deltaXEndmin <= 0.0 && deltaXEndmax >= 0.0) { // line-segment end point also lies on x surface of box isLineSegmentEndOnBox = true; - } } @@ -3834,7 +3842,7 @@ private static boolean intersectionBetweenTwoLine2DsImpl(double start1x, if (Math.abs(determinant) < epsilon) { // The lines are parallel - // Check if they are collinear + // Check if they are collinear double dx = start2x - start1x; double dy = start2y - start1y; double cross = dx * direction1y - dy * direction1x; @@ -4064,7 +4072,7 @@ public static boolean intersectionBetweenLine2DAndLineSegment2D(Point2DReadOnly * intersects with the bounding box are returned as intersection points. * *

- * + * * @param boundingBoxMin the minimum coordinate of the bounding box. Not modified. * @param boundingBoxMax the maximum coordinate of the bounding box. Not modified. * @param firstPointOnLine a first point located on the infinitely long line. Not modified. @@ -4074,10 +4082,10 @@ public static boolean intersectionBetweenLine2DAndLineSegment2D(Point2DReadOnly * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line and the bounding box. It is either equal to - * 0, 1, or 2. + * 0, 1, or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ public static int intersectionBetweenLine3DAndBoundingBox3D(Point3DReadOnly boundingBoxMin, Point3DReadOnly boundingBoxMax, @@ -4120,7 +4128,7 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(Point3DReadOnly boun * intersects with the bounding box are returned as intersection points. * *

- * + * * @param boundingBoxMin the minimum coordinate of the bounding box. Not modified. * @param boundingBoxMax the maximum coordinate of the bounding box. Not modified. * @param pointOnLine a point located on the infinitely long line. Not modified. @@ -4130,10 +4138,10 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(Point3DReadOnly boun * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line and the bounding box. It is either equal to - * 0, 1, or 2. + * 0, 1, or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ public static int intersectionBetweenLine3DAndBoundingBox3D(Point3DReadOnly boundingBoxMin, Point3DReadOnly boundingBoxMax, @@ -4174,7 +4182,7 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(Point3DReadOnly boun * intersects with the bounding box are returned as intersection points. * *

- * + * * @param boundingBoxMinX the minimum x-coordinate of the bounding box. * @param boundingBoxMinY the minimum y-coordinate of the bounding box. * @param boundingBoxMinZ the minimum z-coordinate of the bounding box. @@ -4188,10 +4196,10 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(Point3DReadOnly boun * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line and the bounding box. It is either equal to - * 0, 1, or 2. + * 0, 1, or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ public static int intersectionBetweenLine3DAndBoundingBox3D(double boundingBoxMinX, double boundingBoxMinY, @@ -4248,7 +4256,7 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(double boundingBoxMi * intersects with the bounding box are returned as intersection points. * *

- * + * * @param boundingBoxMinX the minimum x-coordinate of the bounding box. * @param boundingBoxMinY the minimum y-coordinate of the bounding box. * @param boundingBoxMinZ the minimum z-coordinate of the bounding box. @@ -4266,10 +4274,10 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(double boundingBoxMi * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line and the bounding box. It is either equal to - * 0, 1, or 2. + * 0, 1, or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ public static int intersectionBetweenLine3DAndBoundingBox3D(double boundingBoxMinX, double boundingBoxMinY, @@ -4338,7 +4346,7 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(double boundingBoxMi * intersection points. * *

- * + * * @param boundingBoxMin the minimum coordinate of the bounding box. Not modified. * @param boundingBoxMax the maximum coordinate of the bounding box. Not modified. * @param startX the x-coordinate of a point located on the @@ -4362,10 +4370,10 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(double boundingBoxMi * @param secondIntersectionToPack the coordinate of the second intersection. Can be * {@code null}. Modified. * @return the number of intersections between the line/line-segment/ray and the bounding box. It is - * either equal to 0, 1, or 2. + * either equal to 0, 1, or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ private static int intersectionBetweenLine3DAndBoundingBox3DImpl(Point3DReadOnly boundingBoxMin, Point3DReadOnly boundingBoxMax, @@ -4454,10 +4462,10 @@ private static int intersectionBetweenLine3DAndBoundingBox3DImpl(Point3DReadOnly * @param secondIntersectionToPack the coordinate of the second intersection. Can be * {@code null}. Modified. * @return the number of intersections between the line/line-segment/ray and the bounding box. It is - * either equal to 0, 1, or 2. + * either equal to 0, 1, or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ private static int intersectionBetweenLine3DAndBoundingBox3DImpl(double boundingBoxMinX, double boundingBoxMinY, @@ -4692,7 +4700,7 @@ private static int intersectionBetweenLine3DAndBoundingBox3DImpl(double bounding // if we get here we have a line-segment or a ray colinear with one of the box surfaces and we need to check for edge cases if (canIntersectionOccurAfterEnd) {// we have a ray and need to test for the edge-case that can only occur with ray at this point - // check if ray origin lies on bounding box surface and ray or line are colinear with that bounding box surface + // check if ray origin lies on bounding box surface and ray or line are colinear with that bounding box surface boolean isRayOriginOnBox = false; if (isColinearX) @@ -4742,7 +4750,7 @@ else if (isColinearZ) } else {// we have a line-segment and need to test for the edge-case that can only occur with a line-segment at this point - // check if start- and/or end- point lies on bounding box surface and the line-segment is colinear with that bounding box surface + // check if start- and/or end- point lies on bounding box surface and the line-segment is colinear with that bounding box surface boolean isLineSegmentStartOnBox = false; boolean isLineSegmentEndOnBox = false; double deltaXEndmin = boundingBoxMinX - endX; @@ -4763,7 +4771,6 @@ else if (isColinearZ) { // line-segment end point also lies on x surface of box isLineSegmentEndOnBox = true; } - } else if (isColinearY) {// check if start point of ray lies on bounding box surface @@ -4775,7 +4782,6 @@ else if (isColinearY) if (deltaXEndmin <= 0.0 && deltaXEndmax >= 0.0 && deltaZEndmin <= 0.0 && deltaZEndmax >= 0.0) { // line-segment end point also lies on x surface of box isLineSegmentEndOnBox = true; - } } else if (isColinearZ) @@ -4789,7 +4795,6 @@ else if (isColinearZ) { // line-segment end point also lies on x surface of box isLineSegmentEndOnBox = true; } - } if (isLineSegmentStartOnBox && isLineSegmentEndOnBox) { // start and end are on the box, we consider them intersections @@ -4841,7 +4846,6 @@ else if (isLineSegmentStartOnBox) } return numberOfIntersections; - } /** @@ -4880,9 +4884,9 @@ else if (isLineSegmentStartOnBox) * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line and the cylinder. It is either equal to 0, - * 1, or 2. + * 1, or 2. * @throws IllegalArgumentException if either {@code cylinderLength < 0} or - * {@code cylinderRadius < 0}. + * {@code cylinderRadius < 0}. */ public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, double cylinderRadius, @@ -4956,9 +4960,9 @@ public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line and the cylinder. It is either equal to 0, - * 1, or 2. + * 1, or 2. * @throws IllegalArgumentException if either {@code cylinderLength < 0} or - * {@code cylinderRadius < 0}. + * {@code cylinderRadius < 0}. */ public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, double cylinderRadius, @@ -5030,9 +5034,9 @@ public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line and the cylinder. It is either equal to 0, - * 1, or 2. + * 1, or 2. * @throws IllegalArgumentException if either {@code cylinderLength < 0} or - * {@code cylinderRadius < 0}. + * {@code cylinderRadius < 0}. */ public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, double cylinderRadius, @@ -5135,9 +5139,9 @@ public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, * @param secondIntersectionToPack the coordinate of the second intersection. Can be * {@code null}. Modified. * @return the number of intersections between the line/line-segment/ray and the cylinder. It is - * either equal to 0, 1, or 2. + * either equal to 0, 1, or 2. * @throws IllegalArgumentException if either {@code cylinderLength < 0} or - * {@code cylinderRadius < 0}. + * {@code cylinderRadius < 0}. */ private static int intersectionBetweenLine3DAndCylinder3DImpl(double cylinderLength, double cylinderRadius, @@ -5244,8 +5248,8 @@ else if (dBottom < dIntersection1) // If dIntersection2 is not NaN, that means two intersections were found which is the max, so no need to check with the cylinder part. if (Double.isNaN(dIntersection2)) { // Compute possible intersections with the cylinder part - // Notation used: cylinder axis: pa + va * d; line equation: p + v * d - // Need to solve quadratic equation of the form A * d^2 + B * d + C = 0 + // Notation used: cylinder axis: pa + va * d; line equation: p + v * d + // Need to solve quadratic equation of the form A * d^2 + B * d + C = 0 // (v, va)*va double scaledAxisX1 = lineDirectionDotCylinderAxis * axisX; @@ -5430,9 +5434,9 @@ else if (dCylinder2 < dIntersection1) * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line/line-segment/ray and the ellipsoid. It is - * either equal to 0, 1, or 2. + * either equal to 0, 1, or 2. * @throws IllegalArgumentException if either {@code radiusX}, {@code radiusY}, or {@code radiusZ} - * is negative. + * is negative. */ public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, double radiusY, @@ -5495,9 +5499,9 @@ public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line/line-segment/ray and the ellipsoid. It is - * either equal to 0, 1, or 2. + * either equal to 0, 1, or 2. * @throws IllegalArgumentException if either {@code radiusX}, {@code radiusY}, or {@code radiusZ} - * is negative. + * is negative. */ public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, double radiusY, @@ -5556,9 +5560,9 @@ public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line/line-segment/ray and the ellipsoid. It is - * either equal to 0, 1, or 2. + * either equal to 0, 1, or 2. * @throws IllegalArgumentException if either {@code radiusX}, {@code radiusY}, or {@code radiusZ} - * is negative. + * is negative. */ public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, double radiusY, @@ -5642,9 +5646,9 @@ public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, * @param secondIntersectionToPack the coordinate of the second intersection. Can be * {@code null}. Modified. * @return the number of intersections between the line/line-segment/ray and the ellipsoid. It is - * either equal to 0, 1, or 2. + * either equal to 0, 1, or 2. * @throws IllegalArgumentException if either {@code radiusX}, {@code radiusY}, or {@code radiusZ} - * is negative. + * is negative. */ private static int intersectionBetweenLine3DAndEllipsoid3DImpl(double radiusX, double radiusY, @@ -5785,7 +5789,7 @@ private static int intersectionBetweenLine3DAndEllipsoid3DImpl(double radiusX, * @param pointOnLine a point located on the line. Not modified. * @param lineDirection the direction of the line. Not modified. * @return the coordinates of the intersection, or {@code null} if the line is parallel to the - * plane. + * plane. */ public static Point3D intersectionBetweenLine3DAndPlane3D(Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal, @@ -5892,10 +5896,10 @@ public static boolean intersectionBetweenLine3DAndPlane3D(Point3DReadOnly pointO * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line segment and the bounding box. It is either - * equal to 0, 1, or 2. + * equal to 0, 1, or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ public static int intersectionBetweenLineSegment2DAndBoundingBox2D(Point2DReadOnly boundingBoxMin, Point2DReadOnly boundingBoxMax, @@ -5947,7 +5951,7 @@ public static int intersectionBetweenLineSegment2DAndBoundingBox2D(Point2DReadOn * first/last intersects with the bounding box boundary are returned as intersection points. * *

- * + * * @param boundingBoxMin the minimum coordinate of the bounding box. Not modified. * @param boundingBoxMax the maximum coordinate of the bounding box. Not modified. * @param lineSegmentStart the first endpoint of the line segment. Not modified. @@ -5957,10 +5961,10 @@ public static int intersectionBetweenLineSegment2DAndBoundingBox2D(Point2DReadOn * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line segment and the bounding box. It is either - * equal to 0, 1, or 2. + * equal to 0, 1, or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ public static int intersectionBetweenLineSegment3DAndBoundingBox3D(Point3DReadOnly boundingBoxMin, Point3DReadOnly boundingBoxMax, @@ -6011,9 +6015,9 @@ public static int intersectionBetweenLineSegment3DAndBoundingBox3D(Point3DReadOn * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line segment and the cylinder. It is either equal - * to 0, 1, or 2. + * to 0, 1, or 2. * @throws IllegalArgumentException if either {@code cylinderLength < 0} or - * {@code cylinderRadius < 0}. + * {@code cylinderRadius < 0}. */ public static int intersectionBetweenLineSegment3DAndCylinder3D(double cylinderLength, double cylinderRadius, @@ -6085,9 +6089,9 @@ public static int intersectionBetweenLineSegment3DAndCylinder3D(double cylinderL * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line/line-segment/ray and the ellipsoid. It is - * either equal to 0, 1, or 2. + * either equal to 0, 1, or 2. * @throws IllegalArgumentException if either {@code radiusX}, {@code radiusY}, or {@code radiusZ} - * is negative. + * is negative. */ public static int intersectionBetweenLineSegment3DAndEllipsoid3D(double radiusX, double radiusY, @@ -6199,10 +6203,10 @@ public static Point3D intersectionBetweenLineSegment3DAndPlane3D(Point3DReadOnly * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the ray and the bounding box. It is either equal to - * 0, 1, or 2. + * 0, 1, or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ public static int intersectionBetweenRay2DAndBoundingBox2D(Point2DReadOnly boundingBoxMin, Point2DReadOnly boundingBoxMax, @@ -6273,7 +6277,6 @@ public static boolean intersectionBetweenRay2DAndLineSegment2D(double rayOriginX double end2x = lineSegmentEndX; double end2y = lineSegmentEndY; return intersectionBetweenTwoLine2DsImpl(start1x, start1y, false, end1x, end1y, true, start2x, start2y, false, end2x, end2y, false, intersectionToPack); - } /** @@ -6392,7 +6395,7 @@ public static boolean intersectionBetweenRay2DAndLineSegment2D(Point2DReadOnly r * intersection points. * *

- * + * * @param boundingBoxMin the minimum coordinate of the bounding box. Not modified. * @param boundingBoxMax the maximum coordinate of the bounding box. Not modified. * @param rayOrigin the coordinate of the ray origin. Not modified. @@ -6402,10 +6405,10 @@ public static boolean intersectionBetweenRay2DAndLineSegment2D(Point2DReadOnly r * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the ray and the bounding box. It is either equal to - * 0, 1, or 2. + * 0, 1, or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ public static int intersectionBetweenRay3DAndBoundingBox3D(Point3DReadOnly boundingBoxMin, Point3DReadOnly boundingBoxMax, @@ -6457,10 +6460,10 @@ public static int intersectionBetweenRay3DAndBoundingBox3D(Point3DReadOnly bound * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line and the 3D box. It is either equal to 0, 1, - * or 2. If the ray origin is on the surface of the 3D box it is considered an intersection. + * or 2. If the ray origin is on the surface of the 3D box it is considered an intersection. * @throws IllegalArgumentException if {@code boxSize} contains values <= 0.0 * @see #intersectionBetweenRay3DAndBoundingBox3D(Point3DReadOnly, Point3DReadOnly, Point3DReadOnly, - * Vector3DReadOnly, Point3DBasics, Point3DBasics) + * Vector3DReadOnly, Point3DBasics, Point3DBasics) */ public static int intersectionBetweenRay3DAndBox3D(Point3DReadOnly boxPosition, Orientation3DReadOnly boxOrientation, @@ -6561,9 +6564,9 @@ public static int intersectionBetweenRay3DAndBox3D(Point3DReadOnly boxPosition, * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the ray and the bounding box. It is either equal to - * 0, 1, or 2. + * 0, 1, or 2. * @throws IllegalArgumentException if either {@code cylinderLength < 0} or - * {@code cylinderRadius < 0}. + * {@code cylinderRadius < 0}. */ public static int intersectionBetweenRay3DAndCylinder3D(double cylinderLength, double cylinderRadius, @@ -6635,9 +6638,9 @@ public static int intersectionBetweenRay3DAndCylinder3D(double cylinderLength, * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line/line-segment/ray and the ellipsoid. It is - * either equal to 0, 1, or 2. + * either equal to 0, 1, or 2. * @throws IllegalArgumentException if either {@code radiusX}, {@code radiusY}, or {@code radiusZ} - * is negative. + * is negative. */ public static int intersectionBetweenRay3DAndEllipsoid3D(double radiusX, double radiusY, @@ -7096,9 +7099,9 @@ public static boolean intersectionBetweenTwoPlane3Ds(Point3DReadOnly pointOnPlan intersectionDirectionToPack.scale(1.0 / EuclidCoreTools.squareRoot(det)); double normal3DotPoint1 = intersectionDirectionToPack.getX() * pointOnPlane1.getX() + intersectionDirectionToPack.getY() * pointOnPlane1.getY() - + intersectionDirectionToPack.getZ() * pointOnPlane1.getZ(); + + intersectionDirectionToPack.getZ() * pointOnPlane1.getZ(); double normal3DotPoint2 = intersectionDirectionToPack.getX() * pointOnPlane2.getX() + intersectionDirectionToPack.getY() * pointOnPlane2.getY() - + intersectionDirectionToPack.getZ() * pointOnPlane2.getZ(); + + intersectionDirectionToPack.getZ() * pointOnPlane2.getZ(); double d3 = 0.5 * (normal3DotPoint1 + normal3DotPoint2); pointOnIntersectionToPack.set(d1 * normal3Cross2X + d2 * normal1Cross3X + d3 * normal2Cross1X, @@ -7171,7 +7174,7 @@ public static boolean intersectionBetweenTwoPlane3Ds(Point3DReadOnly pointOnPlan * @param lengthSideB the length of the side B. * @param lengthSideC the length of the side C. * @return {@code true} if the lengths represents the three sides of a triangle, {@code false} - * otherwise. + * otherwise. * @throws IllegalArgumentException if any of the three lengths is negative. */ public static boolean isFormingTriangle(double lengthSideA, double lengthSideB, double lengthSideC) @@ -7203,8 +7206,8 @@ public static boolean isFormingTriangle(double lengthSideA, double lengthSideB, * @param rayDirectionX the x-component of the ray's direction. * @param rayDirectionY the y-component of the ray's direction. * @return {@link Location#AHEAD} if the query is located in front of the ray, - * {@link Location#BEHIND} if the query is behind the ray, and {@code null} if the query's - * projection onto the ray is exactly equal to the ray origin. + * {@link Location#BEHIND} if the query is behind the ray, and {@code null} if the query's + * projection onto the ray is exactly equal to the ray origin. */ public static Location whichPartOfRay2DIsPoint2DOn(double pointX, double pointY, @@ -7232,8 +7235,8 @@ else if (dotProduct < 0.0) * @param rayOrigin the ray's origin. Not modified. * @param rayDirection the ray's direction. Not modified. * @return {@link Location#AHEAD} if the query is located in front of the ray, - * {@link Location#BEHIND} if the query is behind the ray, and {@code null} if the query's - * projection onto the ray is exactly equal to the ray origin. + * {@link Location#BEHIND} if the query is behind the ray, and {@code null} if the query's + * projection onto the ray is exactly equal to the ray origin. */ public static Location whichPartOfRay2DIsPoint2DOn(Point2DReadOnly point, Point2DReadOnly rayOrigin, Vector2DReadOnly rayDirection) { @@ -7280,7 +7283,7 @@ public static boolean isPoint2DInFrontOfRay2D(Point2DReadOnly point, Point2DRead * @param b second vertex of the triangle. Not modified. * @param c third vertex of the triangle. Not modified. * @return {@code true} if the query is exactly inside the triangle. {@code false} if the query - * point is outside triangle or exactly on an edge of the triangle. + * point is outside triangle or exactly on an edge of the triangle. */ public static boolean isPoint2DInsideTriangleABC(Point2DReadOnly point, Point2DReadOnly a, Point2DReadOnly b, Point2DReadOnly c) { @@ -7388,7 +7391,7 @@ public static boolean isPoint2DOnLineSegment2D(Point2DReadOnly point, Point2DRea * @param firstPointOnLine a first point located on the line. Not modified. * @param secondPointOnLine a second point located on the line. Not modified. * @return {@link Location.LEFT}/{@link Location.RIGHT} if the point is on the left/right side of - * the line, or {@code null} if the point is exactly on the line. + * the line, or {@code null} if the point is exactly on the line. */ public static Location whichSideOfLine2DIsPoint2DOn(double pointX, double pointY, Point2DReadOnly firstPointOnLine, Point2DReadOnly secondPointOnLine) { @@ -7417,7 +7420,7 @@ public static Location whichSideOfLine2DIsPoint2DOn(double pointX, double pointY * @param pointOnLine a point positioned on the infinite line. Not modified. * @param lineDirection the direction of the infinite line. Not modified. * @return {@link Location.LEFT}/{@link Location.RIGHT} if the point is on the left/right side of - * the line, or {@code null} if the point is exactly on the line. + * the line, or {@code null} if the point is exactly on the line. */ public static Location whichSideOfLine2DIsPoint2DOn(double pointX, double pointY, Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection) { @@ -7446,7 +7449,7 @@ public static Location whichSideOfLine2DIsPoint2DOn(double pointX, double pointY * @param firstPointOnLine a first point located on the line. Not modified. * @param secondPointOnLine a second point located on the line. Not modified. * @return {@link Location.LEFT}/{@link Location.RIGHT} if the point is on the left/right side of - * the line, or {@code null} if the point is exactly on the line. + * the line, or {@code null} if the point is exactly on the line. */ public static Location whichSideOfLine2DIsPoint2DOn(Point2DReadOnly point, Point2DReadOnly firstPointOnLine, Point2DReadOnly secondPointOnLine) { @@ -7470,7 +7473,7 @@ public static Location whichSideOfLine2DIsPoint2DOn(Point2DReadOnly point, Point * @param pointOnLine a point positioned on the infinite line. Not modified. * @param lineDirection the direction of the infinite line. Not modified. * @return {@link Location.LEFT}/{@link Location.RIGHT} if the point is on the left/right side of - * the line, or {@code null} if the point is exactly on the line. + * the line, or {@code null} if the point is exactly on the line. */ public static Location whichSideOfLine2DIsPoint2DOn(Point2DReadOnly point, Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection) { @@ -7497,7 +7500,7 @@ public static Location whichSideOfLine2DIsPoint2DOn(Point2DReadOnly point, Point * @param lineDirectionX the x-component of the direction of the infinite line. * @param lineDirectionY the y-component of the direction of the infinite line. * @return {@link Location.LEFT}/{@link Location.RIGHT} if the point is on the left/right side of - * the line, or {@code null} if the point is exactly on the line. + * the line, or {@code null} if the point is exactly on the line. */ public static Location whichSideOfLine2DIsPoint2DOn(double pointX, double pointY, @@ -7532,7 +7535,7 @@ else if (crossProduct < 0.0) * @param firstPointOnLine a first point located on the line. Not modified. * @param secondPointOnLine a second point located on the line. Not modified. * @return {@code true} if the point is on the left side of the line, {@code false} if the point is - * on the right side or exactly on the line. + * on the right side or exactly on the line. */ public static boolean isPoint2DOnLeftSideOfLine2D(Point2DReadOnly point, Point2DReadOnly firstPointOnLine, Point2DReadOnly secondPointOnLine) { @@ -7554,7 +7557,7 @@ public static boolean isPoint2DOnLeftSideOfLine2D(Point2DReadOnly point, Point2D * @param firstPointOnLine a first point located on the line. Not modified. * @param secondPointOnLine a second point located on the line. Not modified. * @return {@code true} if the point is on the right side of the line, {@code false} if the point is - * on the left side or exactly on the line. + * on the left side or exactly on the line. */ public static boolean isPoint2DOnRightSideOfLine2D(Point2DReadOnly point, Point2DReadOnly firstPointOnLine, Point2DReadOnly secondPointOnLine) { @@ -7583,10 +7586,10 @@ public static boolean isPoint2DOnRightSideOfLine2D(Point2DReadOnly point, Point2 * @param testLeftSide the query of the side, when equal to {@code true} this will test for the * left side, {@code false} this will test for the right side. * @return {@code true} if the point is on the query side of the line, {@code false} if the point is - * on the opposite side or exactly on the line. + * on the opposite side or exactly on the line. * @deprecated Use - * {@link #whichSideOfLine2DIsPoint2DOn(double, double, double, double, double, double)} - * instead. + * {@link #whichSideOfLine2DIsPoint2DOn(double, double, double, double, double, double)} + * instead. */ @Deprecated public static boolean isPoint2DOnSideOfLine2D(double pointX, @@ -7622,10 +7625,10 @@ public static boolean isPoint2DOnSideOfLine2D(double pointX, * @param testLeftSide the query of the side, when equal to {@code true} this will test for the * left side, {@code false} this will test for the right side. * @return {@code true} if the point is on the query side of the line, {@code false} if the point is - * on the opposite side or exactly on the line. + * on the opposite side or exactly on the line. * @deprecated Use - * {@link #whichSideOfLine2DIsPoint2DOn(double, double, Point2DReadOnly, Point2DReadOnly)} - * instead. + * {@link #whichSideOfLine2DIsPoint2DOn(double, double, Point2DReadOnly, Point2DReadOnly)} + * instead. */ @Deprecated public static boolean isPoint2DOnSideOfLine2D(double pointX, @@ -7658,10 +7661,10 @@ public static boolean isPoint2DOnSideOfLine2D(double pointX, * @param testLeftSide the query of the side, when equal to {@code true} this will test for the * left side, {@code false} this will test for the right side. * @return {@code true} if the point is on the query side of the line, {@code false} if the point is - * on the opposite side or exactly on the line. + * on the opposite side or exactly on the line. * @deprecated Use - * {@link #whichSideOfLine2DIsPoint2DOn(double, double, Point2DReadOnly, Vector2DReadOnly)} - * instead. + * {@link #whichSideOfLine2DIsPoint2DOn(double, double, Point2DReadOnly, Vector2DReadOnly)} + * instead. */ @Deprecated public static boolean isPoint2DOnSideOfLine2D(double pointX, @@ -7694,10 +7697,10 @@ public static boolean isPoint2DOnSideOfLine2D(double pointX, * @param testLeftSide the query of the side, when equal to {@code true} this will test for the * left side, {@code false} this will test for the right side. * @return {@code true} if the point is on the query side of the line, {@code false} if the point is - * on the opposite side or exactly on the line. + * on the opposite side or exactly on the line. * @deprecated Use - * {@link #whichSideOfLine2DIsPoint2DOn(Point2DReadOnly, Point2DReadOnly, Point2DReadOnly)} - * instead. + * {@link #whichSideOfLine2DIsPoint2DOn(Point2DReadOnly, Point2DReadOnly, Point2DReadOnly)} + * instead. */ @Deprecated public static boolean isPoint2DOnSideOfLine2D(Point2DReadOnly point, @@ -7728,10 +7731,10 @@ public static boolean isPoint2DOnSideOfLine2D(Point2DReadOnly point, * @param testLeftSide the query of the side, when equal to {@code true} this will test for the * left side, {@code false} this will test for the right side. * @return {@code true} if the point is on the query side of the line, {@code false} if the point is - * on the opposite side or exactly on the line. + * on the opposite side or exactly on the line. * @deprecated Use - * {@link #whichSideOfLine2DIsPoint2DOn(Point2DReadOnly, Point2DReadOnly, Vector2DReadOnly)} - * instead. + * {@link #whichSideOfLine2DIsPoint2DOn(Point2DReadOnly, Point2DReadOnly, Vector2DReadOnly)} + * instead. */ @Deprecated public static boolean isPoint2DOnSideOfLine2D(Point2DReadOnly point, Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection, boolean testLeftSide) @@ -7763,7 +7766,7 @@ public static boolean isPoint2DOnSideOfLine2D(Point2DReadOnly point, Point2DRead * @param planeNormalY the y-component of the normal of the infinite plane. * @param planeNormalZ the z-component of the normal of the infinite plane. * @return {@link Location#ABOVE}/{@link Location#BELOW} if the point is above/below the plane, - * {@code null} if the point is exactly on the plane. + * {@code null} if the point is exactly on the plane. */ public static Location whichSideOfPlane3DIsPoint3DOn(double pointX, double pointY, @@ -7807,7 +7810,7 @@ else if (signedDistance < 0.0) * @param pointOnPlane the coordinates of a point positioned on the infinite plane. Not modified. * @param planeNormal the normal of the infinite plane. Not modified. * @return {@link Location#ABOVE}/{@link Location#BELOW} if the point is above/below the plane, - * {@code null} if the point is exactly on the plane. + * {@code null} if the point is exactly on the plane. */ public static Location whichSideOfPlane3DIsPoint3DOn(double pointX, double pointY, double pointZ, Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal) { @@ -7839,7 +7842,7 @@ public static Location whichSideOfPlane3DIsPoint3DOn(double pointX, double point * @param pointOnPlane the coordinates of a point positioned on the infinite plane. Not modified. * @param planeNormal the normal of the infinite plane. Not modified. * @return {@link Location#ABOVE}/{@link Location#BELOW} if the point is above/below the plane, - * {@code null} if the point is exactly on the plane. + * {@code null} if the point is exactly on the plane. */ public static Location whichSideOfPlane3DIsPoint3DOn(Point3DReadOnly point, Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal) { @@ -7872,9 +7875,9 @@ public static Location whichSideOfPlane3DIsPoint3DOn(Point3DReadOnly point, Poin * @param planeSecondTangentY the y-component of a second tangent of the infinite plane. * @param planeSecondTangentZ the z-component of a second tangent of the infinite plane. * @return {@link Location#ABOVE}/{@link Location#BELOW} if the point is above/below the plane, - * {@code null} if the point is exactly on the plane. + * {@code null} if the point is exactly on the plane. * @see #whichSideOfPlane3DIsPoint3DOn(double, double, double, double, double, double, double, - * double, double) + * double, double) */ public static Location whichSideOfPlane3DIsPoint3DOn(double pointX, double pointY, @@ -7916,9 +7919,9 @@ public static Location whichSideOfPlane3DIsPoint3DOn(double pointX, * @param planeFirstTangent a first tangent of the infinite plane. Not modified. * @param planeSecondTangent a second tangent of the infinite plane. Not modified. * @return {@link Location#ABOVE}/{@link Location#BELOW} if the point is above/below the plane, - * {@code null} if the point is exactly on the plane. + * {@code null} if the point is exactly on the plane. * @see #whichSideOfPlane3DIsPoint3DOn(double, double, double, double, double, double, double, - * double, double) + * double, double) */ public static Location whichSideOfPlane3DIsPoint3DOn(double pointX, double pointY, @@ -7960,9 +7963,9 @@ public static Location whichSideOfPlane3DIsPoint3DOn(double pointX, * @param planeFirstTangent a first tangent of the infinite plane. Not modified. * @param planeSecondTangent a second tangent of the infinite plane. Not modified. * @return {@link Location#ABOVE}/{@link Location#BELOW} if the point is above/below the plane, - * {@code null} if the point is exactly on the plane. + * {@code null} if the point is exactly on the plane. * @see #whichSideOfPlane3DIsPoint3DOn(double, double, double, double, double, double, double, - * double, double) + * double, double) */ public static Location whichSideOfPlane3DIsPoint3DOn(Point3DReadOnly point, Point3DReadOnly pointOnPlane, @@ -7997,10 +8000,10 @@ public static Location whichSideOfPlane3DIsPoint3DOn(Point3DReadOnly point, * @param testForAbove the query of the side, when equal to {@code true} this will test for the * above side, {@code false} this will test for the below side. * @return {@code true} if the point is on the query side of the plane, {@code false} if the point - * is on the opposite side or exactly on the plane. + * is on the opposite side or exactly on the plane. * @deprecated Use - * {@link #whichSideOfPlane3DIsPoint3DOn(double, double, double, double, double, double, double, double, double)} - * instead. + * {@link #whichSideOfPlane3DIsPoint3DOn(double, double, double, double, double, double, double, double, double)} + * instead. */ @Deprecated public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, @@ -8047,10 +8050,10 @@ public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, * @param testForAbove the query of the side, when equal to {@code true} this will test for the * above side, {@code false} this will test for the below side. * @return {@code true} if the point is on the query side of the plane, {@code false} if the point - * is on the opposite side or exactly on the plane. + * is on the opposite side or exactly on the plane. * @deprecated Use - * {@link #whichSideOfPlane3DIsPoint3DOn(double, double, double, Point3DReadOnly, Vector3DReadOnly, boolean)} - * instead. + * {@link #whichSideOfPlane3DIsPoint3DOn(double, double, double, Point3DReadOnly, Vector3DReadOnly, boolean)} + * instead. */ @Deprecated public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, @@ -8083,10 +8086,10 @@ public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, * @param testForAbove the query of the side, when equal to {@code true} this will test for the * above side, {@code false} this will test for the below side. * @return {@code true} if the point is on the query side of the plane, {@code false} if the point - * is on the opposite side or exactly on the plane. + * is on the opposite side or exactly on the plane. * @deprecated Use - * {@link #whichSideOfPlane3DIsPoint3DOn(Point3DReadOnly, Point3DReadOnly, Vector3DReadOnly)} - * instead. + * {@link #whichSideOfPlane3DIsPoint3DOn(Point3DReadOnly, Point3DReadOnly, Vector3DReadOnly)} + * instead. */ @Deprecated public static boolean isPoint3DAboveOrBelowPlane3D(Point3DReadOnly point, Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal, boolean testForAbove) @@ -8114,7 +8117,7 @@ public static boolean isPoint3DAboveOrBelowPlane3D(Point3DReadOnly point, Point3 * @param pointOnPlane the coordinates of a point positioned on the infinite plane. Not modified. * @param planeNormal the normal of the infinite plane. Not modified. * @return {@code true} if the point is strictly above the plane, {@code false} if the point is - * below or exactly on the plane. + * below or exactly on the plane. */ public static boolean isPoint3DAbovePlane3D(double pointX, double pointY, double pointZ, Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal) { @@ -8138,7 +8141,7 @@ public static boolean isPoint3DAbovePlane3D(double pointX, double pointY, double * @param pointOnPlane the coordinates of a point positioned on the infinite plane. Not modified. * @param planeNormal the normal of the infinite plane. Not modified. * @return {@code true} if the point is strictly above the plane, {@code false} if the point is - * below or exactly on the plane. + * below or exactly on the plane. */ public static boolean isPoint3DAbovePlane3D(Point3DReadOnly point, Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal) { @@ -8164,7 +8167,7 @@ public static boolean isPoint3DAbovePlane3D(Point3DReadOnly point, Point3DReadOn * @param pointOnPlane the coordinates of a point positioned on the infinite plane. Not modified. * @param planeNormal the normal of the infinite plane. Not modified. * @return {@code true} if the point is strictly below the plane, {@code false} if the point is - * above or exactly on the plane. + * above or exactly on the plane. */ public static boolean isPoint3DBelowPlane3D(double pointX, double pointY, double pointZ, Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal) { @@ -8188,7 +8191,7 @@ public static boolean isPoint3DBelowPlane3D(double pointX, double pointY, double * @param pointOnPlane the coordinates of a point positioned on the infinite plane. Not modified. * @param planeNormal the normal of the infinite plane. Not modified. * @return {@code true} if the point is strictly below the plane, {@code false} if the point is - * above or exactly on the plane. + * above or exactly on the plane. */ public static boolean isPoint3DBelowPlane3D(Point3DReadOnly point, Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal) { @@ -8223,12 +8226,12 @@ public static boolean isPoint3DBelowPlane3D(Point3DReadOnly point, Point3DReadOn * @param testForAbove the query of the side, when equal to {@code true} this will test for * the above side, {@code false} this will test for the below side. * @return {@code true} if the point is on the query side of the plane, {@code false} if the point - * is on the opposite side or exactly on the plane. + * is on the opposite side or exactly on the plane. * @see #isPoint3DAboveOrBelowPlane3D(double, double, double, double, double, double, double, - * double, double, boolean) + * double, double, boolean) * @deprecated Use - * {@link #whichSideOfPlane3DIsPoint3DOn(double, double, double, double, double, double, double, double, double, double, double, double)} - * instead. + * {@link #whichSideOfPlane3DIsPoint3DOn(double, double, double, double, double, double, double, double, double, double, double, double)} + * instead. */ @Deprecated public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, @@ -8258,7 +8261,6 @@ public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, planeSecondTangentY, planeSecondTangentZ); return testForAbove ? side == Location.ABOVE : side == Location.BELOW; - } /** @@ -8284,12 +8286,12 @@ public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, * @param testForAbove the query of the side, when equal to {@code true} this will test for * the above side, {@code false} this will test for the below side. * @return {@code true} if the point is on the query side of the plane, {@code false} if the point - * is on the opposite side or exactly on the plane. + * is on the opposite side or exactly on the plane. * @see #isPoint3DAboveOrBelowPlane3D(double, double, double, double, double, double, double, - * double, double, boolean) + * double, double, boolean) * @deprecated Use - * {@link #whichSideOfPlane3DIsPoint3DOn(double, double, double, Point3DReadOnly, Vector3DReadOnly, Vector3DReadOnly)} - * instead. + * {@link #whichSideOfPlane3DIsPoint3DOn(double, double, double, Point3DReadOnly, Vector3DReadOnly, Vector3DReadOnly)} + * instead. */ @Deprecated public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, @@ -8302,7 +8304,6 @@ public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, { Location side = whichSideOfPlane3DIsPoint3DOn(pointX, pointY, pointZ, pointOnPlane, planeFirstTangent, planeSecondTangent); return testForAbove ? side == Location.ABOVE : side == Location.BELOW; - } /** @@ -8326,12 +8327,12 @@ public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, * @param testForAbove the query of the side, when equal to {@code true} this will test for * the above side, {@code false} this will test for the below side. * @return {@code true} if the point is on the query side of the plane, {@code false} if the point - * is on the opposite side or exactly on the plane. + * is on the opposite side or exactly on the plane. * @see #isPoint3DAboveOrBelowPlane3D(double, double, double, double, double, double, double, - * double, double, boolean) + * double, double, boolean) * @deprecated Use - * {@link #whichSideOfPlane3DIsPoint3DOn(Point3DReadOnly, Point3DReadOnly, Vector3DReadOnly, Vector3DReadOnly)} - * instead. + * {@link #whichSideOfPlane3DIsPoint3DOn(Point3DReadOnly, Point3DReadOnly, Vector3DReadOnly, Vector3DReadOnly)} + * instead. */ @Deprecated public static boolean isPoint3DAboveOrBelowPlane3D(Point3DReadOnly point, @@ -8342,7 +8343,6 @@ public static boolean isPoint3DAboveOrBelowPlane3D(Point3DReadOnly point, { Location side = whichSideOfPlane3DIsPoint3DOn(point, pointOnPlane, planeFirstTangent, planeSecondTangent); return testForAbove ? side == Location.ABOVE : side == Location.BELOW; - } /** @@ -8366,9 +8366,9 @@ public static boolean isPoint3DAboveOrBelowPlane3D(Point3DReadOnly point, * @param planeFirstTangent a first tangent of the infinite plane. Not modified. * @param planeSecondTangent a second tangent of the infinite plane. Not modified. * @return {@code true} if the point is strictly above the plane, {@code false} if the point is - * below or exactly on the plane. + * below or exactly on the plane. * @see #isPoint3DAboveOrBelowPlane3D(double, double, double, double, double, double, double, - * double, double, boolean) + * double, double, boolean) */ public static boolean isPoint3DAbovePlane3D(double pointX, double pointY, @@ -8399,9 +8399,9 @@ public static boolean isPoint3DAbovePlane3D(double pointX, * @param planeFirstTangent a first tangent of the infinite plane. Not modified. * @param planeSecondTangent a second tangent of the infinite plane. Not modified. * @return {@code true} if the point is strictly above the plane, {@code false} if the point is - * below or exactly on the plane. + * below or exactly on the plane. * @see #isPoint3DAboveOrBelowPlane3D(double, double, double, double, double, double, double, - * double, double, boolean) + * double, double, boolean) */ public static boolean isPoint3DAbovePlane3D(Point3DReadOnly point, Point3DReadOnly pointOnPlane, @@ -8432,9 +8432,9 @@ public static boolean isPoint3DAbovePlane3D(Point3DReadOnly point, * @param planeFirstTangent a first tangent of the infinite plane. Not modified. * @param planeSecondTangent a second tangent of the infinite plane. Not modified. * @return {@code true} if the point is strictly below the plane, {@code false} if the point is - * above or exactly on the plane. + * above or exactly on the plane. * @see #isPoint3DAboveOrBelowPlane3D(double, double, double, double, double, double, double, - * double, double, boolean) + * double, double, boolean) */ public static boolean isPoint3DBelowPlane3D(double pointX, double pointY, @@ -8465,9 +8465,9 @@ public static boolean isPoint3DBelowPlane3D(double pointX, * @param planeFirstTangent a first tangent of the infinite plane. Not modified. * @param planeSecondTangent a second tangent of the infinite plane. Not modified. * @return {@code true} if the point is strictly below the plane, {@code false} if the point is - * above or exactly on the plane. + * above or exactly on the plane. * @see #isPoint3DAboveOrBelowPlane3D(double, double, double, double, double, double, double, - * double, double, boolean) + * double, double, boolean) */ public static boolean isPoint3DBelowPlane3D(Point3DReadOnly point, Point3DReadOnly pointOnPlane, @@ -9245,8 +9245,8 @@ public static boolean orthogonalProjectionOnPlane3D(double x, projectionToPack.set(x, y, z); projectionToPack.sub(pointOnPlane); - double signedDistance = projectionToPack.getX() * planeNormal.getX() + projectionToPack.getY() * planeNormal.getY() - + projectionToPack.getZ() * planeNormal.getZ(); + double signedDistance = + projectionToPack.getX() * planeNormal.getX() + projectionToPack.getY() * planeNormal.getY() + projectionToPack.getZ() * planeNormal.getZ(); signedDistance /= normalMagnitude * normalMagnitude; projectionToPack.set(x - signedDistance * planeNormal.getX(), y - signedDistance * planeNormal.getY(), z - signedDistance * planeNormal.getZ()); @@ -9276,7 +9276,7 @@ public static boolean orthogonalProjectionOnPlane3D(double x, * @param lineDirection2x x-component of the second line direction. * @param lineDirection2y y-component of the second line direction. * @return {@code alpha} the percentage along the first line of the intersection location. This - * method returns {@link Double#NaN} if the lines do not intersect. + * method returns {@link Double#NaN} if the lines do not intersect. */ public static double percentageOfIntersectionBetweenTwoLine2Ds(double pointOnLine1x, double pointOnLine1y, @@ -9301,7 +9301,7 @@ public static double percentageOfIntersectionBetweenTwoLine2Ds(double pointOnLin if (Math.abs(determinant) < ONE_TRILLIONTH) { // The lines are parallel - // Check if they are collinear + // Check if they are collinear double cross = dx * lineDirection1y - dy * lineDirection1x; if (Math.abs(cross) < ONE_TRILLIONTH) { @@ -9347,7 +9347,7 @@ public static double percentageOfIntersectionBetweenTwoLine2Ds(double pointOnLin * @param pointOnLine2 a point located on the second line. Not modified. * @param lineDirection2 the second line direction. Not modified. * @return {@code alpha} the percentage along the first line of the intersection location. This - * method returns {@link Double#NaN} if the lines do not intersect. + * method returns {@link Double#NaN} if the lines do not intersect. */ public static double percentageOfIntersectionBetweenTwoLine2Ds(Point2DReadOnly pointOnLine1, Vector2DReadOnly lineDirection1, @@ -9387,7 +9387,7 @@ public static double percentageOfIntersectionBetweenTwoLine2Ds(Point2DReadOnly p * @param pointOnLine a point located on the line. Not modified. * @param lineDirection the line direction. Not modified. * @return {@code alpha} the percentage along the line segment of the intersection location. This - * method returns {@link Double#NaN} if the line segment and the line do not intersect. + * method returns {@link Double#NaN} if the line segment and the line do not intersect. */ public static double percentageOfIntersectionBetweenLineSegment2DAndLine2D(Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd, @@ -9437,7 +9437,7 @@ else if (alpha > 1.0) * @param pointOnLine a point located on the line. Not modified. * @param lineDirection the direction of the line. Not modified. * @return the computed percentage along the line representing where the point projection is - * located. + * located. */ public static double percentageAlongLine2D(Point2DReadOnly point, Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection) { @@ -9464,7 +9464,7 @@ public static double percentageAlongLine2D(Point2DReadOnly point, Point2DReadOnl * @param pointOnLine a point located on the line. Not modified. * @param lineDirection the direction of the line. Not modified. * @return the computed percentage along the line representing where the point projection is - * located. + * located. */ public static double percentageAlongLine2D(double pointX, double pointY, Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection) { @@ -9493,7 +9493,7 @@ public static double percentageAlongLine2D(double pointX, double pointY, Point2D * @param lineDirectionX x-component of the direction of the line. * @param lineDirectionY y-component of the direction of the line. * @return the computed percentage along the line representing where the point projection is - * located. + * located. */ public static double percentageAlongLine2D(double pointX, double pointY, @@ -9543,7 +9543,7 @@ public static double percentageAlongLine2D(double pointX, * @param lineSegmentEndX the x-coordinate of the line segment second endpoint. * @param lineSegmentEndY the y-coordinate of the line segment second endpoint. * @return the computed percentage along the line segment representing where the point projection is - * located. + * located. */ public static double percentageAlongLineSegment2D(double pointX, double pointY, @@ -9593,7 +9593,7 @@ public static double percentageAlongLineSegment2D(double pointX, * @param lineSegmentStart the line segment first endpoint. Not modified. * @param lineSegmentEnd the line segment second endpoint. Not modified. * @return the computed percentage along the line segment representing where the point projection is - * located. + * located. */ public static double percentageAlongLineSegment2D(double pointX, double pointY, Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd) { @@ -9625,7 +9625,7 @@ public static double percentageAlongLineSegment2D(double pointX, double pointY, * @param lineSegmentStart the line segment first endpoint. Not modified. * @param lineSegmentEnd the line segment second endpoint. Not modified. * @return the computed percentage along the line segment representing where the point projection is - * located. + * located. */ public static double percentageAlongLineSegment2D(Point2DReadOnly point, Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd) { @@ -9651,7 +9651,7 @@ public static double percentageAlongLineSegment2D(Point2DReadOnly point, Point2D * @param pointOnLine a point located on the line. Not modified. * @param lineDirection the direction of the line. Not modified. * @return the computed percentage along the line representing where the point projection is - * located. + * located. */ public static double percentageAlongLine3D(Point3DReadOnly point, Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection) { @@ -9687,7 +9687,7 @@ public static double percentageAlongLine3D(Point3DReadOnly point, Point3DReadOnl * @param pointOnLine a point located on the line. Not modified. * @param lineDirection the direction of the line. Not modified. * @return the computed percentage along the line representing where the point projection is - * located. + * located. */ public static double percentageAlongLine3D(double pointX, double pointY, double pointZ, Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection) { @@ -9727,7 +9727,7 @@ public static double percentageAlongLine3D(double pointX, double pointY, double * @param lineDirectionY y-component of the direction of the line. * @param lineDirectionZ z-component of the direction of the line. * @return the computed percentage along the line representing where the point projection is - * located. + * located. */ public static double percentageAlongLine3D(double pointX, double pointY, @@ -9784,7 +9784,7 @@ public static double percentageAlongLine3D(double pointX, * @param lineSegmentEndY the y-coordinate of the line segment second endpoint. * @param lineSegmentEndZ the z-coordinate of the line segment second endpoint. * @return the computed percentage along the line segment representing where the point projection is - * located. + * located. */ public static double percentageAlongLineSegment3D(double pointX, double pointY, @@ -9840,7 +9840,7 @@ public static double percentageAlongLineSegment3D(double pointX, * @param lineSegmentStart the line segment first endpoint. Not modified. * @param lineSegmentEnd the line segment second endpoint. Not modified. * @return the computed percentage along the line segment representing where the point projection is - * located. + * located. */ public static double percentageAlongLineSegment3D(double pointX, double pointY, @@ -9884,7 +9884,7 @@ public static double percentageAlongLineSegment3D(double pointX, * @param lineSegmentStart the line segment first endpoint. Not modified. * @param lineSegmentEnd the line segment second endpoint. Not modified. * @return the computed percentage along the line segment representing where the point projection is - * located. + * located. */ public static double percentageAlongLineSegment3D(Point3DReadOnly point, Point3DReadOnly lineSegmentStart, Point3DReadOnly lineSegmentEnd) { @@ -10090,7 +10090,7 @@ public static void perpendicularVector2D(Vector2DReadOnly vector, Vector2DBasics * @param orthogonalProjectionToPack a 3D point in which the projection of {@code point} onto the * line is stored. Modified. Can be {@code null}. * @return the vector perpendicular to the line and pointing to the {@code point}, or {@code null} - * when the method fails. + * when the method fails. */ public static Vector3D perpendicularVector3DFromLine3DToPoint3D(Point3DReadOnly point, Point3DReadOnly firstPointOnLine, @@ -10188,7 +10188,7 @@ public static boolean perpendicularVector3DFromLine3DToPoint3D(Point3DReadOnly p * @param cathetusA the length of the cathetus A. * @return the length of the cathetus B. * @throws IllegalArgumentException if the length of the cathetus A is negative or greater than the - * hypotenuse C. + * hypotenuse C. */ public static double pythagorasGetCathetus(double hypotenuseC, double cathetusA) { @@ -10262,7 +10262,7 @@ public static double radiusOfArc(double chordLength, double chordAngle) * @param firstPointOnLine a first point located on the line. Not modified. * @param secondPointOnLine a second point located on the line. Not modified. * @return the minimum distance between the 2D point and the 2D line. The distance is negative if - * the query is located on the right side of the line. + * the query is located on the right side of the line. */ public static double signedDistanceFromPoint2DToLine2D(double pointX, double pointY, Point2DReadOnly firstPointOnLine, Point2DReadOnly secondPointOnLine) { @@ -10292,7 +10292,7 @@ public static double signedDistanceFromPoint2DToLine2D(double pointX, double poi * @param pointOnLine a point located on the line. Not modified. * @param lineDirection the direction of the line. Not modified. * @return the minimum distance between the 2D point and the 2D line. The distance is negative if - * the query is located on the right side of the line. + * the query is located on the right side of the line. */ public static double signedDistanceFromPoint2DToLine2D(double pointX, double pointY, Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection) { @@ -10323,7 +10323,7 @@ public static double signedDistanceFromPoint2DToLine2D(double pointX, double poi * @param firstPointOnLine a first point located on the line. Not modified. * @param secondPointOnLine a second point located on the line. Not modified. * @return the minimum distance between the 2D point and the 2D line. The distance is negative if - * the query is located on the right side of the line. + * the query is located on the right side of the line. */ public static double signedDistanceFromPoint2DToLine2D(Point2DReadOnly point, Point2DReadOnly firstPointOnLine, Point2DReadOnly secondPointOnLine) { @@ -10348,7 +10348,7 @@ public static double signedDistanceFromPoint2DToLine2D(Point2DReadOnly point, Po * @param pointOnLine a point located on the line. Not modified. * @param lineDirection the direction of the line. Not modified. * @return the minimum distance between the 2D point and the 2D line. The distance is negative if - * the query is located on the right side of the line. + * the query is located on the right side of the line. */ public static double signedDistanceFromPoint2DToLine2D(Point2DReadOnly point, Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection) { @@ -10376,7 +10376,7 @@ public static double signedDistanceFromPoint2DToLine2D(Point2DReadOnly point, Po * @param lineDirectionX x-component of the line direction. * @param lineDirectionY y-component of the line direction. * @return the minimum distance between the 2D point and the 2D line. The distance is negative if - * the query is located on the right side of the line. + * the query is located on the right side of the line. */ public static double signedDistanceFromPoint2DToLine2D(double pointX, double pointY, @@ -10631,8 +10631,8 @@ public static double triangleCircumradius(double lengthA, double lengthB, double public static double triangleIsoscelesHeight(double legLength, double baseLength) { if (legLength < 0.5 * baseLength) - throw new IllegalArgumentException("Malformed isosceles triangle, expected legLength > baseLength/2, was legLength: " + legLength + ", baseLength/2: " - + 0.5 * baseLength); + throw new IllegalArgumentException( + "Malformed isosceles triangle, expected legLength > baseLength/2, was legLength: " + legLength + ", baseLength/2: " + 0.5 * baseLength); if (baseLength < 0.0) throw new IllegalArgumentException("The base cannot have a negative length, baseLength = " + baseLength); @@ -10650,14 +10650,15 @@ public static double triangleIsoscelesHeight(double legLength, double baseLength * @param lengthOppositeSideC the length of the side C. * @return the value in radians of the unknown angle. * @throws IllegalArgumentException if the lengths do not describe a triangle, see - * {@link #isFormingTriangle(double, double, double)}. + * {@link #isFormingTriangle(double, double, double)}. */ public static double unknownTriangleAngleByLawOfCosine(double lengthNeighbourSideA, double lengthNeighbourSideB, double lengthOppositeSideC) { if (!isFormingTriangle(lengthNeighbourSideA, lengthNeighbourSideB, lengthOppositeSideC)) { - throw new IllegalArgumentException("Unable to build a Triangle of the given triangle sides a: " + lengthNeighbourSideA + " b: " + lengthNeighbourSideB - + " c: " + lengthOppositeSideC); + throw new IllegalArgumentException( + "Unable to build a Triangle of the given triangle sides a: " + lengthNeighbourSideA + " b: " + lengthNeighbourSideB + " c: " + + lengthOppositeSideC); } double numerator = lengthNeighbourSideA * lengthNeighbourSideA + lengthNeighbourSideB * lengthNeighbourSideB - lengthOppositeSideC * lengthOppositeSideC; @@ -10681,7 +10682,7 @@ public static double unknownTriangleAngleByLawOfCosine(double lengthNeighbourSid * @param angleBetweenAAndB the angle between the sides A and B. * @return the value of the unknown side length. * @throws IllegalArgumentException if {@code lengthSideA} and/or {@code lengthSideB} are negative, - * if {@code angleBetweenAAndB} is greater than pi. + * if {@code angleBetweenAAndB} is greater than pi. */ public static double unknownTriangleSideLengthByLawOfCosine(double lengthSideA, double lengthSideB, double angleBetweenAAndB) { @@ -10692,7 +10693,1006 @@ public static double unknownTriangleSideLengthByLawOfCosine(double lengthSideA, if (Math.abs(angleBetweenAAndB) > Math.PI) throw new IllegalArgumentException("angleBetweenAAndB " + angleBetweenAAndB + " does not define a triangle."); - return EuclidCoreTools.squareRoot(lengthSideA * lengthSideA + lengthSideB * lengthSideB - - 2.0 * lengthSideA * lengthSideB * EuclidCoreTools.cos(angleBetweenAAndB)); + return EuclidCoreTools.squareRoot( + lengthSideA * lengthSideA + lengthSideB * lengthSideB - 2.0 * lengthSideA * lengthSideB * EuclidCoreTools.cos(angleBetweenAAndB)); + } + + /** + * Tests if the point 2D is located on the infinitely long line 2D. + *

+ * The test is performed by computing the distance between the point and the line, if that distance + * is below {@link EuclidGeometryTools#IS_POINT_ON_LINE_EPS} this method returns {@code true}. + *

+ * + * @param lineSegmentStart the first endpoint of the line segment. Not modified. + * @param lineSegmentEnd the second endpoint of the line segment. Not modified. + * @return {@code true} if the query is considered to be lying on the line, {@code false} otherwise. + */ + public static boolean isPoint2DOnLineSegment2D(double pointX, double pointY, Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd) + { + return distanceSquaredFromPoint2DToLineSegment2D(pointX, + pointY, + lineSegmentStart.getX(), + lineSegmentStart.getY(), + lineSegmentEnd.getX(), + lineSegmentEnd.getY()) < IS_POINT_ON_LINE_EPS; + } + + /** + * This method implements the same operation as + * {@link EuclidGeometryTools#orientation3DFromFirstToSecondVector3D(double, double, double, double, double, double, Orientation3DBasics)} + * except that it does not rely on {@code Math#acos(double)} making it faster. + * + * @param firstVectorX x-component of the first vector. + * @param firstVectorY y-component of the first vector. + * @param firstVectorZ z-component of the first vector. + * @param secondVectorX x-component of the second vector that is rotated with respect to the first + * vector. + * @param secondVectorY y-component of the second vector that is rotated with respect to the first + * vector. + * @param secondVectorZ z-component of the second vector that is rotated with respect to the first + * vector. + * @param rotationToPack the minimum rotation from {@code firstVector} to the {@code secondVector}. + * Modified. + * @see EuclidGeometryTools#orientation3DFromFirstToSecondVector3D(double, double, double, double, + * double, double, Orientation3DBasics) + */ + public static void rotationMatrix3DFromFirstToSecondVector3D(double firstVectorX, + double firstVectorY, + double firstVectorZ, + double secondVectorX, + double secondVectorY, + double secondVectorZ, + CommonMatrix3DBasics rotationToPack) + { + double firstVectorLengthInv = 1.0 / Math.sqrt(normSquared(firstVectorX, firstVectorY, firstVectorZ)); + double secondVectorLengthInv = 1.0 / Math.sqrt(normSquared(secondVectorX, secondVectorY, secondVectorZ)); + firstVectorX *= firstVectorLengthInv; + firstVectorY *= firstVectorLengthInv; + firstVectorZ *= firstVectorLengthInv; + secondVectorX *= secondVectorLengthInv; + secondVectorY *= secondVectorLengthInv; + secondVectorZ *= secondVectorLengthInv; + + double rotationAxisX = firstVectorY * secondVectorZ - firstVectorZ * secondVectorY; + double rotationAxisY = firstVectorZ * secondVectorX - firstVectorX * secondVectorZ; + double rotationAxisZ = firstVectorX * secondVectorY - firstVectorY * secondVectorX; + double sinAngle = Math.sqrt(normSquared(rotationAxisX, rotationAxisY, rotationAxisZ)); + + boolean normalsAreParallel = sinAngle < ONE_TEN_MILLIONTH; + + if (normalsAreParallel) + { + rotationToPack.setIdentity(); + return; + } + + rotationAxisX /= sinAngle; + rotationAxisY /= sinAngle; + rotationAxisZ /= sinAngle; + + double cosAngle = firstVectorX * secondVectorX + firstVectorY * secondVectorY + firstVectorZ * secondVectorZ; + + if (cosAngle > 1.0) + cosAngle = 1.0; + else if (cosAngle < -1.0) + cosAngle = -1.0; + + double t = 1.0 - cosAngle; + + double xz = rotationAxisX * rotationAxisZ; + double xy = rotationAxisX * rotationAxisY; + double yz = rotationAxisY * rotationAxisZ; + + double m00 = t * rotationAxisX * rotationAxisX + cosAngle; + double m01 = t * xy - sinAngle * rotationAxisZ; + double m02 = t * xz + sinAngle * rotationAxisY; + double m10 = t * xy + sinAngle * rotationAxisZ; + double m11 = t * rotationAxisY * rotationAxisY + cosAngle; + double m12 = t * yz - sinAngle * rotationAxisX; + double m20 = t * xz - sinAngle * rotationAxisY; + double m21 = t * yz + sinAngle * rotationAxisX; + double m22 = t * rotationAxisZ * rotationAxisZ + cosAngle; + + if (rotationToPack instanceof RotationMatrix) + ((RotationMatrix) rotationToPack).setUnsafe(m00, m01, m02, m10, m11, m12, m20, m21, m22); + else + rotationToPack.set(m00, m01, m02, m10, m11, m12, m20, m21, m22); + } + + /** + * This method implements the same operation as + * {@link EuclidGeometryTools#orientation3DFromFirstToSecondVector3D(Vector3DReadOnly, Vector3DReadOnly, Orientation3DBasics)} + * except that it does not rely on {@code Math#acos(double)} making it faster. + * + * @param firstVector the first vector. Not modified. + * @param secondVector the second vector that is rotated with respect to the first vector. Not + * modified. + * @param rotationToPack the minimum rotation from {@code firstVector} to the {@code secondVector}. + * Modified. + * @see EuclidGeometryTools#orientation3DFromFirstToSecondVector3D(Vector3DReadOnly, + * Vector3DReadOnly, Orientation3DBasics) + */ + public static void rotationMatrix3DFromFirstToSecondVector3D(Vector3DReadOnly firstVector, + Vector3DReadOnly secondVector, + CommonMatrix3DBasics rotationToPack) + { + rotationMatrix3DFromFirstToSecondVector3D(firstVector.getX(), + firstVector.getY(), + firstVector.getZ(), + secondVector.getX(), + secondVector.getY(), + secondVector.getZ(), + rotationToPack); + } + + /** + * This method implements the same operation as + * {@link EuclidGeometryTools#orientation3DFromZUpToVector3D(Vector3DReadOnly, Orientation3DBasics)} + * except that it does not rely on {@code Math#acos(double)} making it faster. + * + * @param vector the vector that is rotated with respect to {@code zUp}. Not modified. + * @param vector the vector that is rotated with respect to {@code zUp}. Not modified. + * @param rotationToPack the minimum rotation from {@code zUp} to the given {@code vector}. + * Modified. + * @see EuclidGeometryTools#orientation3DFromZUpToVector3D(Vector3DReadOnly, Orientation3DBasics) + */ + public static void rotationMatrix3DFromZUpToVector3D(Vector3DReadOnly vector, CommonMatrix3DBasics rotationToPack) + { + rotationMatrix3DFromFirstToSecondVector3D(Axis3D.Z, vector, rotationToPack); + } + + /** + * Returns the square of the minimum distance between a point and a given line segment, and packs the closest point on the line segment in + * {@code pointOnSegmentToPack} + *

+ * Edge cases: + *

    + *
  • if {@code lineSegmentStart.distanceSquared(lineSegmentEnd) < }{@value #ONE_TRILLIONTH}, this + * method returns the distance between {@code lineSegmentStart} and the given {@code point}. + *
+ *

+ * + * @param pointX x coordinate of point to be tested. + * @param pointY y coordinate of point to be tested. + * @param lineSegmentStartX the x-coordinate of the line segment first endpoint. + * @param lineSegmentStartY the y-coordinate of the line segment first endpoint. + * @param lineSegmentEndX the x-coordinate of the line segment second endpoint. + * @param lineSegmentEndY the y-coordinate of the line segment second endpoint. + * @param pointOnSegmentToPack the closest point on the line segment to the point to be tested. Modified. + * @return the square of the minimum distance between the 2D point and the 2D line segment. + */ + public static double distanceSquaredFromPoint2DToLineSegment2D(double pointX, + double pointY, + double lineSegmentStartX, + double lineSegmentStartY, + double lineSegmentEndX, + double lineSegmentEndY, + Point2DBasics pointOnSegmentToPack) + { + double percentage = percentageAlongLineSegment2D(pointX, pointY, lineSegmentStartX, lineSegmentStartY, lineSegmentEndX, lineSegmentEndY); + + if (percentage > 1.0) + percentage = 1.0; + else if (percentage < 0.0) + percentage = 0.0; + + double projectionX = (1.0 - percentage) * lineSegmentStartX + percentage * lineSegmentEndX; + double projectionY = (1.0 - percentage) * lineSegmentStartY + percentage * lineSegmentEndY; + + if (pointOnSegmentToPack != null) + pointOnSegmentToPack.set(projectionX, projectionY); + + double dx = projectionX - pointX; + double dy = projectionY - pointY; + return dx * dx + dy * dy; + } + + /** + * Computes and packs the intersecting points between a 2d line segment and a circle. Returns the number of found intersections. + *

+ * Edge cases: + *

    + *
  • If the line does not intersect the circle, this methods fails and returns {@code 0}. + *
+ *

+ * + * @param circleRadius radius of the circle in question. + * @param circlePosition position of the center of the circle. + * @param startPoint starting position of the line segment. + * @param endPoint end position of the line segment. + * @param firstIntersectionToPack first possible intersecting point. + * @param secondIntersectionToPack second possible intersecting point. + * @return number of intersections found. Can be {@code 0} if no intersections, {@code 1} or {@code 2}. + */ + public static int intersectionBetweenLineSegment2DAndCylinder3D(double circleRadius, + Point2DReadOnly circlePosition, + Point2DReadOnly startPoint, + Point2DReadOnly endPoint, + Point2DBasics firstIntersectionToPack, + Point2DBasics secondIntersectionToPack) + { + return intersectionBetweenLine2DAndCircle(circleRadius, + circlePosition.getX(), + circlePosition.getY(), + startPoint.getX(), + startPoint.getY(), + false, + endPoint.getX(), + endPoint.getY(), + false, + firstIntersectionToPack, + secondIntersectionToPack); + } + + /** + * Computes and packs the intersecting points between a 2d line and a circle. Returns the number of found intersections. If the 2d line is represented as a + * line segment, then {@code canIntersectionOccurBeforeStart} and {@code canIntersectionOccurAfterEnd} should both be false. + *

+ * Edge cases: + *

    + *
  • If the line does not intersect the circle, this methods fails and returns {@code 0}. + *
+ *

+ * + * @param circleRadius radius of the circle in question. + * @param circlePositionX x position of the center of the circle. + * @param circlePositionY y position of the center of the circle. + * @param startX x position of a point on the line, or starting x position of the line segment. + * @param startY y position of a point on the line, or starting y position of the line segment. + * @param canIntersectionOccurBeforeStart {@code true} if this is a line, {@code false} if this is a line segment or a ray. + * @param endX x position of a second point on the line, or end x position of the line segment. + * @param endY y position of a second point on the line, or end y position of the line segment. + * @param canIntersectionOccurAfterEnd {@code true} if this is a line or ray, {@code false} if this is a line segment. + * @param firstIntersectionToPack first possible intersecting point. + * @param secondIntersectionToPack second possible intersecting point. + * @return number of intersections found. Can be {@code 0} if no intersections, {@code 1} or {@code 2}. + */ + public static int intersectionBetweenLine2DAndCircle(double circleRadius, + double circlePositionX, + double circlePositionY, + double startX, + double startY, + boolean canIntersectionOccurBeforeStart, + double endX, + double endY, + boolean canIntersectionOccurAfterEnd, + Point2DBasics firstIntersectionToPack, + Point2DBasics secondIntersectionToPack) + { + if (circleRadius < 0.0) + throw new IllegalArgumentException("The circle radius has to be positive."); + + if (firstIntersectionToPack != null) + firstIntersectionToPack.setToNaN(); + if (secondIntersectionToPack != null) + secondIntersectionToPack.setToNaN(); + + if (circleRadius == 0.0) + return 0; + + double radiusSquared = circleRadius * circleRadius; + + double dx = endX - startX; + double dy = endY - startY; + + double dIntersection1 = Double.NaN; + double dIntersection2 = Double.NaN; + + // Compute possible intersections with the circle + // + double deltaPX = startX - circlePositionX; + double deltaPY = startY - circlePositionY; + + double A = EuclidCoreTools.normSquared(dx, dy); + double B = 2.0 * (dx * deltaPX + dy * deltaPY); + double C = EuclidCoreTools.normSquared(deltaPX, deltaPY) - radiusSquared; + + double delta = EuclidCoreTools.squareRoot(B * B - 4 * A * C); + + if (Double.isFinite(delta)) + { + double oneOverTwoA = 0.5 / A; + double dCircle1 = -(B + delta) * oneOverTwoA; + double dCircle2 = -(B - delta) * oneOverTwoA; + + double intersection1X = dCircle1 * dx + startX; + double intersection1Y = dCircle1 * dy + startY; + + if (Math.abs(percentageAlongLine2D(intersection1X, intersection1Y, circlePositionX, circlePositionY, 1.0, 0.0)) > circleRadius - ONE_TRILLIONTH) + dCircle1 = Double.NaN; + + if (Double.isFinite(dCircle1)) + { + if (Double.isNaN(dIntersection1) || Math.abs(dCircle1 - dIntersection1) < ONE_TRILLIONTH) + { + dIntersection1 = dCircle1; + } + else if (dCircle1 < dIntersection1) + { + dIntersection2 = dIntersection1; + dIntersection1 = dCircle1; + } + else + { + dIntersection2 = dCircle1; + } + } + + double intersection2X = dCircle2 * dx + startX; + double intersection2Y = dCircle2 * dy + startY; + + if (Math.abs(percentageAlongLine2D(intersection2X, intersection2Y, circlePositionX, circlePositionY, 1.0, 0.0)) > circleRadius - ONE_TRILLIONTH) + dCircle2 = Double.NaN; + else if (Math.abs(dCircle1 - dCircle2) < ONE_TRILLIONTH) + dCircle2 = Double.NaN; + + if (Double.isFinite(dCircle2)) + { + if (Double.isNaN(dIntersection1)) + { + dIntersection1 = dCircle2; + } + else if (dCircle2 < dIntersection1) + { + dIntersection2 = dIntersection1; + dIntersection1 = dCircle2; + } + else + { + dIntersection2 = dCircle2; + } + } + } + + if (!canIntersectionOccurBeforeStart) + { + if (dIntersection2 < 0.0) + dIntersection2 = Double.NaN; + + if (dIntersection1 < 0.0) + { + dIntersection1 = dIntersection2; + dIntersection2 = Double.NaN; + } + } + + if (!canIntersectionOccurAfterEnd) + { + if (dIntersection2 > 1.0) + dIntersection2 = Double.NaN; + + if (dIntersection1 > 1.0) + { + dIntersection1 = dIntersection2; + dIntersection2 = Double.NaN; + } + } + + if (Double.isNaN(dIntersection1)) + return 0; + + if (firstIntersectionToPack != null) + { + firstIntersectionToPack.set(dx, dy); + firstIntersectionToPack.scale(dIntersection1); + firstIntersectionToPack.add(startX, startY); + } + + if (Double.isNaN(dIntersection2)) + return 1; + + if (secondIntersectionToPack != null) + { + secondIntersectionToPack.set(dx, dy); + secondIntersectionToPack.scale(dIntersection2); + secondIntersectionToPack.add(startX, startY); + } + + return 2; + } + + /** + * Computes and packs the intersecting points between a 2d line and a circle. Returns the number of found intersections. + *

+ * Edge cases: + *

    + *
  • If the line does not intersect the circle, this methods fails and returns {@code 0}. + *
+ *

+ * + * @param circleRadius radius of the circle in question. + * @param circlePosition position of the center of the circle. + * @param pointOnLine point on the line + * @param direction direction of the line. + * @param firstIntersectionToPack first possible intersecting point. + * @param secondIntersectionToPack second possible intersecting point. + * @return number of intersections found. Can be {@code 0} if no intersections, {@code 1} or {@code 2}. + */ + public static int intersectionBetweenLine2DAndCircle(double circleRadius, + Point2DReadOnly circlePosition, + Point2DReadOnly pointOnLine, + Vector2DReadOnly direction, + Point2DBasics firstIntersectionToPack, + Point2DBasics secondIntersectionToPack) + { + return intersectionBetweenLine2DAndCircle(circleRadius, + circlePosition.getX(), + circlePosition.getY(), + pointOnLine.getX(), + pointOnLine.getY(), + true, + pointOnLine.getX() + direction.getX(), + pointOnLine.getY() + direction.getY(), + true, + firstIntersectionToPack, + secondIntersectionToPack); + } + + /** + * Computes and packs the intersecting points between a 2d ray and a circle. Returns the number of found intersections. + *

+ * Edge cases: + *

    + *
  • If the line does not intersect the circle, this methods fails and returns {@code 0}. + *
+ *

+ * + * @param circleRadius radius of the circle in question. + * @param circlePosition position of the center of the circle. + * @param startPoint starting point of the line. + * @param direction direction of the line. + * @param firstIntersectionToPack first possible intersecting point. + * @param secondIntersectionToPack second possible intersecting point. + * @return number of intersections found. Can be {@code 0} if no intersections, {@code 1} or {@code 2}. + */ + public static int intersectionBetweenRay2DAndCircle(double circleRadius, + Point2DReadOnly circlePosition, + Point2DReadOnly startPoint, + Vector2DReadOnly direction, + Point2DBasics firstIntersectionToPack, + Point2DBasics secondIntersectionToPack) + { + return intersectionBetweenLine2DAndCircle(circleRadius, + circlePosition.getX(), + circlePosition.getY(), + startPoint.getX(), + startPoint.getY(), + false, + startPoint.getX() + direction.getX(), + startPoint.getY() + direction.getY(), + true, + firstIntersectionToPack, + secondIntersectionToPack); + } + + /** + * Computes and packs the intersecting points between a 2d ray and a circle. Returns the number of found intersections. + *

+ * Edge cases: + *

    + *
  • If the line does not intersect the circle, this methods fails and returns {@code 0}. + *
+ *

+ * + * @param circleRadius radius of the circle in question. + * @param circlePosition position of the center of the circle. + * @param startPoint starting point of the line. + * @param pointOnRay additional point on the ray use to determine its direction. + * @param firstIntersectionToPack first possible intersecting point. + * @param secondIntersectionToPack second possible intersecting point. + * @return number of intersections found. Can be {@code 0} if no intersections, {@code 1} or {@code 2}. + */ + public static int intersectionBetweenRay2DAndCircle(double circleRadius, + Point2DReadOnly circlePosition, + Point2DReadOnly startPoint, + Point2DReadOnly pointOnRay, + Point2DBasics firstIntersectionToPack, + Point2DBasics secondIntersectionToPack) + { + return intersectionBetweenLine2DAndCircle(circleRadius, + circlePosition.getX(), + circlePosition.getY(), + startPoint.getX(), + startPoint.getY(), + false, + pointOnRay.getX(), + pointOnRay.getY(), + true, + firstIntersectionToPack, + secondIntersectionToPack); + } + + /** + * Computes the coordinates of the intersection between a plane and an infinitely long line. + * Useful link . + *

+ * Edge cases: + *

    + *
  • If the line is parallel to the plane, this methods fails and returns {@code false}. + *
+ *

+ * + * @param pointOnPlaneX x-value of a point located on the plane. + * @param pointOnPlaneY y-value of a point located on the plane. + * @param pointOnPlaneZ z-value of a point located on the plane. + * @param planeNormalX x-value of the normal of the plane. + * @param planeNormalY y-value of the normal of the plane. + * @param planeNormalZ z-value of the normal of the plane. + * @param pointOnLineX x-value of a point located on the line. + * @param pointOnLineY y-value of a point located on the line. + * @param pointOnLineZ z-value of a point located on the line. + * @param lineDirectionX x-value of the direction of the line. + * @param lineDirectionY y-value of the direction of the line. + * @param lineDirectionZ z-value of the direction of the line. + * @param intersectionToPack point in which the coordinates of the intersection are stored. Modified. + * @return {@code true} if the method succeeds, {@code false} otherwise. + */ + public static boolean intersectionBetweenLine3DAndPlane3D(double pointOnPlaneX, + double pointOnPlaneY, + double pointOnPlaneZ, + double planeNormalX, + double planeNormalY, + double planeNormalZ, + double pointOnLineX, + double pointOnLineY, + double pointOnLineZ, + double lineDirectionX, + double lineDirectionY, + double lineDirectionZ, + Point3DBasics intersectionToPack) + { + // Switching to the notation used in https://en.wikipedia.org/wiki/Line%E2%80%93plane_intersection + // Note: the algorithm is independent from the magnitudes of planeNormal and lineDirection + + // Let's compute the value of the coefficient d = ( (p0 - l0).n ) / ( l.n ) + double numerator, denominator; + numerator = (pointOnPlaneX - pointOnLineX) * planeNormalX; + numerator += (pointOnPlaneY - pointOnLineY) * planeNormalY; + numerator += (pointOnPlaneZ - pointOnLineZ) * planeNormalZ; + denominator = planeNormalX * lineDirectionX + planeNormalY * lineDirectionY + planeNormalZ * lineDirectionZ; + + // Check if the line is parallel to the plane + if (Math.abs(denominator) < ONE_TRILLIONTH) + { + return false; + } + else + { + double d = numerator / denominator; + + intersectionToPack.setX(d * lineDirectionX + pointOnLineX); + intersectionToPack.setY(d * lineDirectionY + pointOnLineY); + intersectionToPack.setZ(d * lineDirectionZ + pointOnLineZ); + return true; + } + } + + /** + * Computes the intersection between an infinitely long 2D line (defined by a 2D point and a 2D + * direction) and a 2D ray segment (defined by the start and direction 2D endpoints). + *

+ * Edge cases: + *

    + *
  • When the line and the ray are parallel but not collinear, they do not intersect. + *
  • When the line and the ray are collinear, they are assumed to intersect at + * {@code rayStart}. + *
  • When the line intersects the ray at the start point, this method returns + * {@code true} and the start point is the intersection. + *
  • When there is no intersection, this method returns {@code false} and + * {@code intersectionToPack} is set to {@link Double#NaN}. + *
+ *

+ * + * @param rayOrigin start point of the ray. + * @param rayDirection direction of the ray. + * @param linePoint1 first point located on the line. + * @param linePoint2 second point located on the line. + * @param intersectionToPack the 2D point in which the result is stored. Can be {@code null}. + * Modified. + * @return {@code true} if the line intersects the line segment, {@code false} otherwise. + */ + public static boolean intersectionBetweenRay2DAndLine2D(Point2DReadOnly rayOrigin, + Vector2DReadOnly rayDirection, + Point2DReadOnly linePoint1, + Point2DReadOnly linePoint2, + Point2DBasics intersectionToPack) + { + return intersectionBetweenRay2DAndLine2D(rayOrigin.getX(), + rayOrigin.getY(), + rayDirection.getX(), + rayDirection.getY(), + linePoint1.getX(), + linePoint1.getY(), + linePoint2.getX() - linePoint1.getX(), + linePoint2.getY() - linePoint1.getY(), + intersectionToPack); + } + + /** + * Computes the intersection between an infinitely long 2D line (defined by a 2D point and a 2D + * direction) and a 2D ray segment (defined by the start and direction 2D endpoints). + *

+ * Edge cases: + *

    + *
  • When the line and the ray are parallel but not collinear, they do not intersect. + *
  • When the line and the ray are collinear, they are assumed to intersect at + * {@code rayStart}. + *
  • When the line intersects the ray at the start point, this method returns + * {@code true} and the start point is the intersection. + *
  • When there is no intersection, this method returns {@code false} and + * {@code intersectionToPack} is set to {@link Double#NaN}. + *
+ *

+ * + * @param rayOriginX x-coordinate of the start point of the ray. + * @param rayOriginY y-coordinate of the start point of the ray. + * @param rayDirectionX x-component of the ray direction. + * @param rayDirectionY y-component of the ray direction. + * @param pointOnLineX x-coordinate of a point located on the line. + * @param pointOnLineY y-coordinate of a point located on the line. + * @param lineDirectionX x-component of the line direction. + * @param lineDirectionY y-component of the line direction. + * @param intersectionToPack the 2D point in which the result is stored. Can be {@code null}. + * Modified. + * @return {@code true} if the line intersects the line segment, {@code false} otherwise. + */ + public static boolean intersectionBetweenRay2DAndLine2D(double rayOriginX, + double rayOriginY, + double rayDirectionX, + double rayDirectionY, + double pointOnLineX, + double pointOnLineY, + double lineDirectionX, + double lineDirectionY, + Point2DBasics intersectionToPack) + { + double start1x = rayOriginX; + double start1y = rayOriginY; + double end1x = rayOriginX + rayDirectionX; + double end1y = rayOriginY + rayDirectionY; + double start2x = pointOnLineX; + double start2y = pointOnLineY; + double end2x = pointOnLineX + lineDirectionX; + double end2y = pointOnLineY + lineDirectionY; + return intersectionBetweenTwoLine2DsImpl(start1x, start1y, false, end1x, end1y, true, start2x, start2y, true, end2x, end2y, true, intersectionToPack); + } + + /** + * Computes the intersection between an infinitely long 2D line (defined by a 2D point and a 2D + * direction) and a 2D ray segment (defined by the start and direction 2D endpoints). + *

+ * Edge cases: + *

    + *
  • When the line and the ray are parallel but not collinear, they do not intersect. + *
  • When the line and the ray are collinear, they are assumed to intersect at + * {@code rayStart}. + *
  • When the line intersects the ray at the start point, this method returns + * {@code true} and the start point is the intersection. + *
  • When there is no intersection, this method returns {@code false} and + * {@code intersectionToPack} is set to {@link Double#NaN}. + *
+ *

+ * + * @param rayOrigin start point of the ray. + * @param rayDirection direction of the ray. + * @param pointOnLine point located on the line. + * @param lineDirection direction of the line. + * @param intersectionToPack the 2D point in which the result is stored. Can be {@code null}. + * Modified. + * @return {@code true} if the line intersects the line segment, {@code false} otherwise. + */ + public static boolean intersectionBetweenRay2DAndLine2D(Point2DReadOnly rayOrigin, + Vector2DReadOnly rayDirection, + Point2DReadOnly pointOnLine, + Vector2DReadOnly lineDirection, + Point2DBasics intersectionToPack) + { + return intersectionBetweenRay2DAndLine2D(rayOrigin.getX(), + rayOrigin.getY(), + rayDirection.getX(), + rayDirection.getY(), + pointOnLine.getX(), + pointOnLine.getY(), + lineDirection.getX(), + lineDirection.getY(), + intersectionToPack); + } + + /** + * Computes the intersection between two infinitely long 2D lines each defined by a 2D point and a + * 2D direction and returns a percentage {@code alpha} along the first line such that the + * intersection coordinates can be computed as follows:
+ * {@code intersection = pointOnLine1 + alpha * lineDirection1} + *

+ * Edge cases: + *

    + *
  • if the two lines are parallel but not collinear, the two lines do not intersect and the + * returned value is {@link Double#NaN}. + *
  • if the two lines are collinear, the two lines are assumed to be intersecting infinitely the + * returned value {@code Double.POSITIVE_INFINITY}. + *
+ *

+ * + * @param startPoint1x x-coordinate of a point located on the first line. + * @param startPoint1y y-coordinate of a point located on the first line. + * @param segmentTravel1x x-component of the first line direction. + * @param segmentTravel1y y-component of the first line direction. + * @param startPoint2x x-coordinate of a point located on the second line. + * @param startPoint2y y-coordinate of a point located on the second line. + * @param segmentTravel2x x-component of the second line direction. + * @param segmentTravel2y y-component of the second line direction. + * @return {@code alpha} the percentage along the first line of the intersection location. This + * method returns {@link Double#NaN} if the lines do not intersect. + */ + public static double percentageOfIntersectionBetweenTwoLine2DsInfCase(double startPoint1x, + double startPoint1y, + double segmentTravel1x, + double segmentTravel1y, + double startPoint2x, + double startPoint2y, + double segmentTravel2x, + double segmentTravel2y) + { + // We solve for x the problem of the form: A * x = b + // A * x = b + // / segmentTravel1x -segmentTravel2x \ / alpha \ / startPoint2x - startPoint1x \ + // | | * | | = | | + // \ segmentTravel1y -segmentTravel2y / \ beta / \ startPoint2y - startPoint1y / + // Here, only alpha or beta is needed. + + double determinant = -segmentTravel1x * segmentTravel2y + segmentTravel1y * segmentTravel2x; + + double dx = startPoint2x - startPoint1x; + double dy = startPoint2y - startPoint1y; + + if (Math.abs(determinant) < ONE_TRILLIONTH) + { // The lines are parallel + // Check if they are collinear + double cross = dx * segmentTravel1y - dy * segmentTravel1x; + if (Math.abs(cross) < ONE_TRILLIONTH) + { + /* + * The two lines are collinear. There's an infinite number of intersection. Let's set the result to + * infinity, i.e. alpha = infinity so it can be handled. + */ + return Double.POSITIVE_INFINITY; + } + else + { + return Double.NaN; + } + } + else + { + double oneOverDeterminant = 1.0 / determinant; + double AInverse00 = oneOverDeterminant * -segmentTravel2y; + double AInverse01 = oneOverDeterminant * segmentTravel2x; + + double alpha = AInverse00 * dx + AInverse01 * dy; + + return alpha; + } + } + + /** + * Computes the z height of a Euclidean point defined by {@code pointX} and {@code pointY} on a plane defined by point {@code pointOnPlane} and normal + * {@code planeNormal}. + * + * @param pointOnPlane point located on a plane + * @param planeNormal vector describing the plane normal + * @param pointX x coordinate to query. + * @param pointY y coordinate to query. + * @return height of the plane located at {@code pointX} and {@code pointY} + */ + public static double getZOnPlane(Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal, double pointX, double pointY) + { + // The three components of the plane origin + double x0 = pointOnPlane.getX(); + double y0 = pointOnPlane.getY(); + double z0 = pointOnPlane.getZ(); + // The three components of the plane normal + double a = planeNormal.getX(); + double b = planeNormal.getY(); + double c = planeNormal.getZ(); + + // Given the plane equation: a*x + b*y + c*z + d = 0, with d = -(a*x0 + b*y0 + c*z0), we find z: + double z = a / c * (x0 - pointX) + b / c * (y0 - pointY) + z0; + return z; + } + + /** + * Finds the intersection of two bounding boxes defined by a bounding box. + *

+ * WARNING: generates garbage + *

+ * Allocates a new BoundingBox2D. TODO: Check, Unit test, move where BoundingBox union is + * + * @param a first bounding box to check + * @param b second bounding box to check + * @return the intersection bounding box, or null if no intersection + */ + public static BoundingBox2D computeIntersectionOfTwoBoundingBoxes(BoundingBox2DReadOnly a, BoundingBox2DReadOnly b) + { + double maxX = Math.min(a.getMaxX(), b.getMaxX()); + double maxY = Math.min(a.getMaxY(), b.getMaxY()); + double minX = Math.max(a.getMinX(), b.getMinX()); + double minY = Math.max(a.getMinY(), b.getMinY()); + + if ((maxX <= minX) || (maxY <= minY)) + return null; + + return new BoundingBox2D(minX, minY, maxX, maxY); + } + + /** + * Finds the intersection of two bounding boxes defined by a bounding box. + *

+ * WARNING: generates garbage + *

+ * Allocates a new boundingBox3D. + * + * @param a first bounding box to check + * @param b second bounding box to check + * @return the intersection bounding box, or null if no intersection + */ + public static BoundingBox3D computeIntersectionOfTwoBoundingBoxes(BoundingBox3DReadOnly a, BoundingBox3DReadOnly b) + { + double maxX = Math.min(a.getMaxX(), b.getMaxX()); + double maxY = Math.min(a.getMaxY(), b.getMaxY()); + double maxZ = Math.min(a.getMaxZ(), b.getMaxZ()); + + double minX = Math.max(a.getMinX(), b.getMinX()); + double minY = Math.max(a.getMinY(), b.getMinY()); + double minZ = Math.max(a.getMinZ(), b.getMinZ()); + + if ((maxX <= minX) || (maxY <= minY) || (maxZ <= minZ)) + return null; + + return new BoundingBox3D(minX, minY, minZ, maxX, maxY, maxZ); + } + + /** + * Computes the volume of the bounding box. + * + * @param boundingBox bounding box to inspect + * @return total volume of the bounding box. + */ + public static double computeBoundingBoxVolume3D(BoundingBox3DReadOnly boundingBox) + { + return Math.abs(boundingBox.getMaxX() - boundingBox.getMinX()) * Math.abs(boundingBox.getMaxY() - boundingBox.getMinY()) * Math.abs( + boundingBox.getMaxZ() - boundingBox.getMinZ()); + } + + /** + * Inspects whether a 2d line segment ever intersects with a 2d convex polygon. + * + * @param lineSegmentStart starting point of the line segment. + * @param lineSegmentEnd ending point of the line segment. + * @param convexPolygonVertices list of points that compose the convex polygon. + * @param numberOfVertices number of vertices contained in the convex polygon. + * @return whether the line segment intersects with the polygon. + */ + public static boolean doLineSegment2DAndConvexPolygon2DIntersect(Point2DReadOnly lineSegmentStart, + Point2DReadOnly lineSegmentEnd, + List convexPolygonVertices, + int numberOfVertices) + { + checkNumberOfVertices(convexPolygonVertices, numberOfVertices); + + if (numberOfVertices == 0) + return false; + + if (numberOfVertices == 1) + return isPoint2DOnLineSegment2D(convexPolygonVertices.get(0), lineSegmentStart, lineSegmentEnd); + + if (numberOfVertices == 2) + return doLineSegment2DsIntersect(convexPolygonVertices.get(0), convexPolygonVertices.get(1), lineSegmentStart, lineSegmentEnd); + + for (int edgeIndex = 0; edgeIndex < numberOfVertices; edgeIndex++) + { + Point2DReadOnly edgeStart = convexPolygonVertices.get(edgeIndex); + Point2DReadOnly edgeEnd = convexPolygonVertices.get(EuclidGeometryPolygonTools.next(edgeIndex, numberOfVertices)); + + if (doLineSegment2DsIntersect(edgeStart, edgeEnd, lineSegmentStart, lineSegmentEnd)) + return true; + } + + return false; + } + + private static void checkNumberOfVertices(List convexPolygon2D, int numberOfVertices) + { + if (numberOfVertices < 0 || numberOfVertices > convexPolygon2D.size()) + throw new IllegalArgumentException("Illegal numberOfVertices: " + numberOfVertices + ", expected a value in ] 0, " + convexPolygon2D.size() + "]."); + } + + /** + * Compute Intersection-over-Union (IoU) of two 3D bounding boxes. This is the percentage of volume of the two bounding boxes that are intersecting. + *

+ * WARNING: generates garbage. + */ + public static double computeIntersectionOverUnionOfTwoBoundingBoxes(BoundingBox3DReadOnly a, BoundingBox3DReadOnly b) + { + BoundingBox3D intersection = computeIntersectionOfTwoBoundingBoxes(a, b); + + if (intersection == null) + return 0.0; + + double intersectionVolume = computeBoundingBoxVolume3D(intersection); + double volumeA = computeBoundingBoxVolume3D(a); + double volumeB = computeBoundingBoxVolume3D(b); + double unionVolume = volumeA + volumeB - intersectionVolume; + + return intersectionVolume / unionVolume; + } + + /** + * Compute Intersection-over-Union (IoU) of two 3D bounding boxes. This is the percentage of volume of the two bounding boxes that are intersecting. + *

+ * WARNING: generates garbage. + */ + public static double computeIntersectionOverSmallerOfTwoBoundingBoxes(BoundingBox3DReadOnly a, BoundingBox3DReadOnly b) + { + BoundingBox3D intersection = computeIntersectionOfTwoBoundingBoxes(a, b); + + if (intersection == null) + return 0.0; + + double intersectionVolume = computeBoundingBoxVolume3D(intersection); + double volumeA = computeBoundingBoxVolume3D(a); + double volumeB = computeBoundingBoxVolume3D(b); + double smallerVolume = Math.min(volumeA, volumeB); + + return intersectionVolume / smallerVolume; + } + + /** + * Determines if the polygonToTest is fully inside the convex polygon by some epsilon distance. + * + * @return {@code true} if {@code polygonToTest} is fully contained inside {@code polygon}. {@code false} otherwise. + */ + public static boolean isPolygonInside(ConvexPolygon2DReadOnly polygonToTest, double epsilon, ConvexPolygon2DReadOnly polygon) + { + for (int i = 0; i < polygonToTest.getNumberOfVertices(); i++) + { + if (!polygon.isPointInside(polygonToTest.getVertex(i), epsilon)) + return false; + } + + return true; + } + + /** + * Determines if the polygonToTest is inside the convex polygon. + * + * @return {@code true} if {@code polygonToTest} is fully contained inside {@code polygon}. {@code false} otherwise. + */ + public static boolean isPolygonInside(ConvexPolygon2DReadOnly polygonToTest, ConvexPolygon2DReadOnly polygon) + { + return isPolygonInside(polygonToTest, 0.0, polygon); + } + + /** + * Finds the projection of a 3D point onto a 3D plane given in general form. + * Uses: projectedPoint = point - (normal.dot(point) + planeScalar) * (normal) + *

+ * WARNING: generates garbage. + * + * @param plane Coefficients of the general form of plane equation (ax + by + cz + d = 0) as Vector4D + * @param point Point to be projected onto the plane as Point3D + * @return Projected point onto the plane as Point3D + */ + public static Point3D projectPointOntoPlane(Vector4DReadOnly plane, Point3DReadOnly point) + { + UnitVector3D planeNormal = new UnitVector3D(plane.getX(), plane.getY(), plane.getZ()); + + Vector3D scaledNormal = new Vector3D(planeNormal); + scaledNormal.scale(planeNormal.dot(point) + plane.getS()); + + Point3D projectedPoint = new Point3D(); + projectedPoint.sub(point, scaledNormal); + + return projectedPoint; } } diff --git a/src/main/java/us/ihmc/euclid/QuaternionCalculus.java b/src/main/java/us/ihmc/euclid/QuaternionCalculus.java new file mode 100644 index 000000000..31b6196c1 --- /dev/null +++ b/src/main/java/us/ihmc/euclid/QuaternionCalculus.java @@ -0,0 +1,500 @@ +package us.ihmc.euclid; + +import us.ihmc.euclid.axisAngle.AxisAngle; +import us.ihmc.euclid.rotationConversion.QuaternionConversion; +import us.ihmc.euclid.tools.EuclidCoreTools; +import us.ihmc.euclid.tools.QuaternionTools; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics; +import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; +import us.ihmc.euclid.tuple4D.Quaternion; +import us.ihmc.euclid.tuple4D.Vector4D; +import us.ihmc.euclid.tuple4D.interfaces.QuaternionBasics; +import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; +import us.ihmc.euclid.tuple4D.interfaces.Vector4DBasics; +import us.ihmc.euclid.tuple4D.interfaces.Vector4DReadOnly; + +/** + * This class contains a number of tools related to computing dynamics of quaternions. Each method is guaranteed to be free of allocations, which requires + * instantiation for some methods. + * + *

+ * Specifically, it provides tools for exponential + * ({@link #exp(Vector3DReadOnly, QuaternionBasics)}) and logarithmic maps of quaternions, {@link #log(QuaternionReadOnly, Vector3DBasics)}. + *

+ * + *

+ * Importantly, it also provides the interfaces for the ability to perform spherical linear interpolation, which is smooth interpolation on the + * SO3 manifold. This is commonly referred to as the SLERP algorithm, and can be performed calling + * {@link #interpolate(double, QuaternionReadOnly, QuaternionReadOnly, QuaternionBasics)}. + * Additional tools are provided to perform SLERP interpolation guaranteeing interpolation along the minimal path, with + * {@link #interpolate(double, QuaternionReadOnly, QuaternionReadOnly, QuaternionBasics, boolean)}. + *

+ * + *

+ * This class also provides tools for mapping from and to angular velocities and accelerations and quaternion rates and accelerations. These can be done in + * both + * the local frame defined by the orientation, or the parent frame. These can be accessed using + * {@link #computeQDotInParentFrame(QuaternionReadOnly, Vector3DReadOnly, Vector4DBasics)}, + * {@link #computeQDotInRotatedFrame(QuaternionReadOnly, Vector3DReadOnly, Vector4DBasics)}, + * and various other methods. + *

+ *

+ * Lastly, this class provides tools for estimating quaternion rates and accelerations via finite differences, provided various quaternion samples. These are + * useful for testing quaternion changes from generating trajectories, but can be used for a variety of other things. These can be accessed by + * {@link #computeQDotByFiniteDifferenceCentral(QuaternionReadOnly, QuaternionReadOnly, double, Vector4DBasics)} and + * {@link #computeQDDotByFiniteDifferenceCentral(QuaternionReadOnly, QuaternionReadOnly, QuaternionReadOnly, double, Vector4DBasics)}. + *

+ */ +public class QuaternionCalculus +{ + private final AxisAngle axisAngleForLog = new AxisAngle(); + private final Quaternion tempQ1ForInterpolation = new Quaternion(); + private final AxisAngle axisAngleForPow = new AxisAngle(); + private final Quaternion qConj = new Quaternion(); + + private final Vector4D intermediateQDot = new Vector4D(); + private final Vector4D qDotConj = new Vector4D(); + private final Vector3D intermediateAngularAcceleration = new Vector3D(); + private final Vector3D intermediateAngularVelocity = new Vector3D(); + private final Vector4D intermediateQDDot = new Vector4D(); + private final Vector4D pureQuatForMultiply = new Vector4D(); + + public QuaternionCalculus() + { + } + + /** + * This computes the logarithm of the quaternion a stores it in {@code resultToPack}, which is a pure quaternion. + * This is the equation: + *

resultToPack = log(q)

+ * where q is a {@link QuaternionReadOnly}. + * + * @param q is a unit quaternion and describes a orientation. Not modified. + * @param resultToPack is used to store the result and is a pure quaternion (w = 0.0). Modified. + */ + public void log(QuaternionReadOnly q, Vector4DBasics resultToPack) + { + axisAngleForLog.set(q); + resultToPack.set(axisAngleForLog.getX(), axisAngleForLog.getY(), axisAngleForLog.getZ(), 0.0); + resultToPack.scale(axisAngleForLog.getAngle()); + } + + /** + * This computes the logarithm of the quaternion a stores it in {@code resultToPack}, which is a vector. + * This is the equation: + *

resultToPack = log(q)

+ * where q is a {@link QuaternionReadOnly}. + * + * @param q is a unit quaternion and describes a orientation. Not modified. + * @param resultToPack is used to store the result. Modified. + */ + public void log(QuaternionReadOnly q, Vector3DBasics resultToPack) + { + axisAngleForLog.set(q); + resultToPack.set(axisAngleForLog.getX(), axisAngleForLog.getY(), axisAngleForLog.getZ()); + resultToPack.scale(axisAngleForLog.getAngle()); + } + + /** + * Applies the exponential transformation to the rotation vector. This is the same as computing the equivalent quaternion from a rotation vector. + * + * @param rotationVector rotation of which to compute the exponential. Not modified. + * @param quaternionToPack is used to store the result. Modified. + */ + public static void exp(Vector3DReadOnly rotationVector, QuaternionBasics quaternionToPack) + { + QuaternionConversion.convertRotationVectorToQuaternion(rotationVector, quaternionToPack); + } + + /** + * Performs a spherical linear interpolation (SLERP) from a quaternion between the start {@code q0} and end {@code q1} for a given alpha = [0, 1] using + * SLERP. The results are stored in {@code qInterpolatedToPack}. This effectively applies a constant angular velocity to go from {@code q0} to {@code q1}. + *

+ * At {@code alpha = 0}, the output matches {@code q0}. At {@code alpha = 1}, the output matches {@code q1}. {@code alpha} is not constrained to be between + * [0, 1] + *

+ * SLERP Descriptions. + * Computes: resultToPack = q0 (q0-1 q1)alpha. + * + * @param alpha fraction to interpolate between {@code q0} and {@code q1}. Should be between [0, 1]. + * @param q0 quaternion value at the beginning of the interpolation (when {@code alpha} = 0). Not Modified. + * @param q1 quaternion value at the end of the interpolation (when {@code alpha} = 1). Not Modified. + * @param qInterpolatedToPack interpolated quaternion to store the results. Modified. + */ + public void interpolate(double alpha, QuaternionReadOnly q0, QuaternionReadOnly q1, QuaternionBasics qInterpolatedToPack) + { + interpolate(alpha, q0, q1, qInterpolatedToPack, true); + } + + /** + * Performs a spherical linear interpolation (SLERP) from a quaternion between the start {@code q0} and end {@code q1} for a given alpha = [0, 1] using + * SLERP. The results are stored in {@code qInterpolatedToPack}. This effectively applies a constant angular velocity to go from {@code q0} to {@code q1}. + *

+ * At {@code alpha = 0}, the output matches {@code q0}. At {@code alpha = 1}, the output matches {@code q1}. {@code alpha} is not constrained to be between + * [0, 1] + *

+ *

+ * When {@code preventExtraSpin} is true, this check ensures that the interpolation goes the minimum distance along the shortest path. As it's a spherical + * interpolation, {@code q0} can rotate either direction to reach {@code q1}. When {@code preventExtraSpin} is true, the math checks to make that the + * rotation + * from {@code q0} to {@code q1} is less than {@link Math#PI}. If it is greater than this value, then it negates {@code q1} to reverse the directionality + * of the rotation, ensuring that the interpolation is along the shortest path. + *

+ * SLERP Descriptions. + * Computes: resultToPack = q0 (q0-1 q1)alpha. + * + * @param alpha fraction to interpolate between {@code q0} and {@code q1}. Should be between [0, 1]. + * @param q0 quaternion value at the beginning of the interpolation (when {@code alpha} = 0). Not Modified. + * @param q1 quaternion value at the end of the interpolation (when {@code alpha} = 1). Not Modified. + * @param qInterpolatedToPack interpolated quaternion to store the results. Modified. + */ + public void interpolate(double alpha, QuaternionReadOnly q0, QuaternionReadOnly q1, QuaternionBasics qInterpolatedToPack, boolean preventExtraSpin) + { + tempQ1ForInterpolation.set(q1); + + // Checks if the interpolation is taking the shortest path. + if (preventExtraSpin && q0.dot(tempQ1ForInterpolation) < 0.0) + { + tempQ1ForInterpolation.negate(); + } + + qInterpolatedToPack.difference(q0, tempQ1ForInterpolation); + pow(qInterpolatedToPack, alpha, qInterpolatedToPack); + qInterpolatedToPack.multiply(q0, qInterpolatedToPack); + } + + /** + * This computes: resultToPack = q^power. + * + * @param q is a unit quaternion and describes an orientation. Not modifeid. + * @param power exponential to apply to the quaternion. + * @param resultToPack is used to store the result. Modified. + */ + public void pow(QuaternionReadOnly q, double power, QuaternionBasics resultToPack) + { + axisAngleForPow.set(q); + axisAngleForPow.setAngle(power * axisAngleForPow.getAngle()); + resultToPack.set(axisAngleForPow); + } + + /** + * This computes the angular velocity expressed in the rotation frame of a body from the rotation of the body expressed as a quaternion and its quaternion + * rate. + * + *

+ * The rotation from the parent frame, Fparent to the current frame Fchild is defined by a quaternion {@code q}. This quaternion has a + * corresponding velocity, which is defined by the rate of change of its quaternion coordinates, {@code qDot}. + *

+ *

+ * This method computes the local 3-dimensional angular velocity, expressed in the child frame Fchild, and stores it in + * {@code angularVelocityToPack}. + *

+ * + * @param q quaternion defining the rotation to the local frame. Not modified. + * @param qDot quaternion rate of change of {@code q}. Not modified. + * @param angularVelocityToPack angular velocity in the local frame. Modified. + */ + public void computeAngularVelocityInRotatedFrame(QuaternionReadOnly q, Vector4DReadOnly qDot, Vector3DBasics angularVelocityToPack) + { + qConj.setAndConjugate(q); + multiply(qConj, qDot, angularVelocityToPack); + angularVelocityToPack.scale(2.0); + } + + /** + * This computes the angular velocity expressed in the parent frame of a body from the rotation of the body expressed as a quaternion and its quaternion + * rate. + * + *

+ * The rotation from the parent frame, Fparent to the current frame Fchild is defined by a quaternion {@code q}. This quaternion has a + * corresponding velocity, which is defined by the rate of change of its quaternion coordinates, {@code qDot}. + *

+ *

+ * This method computes the 3-dimensional angular velocity, expressed in the parent frame Fparent, and stores it in + * {@code angularVelocityToPack}. + *

+ * + * @param q quaternion defining the rotation to the local frame. Not modified. + * @param qDot quaternion rate of change of {@code q}. Not modified. + * @param angularVelocityToPack angular velocity in the parent frame. Modified. + */ + public void computeAngularVelocityInParentFrame(QuaternionReadOnly q, Vector4DReadOnly qDot, Vector3DBasics angularVelocityToPack) + { + qConj.setAndConjugate(q); + multiply(qDot, qConj, angularVelocityToPack); + angularVelocityToPack.scale(2.0); + } + + /** + * This computes the quaternion rate from the rotation of a body expressed as a quaternion and its angular velocity expressed in the frame of its parent. + * + *

+ * The rotation from the parent frame, Fparent to the current frame Fchild is defined by a quaternion {@code q}. This quaternion has a + * corresponding velocity, which is defined by the rate of change of its quaternion coordinates, {@code qDot}. + *

+ *

+ * This method computes the quaternion rate, and stores it in {@code qDotToPack} + *

+ * + * @param q quaternion defining the rotation to the local frame. Not Modified. + * @param angularVelocityInParent angular velocity in the parent frame. Not Modified. + * @param qDotToPack quaternion rate of change of {@code q} to compute. Modified. + */ + public static void computeQDotInParentFrame(QuaternionReadOnly q, Vector3DReadOnly angularVelocityInParent, Vector4DBasics qDotToPack) + { + multiply(angularVelocityInParent, q, qDotToPack); + qDotToPack.scale(0.5); + } + + /** + * This computes the quaternion rate from the rotation of a body expressed as a quaternion and its angular velocity expressed in the rotated frame. + * + *

+ * The rotation from the parent frame, Fparent to the current frame Fchild is defined by a quaternion {@code q}. This quaternion has a + * corresponding velocity, which is defined by the rate of change of its quaternion coordinates, {@code qDot}. + *

+ *

+ * This method computes the quaternion rate, and stores it in {@code qDotToPack} + *

+ * + * @param q quaternion defining the rotation to the local frame. Not Modified. + * @param angularVelocityInRotated angular velocity in the rotated frame. Not Modified. + * @param qDotToPack quaternion rate of change of {@code q} to compute. Modified. + */ + public static void computeQDotInRotatedFrame(QuaternionReadOnly q, Vector3DReadOnly angularVelocityInRotated, Vector4DBasics qDotToPack) + { + multiply(q, angularVelocityInRotated, qDotToPack); + qDotToPack.scale(0.5); + } + + /** + * This computes the quaternion acceleration from the rotation of a body expressed as a quaternion, its quaternion rate, and its angular acceleration + * expressed in the frame of its parent. + * + *

+ * The rotation from the parent frame, Fparent to the current frame Fchild is defined by a quaternion {@code q}. This quaternion has a + * corresponding velocity, which is defined by the rate of change of its quaternion coordinates, {@code qDot}, and acceleratoin, which is defined by the + * acceleration of its quaternion coordinates, {@code qDDot}. + *

+ *

+ * This method computes the quaternion acceleration, and stores it in {@code qDDotToPack} + *

+ * + * @param q quaternion defining the rotation to the local frame. Not Modified. + * @param qDot quaternion rate of change of {@code q}. Not modified. + * @param angularAccelerationInParent angular acceleration in the parent frame. Not Modified. + * @param qDDotToPack quaternion acceleration of {@code q} to compute. Not modified. + */ + public void computeQDDotInParentFrame(QuaternionReadOnly q, Vector4DReadOnly qDot, Vector3DReadOnly angularAccelerationInParent, Vector4DBasics qDDotToPack) + { + computeAngularVelocityInParentFrame(q, qDot, intermediateAngularVelocity); + computeQDDotInParentFrame(q, qDot, intermediateAngularVelocity, angularAccelerationInParent, qDDotToPack); + } + + /** + * This computes the quaternion acceleration from the rotation of a body expressed as a quaternion and its angular velocity and acceleration + * expressed in the frame of its parent. + * + *

+ * The rotation from the parent frame, Fparent to the current frame Fchild is defined by a quaternion {@code q}. This quaternion has a + * corresponding velocity, which is defined by the rate of change of its quaternion coordinates, {@code qDot}, and acceleratoin, which is defined by the + * acceleration of its quaternion coordinates, {@code qDDot}. + *

+ *

+ * This method computes the quaternion acceleration, and stores it in {@code qDDotToPack} + *

+ * + * @param q quaternion defining the rotation to the local frame. Not Modified. + * @param angularVelocityInParent angular velocity in the parent frame. Not modified. + * @param angularAccelerationInParent angular acceleration in the parent frame. Not Modified. + * @param qDDotToPack quaternion acceleration of {@code q} to compute. Not modified. + */ + public void computeQDDotInParentFrame(QuaternionReadOnly q, + Vector3DReadOnly angularVelocityInParent, + Vector3DReadOnly angularAccelerationInParent, + Vector4DBasics qDDotToPack) + { + computeQDotInParentFrame(q, angularVelocityInParent, intermediateQDot); + computeQDDotInParentFrame(q, intermediateQDot, angularVelocityInParent, angularAccelerationInParent, qDDotToPack); + } + + /** + * This computes the angular acceleration expressed in the parent frame of a body from the rotation of the body expressed as a quaternion and its quaternion + * rate and acceleration. + * + *

+ * The rotation from the parent frame, Fparent to the current frame Fchild is defined by a quaternion {@code q}. This quaternion has a + * corresponding velocity, which is defined by the rate of change and acceleration of its quaternion coordinates, {@code qDot} and {@code qDDot}. + *

+ *

+ * This method computes the 3-dimensional angular acceleration, expressed in the parent frame Fparent, and stores it in + * {@code angularAccelerationToPack}. + *

+ * + * @param q quaternion defining the rotation to the local frame. Not modified. + * @param qDDot quaternion acceleration of {@code q}. Not modified. + * @param angularVelocityInParent angular velocity in the parent frame. Not modified. + * @param angularAccelerationInParentToPack angular acceleration in the parent frame. Modified. + */ + public void computeAngularAccelerationInParentFrame(QuaternionReadOnly q, + Vector4DReadOnly qDDot, + Vector3DReadOnly angularVelocityInParent, + Vector3DBasics angularAccelerationInParentToPack) + { + computeQDotInParentFrame(q, angularVelocityInParent, intermediateQDot); + computeAngularAccelerationInRotatedFrame(q, intermediateQDot, qDDot, angularAccelerationInParentToPack); + } + + /** + * This computes the angular acceleration expressed in the child frame of a body from the rotation of the body expressed as a quaternion and its quaternion + * rate and acceleration. + * + *

+ * The rotation from the parent frame, Fparent to the current frame Fchild is defined by a quaternion {@code q}. This quaternion has a + * corresponding velocity, which is defined by the rate of change and acceleration of its quaternion coordinates, {@code qDot} and {@code qDDot}. + *

+ *

+ * This method computes the 3-dimensional angular acceleration, expressed in the parent frame Fparent, and stores it in + * {@code angularAccelerationToPack}. + *

+ * + * @param q quaternion defining the rotation to the local frame. Not modified. + * @param qDot quaternion rate of change of {@code q}. Not modified. + * @param qDDot quaternion acceleration of {@code q}. Not modified. + * @param angularAccelerationInRotatedToPack angular acceleration in the child frame. Modified. + */ + public void computeAngularAccelerationInRotatedFrame(QuaternionReadOnly q, + Vector4DReadOnly qDot, + Vector4DReadOnly qDDot, + Vector3DBasics angularAccelerationInRotatedToPack) + { + qConj.setAndConjugate(q); + qDotConj.set(-qDot.getX(), -qDot.getY(), -qDot.getZ(), qDot.getS()); + multiply(qDot, qDotConj, intermediateAngularAcceleration); + multiply(qDDot, qConj, angularAccelerationInRotatedToPack); + angularAccelerationInRotatedToPack.add(intermediateAngularAcceleration); + angularAccelerationInRotatedToPack.scale(2.0); + } + + /** + * This is used to estimate the rate of change of the quaternion coordinates of a time series of data. + * + *

If the time series is represented by

+ *

[qprev, qcurrent, qnext]

+ *

where the data measurements are separated by a time discretisation of {@param dt}, this method is used to compute the quaternion rate of the + * quaternion at the current time. It uses the previous {@param qPrevious} and next {@param qNext} values. The results are stored in {@param qDotToPack}.

+ * + * @param qPrevious previous quaternion estimate. Not modified. + * @param qNext next quaternion estimate. Not modified. + * @param dt time between quaternion estimates + * @param qDotToPack estimated quaternion rate at the current value. Modified. + */ + public static void computeQDotByFiniteDifferenceCentral(QuaternionReadOnly qPrevious, QuaternionReadOnly qNext, double dt, Vector4DBasics qDotToPack) + { + qDotToPack.sub(qNext, qPrevious); + qDotToPack.scale(0.5 / dt); + } + + /** + * This is used to estimate the acceleration of the quaternion coordinates of a time series of data. + * + *

If the time series is represented by

+ *

[qprev, qcurrent, qnext]

+ *

where the data measurements are separated by a time discretisation of {@param dt}, this method is used to compute the quaternion acceleration + * at the current time. It uses the previous {@param qPrevious}, current {@param q}, and next {@param qNext} values. The results are stored in + * {@param qDotToPack}.

+ * + * @param qPrevious previous quaternion estimate. Not modified. + * @param q next quaternion estimate. Not modified. + * @param qNext next quaternion estimate. Not modified. + * @param dt time between quaternion estimates. + * @param qDDotToPack estimated quaternion rate at the current value. Modified. + */ + public static void computeQDDotByFiniteDifferenceCentral(QuaternionReadOnly qPrevious, + QuaternionReadOnly q, + QuaternionReadOnly qNext, + double dt, + Vector4DBasics qDDotToPack) + { + qDDotToPack.sub(qNext, q); + qDDotToPack.sub(q); + qDDotToPack.add(qPrevious); + qDDotToPack.scale(1.0 / EuclidCoreTools.square(dt)); + } + + /** + * This is an internal method used to compute the quaternion acceleration from the quaternion value, its rate, and the angular velocity and acceleration of + * that frame relative to the parent expressed in the parent. + */ + void computeQDDotInParentFrame(QuaternionReadOnly q, + Vector4DReadOnly qDot, + Vector3DReadOnly angularVelocityInParent, + Vector3DReadOnly angularAccelerationInParent, + Vector4DBasics qDDotToPack) + { + multiply(angularAccelerationInParent, q, intermediateQDDot); + multiply(angularVelocityInParent, qDot, qDDotToPack); + qDDotToPack.add(intermediateQDDot); + qDDotToPack.scale(0.5); + } + + private static void multiply(QuaternionReadOnly q, Vector3DReadOnly v, Vector4DBasics resultToPack) + { + setAsPureQuaternion(v, resultToPack); + QuaternionTools.multiply(q, resultToPack, resultToPack); + } + + private static void multiply(Vector3DReadOnly v, Vector4DReadOnly q, Vector4DBasics resultToPack) + { + setAsPureQuaternion(v, resultToPack); + QuaternionTools.multiply(resultToPack, q, resultToPack); + } + + private static void multiply(Vector3DReadOnly v, QuaternionReadOnly q, Vector4DBasics resultToPack) + { + setAsPureQuaternion(v, resultToPack); + QuaternionTools.multiply(resultToPack, q, resultToPack); + } + + private void multiply(Vector4DReadOnly q1, Vector4DReadOnly q2, Vector3DBasics resultToPack) + { + QuaternionTools.multiply(q1, q2, pureQuatForMultiply); + setVectorFromPureQuaternion(pureQuatForMultiply, resultToPack); + } + + private void multiply(Vector4DReadOnly q1, QuaternionReadOnly q2, Vector3DBasics resultToPack) + { + QuaternionTools.multiply(q1, q2, pureQuatForMultiply); + setVectorFromPureQuaternion(pureQuatForMultiply, resultToPack); + } + + private void multiply(QuaternionReadOnly q1, Vector4DReadOnly q2, Vector3DBasics resultToPack) + { + QuaternionTools.multiply(q1, q2, pureQuatForMultiply); + setVectorFromPureQuaternion(pureQuatForMultiply, resultToPack); + } + + /** + * Provides a convenience function for setting a 4D pure quaternion vector from a 3D vector, where the 4D vector is assumed to represent a pure quaternion (w + * = 0.0). + * + * @param vector rotation data source. Not modified. + * @param pureQuaternionToSet results to pack. Modified. + */ + private static void setAsPureQuaternion(Vector3DReadOnly vector, Vector4DBasics pureQuaternionToSet) + { + pureQuaternionToSet.set(vector.getX(), vector.getY(), vector.getZ(), 0.0); + } + + /** + * Provides a convenience function for setting a 3D vector from a 4D vector, where the 4D vector is assumed to represent a pure quaternion (w = 0.0). + * + * @param pureQuaternion quaternion data source. Not modified. + * @param vectorToPack results to pack. Modified. + */ + private static void setVectorFromPureQuaternion(Vector4DReadOnly pureQuaternion, Vector3DBasics vectorToPack) + { + vectorToPack.set(pureQuaternion.getX(), pureQuaternion.getY(), pureQuaternion.getZ()); + } +} diff --git a/src/main/java/us/ihmc/euclid/tools/EuclidCoreTools.java b/src/main/java/us/ihmc/euclid/tools/EuclidCoreTools.java index 9850d624c..7b25e94ce 100644 --- a/src/main/java/us/ihmc/euclid/tools/EuclidCoreTools.java +++ b/src/main/java/us/ihmc/euclid/tools/EuclidCoreTools.java @@ -14,22 +14,9 @@ import us.ihmc.euclid.orientation.interfaces.Orientation3DReadOnly; import us.ihmc.euclid.transform.interfaces.RigidBodyTransformBasics; import us.ihmc.euclid.transform.interfaces.RigidBodyTransformReadOnly; -import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly; -import us.ihmc.euclid.tuple2D.interfaces.Tuple2DBasics; -import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; -import us.ihmc.euclid.tuple2D.interfaces.Vector2DBasics; -import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Tuple3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; -import us.ihmc.euclid.tuple4D.interfaces.QuaternionBasics; -import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; -import us.ihmc.euclid.tuple4D.interfaces.Tuple4DBasics; -import us.ihmc.euclid.tuple4D.interfaces.Tuple4DReadOnly; -import us.ihmc.euclid.tuple4D.interfaces.Vector4DBasics; -import us.ihmc.euclid.tuple4D.interfaces.Vector4DReadOnly; +import us.ihmc.euclid.tuple2D.interfaces.*; +import us.ihmc.euclid.tuple3D.interfaces.*; +import us.ihmc.euclid.tuple4D.interfaces.*; import us.ihmc.euclid.yawPitchRoll.interfaces.YawPitchRollBasics; import us.ihmc.euclid.yawPitchRoll.interfaces.YawPitchRollReadOnly; @@ -285,7 +272,9 @@ public String toString() } }; - /** Constant representing the zero-matrix 3D, i.e. the 9 elements are zero. */ + /** + * Constant representing the zero-matrix 3D, i.e. the 9 elements are zero. + */ public static final Matrix3DReadOnly zeroMatrix3D = new Matrix3DReadOnly() { @Override @@ -1592,7 +1581,7 @@ else if (currentOrientation instanceof AxisAngleReadOnly aaCurr) *
  • the angular velocity is expressed in the local coordinates of the transform. * *

    - * + *

    * @param previousTransform the transform at the previous time step. Not modified. * * @param currentTransform the transform at the current time step. Not modified. @@ -1768,4 +1757,316 @@ private static UnsupportedOperationException newUnsupportedOrientationException( previousOrientation.getClass() .getSimpleName())); } + + /** + * Checks whether all the elements contained within this tuple are finite. + * + * @param tuple Tuple to inspect. + * @return {@code true} if all elements are finite, {@code false} if any is not. + */ + public static boolean isFinite(Tuple3DReadOnly tuple) + { + return Double.isFinite(tuple.getX()) && Double.isFinite(tuple.getY()) && Double.isFinite(tuple.getZ()); + } + + /** + * Checks whether two rigid body transforms are equal within some epislon. + * + * @param a first transform to check. + * @param b first transform to check. + * @param rotationEpsilon epsilon tolerance for the rotation between the two. + * @param translationEpsilon epsilon tolerance for the translation between the two. + * @return {@code true} if the two transforms are equal, {@code false} if not. + */ + public static boolean epsilonEquals(RigidBodyTransformReadOnly a, RigidBodyTransformReadOnly b, double rotationEpsilon, double translationEpsilon) + { + return a.getRotation().geometricallyEquals(b.getRotation(), rotationEpsilon) && a.getTranslation() + .geometricallyEquals(b.getTranslation(), translationEpsilon); + } + + /** + * Computes the angle in radians from the first 3D vector to the second 3D vector. The computed + * angle is in the range [0; pi]. + * + * @param firstVectorX x value of the first vector. + * @param firstVectorY y value of the first vector. + * @param firstVectorZ z value of the first vector. + * @param isFirstVectorUnitary whether the first vector is a unit vector. + * @param secondVectorX x value of the second vector. + * @param secondVectorY y value of the second vector. + * @param secondVectorZ z value of the second vector. + * @param isSecondVectorUnitary whether the second vector is a unit vector. + * @return the angle in radians from the first vector to the second vector. + */ + public static double angleFromFirstToSecondVector3D(double firstVectorX, + double firstVectorY, + double firstVectorZ, + boolean isFirstVectorUnitary, + double secondVectorX, + double secondVectorY, + double secondVectorZ, + boolean isSecondVectorUnitary) + { + double firstVectorLength = isFirstVectorUnitary ? 1.0 : norm(firstVectorX, firstVectorY, firstVectorZ); + double secondVectorLength = isSecondVectorUnitary ? 1.0 : norm(secondVectorX, secondVectorY, secondVectorZ); + + double dotProduct = firstVectorX * secondVectorX + firstVectorY * secondVectorY + firstVectorZ * secondVectorZ; + dotProduct /= firstVectorLength * secondVectorLength; + + if (dotProduct > 1.0) + dotProduct = 1.0; + else if (dotProduct < -1.0) + dotProduct = -1.0; + + return acos(dotProduct); + } + + /** + * Computes the angle in radians from the first 3D vector to the second 3D vector. The computed + * angle is in the range [0; pi]. + * + * @param firstVector the first vector. Not modified. + * @param secondVector the second vector. Not modified. + * @return the angle in radians from the first vector to the second vector. + */ + public static double angleFromFirstToSecondVector3D(Vector3DReadOnly firstVector, Vector3DReadOnly secondVector) + { + return angleFromFirstToSecondVector3D(firstVector.getX(), + firstVector.getY(), + firstVector.getZ(), + firstVector instanceof UnitVector3DReadOnly, + secondVector.getX(), + secondVector.getY(), + secondVector.getZ(), + secondVector instanceof UnitVector3DReadOnly); + } + + /** + * Calculate the angular velocity by differentiating orientation. + * + * @param qStart the initial orientation at time t. + * @param qEnd the final orientation at time t + duration. + * @param duration the time interval between the 2 orientations. + * @param angularVelocityToPack the angular velocity. + */ + public static void differentiateOrientation(QuaternionReadOnly qStart, QuaternionReadOnly qEnd, double duration, Vector3DBasics angularVelocityToPack) + { + double q1x = qStart.getX(); + double q1y = qStart.getY(); + double q1z = qStart.getZ(); + double q1s = qStart.getS(); + + double q2x = qEnd.getX(); + double q2y = qEnd.getY(); + double q2z = qEnd.getZ(); + double q2s = qEnd.getS(); + + double diffx = q1s * q2x - q1x * q2s - q1y * q2z + q1z * q2y; + double diffy = q1s * q2y + q1x * q2z - q1y * q2s - q1z * q2x; + double diffz = q1s * q2z - q1x * q2y + q1y * q2x - q1z * q2s; + double diffs = q1s * q2s + q1x * q2x + q1y * q2y + q1z * q2z; + + if (diffs < 0.0) + { + diffx = -diffx; + diffy = -diffy; + diffz = -diffz; + diffs = -diffs; + } + + double sinHalfTheta = norm(diffx, diffy, diffz); + + double angle; + if (epsilonEquals(1.0, diffs, 1.0e-12)) + angle = 2.0 * sinHalfTheta / diffs; + else + angle = 2.0 * atan2(sinHalfTheta, diffs); + angularVelocityToPack.set(diffx, diffy, diffz); + angularVelocityToPack.scale(angle / (sinHalfTheta * duration)); + } + + /** + * Sets the yaw pitch roll but the doubles are given in degrees. + */ + public static void setYawPitchRollDegrees(Orientation3DBasics orientation3DBasics, double yaw, double pitch, double roll) + { + orientation3DBasics.setYawPitchRoll(Math.toRadians(yaw), Math.toRadians(pitch), Math.toRadians(roll)); + } + + /** + * Projects the provided {@code rotation} onto {@code axis} such that the original rotation can be + * decomposed into a rotation around {@code axis} and one around an orthogonal axis. + *

    + * rotation = orthogonalRotation * result + *

    + * + * @param rotation is the original rotation to be projected onto {@code axis} + * @param axis is the desired rotation axis of the result. + * @param result will be modified to contain the component of {@code rotation} that is around + * {@code axis} + */ + public static void projectRotationOnAxis(QuaternionReadOnly rotation, Vector3DReadOnly axis, QuaternionBasics result) + { + double dotProduct = rotation.getX() * axis.getX() + rotation.getY() * axis.getY() + rotation.getZ() * axis.getZ(); + + double scale = dotProduct / axis.normSquared(); + double projectedX = scale * axis.getX(); + double projectedY = scale * axis.getY(); + double projectedZ = scale * axis.getZ(); + + result.set(projectedX, projectedY, projectedZ, rotation.getS()); + result.normalize(); + } + + /** + * Calculates the 3D part of the given {@code input} that is parallel to the given + * {@code normalAxis} and stores the result in {@code inputNormalPartToPack}. + *

    + * The result has the following properties: + *

      + *
    • x.n = xn.n + *
    • |xn×n| = 0 + *
    + * where: + *
      + *
    • x is {@code input}. + *
    • n is {@code normalAxis}. + *
    • xn is {@code inputNormalPartToPack}. + *
    + *

    + * + * @param input the tuple to extract the normal part of. Not modified. + * @param normalAxis the normal vector. It is normalized internally if needed. Not + * modified. + * @param inputNormalPartToPack the tuple used to store the normal part of the input. Modified. + */ + public static void extractNormalPart(Tuple3DReadOnly input, Vector3DReadOnly normalAxis, Tuple3DBasics inputNormalPartToPack) + { + double normalX = normalAxis.getX(); + double normalY = normalAxis.getY(); + double normalZ = normalAxis.getZ(); + double normalLengthSquared = normSquared(normalX, normalY, normalZ); + + double dot = TupleTools.dot(normalAxis, input) / normalLengthSquared; + inputNormalPartToPack.setAndScale(dot, normalAxis); + } + + /** + * Calculates the 3D part of the given {@code input} that is orthogonal to the given + * {@code normalAxis} and stores the result in {@code inputTangentialPartToPack}. + *

    + * The result has the following properties: + *

      + *
    • xt.n = 0 + *
    • |x - (x.n)n| = |xt| + *
    + * where: + *
      + *
    • x is {@code input}. + *
    • n is {@code normalAxis}. + *
    • xt is {@code inputTangentialPartToPack}. + *
    + *

    + * + * @param input the tuple to extract the tangential part of. Not modified. + * @param normalAxis the normal vector. It is normalized internally if needed. Not + * modified. + * @param inputTagentialPartToPack the tuple used to store the tangential part of the input. + * Modified. + */ + public static void extractTangentialPart(Tuple3DReadOnly input, Vector3DReadOnly normalAxis, Tuple3DBasics inputTagentialPartToPack) + { + double normalX = normalAxis.getX(); + double normalY = normalAxis.getY(); + double normalZ = normalAxis.getZ(); + double normalLengthSquared = normSquared(normalX, normalY, normalZ); + + double dot = TupleTools.dot(normalX, normalY, normalZ, input) / normalLengthSquared; + double normalPartX = dot * normalX; + double normalPartY = dot * normalY; + double normalPartZ = dot * normalZ; + + inputTagentialPartToPack.set(input); + inputTagentialPartToPack.sub(normalPartX, normalPartY, normalPartZ); + } + + /** + * Modifies {@code tupleToModify} such that its normal part is equal to the normal part of + * {@code input}. + *

    + * This method performs the following calculation: x = x - (x.n)n + (y.n)n, where: + *

      + *
    • x is {@code tupleToModify}. + *
    • n is {@code normalAxis}. + *
    • y is {@code input}. + *
    + *

    + * + * @param input the tuple containing the normal part used to update {@code tupleToModify}. + * Not modified. + * @param normalAxis the normal vector. It is normalized internally if needed. Not modified. + * @param tupleToModify the tuple to modify the normal part of. Modified. + */ + public static void setNormalPart(Tuple3DReadOnly input, Vector3DReadOnly normalAxis, Tuple3DBasics tupleToModify) + { + double normalX = normalAxis.getX(); + double normalY = normalAxis.getY(); + double normalZ = normalAxis.getZ(); + double normalLengthSquared = normSquared(normalX, normalY, normalZ); + double dot = (TupleTools.dot(normalAxis, input) - TupleTools.dot(normalAxis, tupleToModify)) / normalLengthSquared; + tupleToModify.scaleAdd(dot, normalAxis, tupleToModify); + } + + /** + * Generic transform operation to apply a rotation matrix {@code matrix} to a tuple 3d. + * + * @param matrix rotation matrix to apply + * @param xOriginal x value of the tuple + * @param yOriginal y value of the tuple + * @param zOriginal z value of the tuple + * @param tupleTransformed transformed tuple 3D to compute. + */ + public static void transform(Matrix3DReadOnly matrix, double xOriginal, double yOriginal, double zOriginal, Tuple3DBasics tupleTransformed) + { + double x = matrix.getM00() * xOriginal + matrix.getM01() * yOriginal + matrix.getM02() * zOriginal; + double y = matrix.getM10() * xOriginal + matrix.getM11() * yOriginal + matrix.getM12() * zOriginal; + double z = matrix.getM20() * xOriginal + matrix.getM21() * yOriginal + matrix.getM22() * zOriginal; + tupleTransformed.set(x, y, z); + } + + /** + * Assert on a component basis is the {@code tuple} is equal to (0, 0, 0) given the tolerance + * {@code epsilon}. + * + * @param tuple the query. Not modified. + * @param epsilon the tolerance. + * @return {@code true} if the tuple's component are all equal to zero, {@code false} otherwise. + */ + public static boolean isZero(Tuple3DReadOnly tuple, double epsilon) + { + if (!epsilonEquals(tuple.getX(), 0.0, epsilon)) + return false; + if (!epsilonEquals(tuple.getY(), 0.0, epsilon)) + return false; + if (!epsilonEquals(tuple.getZ(), 0.0, epsilon)) + return false; + return true; + } + + /** + * Assert on a component basis is the {@code tuple} is equal to (0, 0) given the tolerance + * {@code epsilon}. + * + * @param tuple the query. Not modified. + * @param epsilon the tolerance. + * @return {@code true} if the tuple's component are all equal to zero, {@code false} otherwise. + */ + public static boolean isZero(Tuple2DReadOnly tuple, double epsilon) + { + if (!epsilonEquals(tuple.getX(), 0.0, epsilon)) + return false; + if (!epsilonEquals(tuple.getY(), 0.0, epsilon)) + return false; + return true; + } } diff --git a/src/test/java/us/ihmc/euclid/QuaternionCalculusTest.java b/src/test/java/us/ihmc/euclid/QuaternionCalculusTest.java new file mode 100644 index 000000000..dd9536136 --- /dev/null +++ b/src/test/java/us/ihmc/euclid/QuaternionCalculusTest.java @@ -0,0 +1,331 @@ +package us.ihmc.euclid; + +import java.util.Random; + +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.Test; + +import us.ihmc.euclid.axisAngle.AxisAngle; +import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools; +import us.ihmc.euclid.tools.EuclidCoreTestTools; +import us.ihmc.euclid.tools.EuclidCoreTools; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.euclid.tuple4D.Quaternion; +import us.ihmc.euclid.tuple4D.Vector4D; +import us.ihmc.euclid.tools.EuclidCoreRandomTools; + +import static java.lang.Math.PI; +import static org.junit.jupiter.api.Assertions.*; + +public class QuaternionCalculusTest +{ + private static final double EPSILON = 1.0e-10; + + @AfterEach + public void tearDown() + { + ReferenceFrameTools.clearWorldFrameTree(); + } + + @Test + public void testLogAndExpAlgebra() + { + Random random = new Random(651651961L); + + for (int i = 0; i < 10000; i++) + { + QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); + Quaternion q = EuclidCoreRandomTools.nextQuaternion(random); + + Vector4D qLog = new Vector4D(); + Quaternion vExp = new Quaternion(); + + quaternionCalculus.log(q, qLog); + Vector3D v = new Vector3D(qLog.getX(),qLog.getY(),qLog.getZ()); + + QuaternionCalculus.exp(v, vExp); + + assertTrue(Math.abs(q.getX() - vExp.getX()) < 10e-10); + assertTrue(Math.abs(q.getY() - vExp.getY()) < 10e-10); + assertTrue(Math.abs(q.getZ() - vExp.getZ()) < 10e-10); + assertTrue(Math.abs(q.getS() - vExp.getS()) < 10e-10); + + } + } + + + @Test + public void testConversionQDotToAngularVelocityBackAndForth() + { + Random random = new Random(651651961L); + + for (int i = 0; i < 10000; i++) + { + QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); + Quaternion q = EuclidCoreRandomTools.nextQuaternion(random); + double length = EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0); + Vector3D expectedAngularVelocity = EuclidCoreRandomTools.nextVector3D(random, length); + if (random.nextBoolean()) + expectedAngularVelocity.negate(); + Vector3D actualAngularVelocity = new Vector3D(); + Vector4D qDot = new Vector4D(); + + QuaternionCalculus.computeQDotInParentFrame(q, expectedAngularVelocity, qDot); + quaternionCalculus.computeAngularVelocityInParentFrame(q, qDot, actualAngularVelocity); + + EuclidCoreTestTools.assertEquals(expectedAngularVelocity, actualAngularVelocity, EPSILON); + } + } + + @Test + public void testConversionQDDotToAngularAccelerationBackAndForth() + { + Random random = new Random(651651961L); + + for (int i = 0; i < 10000; i++) + { + QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); + Quaternion q = EuclidCoreRandomTools.nextQuaternion(random); + double length = EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0); + Vector3D angularVelocity = EuclidCoreRandomTools.nextVector3D(random, length); + if (random.nextBoolean()) + angularVelocity.negate(); + Vector3D expectedAngularAcceleration = EuclidCoreRandomTools.nextVector3D(random, length); + if (random.nextBoolean()) + expectedAngularAcceleration.negate(); + Vector3D actualAngularAcceleration = new Vector3D(); + Vector4D qDot = new Vector4D(); + Vector4D qDDot = new Vector4D(); + + QuaternionCalculus.computeQDotInParentFrame(q, angularVelocity, qDot); + + quaternionCalculus.computeQDDotInParentFrame(q, qDot, expectedAngularAcceleration, qDDot); + quaternionCalculus.computeAngularAccelerationInRotatedFrame(q, qDot, qDDot, actualAngularAcceleration); + EuclidCoreTestTools.assertEquals(expectedAngularAcceleration, actualAngularAcceleration, EPSILON); + + quaternionCalculus.computeQDDotInParentFrame(q, angularVelocity, actualAngularAcceleration, qDDot); + quaternionCalculus.computeAngularAccelerationInRotatedFrame(q, qDot, qDDot, actualAngularAcceleration); + EuclidCoreTestTools.assertEquals(expectedAngularAcceleration, actualAngularAcceleration, EPSILON); + + quaternionCalculus.computeQDDotInParentFrame(q, qDot, angularVelocity, actualAngularAcceleration, qDDot); + quaternionCalculus.computeAngularAccelerationInRotatedFrame(q, qDot, qDDot, actualAngularAcceleration); + EuclidCoreTestTools.assertEquals(expectedAngularAcceleration, actualAngularAcceleration, EPSILON); + + quaternionCalculus.computeQDDotInParentFrame(q, qDot, expectedAngularAcceleration, qDDot); + quaternionCalculus.computeAngularAccelerationInParentFrame(q, qDDot, angularVelocity, actualAngularAcceleration); + EuclidCoreTestTools.assertEquals(expectedAngularAcceleration, actualAngularAcceleration, EPSILON); + } + } + +// @Test +// public void testVelocityFromFDAgainstTrajectory() +// { +// QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); +// SimpleOrientationTrajectoryGenerator traj = new SimpleOrientationTrajectoryGenerator("traj", ReferenceFrame.getWorldFrame(), new YoRegistry("null")); +// double trajectoryTime = 1.0; +// traj.setTrajectoryTime(trajectoryTime); +// Random random = new Random(65265L); +// FrameQuaternion initialOrientation = EuclidFrameRandomTools.nextFrameQuaternion(random, ReferenceFrame.getWorldFrame()); +// FrameQuaternion finalOrientation = EuclidFrameRandomTools.nextFrameQuaternion(random, ReferenceFrame.getWorldFrame()); +// traj.setInitialOrientation(initialOrientation); +// traj.setFinalOrientation(finalOrientation); +// traj.initialize(); +// +// double dt = 1.0e-4; +// double dtForFD = 1.0e-6; +// +// FrameQuaternion orientation = new FrameQuaternion(); +// FrameVector3D expectedAngularVelocity = new FrameVector3D(); +// Quaternion q = new Quaternion(); +// Vector4D qDot = new Vector4D(); +// Quaternion qPrevious = new Quaternion(); +// Quaternion qNext = new Quaternion(); +// Vector3D actualAngularVelocity = new Vector3D(); +// +// for (double time = dt; time <= trajectoryTime - dt; time += dt) +// { +// traj.compute(time); +// expectedAngularVelocity.setIncludingFrame(traj.getAngularVelocity()); +// q.set(traj.getOrientation()); +// traj.compute(time - dtForFD); +// qPrevious.set(traj.getOrientation()); +// traj.compute(time + dtForFD); +// qNext.set(traj.getOrientation()); +// +// quaternionCalculus.computeQDotByFiniteDifferenceCentral(qPrevious, qNext, dtForFD, qDot); +// quaternionCalculus.computeAngularVelocityInWorldFrame(q, qDot, actualAngularVelocity); +// +// EuclidCoreTestTools.assertEquals(expectedAngularVelocity, actualAngularVelocity, 1e-8); +// } +// } + + @Test + public void testFDSimpleCase() + { + QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); + Random random = new Random(65265L); + double integrationTime = 1.0; + double angleVelocity = EuclidCoreRandomTools.nextDouble(random, 0.0, 2.0 * PI) / integrationTime; + Vector3D expectedAngularVelocity = new Vector3D(angleVelocity, 0.0, 0.0); + Vector3D expectedAngularAcceleration = new Vector3D(); + AxisAngle axisAnglePrevious = new AxisAngle(1.0, 0.0, 0.0, 0.0); + AxisAngle axisAngleCurrent = new AxisAngle(1.0, 0.0, 0.0, 0.0); + AxisAngle axisAngleNext = new AxisAngle(1.0, 0.0, 0.0, 0.0); + Quaternion qPrevious = new Quaternion(); + Quaternion qCurrent = new Quaternion(); + Quaternion qNext = new Quaternion(); + Vector4D qDot = new Vector4D(); + Vector4D qDDot = new Vector4D(); + + Vector3D actualAngularVelocity = new Vector3D(); + Vector3D actualAngularAcceleration = new Vector3D(); + + double dt = 1.0e-4; + for (double time = dt; time < integrationTime; time += dt) + { + axisAnglePrevious.setAngle(EuclidCoreTools.trimAngleMinusPiToPi(angleVelocity * (time - dt)) - PI); + qPrevious.set(axisAnglePrevious); + axisAngleCurrent.setAngle(EuclidCoreTools.trimAngleMinusPiToPi(angleVelocity * time) - PI); + qCurrent.set(axisAngleCurrent); + axisAngleNext.setAngle(EuclidCoreTools.trimAngleMinusPiToPi(angleVelocity * (time + dt)) - PI); + qNext.set(axisAngleNext); + + QuaternionCalculus.computeQDotByFiniteDifferenceCentral(qPrevious, qNext, dt, qDot); + quaternionCalculus.computeAngularVelocityInParentFrame(qCurrent, qDot, actualAngularVelocity); + + QuaternionCalculus.computeQDDotByFiniteDifferenceCentral(qPrevious, qCurrent, qNext, dt, qDDot); + quaternionCalculus.computeAngularAccelerationInRotatedFrame(qCurrent, qDot, qDDot, actualAngularAcceleration); + + boolean sameVelocity = expectedAngularVelocity.epsilonEquals(actualAngularVelocity, 1.0e-7); + if (!sameVelocity) + { + System.out.println("Expected angular velocity: " + expectedAngularVelocity); + System.out.println("Actual angular velocity: " + actualAngularVelocity); + } + assertTrue(sameVelocity); + EuclidCoreTestTools.assertEquals(expectedAngularAcceleration, actualAngularAcceleration, 1e-7); + } + } + +// @Test +// public void testAccelerationFromFDAgainstTrajectory() +// { +// QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); +// SimpleOrientationTrajectoryGenerator traj = new SimpleOrientationTrajectoryGenerator("traj", ReferenceFrame.getWorldFrame(), new YoRegistry("null")); +// double trajectoryTime = 1.0; +// traj.setTrajectoryTime(trajectoryTime); +// Random random = new Random(65265L); +// FrameQuaternion initialOrientation = EuclidFrameRandomTools.nextFrameQuaternion(random, ReferenceFrame.getWorldFrame()); +// FrameQuaternion finalOrientation = EuclidFrameRandomTools.nextFrameQuaternion(random, ReferenceFrame.getWorldFrame()); +// traj.setInitialOrientation(initialOrientation); +// traj.setFinalOrientation(finalOrientation); +// traj.initialize(); +// +// double dt = 1.0e-4; +// double dtForFD = 1.0e-4; +// +// FrameQuaternion orientation = new FrameQuaternion(); +// FrameVector3D expectedAngularAcceleration = new FrameVector3D(); +// Quaternion q = new Quaternion(); +// Vector4D qDot = new Vector4D(); +// Vector4D qDDot = new Vector4D(); +// Quaternion qPrevious = new Quaternion(); +// Quaternion qNext = new Quaternion(); +// Vector3D actualAngularAcceleration = new Vector3D(); +// +// for (double time = dt; time <= trajectoryTime - dt; time += dt) +// { +// traj.compute(time); +// expectedAngularAcceleration.setIncludingFrame(traj.getAngularAcceleration()); +// q.set(traj.getOrientation()); +// traj.compute(time - dtForFD); +// qPrevious.set(traj.getOrientation()); +// traj.compute(time + dtForFD); +// qNext.set(traj.getOrientation()); +// +// quaternionCalculus.computeQDotByFiniteDifferenceCentral(qPrevious, qNext, dtForFD, qDot); +// quaternionCalculus.computeQDDotByFiniteDifferenceCentral(qPrevious, q, qNext, dtForFD, qDDot); +// quaternionCalculus.computeAngularAcceleration(q, qDot, qDDot, actualAngularAcceleration); +// +// EuclidCoreTestTools.assertEquals(expectedAngularAcceleration, actualAngularAcceleration, 1e-5); +// } +// } + + @Test + public void testInterpolateAgainstQuat4d() + { + QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); + Random random = new Random(6546545L); + Quaternion q0 = EuclidCoreRandomTools.nextQuaternion(random); + Quaternion q1 = EuclidCoreRandomTools.nextQuaternion(random); + Quaternion expectedQInterpolated = new Quaternion(); + Quaternion actualQInterpolated = new Quaternion(); + + for (double alpha = 0.0; alpha <= 1.0; alpha += 1.0e-6) + { + expectedQInterpolated.interpolate(q0, q1, alpha); + quaternionCalculus.interpolate(alpha, q0, q1, actualQInterpolated); + + EuclidCoreTestTools.assertEquals(expectedQInterpolated, actualQInterpolated, EPSILON); + } + } + + static double shiftAngleToStartOfRange(double angleToShift, double startOfAngleRange, double endOfAngleRange) + { + double ret = angleToShift; + startOfAngleRange = startOfAngleRange - EPSILON; + + if (angleToShift < startOfAngleRange) + { + ret = angleToShift + Math.ceil((startOfAngleRange - angleToShift) / endOfAngleRange) * endOfAngleRange; + } + + if (angleToShift >= (startOfAngleRange + endOfAngleRange)) + { + ret = angleToShift - Math.floor((angleToShift - startOfAngleRange) / endOfAngleRange) * endOfAngleRange; + } + + return ret; + } + @Test + public void testShiftAngleToStartOfRangeUnitless() + { + double range = Math.pow(2.0, 13.0); + + double endOfAngleRange = range; + double angleToShift = 1.6 * range; + double expectedReturn = 0.6 * range; + double actualReturn = shiftAngleToStartOfRange(angleToShift, 0.0, endOfAngleRange); + assertEquals(expectedReturn, actualReturn, 1e-12); + + angleToShift = -0.4 * range; + expectedReturn = 0.6 * range; + actualReturn = shiftAngleToStartOfRange(angleToShift, 0.0, endOfAngleRange); + assertEquals(expectedReturn, actualReturn, 1e-12); + + angleToShift = 0.4 * range; + expectedReturn = 0.4 * range; + actualReturn = shiftAngleToStartOfRange(angleToShift, 0.0, endOfAngleRange); + assertEquals(expectedReturn, actualReturn, 1e-12); + + int iters = 1000; + Random random = new Random(); + for (int i = 0; i < iters; i++) + { + double ratio = -6.0 + 12.0 * random.nextDouble(); + + angleToShift = ratio * range; + expectedReturn = angleToShift; + + if (angleToShift < 0.0) + expectedReturn = angleToShift + Math.ceil((-angleToShift) / endOfAngleRange) * endOfAngleRange; + + if (angleToShift >= endOfAngleRange) + expectedReturn = angleToShift - Math.floor((angleToShift) / endOfAngleRange) * endOfAngleRange; + + actualReturn = shiftAngleToStartOfRange(angleToShift, 0.0, endOfAngleRange); + assertEquals(expectedReturn, actualReturn, 1e-12); + } + } + +} diff --git a/src/test/java/us/ihmc/euclid/geometry/tools/ConvexPolygon2DTest.java b/src/test/java/us/ihmc/euclid/geometry/tools/ConvexPolygon2DTest.java new file mode 100644 index 000000000..7faadd770 --- /dev/null +++ b/src/test/java/us/ihmc/euclid/geometry/tools/ConvexPolygon2DTest.java @@ -0,0 +1,35 @@ +package us.ihmc.euclid.geometry.tools; + +import org.junit.jupiter.api.Test; +import us.ihmc.euclid.geometry.ConvexPolygon2D; +import us.ihmc.euclid.geometry.interfaces.ConvexPolygon2DBasics; +import us.ihmc.euclid.tuple2D.Point2D; +import us.ihmc.euclid.tuple2D.Vector2D; + +import static org.junit.jupiter.api.Assertions.*; + +public class ConvexPolygon2DTest +{ + private static final double epsilon = 1e-7; + + + @Test + public void testTranslatePolygon1() + { + ConvexPolygon2D polygon = new ConvexPolygon2D(); + polygon.addVertex(new Point2D(0.0, 0.0)); + polygon.addVertex(new Point2D(10.0, 0.0)); + polygon.addVertex(new Point2D(0.0, 10.0)); + polygon.update(); + + Vector2D translation1 = new Vector2D(0.0, 0.0); + ConvexPolygon2DBasics polygon1 = polygon.translateCopy(translation1); + assertTrue(polygon1.epsilonEquals(polygon, epsilon)); + + Vector2D translation2 = new Vector2D(1.0, 0.5); + ConvexPolygon2DBasics polygon2 = polygon.translateCopy(translation2); + assertTrue(polygon2.getVertex(2).epsilonEquals(new Point2D(1.0, 0.5), epsilon)); + assertTrue(polygon2.getVertex(1).epsilonEquals(new Point2D(11.0, 0.5), epsilon)); + assertTrue(polygon2.getVertex(0).epsilonEquals(new Point2D(1.0, 10.5), epsilon)); + } +} diff --git a/src/test/java/us/ihmc/euclid/geometry/tools/EuclidGeometryToolsTest.java b/src/test/java/us/ihmc/euclid/geometry/tools/EuclidGeometryToolsTest.java index d117e3492..eebcfe4d4 100644 --- a/src/test/java/us/ihmc/euclid/geometry/tools/EuclidGeometryToolsTest.java +++ b/src/test/java/us/ihmc/euclid/geometry/tools/EuclidGeometryToolsTest.java @@ -25,6 +25,9 @@ import us.ihmc.euclid.Axis3D; import us.ihmc.euclid.Location; import us.ihmc.euclid.axisAngle.AxisAngle; +import us.ihmc.euclid.geometry.BoundingBox3D; +import us.ihmc.euclid.geometry.ConvexPolygon2D; +import us.ihmc.euclid.geometry.Plane3D; import us.ihmc.euclid.geometry.exceptions.BoundingBoxException; import us.ihmc.euclid.matrix.RotationMatrix; import us.ihmc.euclid.tools.EuclidCoreRandomTools; @@ -35,12 +38,14 @@ import us.ihmc.euclid.tuple2D.Point2D; import us.ihmc.euclid.tuple2D.Vector2D; import us.ihmc.euclid.tuple3D.Point3D; +import us.ihmc.euclid.tuple3D.UnitVector3D; import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.euclid.tuple3D.interfaces.Point3DBasics; import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics; import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; import us.ihmc.euclid.tuple4D.Quaternion; +import us.ihmc.euclid.tuple4D.Vector4D; public class EuclidGeometryToolsTest { @@ -14369,4 +14374,105 @@ public void testUnknownTriangleSideLengthByLawOfCosine() throws Exception // good } } + + + @Test + public void testGetZOnPlane() + { + Point3D point = new Point3D(1.0, 2.0, -3.0); + Vector3D normal = new Vector3D(0.2, 1.7, 0.4); + Plane3D plane = new Plane3D(point, normal); + + double x = 2.33; + double y = 1.97; + + double z = EuclidGeometryTools.getZOnPlane(point, normal, x, y); + + Point3D testPoint = new Point3D(x, y, z); + assertTrue(plane.distance(testPoint) < 1e-10); + + normal = new Vector3D(0.2, 1.7, 0.0); + + z = EuclidGeometryTools.getZOnPlane(point, normal, x, y); + assertTrue(Double.isNaN(z)); + } + + + @Test + public void testIsPolygonInside1() + { + ConvexPolygon2D polygon = new ConvexPolygon2D(); + polygon.addVertex(new Point2D(0.0, 0.0)); + polygon.addVertex(new Point2D(2.0, 1.0)); + polygon.addVertex(new Point2D(1.0, 2.0)); + polygon.update(); + + ConvexPolygon2D polygonToTest1 = new ConvexPolygon2D(); + polygonToTest1.addVertex(new Point2D(0.1, 0.1)); + polygonToTest1.addVertex(new Point2D(0.2, 0.2)); + polygonToTest1.update(); + assertTrue(EuclidGeometryTools.isPolygonInside(polygonToTest1, polygon)); + + ConvexPolygon2D polygonToTest2 = new ConvexPolygon2D(polygon); + assertTrue(EuclidGeometryTools.isPolygonInside(polygonToTest2, polygon)); + assertTrue(EuclidGeometryTools.isPolygonInside(polygonToTest2, EPSILON, polygon)); + assertFalse(EuclidGeometryTools.isPolygonInside(polygonToTest2, -EPSILON, polygon)); + + ConvexPolygon2D polygonToTest3 = new ConvexPolygon2D(); + polygonToTest3.addVertex(new Point2D(0.3, 0.9)); + polygonToTest3.addVertex(new Point2D(0.1, 0.1)); + polygonToTest3.addVertex(new Point2D(1.0, 1.2)); + polygonToTest3.update(); + assertFalse(EuclidGeometryTools.isPolygonInside(polygonToTest3, polygon)); + + ConvexPolygon2D polygonToTest4 = new ConvexPolygon2D(); + assertTrue(EuclidGeometryTools.isPolygonInside(polygonToTest4, polygon)); + + ConvexPolygon2D polygonToTest5 = new ConvexPolygon2D(); + polygonToTest5.addVertex(new Point2D(-0.1, 0.1)); + polygonToTest5.update(); + assertFalse(EuclidGeometryTools.isPolygonInside(polygonToTest5, polygon)); + } + + + @Test + public void testComputeBoundingBoxIntersection3D() + { + BoundingBox3D boundingBox1 = new BoundingBox3D(0.0, 0.0, 0.0, 1.0, 1.0, 1.0); + BoundingBox3D boundingBox2 = new BoundingBox3D(0.5, 0.0, 0.0, 1.5, 1.0, 1.0); + + assertEquals(1.0, EuclidGeometryTools.computeBoundingBoxVolume3D(boundingBox1), EPSILON, "[FAILED] Volume should be 1.0"); + assertEquals(1.0, EuclidGeometryTools.computeBoundingBoxVolume3D(boundingBox2), EPSILON, "[FAILED] Volume should be 1.0"); + + assertEquals(1/3.0, EuclidGeometryTools.computeIntersectionOverUnionOfTwoBoundingBoxes(boundingBox1, boundingBox2), EPSILON, "[FAILED] Intersection should be 1/3.0"); + assertEquals(0.5, EuclidGeometryTools.computeIntersectionOverSmallerOfTwoBoundingBoxes(boundingBox2, boundingBox1), EPSILON, "[FAILED] Intersection should be 0.5"); + + boundingBox2 = new BoundingBox3D(0.5, 0.5, 0.5, 1.5, 1.5, 1.5); + + assertEquals(1.0, EuclidGeometryTools.computeBoundingBoxVolume3D(boundingBox2), EPSILON, "[FAILED] Volume should be 1.0"); + + assertEquals(1/15.0, EuclidGeometryTools.computeIntersectionOverUnionOfTwoBoundingBoxes(boundingBox1, boundingBox2), EPSILON, "[FAILED] Intersection should be 1/3.0"); + assertEquals( 1/8.0, EuclidGeometryTools.computeIntersectionOverSmallerOfTwoBoundingBoxes(boundingBox2, boundingBox1), EPSILON, "[FAILED] Intersection should be 1/15.0"); + } + + @Test + public void testPointProjectionsOntoPlane() + { + Point3D pointOnPlane = new Point3D(1.0, 1.0, 1.0); + UnitVector3D planeNormal = new UnitVector3D(1.0, 1.0, 1.0); + + Vector4D plane = new Vector4D(planeNormal.getX(), planeNormal.getY(), planeNormal.getZ(), -pointOnPlane.dot(planeNormal)); + + Point3D pointToProject = new Point3D(0.0, 0.0, 0.0); + + Point3D projectedPoint = EuclidGeometryTools.projectPointOntoPlane(plane, pointToProject); + + + Point3D expectedProjection = new Point3D(1.0, 1.0, 1.0); + + System.out.println("Projected point: " + projectedPoint); + System.out.println("Expected projection: " + expectedProjection); + + assertEquals(0.0, projectedPoint.distance(expectedProjection), EPSILON); + } } diff --git a/src/test/java/us/ihmc/euclid/referenceFrame/FramePlane3DTest.java b/src/test/java/us/ihmc/euclid/referenceFrame/FramePlane3DTest.java new file mode 100644 index 000000000..5ad001cb3 --- /dev/null +++ b/src/test/java/us/ihmc/euclid/referenceFrame/FramePlane3DTest.java @@ -0,0 +1,120 @@ +package us.ihmc.euclid.referenceFrame; + +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.Test; + +import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools; +import us.ihmc.euclid.transform.RigidBodyTransform; +import us.ihmc.euclid.tuple3D.Point3D; +import us.ihmc.euclid.tuple3D.Vector3D; + +import static org.junit.jupiter.api.Assertions.*; + +public class FramePlane3DTest +{ + private static ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); + private static double epsilon = 1e-14; + + @AfterEach + public void tearDown() + { + ReferenceFrameTools.clearWorldFrameTree(); + } + + @Test + public void testIsOnOrAbove() + { + FramePlane3D plane = new FramePlane3D(worldFrame, new Point3D(), new Vector3D(0.0, 0.0, 1.0)); + FramePoint3D q = new FramePoint3D(worldFrame, 0.0, 0.0, 2.0); + assertTrue(plane.isOnOrAbove(q)); + q = new FramePoint3D(worldFrame, 0.0, 0.0, 0.0); + assertTrue(plane.isOnOrAbove(q)); + q = new FramePoint3D(worldFrame, 0.0, 0.0, -2.0); + assertFalse(plane.isOnOrAbove(q)); + } + + @Test + public void testIsOnOrBelow() + { + FramePlane3D plane = new FramePlane3D(worldFrame, new Point3D(), new Vector3D(0.0, 0.0, 1.0)); + FramePoint3D q = new FramePoint3D(worldFrame, 0.0, 0.0, -2.0); + assertTrue(plane.isOnOrBelow(q)); + q.set(0.0, 0.0, 1.0); + assertFalse(plane.isOnOrBelow(q)); + } + + @Test + public void testOrthogonalProjection() + { + FramePoint3D point = new FramePoint3D(worldFrame, 1.0, 2.0, -3.0); + FramePoint3D expectedPoint = new FramePoint3D(worldFrame, 1.0, 2.0, 0.0); + FramePlane3D plane = new FramePlane3D(worldFrame, new Point3D(), new Vector3D(0.0, 0.0, 1.0)); + assertTrue(expectedPoint.epsilonEquals(plane.orthogonalProjectionCopy(point), 1e-14)); + + point.set(3.0, 3.0, -4.0); + plane.getPoint().set(1.0, 1.0, 1.0); + plane.getNormal().set(2.0, 0.0, 0.0); + expectedPoint.set(1.0, 3.0, -4.0); + assertTrue(expectedPoint.epsilonEquals(plane.orthogonalProjectionCopy(point), 1e-14)); + } + + @Test + public void testDistance() + { + FramePoint3D point = new FramePoint3D(worldFrame, 1.0, 1.0, 1.0); + FramePlane3D plane = new FramePlane3D(worldFrame, new Point3D(), new Vector3D(0.0, 0.0, 1.0)); + assertEquals(1.0, plane.distance(point), 1e-14); + + point.set(0.0, 0.0, 0.0); + plane.getNormal().set(1.0, 1.0, 0.0); + plane.getPoint().set(1.0, 1.0, 0.0); + assertEquals(Math.sqrt(2), plane.distance(point), epsilon); + } + + @Test + public void testApplyTransform() + { + RigidBodyTransform transformation = new RigidBodyTransform(); + transformation.setRotationYawAndZeroTranslation(2.3); + FramePlane3D plane = new FramePlane3D(worldFrame, new Point3D(), new Vector3D(0.0, 0.0, 1.0)); + plane.applyTransform(transformation); + FrameVector3D expectedNormal = new FrameVector3D(worldFrame, 0.0, 0.0, 1.0); + FramePoint3D expectedPoint = new FramePoint3D(worldFrame, 0.0, 0.0, 0.0); + assertTrue(plane.epsilonEquals(new FramePlane3D(expectedPoint, expectedNormal), epsilon)); + + RigidBodyTransform transformation2 = new RigidBodyTransform(); + transformation2.getTranslation().set(new Vector3D(1.0, 2.0, 3.0)); + FramePlane3D plane2 = new FramePlane3D(worldFrame, new Point3D(), new Vector3D(0.0, 0.0, 1.0)); + plane2.applyTransform(transformation2); + expectedNormal.set(0.0, 0.0, 1.0); + expectedPoint.set(1.0, 2.0, 3.0); + assertTrue(plane2.epsilonEquals(new FramePlane3D(expectedPoint, expectedNormal), epsilon)); + + RigidBodyTransform transformation3 = new RigidBodyTransform(); + transformation3.setRotationPitchAndZeroTranslation(Math.PI / 2); + transformation3.getTranslation().set(new Vector3D(1.0, 2.0, 3.0)); + FramePlane3D plane3 = new FramePlane3D(worldFrame, new Point3D(), new Vector3D(0.0, 0.0, 1.0)); + plane3.applyTransform(transformation3); + expectedNormal.set(1.0, 0.0, 0.0); + expectedPoint.set(1.0, 2.0, 3.0); + assertTrue(plane3.epsilonEquals(new FramePlane3D(expectedPoint, expectedNormal), epsilon)); + } + + @Test + public void testIntersectionWithLine() + { + FrameVector3D normal = new FrameVector3D(worldFrame, 0.0, 0.0, 1.0); + FramePoint3D point = new FramePoint3D(worldFrame, 0.0, 0.0, 0.0); + FramePlane3D plane = new FramePlane3D(point, normal); + + FramePoint3D origin = new FramePoint3D(worldFrame, 0.0, 1.0, -1.0); + FrameVector3D direction = new FrameVector3D(worldFrame, 1.0, 0.0, 1.0); + FrameLine3D line = new FrameLine3D(origin, direction); + + FramePoint3D pointToPack = new FramePoint3D(worldFrame); + plane.intersectionWith(line, pointToPack); + + FramePoint3D expectedIntersection = new FramePoint3D(worldFrame, 1.0, 1.0, 0.0); + assertTrue(pointToPack.epsilonEquals(expectedIntersection, epsilon)); + } +} diff --git a/src/test/java/us/ihmc/euclid/referenceFrame/Pose2DReferenceFrameTest.java b/src/test/java/us/ihmc/euclid/referenceFrame/Pose2DReferenceFrameTest.java new file mode 100644 index 000000000..31845df95 --- /dev/null +++ b/src/test/java/us/ihmc/euclid/referenceFrame/Pose2DReferenceFrameTest.java @@ -0,0 +1,164 @@ + +package us.ihmc.euclid.referenceFrame; + +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.Test; +import us.ihmc.euclid.referenceFrame.tools.EuclidFrameTestTools; +import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools; +import us.ihmc.euclid.tools.EuclidCoreRandomTools; +import us.ihmc.euclid.tuple2D.Point2D; + +import java.util.Random; + +import static org.junit.jupiter.api.Assertions.*; + +public class Pose2DReferenceFrameTest +{ + @AfterEach + public void tearDown() + { + ReferenceFrameTools.clearWorldFrameTree(); + } + + + @Test + public void testAsynchronousUpdatesOne() + { + ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); + Pose2DReferenceFrame poseFrame0 = new Pose2DReferenceFrame("poseFrame0", worldFrame); + Pose2DReferenceFrame poseFrame00 = new Pose2DReferenceFrame("poseFrame00", poseFrame0); + Pose2DReferenceFrame poseFrame000 = new Pose2DReferenceFrame("poseFrame000", poseFrame00); + FramePoint3D framePoint = new FramePoint3D(poseFrame000, 1.0, 2.8, 4.4); + + Random random = new Random(1776L); + + doRandomPoseChangeAndUpdate(poseFrame0, random); + doRandomPoseChangeAndUpdate(poseFrame00, random); + doRandomPoseChangeAndUpdate(poseFrame000, random); + + // Make sure that after a pose change of an intermediate frame, that the point before and after is different. + FramePoint3D framePointInWorldOne = new FramePoint3D(framePoint); + framePointInWorldOne.changeFrame(worldFrame); + doRandomPoseChangeAndUpdate(poseFrame0, random); + FramePoint3D framePointInWorldTwo = new FramePoint3D(framePoint); + framePointInWorldTwo.changeFrame(worldFrame); + + assertFalse(framePointInWorldOne.epsilonEquals(framePointInWorldTwo, 1e-7)); + + // But make sure that additional updates doesn't make it any different. + poseFrame0.update(); + poseFrame00.update(); + poseFrame000.update(); + + FramePoint3D framePointInWorldThree = new FramePoint3D(framePoint); + framePointInWorldThree.changeFrame(worldFrame); + + EuclidFrameTestTools.assertEquals(framePointInWorldThree, framePointInWorldTwo, 1e-7); + } + + @Test + public void testLongChainEfficiency() + { + ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); + + int numberOfFramesInChain = 100; + + ReferenceFrame previousFrame = worldFrame; + for (int i=0; i P2_RO = Point2DReadOnly.class; private static final Class P2_BA = Point2DBasics.class; private static final Class D = double.class; + private static final Class B = boolean.class; + private static final Class I = int.class; private static final double EPSILON = 1.0e-12; @Test @@ -54,6 +56,11 @@ public void testAPIIsComplete() signaturesToIgnore.add(new MethodSignature("closestPoint2DsBetweenTwoLineSegment2Ds", D, D, D, D, D, D, D, D, P2_BA, P2_BA)); signaturesToIgnore.add(new MethodSignature("closestPoint3DsBetweenTwoLineSegment3Ds", D, D, D, D, D, D, D, D, D, D, D, D, P3_BA, P3_BA)); signaturesToIgnore.add(new MethodSignature("axisAngleFromZUpToVector3D", Vector3DReadOnly.class, AxisAngleBasics.class)); + signaturesToIgnore.add(new MethodSignature("getZOnPlane", Vector3DReadOnly.class, AxisAngleBasics.class)); + signaturesToIgnore.add(new MethodSignature("getZOnPlane", Point3DReadOnly.class, Vector3DReadOnly.class, D, D)); + signaturesToIgnore.add(new MethodSignature("isPoint2DOnLineSegment2D", D, D, Point2DReadOnly.class, Point2DReadOnly.class)); + signaturesToIgnore.add(new MethodSignature("intersectionBetweenLine2DAndCircle", D, D, D, D, D, B, D, D, B, Point2DBasics.class, Point2DBasics.class)); + signaturesToIgnore.add(new MethodSignature("doLineSegment2DAndConvexPolygon2DIntersect", Point2DReadOnly.class, Point2DReadOnly.class, List.class, I)); Predicate methodFilter = EuclidFrameAPITester.methodFilterFromSignature(signaturesToIgnore); EuclidFrameAPITester tester = new EuclidFrameAPITester(new EuclidFrameAPIDefaultConfiguration()); diff --git a/src/test/java/us/ihmc/euclid/tools/EuclidCoreToolsTest.java b/src/test/java/us/ihmc/euclid/tools/EuclidCoreToolsTest.java index ed9a8e511..a21b8800f 100644 --- a/src/test/java/us/ihmc/euclid/tools/EuclidCoreToolsTest.java +++ b/src/test/java/us/ihmc/euclid/tools/EuclidCoreToolsTest.java @@ -1,8 +1,11 @@ package us.ihmc.euclid.tools; import org.junit.jupiter.api.Test; +import us.ihmc.euclid.Axis3D; import us.ihmc.euclid.axisAngle.AxisAngle; +import us.ihmc.euclid.geometry.tools.EuclidGeometryTools; import us.ihmc.euclid.matrix.Matrix3D; +import us.ihmc.euclid.matrix.RotationMatrix; import us.ihmc.euclid.orientation.Orientation2D; import us.ihmc.euclid.orientation.interfaces.Orientation2DReadOnly; import us.ihmc.euclid.orientation.interfaces.Orientation3DBasics; @@ -13,7 +16,9 @@ import us.ihmc.euclid.tuple2D.interfaces.Tuple2DBasics; import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; import us.ihmc.euclid.tuple2D.interfaces.Vector2DBasics; +import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; import us.ihmc.euclid.tuple3D.Point3D; +import us.ihmc.euclid.tuple3D.UnitVector3D; import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.euclid.tuple3D.interfaces.Tuple3DBasics; import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; @@ -1121,4 +1126,456 @@ public void testIntegrate() EuclidCoreTestTools.assertEquals(expectedCurrTransform, actualCurrTransform, 1.0e-12); } } + + private static final int iters = 1000; + + + @Test + public void testProjectRotationOnAxis() + { + Random random = new Random(9429424L); + Quaternion fullRotation = new Quaternion(); + Quaternion actualRotation = new Quaternion(); + + for (int i = 0; i < 10000; i++) + { + // Create random axis and a rotation around that axis. + Vector3D axis = EuclidCoreRandomTools.nextVector3DWithFixedLength(random, 1.0); + double angle = EuclidCoreRandomTools.nextDouble(random, -Math.PI, Math.PI); + Quaternion expectedRotation = new Quaternion(new AxisAngle(axis, angle)); + + // Create an orthogonal rotation. + Vector3D orthogonalAxis = EuclidCoreRandomTools.nextOrthogonalVector3D(random, axis, true); + double orthogonalAngle = EuclidCoreRandomTools.nextDouble(random, -Math.PI, Math.PI); + Quaternion orthogonalRotation = new Quaternion(new AxisAngle(orthogonalAxis, orthogonalAngle)); + + // From the combined rotation and the original axis back out the rotation around the original axis. + fullRotation.multiply(orthogonalRotation, expectedRotation); + EuclidCoreTools.projectRotationOnAxis(fullRotation, axis, actualRotation); + EuclidCoreTestTools.assertOrientation3DGeometricallyEquals(expectedRotation, actualRotation, 1.0e-10); + } + } + + @Test + public void testRotationMatrix3DFromFirstToSecondVector3D() + { + Random random = new Random(43634); + + for (int i = 0; i < 10000; i++) + { + Vector3D firstVector = EuclidCoreRandomTools.nextVector3D(random); + Vector3D secondVector = EuclidCoreRandomTools.nextVector3D(random); + + RotationMatrix actualRotationMatrix = new RotationMatrix(); + AxisAngle actualAxisAngle = new AxisAngle(); + AxisAngle expectedAxisAngle = new AxisAngle(); + + EuclidGeometryTools.rotationMatrix3DFromFirstToSecondVector3D(firstVector, secondVector, actualRotationMatrix); + actualAxisAngle.set(actualRotationMatrix); + EuclidGeometryTools.orientation3DFromFirstToSecondVector3D(firstVector, secondVector, expectedAxisAngle); + EuclidCoreTestTools.assertOrientation3DGeometricallyEquals(expectedAxisAngle, actualAxisAngle, 1.0e-7); + } + } + + @Test + public void testDistanceBetweenTwoLineSegment2Ds() + { + Point2D closestPointOnLineSegment1 = new Point2D(); + Point2D closestPointOnLineSegment2 = new Point2D(); + + Vector2D lineSegmentDirection1 = new Vector2D(); + Vector2D lineSegmentDirection2 = new Vector2D(); + + Random random = new Random(11762L); + + // Parallel case, expecting expectedPointOnLineSegment1 = + // lineSegmentStart1 + for (int i = 0; i < iters; i++) + { + Point2D lineSegmentStart1 = EuclidCoreRandomTools.nextPoint2D(random); + lineSegmentStart1.scale(EuclidCoreRandomTools.nextDouble(random, 10.0)); + Point2D lineSegmentEnd1 = EuclidCoreRandomTools.nextPoint2D(random); + lineSegmentEnd1.scale(EuclidCoreRandomTools.nextDouble(random, 10.0)); + + lineSegmentDirection1.sub(lineSegmentEnd1, lineSegmentStart1); + lineSegmentDirection1.normalize(); + + // expectedPointOnLineSegment1 = lineSegmentStart1 + closestPointOnLineSegment1.set(lineSegmentStart1); + + // Create the closest point of line segment 2 + Vector2D orthogonalToLineSegment1 = nextOrthogonalVector2D(random, lineSegmentDirection1, true); + double expectedMinimumDistance = EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0); + closestPointOnLineSegment2.scaleAdd(expectedMinimumDistance, orthogonalToLineSegment1, closestPointOnLineSegment1); + + // Set the lineSegmentDirection2 = lineSegmentDirection1 + lineSegmentDirection2.set(lineSegmentDirection1); + + // Set the end points of the line segment 2 around the expected + // closest point. + Point2D lineSegmentStart2 = new Point2D(); + Point2D lineSegmentEnd2 = new Point2D(); + lineSegmentStart2.scaleAdd(EuclidCoreRandomTools.nextDouble(random, -10.0, 0.0), lineSegmentDirection2, closestPointOnLineSegment2); + lineSegmentEnd2.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0), lineSegmentDirection2, closestPointOnLineSegment2); + + double actualMinimumDistance = EuclidGeometryTools.distanceBetweenTwoLineSegment2Ds(lineSegmentStart1, + lineSegmentEnd1, + lineSegmentStart2, + lineSegmentEnd2); + assertEquals(expectedMinimumDistance, actualMinimumDistance, 1e-12); + + // Set the end points of the line segment 2 before the expected + // closest point, so we have expectedClosestPointOnLineSegment2 = + // lineSegmentEnd2 + double shiftStartFromExpected = EuclidCoreRandomTools.nextDouble(random, -20.0, -10.0); + double shiftEndFromExpected = EuclidCoreRandomTools.nextDouble(random, -10.0, 0.0); + lineSegmentStart2.scaleAdd(shiftStartFromExpected, lineSegmentDirection2, closestPointOnLineSegment2); + lineSegmentEnd2.scaleAdd(shiftEndFromExpected, lineSegmentDirection2, closestPointOnLineSegment2); + closestPointOnLineSegment2.set(lineSegmentEnd2); + expectedMinimumDistance = closestPointOnLineSegment1.distance(closestPointOnLineSegment2); + + actualMinimumDistance = EuclidGeometryTools.distanceBetweenTwoLineSegment2Ds(lineSegmentStart1, + lineSegmentEnd1, + lineSegmentStart2, + lineSegmentEnd2); + assertEquals(expectedMinimumDistance, actualMinimumDistance, 1e-12); + + actualMinimumDistance = EuclidGeometryTools.distanceBetweenTwoLineSegment2Ds(lineSegmentStart1, + lineSegmentEnd1, + lineSegmentEnd2, + lineSegmentStart2); + assertEquals(expectedMinimumDistance, actualMinimumDistance, 1e-12); + } + + // Case: on closest point on lineSegment1 outside end points. + for (int i = 0; i < iters; i++) + { + Point2D lineSegmentStart1 = EuclidCoreRandomTools.nextPoint2D(random); + lineSegmentStart1.scale(EuclidCoreRandomTools.nextDouble(random, 10.0)); + Point2D lineSegmentEnd1 = EuclidCoreRandomTools.nextPoint2D(random); + lineSegmentEnd1.scale(EuclidCoreRandomTools.nextDouble(random, 10.0)); + + lineSegmentDirection1.sub(lineSegmentEnd1, lineSegmentStart1); + lineSegmentDirection1.normalize(); + + // Put the first closest to the start of line segment 1 + closestPointOnLineSegment1.set(lineSegmentStart1); + + // Create the closest point of line segment 2 such that it reaches + // out of line segment 1 + Vector2D oppositeOflineSegmentDirection1 = new Vector2D(); + oppositeOflineSegmentDirection1.setAndNegate(lineSegmentDirection1); + Vector2D orthogonalToLineSegment1 = nextOrthogonalVector2D(random, lineSegmentDirection1, true); + Vector2D shiftVector = new Vector2D(); + shiftVector.interpolate(orthogonalToLineSegment1, oppositeOflineSegmentDirection1, EuclidCoreRandomTools.nextDouble(random, 0.0, 1.0)); + closestPointOnLineSegment2.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0), shiftVector, closestPointOnLineSegment1); + + // Set the line direction 2 to orthogonal to the shift vector + lineSegmentDirection2 = nextOrthogonalVector2D(random, shiftVector, true); + + // Set the end points of the line segment 2 around the expected + // closest point. + Point2D lineSegmentStart2 = new Point2D(); + Point2D lineSegmentEnd2 = new Point2D(); + lineSegmentStart2.scaleAdd(EuclidCoreRandomTools.nextDouble(random, -10.0, 0.0), lineSegmentDirection2, closestPointOnLineSegment2); + lineSegmentEnd2.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0), lineSegmentDirection2, closestPointOnLineSegment2); + double expectedMinimumDistance = closestPointOnLineSegment1.distance(closestPointOnLineSegment2); + + double actualMinimumDistance = EuclidGeometryTools.distanceBetweenTwoLineSegment2Ds(lineSegmentStart1, + lineSegmentEnd1, + lineSegmentStart2, + lineSegmentEnd2); + assertEquals(expectedMinimumDistance, actualMinimumDistance, 1e-12); + actualMinimumDistance = EuclidGeometryTools.distanceBetweenTwoLineSegment2Ds(lineSegmentStart1, + lineSegmentEnd1, + lineSegmentEnd2, + lineSegmentStart2); + assertEquals(expectedMinimumDistance, actualMinimumDistance, 1e-12); + actualMinimumDistance = EuclidGeometryTools.distanceBetweenTwoLineSegment2Ds(lineSegmentEnd1, + lineSegmentStart1, + lineSegmentStart2, + lineSegmentEnd2); + assertEquals(expectedMinimumDistance, actualMinimumDistance, 1e-12); + actualMinimumDistance = EuclidGeometryTools.distanceBetweenTwoLineSegment2Ds(lineSegmentEnd1, + lineSegmentStart1, + lineSegmentEnd2, + lineSegmentStart2); + assertEquals(expectedMinimumDistance, actualMinimumDistance, 1e-12); + } + + // Edge case: both closest points are outside bounds of each line + // segment + for (int i = 0; i < iters; i++) + { + Point2D lineSegmentStart1 = EuclidCoreRandomTools.nextPoint2D(random); + lineSegmentStart1.scale(EuclidCoreRandomTools.nextDouble(random, 10.0)); + Point2D lineSegmentEnd1 = EuclidCoreRandomTools.nextPoint2D(random); + lineSegmentEnd1.scale(EuclidCoreRandomTools.nextDouble(random, 10.0)); + + lineSegmentDirection1.sub(lineSegmentEnd1, lineSegmentStart1); + lineSegmentDirection1.normalize(); + + // Put the first closest to the start of line segment 1 + closestPointOnLineSegment1.set(lineSegmentStart1); + + // Create the closest point of line segment 2 such that it reaches + // out of line segment 1 + Vector2D oppositeOflineSegmentDirection1 = new Vector2D(); + oppositeOflineSegmentDirection1.setAndNegate(lineSegmentDirection1); + Vector2D orthogonalToLineSegment1 = nextOrthogonalVector2D(random, lineSegmentDirection1, true); + Vector2D shiftVector = new Vector2D(); + double alpha = EuclidCoreRandomTools.nextDouble(random, 0.0, 1.0); + shiftVector.interpolate(orthogonalToLineSegment1, oppositeOflineSegmentDirection1, alpha); + closestPointOnLineSegment2.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0), shiftVector, closestPointOnLineSegment1); + + // set the start of the second line segment to the expected closest + // point + Point2D lineSegmentStart2 = new Point2D(closestPointOnLineSegment2); + + // Set the line direction 2 to point somewhat in the same direction + // as the shift vector + Vector2D orthogonalToShiftVector = nextOrthogonalVector2D(random, shiftVector, true); + lineSegmentDirection2.interpolate(shiftVector, orthogonalToShiftVector, EuclidCoreRandomTools.nextDouble(random, 0.0, 1.0)); + + // Set the end points of the line segment 2 around the expected + // closest point. + Point2D lineSegmentEnd2 = new Point2D(); + double alpha2 = EuclidCoreRandomTools.nextDouble(random, 0.1, 10.0); + lineSegmentEnd2.scaleAdd(alpha2, lineSegmentDirection2, closestPointOnLineSegment2); + + double expectedMinimumDistance = closestPointOnLineSegment1.distance(closestPointOnLineSegment2); + double actualMinimumDistance = EuclidGeometryTools.distanceBetweenTwoLineSegment2Ds(lineSegmentStart1, + lineSegmentEnd1, + lineSegmentStart2, + lineSegmentEnd2); + assertEquals(expectedMinimumDistance, actualMinimumDistance, 1e-12); + actualMinimumDistance = EuclidGeometryTools.distanceBetweenTwoLineSegment2Ds(lineSegmentStart1, + lineSegmentEnd1, + lineSegmentEnd2, + lineSegmentStart2); + assertEquals(expectedMinimumDistance, actualMinimumDistance, 1e-12); + actualMinimumDistance = EuclidGeometryTools.distanceBetweenTwoLineSegment2Ds(lineSegmentEnd1, + lineSegmentStart1, + lineSegmentStart2, + lineSegmentEnd2); + assertEquals(expectedMinimumDistance, actualMinimumDistance, 1e-12); + actualMinimumDistance = EuclidGeometryTools.distanceBetweenTwoLineSegment2Ds(lineSegmentEnd1, + lineSegmentStart1, + lineSegmentEnd2, + lineSegmentStart2); + assertEquals(expectedMinimumDistance, actualMinimumDistance, 1e-12); + } + } + + @Test + public void testExtractNormalPart() + { + Random random = new Random(6457); + + for (int i = 0; i < iters; i++) + { // We build the normal and tangential parts of the vector and then assemble it and expect to get the normal part pack. + Vector3D normalAxis = EuclidCoreRandomTools.nextVector3DWithFixedLength(random, 1.0); + Vector3D tangentialAxis = EuclidCoreRandomTools.nextOrthogonalVector3D(random, normalAxis, true); + Vector3D normalPart = new Vector3D(); + Vector3D tangentialPart = new Vector3D(); + + double normalMagnitude = EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0); + double tangentialMagnitude = EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0); + normalPart.setAndScale(normalMagnitude, normalAxis); + tangentialPart.setAndScale(tangentialMagnitude, tangentialAxis); + + Vector3D input = new Vector3D(); + input.add(normalPart, tangentialPart); + + Vector3D actualNormalPart = new Vector3D(); + EuclidCoreTools.extractNormalPart(input, normalAxis, actualNormalPart); + EuclidCoreTestTools.assertEquals(normalPart, actualNormalPart, 1e-12); + + // Randomize the axis + normalAxis.scale(EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0)); + EuclidCoreTools.extractNormalPart(input, normalAxis, actualNormalPart); + EuclidCoreTestTools.assertEquals(normalPart, actualNormalPart, 1e-12); + } + } + + @Test + public void testExtractTangentialPart() + { + Random random = new Random(63457); + + for (int i = 0; i < iters; i++) + { // We build the normal and tangential parts of the vector and then assemble it and expect to get the normal part pack. + // Setting trivial setup using Axis3D + Axis3D normalAxis = EuclidCoreRandomTools.nextAxis3D(random); + Axis3D tangentialAxis = random.nextBoolean() ? normalAxis.next() : normalAxis.previous(); + Vector3D normalPart = new Vector3D(); + Vector3D tangentialPart = new Vector3D(); + + double normalMagnitude = EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0); + double tangentialMagnitude = EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0); + normalPart.setAndScale(normalMagnitude, normalAxis); + tangentialPart.setAndScale(tangentialMagnitude, tangentialAxis); + + Vector3D input = new Vector3D(); + input.add(normalPart, tangentialPart); + + Vector3D actualTangentialPart = new Vector3D(); + EuclidCoreTools.extractTangentialPart(input, normalAxis, actualTangentialPart); + EuclidCoreTestTools.assertEquals("Iteration: " + i, tangentialPart, actualTangentialPart, 1e-12); + } + + for (int i = 0; i < iters; i++) + { // We build the normal and tangential parts of the vector and then assemble it and expect to get the normal part pack. + Vector3D normalAxis = EuclidCoreRandomTools.nextVector3DWithFixedLength(random, 1.0); + Vector3D tangentialAxis = EuclidCoreRandomTools.nextOrthogonalVector3D(random, normalAxis, true); + Vector3D normalPart = new Vector3D(); + Vector3D tangentialPart = new Vector3D(); + + double normalMagnitude = EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0); + double tangentialMagnitude = EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0); + normalPart.setAndScale(normalMagnitude, normalAxis); + tangentialPart.setAndScale(tangentialMagnitude, tangentialAxis); + + Vector3D input = new Vector3D(); + input.add(normalPart, tangentialPart); + + Vector3D actualTangentialPart = new Vector3D(); + EuclidCoreTools.extractTangentialPart(input, normalAxis, actualTangentialPart); + EuclidCoreTestTools.assertEquals(tangentialPart, actualTangentialPart, 1e-12); + + // Randomize the axis + normalAxis.scale(EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0)); + EuclidCoreTools.extractTangentialPart(input, normalAxis, actualTangentialPart); + EuclidCoreTestTools.assertEquals(tangentialPart, actualTangentialPart, 1e-12); + } + } + + @Test + public void testIntersectionBetweenRay2DAndLine2D() + { + Point2D rayOrigin = new Point2D(8.689, 0.5687); + Point2D pointOnRay = new Point2D(8.6432, 0.4951); + Vector2D rayDirection = new Vector2D(); + rayDirection.sub(pointOnRay, rayOrigin); + + Point2D lineStart = new Point2D(8.4521, 0.4323); + Point2D lineEnd = new Point2D(8.776, 0.5267); + Vector2D lineDirection = new Vector2D(); + lineDirection.sub(lineEnd, lineStart); + + Point2D intersectionToPac = new Point2D(); + assertTrue(EuclidGeometryTools.intersectionBetweenRay2DAndLine2D(rayOrigin, rayDirection, lineStart, lineDirection, intersectionToPac)); + + Point2D intersectionExpected = new Point2D(); + EuclidGeometryTools.intersectionBetweenLine2DAndLineSegment2D(rayOrigin, rayDirection, lineStart, lineEnd, intersectionExpected); + + EuclidCoreTestTools.assertPoint2DGeometricallyEquals(intersectionExpected, intersectionToPac, 1e-5); + } + + @Test + public void testSetNormalPart() + { + Random random = new Random(897632); + + for (int i = 0; i < iters; i++) + { + // Setting trivial setup using Axis3D + Axis3D normalAxis = EuclidCoreRandomTools.nextAxis3D(random); + Axis3D tangentialAxis = random.nextBoolean() ? normalAxis.next() : normalAxis.previous(); + Vector3D normalPart = new Vector3D(); + Vector3D tangentialPart = new Vector3D(); + double normalMagnitude = EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0); + double tangentialMagnitude = EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0); + normalPart.setAndScale(normalMagnitude, normalAxis); + tangentialPart.setAndScale(tangentialMagnitude, tangentialAxis); + + Point3D input = new Point3D(normalPart); + input.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), tangentialPart, input); + + Vector3D tupleToModify = new Vector3D(tangentialPart); + tupleToModify.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), normalPart, tupleToModify); + + Vector3D expected = new Vector3D(); + expected.add(normalPart, tangentialPart); + + EuclidCoreTools.setNormalPart(input, normalAxis, tupleToModify); + EuclidCoreTestTools.assertEquals(expected, tupleToModify, 1e-12); + } + + for (int i = 0; i < iters; i++) + { + Vector3D normalAxis = EuclidCoreRandomTools.nextVector3DWithFixedLength(random, 1.0); + Vector3D tangentialAxis = EuclidCoreRandomTools.nextOrthogonalVector3D(random, normalAxis, true); + Vector3D normalPart = new Vector3D(); + Vector3D tangentialPart = new Vector3D(); + double normalMagnitude = EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0); + double tangentialMagnitude = EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0); + normalPart.setAndScale(normalMagnitude, normalAxis); + tangentialPart.setAndScale(tangentialMagnitude, tangentialAxis); + + Point3D input = new Point3D(normalPart); + input.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), tangentialPart, input); + + Vector3D tupleToModify = new Vector3D(tangentialPart); + tupleToModify.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), normalPart, tupleToModify); + + Vector3D expected = new Vector3D(); + expected.add(normalPart, tangentialPart); + + EuclidCoreTools.setNormalPart(input, normalAxis, tupleToModify); + EuclidCoreTestTools.assertEquals(expected, tupleToModify, 1e-12); + + // Randomize the axis + normalAxis.scale(EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0)); + EuclidCoreTools.setNormalPart(input, normalAxis, tupleToModify); + EuclidCoreTestTools.assertEquals(expected, tupleToModify, 1e-12); + } + } + + @Test + public void testDifferentiateOrientation() + { + Random random = new Random(23423); + + for (int i = 0; i < iters; i++) + { + Quaternion qStart = EuclidCoreRandomTools.nextQuaternion(random); + double duration = EuclidCoreRandomTools.nextDouble(random, 0.0, 1.0e-2); + double angle = EuclidCoreRandomTools.nextDouble(random, 0.0, Math.PI); + UnitVector3D velocityAxis = EuclidCoreRandomTools.nextUnitVector3D(random); + Vector3D expectedAngularVelocity = new Vector3D(); + expectedAngularVelocity.setAndScale(angle / duration, velocityAxis); + + Quaternion qEnd = new Quaternion(); + qEnd.setRotationVector(velocityAxis.getX() * angle, velocityAxis.getY() * angle, velocityAxis.getZ() * angle); + qEnd.prepend(qStart); + + Vector3D actualAngularVelocity = new Vector3D(); + EuclidCoreTools.differentiateOrientation(qStart, qEnd, duration, actualAngularVelocity); + + EuclidCoreTestTools.assertEquals(expectedAngularVelocity, actualAngularVelocity, 1e-12 * Math.max(1.0, expectedAngularVelocity.norm())); + } + } + + /** + * Generates a random vector that is perpendicular to {@code vectorToBeOrthogonalTo}. + * + * @param random the random generator to use. + * @param vectorToBeOrthogonalTo the vector to be orthogonal to. Not modified. + * @param normalize whether to normalize the generated vector or not. + * @return the random vector. + */ + public static Vector2D nextOrthogonalVector2D(Random random, Vector2DReadOnly vectorToBeOrthogonalTo, boolean normalize) + { + Vector2D v1 = new Vector2D(vectorToBeOrthogonalTo.getY(), -vectorToBeOrthogonalTo.getX()); + + Vector2D randomPerpendicular = new Vector2D(); + double a = EuclidCoreRandomTools.nextDouble(random, 1.0); + randomPerpendicular.scaleAdd(a, v1, randomPerpendicular); + + if (normalize) + randomPerpendicular.normalize(); + + return randomPerpendicular; + } }