From 61bd4a054c08a82f38f8d4f603968e996d693281 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Mon, 14 Mar 2022 12:57:58 -0500 Subject: [PATCH 1/7] Formatted all the code. --- .../referenceFrame/FrameEllipsoid3D.java | 6 +- .../EuclidFrameShapeCollisionTools.java | 57 +- .../epa/FrameExpandingPolytopeAlgorithm.java | 18 +- ...ilbertJohnsonKeerthiCollisionDetector.java | 18 +- ...idFrameShape3DCollisionResultReadOnly.java | 4 +- .../interfaces/FixedFrameBox3DBasics.java | 6 +- .../FixedFrameEllipsoid3DBasics.java | 6 +- .../interfaces/FixedFrameRamp3DBasics.java | 6 +- .../interfaces/FrameBox3DBasics.java | 6 +- .../interfaces/FrameBox3DReadOnly.java | 20 +- .../interfaces/FrameCylinder3DReadOnly.java | 20 +- .../interfaces/FrameEllipsoid3DBasics.java | 6 +- .../interfaces/FrameEllipsoid3DReadOnly.java | 20 +- .../interfaces/FrameRamp3DBasics.java | 6 +- .../interfaces/FrameShape3DReadOnly.java | 15 +- .../interfaces/FrameSphere3DReadOnly.java | 20 +- .../referenceFrame/polytope/FrameFace3D.java | 4 +- .../tools/EuclidFrameShapeRandomTools.java | 94 +- .../tools/EuclidFrameShapeTestTools.java | 189 ++- .../tools/EuclidFrameShapeTools.java | 183 ++- .../referenceFrame/FrameRotationMatrix.java | 10 +- .../api/EuclidFrameAPITester.java | 66 +- .../FrameBoundingBox2DReadOnly.java | 82 +- .../FrameBoundingBox3DReadOnly.java | 82 +- .../interfaces/FrameCommonMatrix3DBasics.java | 10 +- .../FrameConvexPolygon2DReadOnly.java | 15 +- .../interfaces/FrameLine2DReadOnly.java | 15 +- .../interfaces/FrameLine3DBasics.java | 9 +- .../interfaces/FrameLine3DReadOnly.java | 15 +- .../FrameLineSegment2DReadOnly.java | 15 +- .../tools/EuclidFrameFactories.java | 40 +- .../tools/EuclidFrameRandomTools.java | 25 +- .../tools/EuclidFrameTestTools.java | 175 ++- .../tools/EuclidFrameTools.java | 963 +++++++++---- .../ihmc/euclid/geometry/LineSegment3D.java | 6 +- .../exceptions/BoundingBoxException.java | 6 +- .../interfaces/BoundingBox2DReadOnly.java | 12 +- .../interfaces/BoundingBox3DReadOnly.java | 12 +- .../tools/EuclidGeometryFactories.java | 6 +- .../tools/EuclidGeometryPolygonTools.java | 226 +++- .../tools/EuclidGeometryTestTools.java | 71 +- .../geometry/tools/EuclidGeometryTools.java | 1188 +++++++++++++---- .../java/us/ihmc/euclid/matrix/Matrix3D.java | 4 +- .../AxisAngleConversion.java | 10 +- .../QuaternionConversion.java | 10 +- .../RotationVectorConversion.java | 10 +- .../YawPitchRollConversion.java | 10 +- .../us/ihmc/euclid/tools/AxisAngleTools.java | 40 +- .../euclid/tools/EuclidCoreFactories.java | 21 +- .../ihmc/euclid/tools/EuclidCoreIOTools.java | 15 +- .../euclid/tools/EuclidCoreTestTools.java | 96 +- .../ihmc/euclid/tools/Matrix3DFeatures.java | 30 +- .../us/ihmc/euclid/tools/Matrix3DTools.java | 27 +- .../us/ihmc/euclid/tools/QuaternionTools.java | 73 +- .../euclid/tools/RotationMatrixTools.java | 44 +- .../tools/SingularValueDecomposition3D.java | 11 +- .../ihmc/euclid/tools/YawPitchRollTools.java | 74 +- .../euclid/transform/RigidBodyTransform.java | 36 +- .../interfaces/AffineTransformBasics.java | 12 +- .../collision/EuclidShapeCollisionTools.java | 25 +- .../euclid/shape/collision/epa/EPATools.java | 9 +- .../epa/ExpandingPolytopeAlgorithm.java | 4 +- .../euclid/shape/collision/gjk/GJKTools.java | 24 +- .../EuclidPolytopeConstructionTools.java | 13 +- .../tools/EuclidPolytopeTools.java | 69 +- .../primitives/interfaces/Box3DReadOnly.java | 4 +- .../interfaces/Cylinder3DReadOnly.java | 4 +- .../interfaces/Ellipsoid3DReadOnly.java | 4 +- .../interfaces/Sphere3DReadOnly.java | 4 +- .../shape/tools/EuclidEllipsoid3DTools.java | 16 +- .../shape/tools/EuclidShapeRandomTools.java | 40 +- .../shape/tools/EuclidShapeTestTools.java | 106 +- .../euclid/shape/tools/EuclidShapeTools.java | 196 ++- .../geometry/ConvexPolygon2DBasicsTest.java | 2 +- .../euclid/geometry/LineSegment2DTest.java | 14 +- .../euclid/geometry/Triangle3DBasicsTest.java | 67 +- .../ihmc/euclid/geometry/Triangle3DTest.java | 146 +- .../tools/EuclidGeometryFactoriesTest.java | 6 +- .../tools/EuclidGeometryTestToolsTest.java | 6 +- .../tools/EuclidGeometryToolsTest.java | 43 +- .../us/ihmc/euclid/matrix/Matrix3DTest.java | 12 +- .../EuclidFrameShapeCollisionToolsTest.java | 7 +- .../FrameExpandingPolytopeAlgorithmTest.java | 4 +- ...rtJohnsonKeerthiCollisionDetectorTest.java | 4 +- .../tools/EuclidFrameFactoriesTest.java | 6 +- .../tools/EuclidFrameTestToolsTest.java | 6 +- .../EuclidShapeCollisionToolsTest.java | 5 +- .../ExpandingPolytopeAlgorithmTest.java | 7 +- ...rtJohnsonKeerthiCollisionDetectorTest.java | 16 +- .../shape/tools/EuclidShapeToolsTest.java | 40 +- .../euclid/tools/EuclidCoreTestToolsTest.java | 15 +- .../ihmc/euclid/tools/Matrix3DToolsTest.java | 6 +- .../SymmetricEigenDecomposition3DTest.java | 9 +- .../euclid/transform/AffineTransformTest.java | 14 +- 94 files changed, 3843 insertions(+), 1386 deletions(-) diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/FrameEllipsoid3D.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/FrameEllipsoid3D.java index 23557fd1a..833fa9c3e 100644 --- a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/FrameEllipsoid3D.java +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/FrameEllipsoid3D.java @@ -104,7 +104,11 @@ public FrameEllipsoid3D(ReferenceFrame referenceFrame, Vector3DReadOnly radii) * @param radiusZ the size of the ellipsoid along the z-axis. * @throws IllegalArgumentException if any of the three radii is negative. */ - public FrameEllipsoid3D(ReferenceFrame referenceFrame, Point3DReadOnly position, Orientation3DReadOnly orientation, double radiusX, double radiusY, + public FrameEllipsoid3D(ReferenceFrame referenceFrame, + Point3DReadOnly position, + Orientation3DReadOnly orientation, + double radiusX, + double radiusY, double radiusZ) { setIncludingFrame(referenceFrame, position, orientation, radiusX, radiusY, radiusZ); diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/collision/EuclidFrameShapeCollisionTools.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/collision/EuclidFrameShapeCollisionTools.java index 1569e33ca..b5334bb81 100644 --- a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/collision/EuclidFrameShapeCollisionTools.java +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/collision/EuclidFrameShapeCollisionTools.java @@ -43,7 +43,8 @@ private EuclidFrameShapeCollisionTools() * @param shapeB the box. Not modified. * @param resultToPack the object in which the collision result is stored. Modified. */ - public static void evaluatePointShape3DBox3DCollision(FramePointShape3DReadOnly shapeA, FrameBox3DReadOnly shapeB, + public static void evaluatePointShape3DBox3DCollision(FramePointShape3DReadOnly shapeA, + FrameBox3DReadOnly shapeB, EuclidFrameShape3DCollisionResultBasics resultToPack) { evaluatePoint3DBox3DCollision(shapeA, shapeB, resultToPack); @@ -58,7 +59,8 @@ public static void evaluatePointShape3DBox3DCollision(FramePointShape3DReadOnly * @param shapeB the box. Not modified. * @param resultToPack the object in which the collision result is stored. Modified. */ - public static void evaluateSphere3DBox3DCollision(FrameSphere3DReadOnly shapeA, FrameBox3DReadOnly shapeB, + public static void evaluateSphere3DBox3DCollision(FrameSphere3DReadOnly shapeA, + FrameBox3DReadOnly shapeB, EuclidFrameShape3DCollisionResultBasics resultToPack) { evaluatePoint3DBox3DCollision(shapeA.getPosition(), shapeB, resultToPack); @@ -72,7 +74,8 @@ public static void evaluateSphere3DBox3DCollision(FrameSphere3DReadOnly shapeA, resultToPack.setShapesAreColliding(distance < 0.0); } - private static void evaluatePoint3DBox3DCollision(FramePoint3DReadOnly point3D, FrameBox3DReadOnly box3D, + private static void evaluatePoint3DBox3DCollision(FramePoint3DReadOnly point3D, + FrameBox3DReadOnly box3D, EuclidFrameShape3DCollisionResultBasics resultToPack) { FramePoint3DBasics pointOnA = resultToPack.getPointOnA(); @@ -111,7 +114,8 @@ private static void evaluatePoint3DBox3DCollision(FramePoint3DReadOnly point3D, * @param shapeB the capsule. Not modified. * @param resultToPack the object in which the collision result is stored. Modified. */ - public static void evaluatePointShape3DCapsule3DCollision(FramePointShape3DReadOnly shapeA, FrameCapsule3DReadOnly shapeB, + public static void evaluatePointShape3DCapsule3DCollision(FramePointShape3DReadOnly shapeA, + FrameCapsule3DReadOnly shapeB, EuclidFrameShape3DCollisionResultBasics resultToPack) { evaluatePoint3DCapsule3DCollision(shapeA, shapeB, resultToPack); @@ -126,7 +130,8 @@ public static void evaluatePointShape3DCapsule3DCollision(FramePointShape3DReadO * @param shapeB the second capsule. Not modified. * @param resultToPack the object in which the collision result is stored. Modified. */ - public static void evaluateCapsule3DCapsule3DCollision(FrameCapsule3DReadOnly shapeA, FrameCapsule3DReadOnly shapeB, + public static void evaluateCapsule3DCapsule3DCollision(FrameCapsule3DReadOnly shapeA, + FrameCapsule3DReadOnly shapeB, EuclidFrameShape3DCollisionResultBasics resultToPack) { ReferenceFrame frameB = shapeB.getReferenceFrame(); @@ -195,7 +200,8 @@ public static void evaluateCapsule3DCapsule3DCollision(FrameCapsule3DReadOnly sh * @param shapeB the capsule. Not modified. * @param resultToPack the object in which the collision result is stored. Modified. */ - public static void evaluateSphere3DCapsule3DCollision(FrameSphere3DReadOnly shapeA, FrameCapsule3DReadOnly shapeB, + public static void evaluateSphere3DCapsule3DCollision(FrameSphere3DReadOnly shapeA, + FrameCapsule3DReadOnly shapeB, EuclidFrameShape3DCollisionResultBasics resultToPack) { evaluatePoint3DCapsule3DCollision(shapeA.getPosition(), shapeB, resultToPack); @@ -209,7 +215,8 @@ public static void evaluateSphere3DCapsule3DCollision(FrameSphere3DReadOnly shap resultToPack.setSignedDistance(distance); } - private static void evaluatePoint3DCapsule3DCollision(FramePoint3DReadOnly point3D, FrameCapsule3DReadOnly capsule3D, + private static void evaluatePoint3DCapsule3DCollision(FramePoint3DReadOnly point3D, + FrameCapsule3DReadOnly capsule3D, EuclidFrameShape3DCollisionResultBasics resultToPack) { FramePoint3DBasics pointOnA = resultToPack.getPointOnA(); @@ -245,7 +252,8 @@ private static void evaluatePoint3DCapsule3DCollision(FramePoint3DReadOnly point * @param shapeB the cylinder. Not modified. * @param resultToPack the object in which the collision result is stored. Modified. */ - public static void evaluatePointShape3DCylinder3DCollision(FramePointShape3DReadOnly shapeA, FrameCylinder3DReadOnly shapeB, + public static void evaluatePointShape3DCylinder3DCollision(FramePointShape3DReadOnly shapeA, + FrameCylinder3DReadOnly shapeB, EuclidFrameShape3DCollisionResultBasics resultToPack) { evaluatePoint3DCylinder3DCollision(shapeA, shapeB, resultToPack); @@ -260,7 +268,8 @@ public static void evaluatePointShape3DCylinder3DCollision(FramePointShape3DRead * @param shapeB the cylinder. Not modified. * @param resultToPack the object in which the collision result is stored. Modified. */ - public static void evaluateSphere3DCylinder3DCollision(FrameSphere3DReadOnly shapeA, FrameCylinder3DReadOnly shapeB, + public static void evaluateSphere3DCylinder3DCollision(FrameSphere3DReadOnly shapeA, + FrameCylinder3DReadOnly shapeB, EuclidFrameShape3DCollisionResultBasics resultToPack) { evaluatePoint3DCylinder3DCollision(shapeA.getPosition(), shapeB, resultToPack); @@ -274,7 +283,8 @@ public static void evaluateSphere3DCylinder3DCollision(FrameSphere3DReadOnly sha resultToPack.setSignedDistance(distance); } - private static void evaluatePoint3DCylinder3DCollision(FramePoint3DReadOnly point3D, FrameCylinder3DReadOnly cylinder3D, + private static void evaluatePoint3DCylinder3DCollision(FramePoint3DReadOnly point3D, + FrameCylinder3DReadOnly cylinder3D, EuclidFrameShape3DCollisionResultBasics resultToPack) { FramePoint3DBasics pointOnA = resultToPack.getPointOnA(); @@ -310,7 +320,8 @@ private static void evaluatePoint3DCylinder3DCollision(FramePoint3DReadOnly poin * @param shapeB the ellipsoid. Not modified. * @param resultToPack the object in which the collision result is stored. Modified. */ - public static void evaluatePointShape3DEllipsoid3DCollision(FramePointShape3DReadOnly shapeA, FrameEllipsoid3DReadOnly shapeB, + public static void evaluatePointShape3DEllipsoid3DCollision(FramePointShape3DReadOnly shapeA, + FrameEllipsoid3DReadOnly shapeB, EuclidFrameShape3DCollisionResultBasics resultToPack) { evaluatePoint3DEllipsoid3DCollision(shapeA, shapeB, resultToPack); @@ -325,7 +336,8 @@ public static void evaluatePointShape3DEllipsoid3DCollision(FramePointShape3DRea * @param shapeB the ellipsoid. Not modified. * @param resultToPack the object in which the collision result is stored. Modified. */ - public static void evaluateSphere3DEllipsoid3DCollision(FrameSphere3DReadOnly shapeA, FrameEllipsoid3DReadOnly shapeB, + public static void evaluateSphere3DEllipsoid3DCollision(FrameSphere3DReadOnly shapeA, + FrameEllipsoid3DReadOnly shapeB, EuclidFrameShape3DCollisionResultBasics resultToPack) { evaluatePoint3DEllipsoid3DCollision(shapeA.getPosition(), shapeB, resultToPack); @@ -339,7 +351,8 @@ public static void evaluateSphere3DEllipsoid3DCollision(FrameSphere3DReadOnly sh resultToPack.setSignedDistance(distance); } - private static void evaluatePoint3DEllipsoid3DCollision(FramePoint3DReadOnly point3D, FrameEllipsoid3DReadOnly ellipsoid3D, + private static void evaluatePoint3DEllipsoid3DCollision(FramePoint3DReadOnly point3D, + FrameEllipsoid3DReadOnly ellipsoid3D, EuclidFrameShape3DCollisionResultBasics resultToPack) { FramePoint3DBasics pointOnA = resultToPack.getPointOnA(); @@ -381,7 +394,8 @@ private static void evaluatePoint3DEllipsoid3DCollision(FramePoint3DReadOnly poi * @param shapeB the second point shape. Not modified. * @param resultToPack the object in which the collision result is stored. Modified. */ - public static void evaluatePointShape3DPointShape3DCollision(FramePointShape3DReadOnly shapeA, FramePointShape3DReadOnly shapeB, + public static void evaluatePointShape3DPointShape3DCollision(FramePointShape3DReadOnly shapeA, + FramePointShape3DReadOnly shapeB, EuclidFrameShape3DCollisionResultBasics resultToPack) { FramePoint3DBasics pointOnA = resultToPack.getPointOnA(); @@ -412,7 +426,8 @@ public static void evaluatePointShape3DPointShape3DCollision(FramePointShape3DRe * @param shapeB the ramp. Not modified. * @param resultToPack the object in which the collision result is stored. Modified. */ - public static void evaluatePointShape3DRamp3DCollision(FramePointShape3DReadOnly shapeA, FrameRamp3DReadOnly shapeB, + public static void evaluatePointShape3DRamp3DCollision(FramePointShape3DReadOnly shapeA, + FrameRamp3DReadOnly shapeB, EuclidFrameShape3DCollisionResultBasics resultToPack) { evaluatePoint3DRamp3DCollision(shapeA, shapeB, resultToPack); @@ -427,7 +442,8 @@ public static void evaluatePointShape3DRamp3DCollision(FramePointShape3DReadOnly * @param shapeB the ramp. Not modified. * @param resultToPack the object in which the collision result is stored. Modified. */ - public static void evaluateSphere3DRamp3DCollision(FrameSphere3DReadOnly shapeA, FrameRamp3DReadOnly shapeB, + public static void evaluateSphere3DRamp3DCollision(FrameSphere3DReadOnly shapeA, + FrameRamp3DReadOnly shapeB, EuclidFrameShape3DCollisionResultBasics resultToPack) { evaluatePoint3DRamp3DCollision(shapeA.getPosition(), shapeB, resultToPack); @@ -441,7 +457,8 @@ public static void evaluateSphere3DRamp3DCollision(FrameSphere3DReadOnly shapeA, resultToPack.setSignedDistance(distance); } - private static void evaluatePoint3DRamp3DCollision(FramePoint3DReadOnly point3D, FrameRamp3DReadOnly ramp3D, + private static void evaluatePoint3DRamp3DCollision(FramePoint3DReadOnly point3D, + FrameRamp3DReadOnly ramp3D, EuclidFrameShape3DCollisionResultBasics resultToPack) { FramePoint3DBasics pointOnA = resultToPack.getPointOnA(); @@ -479,7 +496,8 @@ private static void evaluatePoint3DRamp3DCollision(FramePoint3DReadOnly point3D, * @param shapeB the sphere. Not modified. * @param resultToPack the object in which the collision result is stored. Modified. */ - public static void evaluatePointShape3DSphere3DCollision(FramePointShape3DReadOnly shapeA, FrameSphere3DReadOnly shapeB, + public static void evaluatePointShape3DSphere3DCollision(FramePointShape3DReadOnly shapeA, + FrameSphere3DReadOnly shapeB, EuclidFrameShape3DCollisionResultBasics resultToPack) { FramePoint3DBasics pointOnA = resultToPack.getPointOnA(); @@ -510,7 +528,8 @@ public static void evaluatePointShape3DSphere3DCollision(FramePointShape3DReadOn * @param shapeB the second sphere. Not modified. * @param resultToPack the object in which the collision result is stored. Modified. */ - public static void evaluateSphere3DSphere3DCollision(FrameSphere3DReadOnly shapeA, FrameSphere3DReadOnly shapeB, + public static void evaluateSphere3DSphere3DCollision(FrameSphere3DReadOnly shapeA, + FrameSphere3DReadOnly shapeB, EuclidFrameShape3DCollisionResultBasics resultToPack) { FramePoint3DBasics pointOnA = resultToPack.getPointOnA(); diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/collision/epa/FrameExpandingPolytopeAlgorithm.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/collision/epa/FrameExpandingPolytopeAlgorithm.java index ec50eae68..7ce0702c4 100644 --- a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/collision/epa/FrameExpandingPolytopeAlgorithm.java +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/collision/epa/FrameExpandingPolytopeAlgorithm.java @@ -303,7 +303,8 @@ public EuclidFrameShape3DCollisionResult evaluateCollision(SupportingFrameVertex * @param resultToPack the object in which the collision result is stored. Modified. * @return {@code true} if the shapes are colliding, {@code false} otherwise. */ - public boolean evaluateCollision(SupportingFrameVertexHolder shapeA, SupportingFrameVertexHolder shapeB, + public boolean evaluateCollision(SupportingFrameVertexHolder shapeA, + SupportingFrameVertexHolder shapeB, EuclidFrameShape3DCollisionResultBasics resultToPack) { shapeA.getReferenceFrame().getTransformToDesiredFrame(transform, shapeB.getReferenceFrame()); @@ -315,21 +316,28 @@ public boolean evaluateCollision(SupportingFrameVertexHolder shapeA, SupportingF return areColliding; } - private boolean evaluateCollision(ReferenceFrame detectorFrame, SupportingVertexHolder shapeA, SupportingVertexHolder shapeB, - RigidBodyTransformReadOnly transformFromAToB, EuclidFrameShape3DCollisionResultBasics resultToPack) + private boolean evaluateCollision(ReferenceFrame detectorFrame, + SupportingVertexHolder shapeA, + SupportingVertexHolder shapeB, + RigidBodyTransformReadOnly transformFromAToB, + EuclidFrameShape3DCollisionResultBasics resultToPack) { supportingVertexTransformer.initialize(shapeA, transformFromAToB); return evaluateCollision(detectorFrame, supportingVertexTransformer, shapeB, resultToPack); } - private boolean evaluateCollision(ReferenceFrame detectorFrame, SupportingVertexHolder shapeA, SupportingVertexHolder shapeB, + private boolean evaluateCollision(ReferenceFrame detectorFrame, + SupportingVertexHolder shapeA, + SupportingVertexHolder shapeB, EuclidFrameShape3DCollisionResultBasics resultToPack) { initializeGJK(detectorFrame); return epaAlgorithm.evaluateCollision(shapeA, shapeB, resultToPack); } - private boolean evaluateCollision(ReferenceFrame detectorFrame, Shape3DReadOnly shapeA, Shape3DReadOnly shapeB, + private boolean evaluateCollision(ReferenceFrame detectorFrame, + Shape3DReadOnly shapeA, + Shape3DReadOnly shapeB, EuclidFrameShape3DCollisionResultBasics resultToPack) { initializeGJK(detectorFrame); diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/collision/gjk/FrameGilbertJohnsonKeerthiCollisionDetector.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/collision/gjk/FrameGilbertJohnsonKeerthiCollisionDetector.java index 542a0cdf0..4c5efffd8 100644 --- a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/collision/gjk/FrameGilbertJohnsonKeerthiCollisionDetector.java +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/collision/gjk/FrameGilbertJohnsonKeerthiCollisionDetector.java @@ -291,7 +291,8 @@ public EuclidFrameShape3DCollisionResult evaluateCollision(SupportingFrameVertex * @param resultToPack the object in which the collision result is stored. Modified. * @return {@code true} if the shapes are colliding, {@code false} otherwise. */ - public boolean evaluateCollision(SupportingFrameVertexHolder shapeA, SupportingFrameVertexHolder shapeB, + public boolean evaluateCollision(SupportingFrameVertexHolder shapeA, + SupportingFrameVertexHolder shapeB, EuclidFrameShape3DCollisionResultBasics resultToPack) { shapeA.getReferenceFrame().getTransformToDesiredFrame(transform, shapeB.getReferenceFrame()); @@ -301,21 +302,28 @@ public boolean evaluateCollision(SupportingFrameVertexHolder shapeA, SupportingF return areColliding; } - private boolean evaluateCollision(ReferenceFrame detectorFrame, SupportingVertexHolder shapeA, SupportingVertexHolder shapeB, - RigidBodyTransformReadOnly transformFromAToB, EuclidFrameShape3DCollisionResultBasics resultToPack) + private boolean evaluateCollision(ReferenceFrame detectorFrame, + SupportingVertexHolder shapeA, + SupportingVertexHolder shapeB, + RigidBodyTransformReadOnly transformFromAToB, + EuclidFrameShape3DCollisionResultBasics resultToPack) { supportingVertexTransformer.initialize(shapeA, transformFromAToB); return evaluateCollision(detectorFrame, supportingVertexTransformer, shapeB, resultToPack); } - private boolean evaluateCollision(ReferenceFrame detectorFrame, SupportingVertexHolder shapeA, SupportingVertexHolder shapeB, + private boolean evaluateCollision(ReferenceFrame detectorFrame, + SupportingVertexHolder shapeA, + SupportingVertexHolder shapeB, EuclidFrameShape3DCollisionResultBasics resultToPack) { initializeGJK(detectorFrame); return gjkCollisionDetector.evaluateCollision(shapeA, shapeB, resultToPack); } - private boolean evaluateCollision(ReferenceFrame detectorFrame, Shape3DReadOnly shapeA, Shape3DReadOnly shapeB, + private boolean evaluateCollision(ReferenceFrame detectorFrame, + Shape3DReadOnly shapeA, + Shape3DReadOnly shapeB, EuclidFrameShape3DCollisionResultBasics resultToPack) { initializeGJK(detectorFrame); diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/collision/interfaces/EuclidFrameShape3DCollisionResultReadOnly.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/collision/interfaces/EuclidFrameShape3DCollisionResultReadOnly.java index 7914857db..252a2b8a5 100644 --- a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/collision/interfaces/EuclidFrameShape3DCollisionResultReadOnly.java +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/collision/interfaces/EuclidFrameShape3DCollisionResultReadOnly.java @@ -89,7 +89,9 @@ default boolean geometricallyEquals(EuclidFrameShape3DCollisionResultReadOnly ot * @return {@code true} if the two collision results are considered geometrically similar, * {@code false} otherwise. */ - default boolean geometricallyEquals(EuclidFrameShape3DCollisionResultReadOnly other, double distanceEpsilon, double pointTangentialEpsilon, + default boolean geometricallyEquals(EuclidFrameShape3DCollisionResultReadOnly other, + double distanceEpsilon, + double pointTangentialEpsilon, double normalEpsilon) { boolean swap; diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FixedFrameBox3DBasics.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FixedFrameBox3DBasics.java index 8ca8ae3ad..c227fa143 100644 --- a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FixedFrameBox3DBasics.java +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FixedFrameBox3DBasics.java @@ -400,7 +400,11 @@ default void setMatchingFrame(FrameBox3DReadOnly other) * @param sizeZ the size along the z-axis. * @throws IllegalArgumentException if any of the three size arguments is negative. */ - default void setMatchingFrame(ReferenceFrame referenceFrame, Point3DReadOnly position, Orientation3DReadOnly orientation, double sizeX, double sizeY, + default void setMatchingFrame(ReferenceFrame referenceFrame, + Point3DReadOnly position, + Orientation3DReadOnly orientation, + double sizeX, + double sizeY, double sizeZ) { set(position, orientation, sizeX, sizeY, sizeZ); diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FixedFrameEllipsoid3DBasics.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FixedFrameEllipsoid3DBasics.java index 22efaa853..ebdd84ff1 100644 --- a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FixedFrameEllipsoid3DBasics.java +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FixedFrameEllipsoid3DBasics.java @@ -401,7 +401,11 @@ default void setMatchingFrame(FrameEllipsoid3DReadOnly other) * @param radiusZ radius of the ellipsoid along the z-axis. * @throws IllegalArgumentException if any of the three radii is negative. */ - default void setMatchingFrame(ReferenceFrame referenceFrame, Point3DReadOnly position, Orientation3DReadOnly orientation, double radiusX, double radiusY, + default void setMatchingFrame(ReferenceFrame referenceFrame, + Point3DReadOnly position, + Orientation3DReadOnly orientation, + double radiusX, + double radiusY, double radiusZ) { set(position, orientation, radiusX, radiusY, radiusZ); diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FixedFrameRamp3DBasics.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FixedFrameRamp3DBasics.java index b660328af..b6536aae5 100644 --- a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FixedFrameRamp3DBasics.java +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FixedFrameRamp3DBasics.java @@ -408,7 +408,11 @@ default void setMatchingFrame(FrameRamp3DReadOnly other) * @param sizeZ the size along the z-axis. * @throws IllegalArgumentException if any of the three size arguments is negative. */ - default void setMatchingFrame(ReferenceFrame referenceFrame, Point3DReadOnly position, Orientation3DReadOnly orientation, double sizeX, double sizeY, + default void setMatchingFrame(ReferenceFrame referenceFrame, + Point3DReadOnly position, + Orientation3DReadOnly orientation, + double sizeX, + double sizeY, double sizeZ) { set(position, orientation, sizeX, sizeY, sizeZ); diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameBox3DBasics.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameBox3DBasics.java index 8adb28f52..a66ea9798 100644 --- a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameBox3DBasics.java +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameBox3DBasics.java @@ -53,7 +53,11 @@ default void setIncludingFrame(FrameBox3DReadOnly other) * @param sizeZ the size along the z-axis. * @throws IllegalArgumentException if any of the three size arguments is negative. */ - default void setIncludingFrame(ReferenceFrame referenceFrame, Point3DReadOnly position, Orientation3DReadOnly orientation, double sizeX, double sizeY, + default void setIncludingFrame(ReferenceFrame referenceFrame, + Point3DReadOnly position, + Orientation3DReadOnly orientation, + double sizeX, + double sizeY, double sizeZ) { setReferenceFrame(referenceFrame); diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameBox3DReadOnly.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameBox3DReadOnly.java index 4529c2b8b..c25c47d6e 100644 --- a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameBox3DReadOnly.java +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameBox3DReadOnly.java @@ -188,7 +188,9 @@ default int intersectionWith(FrameLine3DReadOnly line, FixedFramePoint3DBasics f * @throws ReferenceFrameMismatchException if any of the frame arguments is not expressed in the * same reference frame as {@code this}. */ - default int intersectionWith(FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection, Point3DBasics firstIntersectionToPack, + default int intersectionWith(FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(pointOnLine, lineDirection); @@ -213,7 +215,9 @@ default int intersectionWith(FramePoint3DReadOnly pointOnLine, FrameVector3DRead * @return the number of intersections between the line and this box. It is either equal to 0, 1, or * 2. */ - default int intersectionWith(Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection, FramePoint3DBasics firstIntersectionToPack, + default int intersectionWith(Point3DReadOnly pointOnLine, + Vector3DReadOnly lineDirection, + FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) { if (firstIntersectionToPack != null) @@ -244,7 +248,9 @@ default int intersectionWith(Point3DReadOnly pointOnLine, Vector3DReadOnly lineD * @throws ReferenceFrameMismatchException if any of the frame arguments is not expressed in the * same reference frame as {@code this}. */ - default int intersectionWith(Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection, FixedFramePoint3DBasics firstIntersectionToPack, + default int intersectionWith(Point3DReadOnly pointOnLine, + Vector3DReadOnly lineDirection, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { if (firstIntersectionToPack != null) @@ -275,7 +281,9 @@ default int intersectionWith(Point3DReadOnly pointOnLine, Vector3DReadOnly lineD * not expressed in the same reference frame as * {@code this}. */ - default int intersectionWith(FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection, FramePoint3DBasics firstIntersectionToPack, + default int intersectionWith(FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, + FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(pointOnLine, lineDirection); @@ -306,7 +314,9 @@ default int intersectionWith(FramePoint3DReadOnly pointOnLine, FrameVector3DRead * @throws ReferenceFrameMismatchException if any of the arguments is not expressed in the same * reference frame as {@code this}. */ - default int intersectionWith(FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection, FixedFramePoint3DBasics firstIntersectionToPack, + default int intersectionWith(FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(pointOnLine, lineDirection); diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameCylinder3DReadOnly.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameCylinder3DReadOnly.java index 8ec37f57c..ba54cace3 100644 --- a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameCylinder3DReadOnly.java +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameCylinder3DReadOnly.java @@ -192,7 +192,9 @@ default int intersectionWith(FrameLine3DReadOnly line, FixedFramePoint3DBasics f * @throws ReferenceFrameMismatchException if any of the arguments is not expressed in the same * reference frame as {@code this}. */ - default int intersectionWith(FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection, Point3DBasics firstIntersectionToPack, + default int intersectionWith(FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(pointOnLine, lineDirection); @@ -216,7 +218,9 @@ default int intersectionWith(FramePoint3DReadOnly pointOnLine, FrameVector3DRead * @return the number of intersections between the line and this cylinder. It is either equal to 0, * 1, or 2. */ - default int intersectionWith(Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection, FramePoint3DBasics firstIntersectionToPack, + default int intersectionWith(Point3DReadOnly pointOnLine, + Vector3DReadOnly lineDirection, + FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) { if (firstIntersectionToPack != null) @@ -245,7 +249,9 @@ default int intersectionWith(Point3DReadOnly pointOnLine, Vector3DReadOnly lineD * @throws ReferenceFrameMismatchException if any of the arguments is not expressed in the same * reference frame as {@code this}. */ - default int intersectionWith(Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection, FixedFramePoint3DBasics firstIntersectionToPack, + default int intersectionWith(Point3DReadOnly pointOnLine, + Vector3DReadOnly lineDirection, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { if (firstIntersectionToPack != null) @@ -275,7 +281,9 @@ default int intersectionWith(Point3DReadOnly pointOnLine, Vector3DReadOnly lineD * not expressed in the same reference frame as * {@code this}. */ - default int intersectionWith(FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection, FramePoint3DBasics firstIntersectionToPack, + default int intersectionWith(FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, + FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(pointOnLine, lineDirection); @@ -303,7 +311,9 @@ default int intersectionWith(FramePoint3DReadOnly pointOnLine, FrameVector3DRead * @return the number of intersections between the line and this cylinder. It is either equal to 0, * 1, or 2. */ - default int intersectionWith(FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection, FixedFramePoint3DBasics firstIntersectionToPack, + default int intersectionWith(FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(pointOnLine, lineDirection); diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameEllipsoid3DBasics.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameEllipsoid3DBasics.java index bb4860f9a..3ede84675 100644 --- a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameEllipsoid3DBasics.java +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameEllipsoid3DBasics.java @@ -53,7 +53,11 @@ default void setIncludingFrame(FrameEllipsoid3DReadOnly other) * @param radiusZ radius of the ellipsoid along the z-axis. * @throws IllegalArgumentException if any of the three radii is negative. */ - default void setIncludingFrame(ReferenceFrame referenceFrame, Point3DReadOnly position, Orientation3DReadOnly orientation, double radiusX, double radiusY, + default void setIncludingFrame(ReferenceFrame referenceFrame, + Point3DReadOnly position, + Orientation3DReadOnly orientation, + double radiusX, + double radiusY, double radiusZ) { setReferenceFrame(referenceFrame); diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameEllipsoid3DReadOnly.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameEllipsoid3DReadOnly.java index e5723e7a0..0675f0413 100644 --- a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameEllipsoid3DReadOnly.java +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameEllipsoid3DReadOnly.java @@ -187,7 +187,9 @@ default int intersectionWith(FrameLine3DReadOnly line, FixedFramePoint3DBasics f * @throws ReferenceFrameMismatchException if any of the frame arguments is not expressed in the * same reference frame as {@code this}. */ - default int intersectionWith(FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection, Point3DBasics firstIntersectionToPack, + default int intersectionWith(FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(pointOnLine, lineDirection); @@ -212,7 +214,9 @@ default int intersectionWith(FramePoint3DReadOnly pointOnLine, FrameVector3DRead * @return the number of intersections between the line and this ellipsoid. It is either equal to 0, * 1, or 2. */ - default int intersectionWith(Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection, FramePoint3DBasics firstIntersectionToPack, + default int intersectionWith(Point3DReadOnly pointOnLine, + Vector3DReadOnly lineDirection, + FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) { if (firstIntersectionToPack != null) @@ -242,7 +246,9 @@ default int intersectionWith(Point3DReadOnly pointOnLine, Vector3DReadOnly lineD * @throws ReferenceFrameMismatchException if any of the frame arguments is not expressed in the * same reference frame as {@code this}. */ - default int intersectionWith(Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection, FixedFramePoint3DBasics firstIntersectionToPack, + default int intersectionWith(Point3DReadOnly pointOnLine, + Vector3DReadOnly lineDirection, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { if (firstIntersectionToPack != null) @@ -273,7 +279,9 @@ default int intersectionWith(Point3DReadOnly pointOnLine, Vector3DReadOnly lineD * not expressed in the same reference frame as * {@code this}. */ - default int intersectionWith(FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection, FramePoint3DBasics firstIntersectionToPack, + default int intersectionWith(FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, + FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(pointOnLine, lineDirection); @@ -304,7 +312,9 @@ default int intersectionWith(FramePoint3DReadOnly pointOnLine, FrameVector3DRead * @throws ReferenceFrameMismatchException if any of the arguments is not expressed in the same * reference frame as {@code this}. */ - default int intersectionWith(FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection, FixedFramePoint3DBasics firstIntersectionToPack, + default int intersectionWith(FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(pointOnLine, lineDirection); diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameRamp3DBasics.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameRamp3DBasics.java index 085cc508d..a6c1f4c44 100644 --- a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameRamp3DBasics.java +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameRamp3DBasics.java @@ -61,7 +61,11 @@ default void setIncludingFrame(FrameRamp3DReadOnly other) * @param sizeZ the size along the z-axis. * @throws IllegalArgumentException if any of the three size arguments is negative. */ - default void setIncludingFrame(ReferenceFrame referenceFrame, Point3DReadOnly position, Orientation3DReadOnly orientation, double sizeX, double sizeY, + default void setIncludingFrame(ReferenceFrame referenceFrame, + Point3DReadOnly position, + Orientation3DReadOnly orientation, + double sizeX, + double sizeY, double sizeZ) { setReferenceFrame(referenceFrame); diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameShape3DReadOnly.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameShape3DReadOnly.java index 64721f46b..c14e416f4 100644 --- a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameShape3DReadOnly.java +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameShape3DReadOnly.java @@ -39,7 +39,8 @@ public interface FrameShape3DReadOnly extends Shape3DReadOnly, SupportingFrameVe * @throws ReferenceFrameMismatchException if any of the frame arguments is not expressed in the * same reference frame as {@code this}. */ - default boolean evaluatePoint3DCollision(Point3DReadOnly pointToCheck, FixedFramePoint3DBasics closestPointOnSurfaceToPack, + default boolean evaluatePoint3DCollision(Point3DReadOnly pointToCheck, + FixedFramePoint3DBasics closestPointOnSurfaceToPack, FixedFrameVector3DBasics normalAtClosestPointToPack) { checkReferenceFrameMatch(closestPointOnSurfaceToPack); @@ -58,7 +59,8 @@ default boolean evaluatePoint3DCollision(Point3DReadOnly pointToCheck, FixedFram * @return {@code true} if the query is inside this shape or exactly on its surface, {@code false} * otherwise. */ - default boolean evaluatePoint3DCollision(Point3DReadOnly pointToCheck, FramePoint3DBasics closestPointOnSurfaceToPack, + default boolean evaluatePoint3DCollision(Point3DReadOnly pointToCheck, + FramePoint3DBasics closestPointOnSurfaceToPack, FrameVector3DBasics normalAtClosestPointToPack) { closestPointOnSurfaceToPack.setReferenceFrame(getReferenceFrame()); @@ -79,7 +81,8 @@ default boolean evaluatePoint3DCollision(Point3DReadOnly pointToCheck, FramePoin * @throws ReferenceFrameMismatchException if the frame argument is not expressed in the same * reference frame as {@code this}. */ - default boolean evaluatePoint3DCollision(FramePoint3DReadOnly pointToCheck, Point3DBasics closestPointOnSurfaceToPack, + default boolean evaluatePoint3DCollision(FramePoint3DReadOnly pointToCheck, + Point3DBasics closestPointOnSurfaceToPack, Vector3DBasics normalAtClosestPointToPack) { checkReferenceFrameMatch(pointToCheck); @@ -99,7 +102,8 @@ default boolean evaluatePoint3DCollision(FramePoint3DReadOnly pointToCheck, Poin * @throws ReferenceFrameMismatchException if any of the arguments is not expressed in the same * reference frame as {@code this}. */ - default boolean evaluatePoint3DCollision(FramePoint3DReadOnly pointToCheck, FixedFramePoint3DBasics closestPointOnSurfaceToPack, + default boolean evaluatePoint3DCollision(FramePoint3DReadOnly pointToCheck, + FixedFramePoint3DBasics closestPointOnSurfaceToPack, FixedFrameVector3DBasics normalAtClosestPointToPack) { checkReferenceFrameMatch(pointToCheck); @@ -119,7 +123,8 @@ default boolean evaluatePoint3DCollision(FramePoint3DReadOnly pointToCheck, Fixe * @throws ReferenceFrameMismatchException if {@code pointToCheck} is not expressed in the same * reference frame as {@code this}. */ - default boolean evaluatePoint3DCollision(FramePoint3DReadOnly pointToCheck, FramePoint3DBasics closestPointOnSurfaceToPack, + default boolean evaluatePoint3DCollision(FramePoint3DReadOnly pointToCheck, + FramePoint3DBasics closestPointOnSurfaceToPack, FrameVector3DBasics normalAtClosestPointToPack) { checkReferenceFrameMatch(pointToCheck); diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameSphere3DReadOnly.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameSphere3DReadOnly.java index f122f2d6e..54d6dead2 100644 --- a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameSphere3DReadOnly.java +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/interfaces/FrameSphere3DReadOnly.java @@ -169,7 +169,9 @@ default int intersectionWith(FrameLine3DReadOnly line, FixedFramePoint3DBasics f * @throws ReferenceFrameMismatchException if any of the frame arguments is not expressed in the * same reference frame as {@code this}. */ - default int intersectionWith(FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection, Point3DBasics firstIntersectionToPack, + default int intersectionWith(FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(pointOnLine, lineDirection); @@ -194,7 +196,9 @@ default int intersectionWith(FramePoint3DReadOnly pointOnLine, FrameVector3DRead * @return the number of intersections between the line and this sphere. It is either equal to 0, 1, * or 2. */ - default int intersectionWith(Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection, FramePoint3DBasics firstIntersectionToPack, + default int intersectionWith(Point3DReadOnly pointOnLine, + Vector3DReadOnly lineDirection, + FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) { if (firstIntersectionToPack != null) @@ -224,7 +228,9 @@ default int intersectionWith(Point3DReadOnly pointOnLine, Vector3DReadOnly lineD * @throws ReferenceFrameMismatchException if any of the frame arguments is not expressed in the * same reference frame as {@code this}. */ - default int intersectionWith(Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection, FixedFramePoint3DBasics firstIntersectionToPack, + default int intersectionWith(Point3DReadOnly pointOnLine, + Vector3DReadOnly lineDirection, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { if (firstIntersectionToPack != null) @@ -255,7 +261,9 @@ default int intersectionWith(Point3DReadOnly pointOnLine, Vector3DReadOnly lineD * not expressed in the same reference frame as * {@code this}. */ - default int intersectionWith(FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection, FramePoint3DBasics firstIntersectionToPack, + default int intersectionWith(FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, + FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(pointOnLine, lineDirection); @@ -286,7 +294,9 @@ default int intersectionWith(FramePoint3DReadOnly pointOnLine, FrameVector3DRead * @throws ReferenceFrameMismatchException if any of the arguments is not expressed in the same * reference frame as {@code this}. */ - default int intersectionWith(FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection, FixedFramePoint3DBasics firstIntersectionToPack, + default int intersectionWith(FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(pointOnLine, lineDirection); diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/polytope/FrameFace3D.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/polytope/FrameFace3D.java index 750e69c5b..dde051c3a 100644 --- a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/polytope/FrameFace3D.java +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/polytope/FrameFace3D.java @@ -82,7 +82,9 @@ public FrameFace3D(ReferenceFrameHolder referenceFrameHolder, Vector3DReadOnly i * @param constructionEpsilon tolerance used when adding vertices to a face to trigger a series of * edge-cases. */ - public FrameFace3D(ReferenceFrameHolder referenceFrameHolder, Collection faceEdges, FrameVector3DReadOnly normal, + public FrameFace3D(ReferenceFrameHolder referenceFrameHolder, + Collection faceEdges, + FrameVector3DReadOnly normal, double constructionEpsilon) { super(edgeFactory(referenceFrameHolder), constructionEpsilon); diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameShapeRandomTools.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameShapeRandomTools.java index 67852cef2..79da46e87 100644 --- a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameShapeRandomTools.java +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameShapeRandomTools.java @@ -150,7 +150,11 @@ public static FrameCapsule3D nextFrameCapsule3D(Random random, ReferenceFrame re * @return the random capsule 3D. * @throws RuntimeException if {@code minLength > maxLength} or {@code minRadius > maxRadius}. */ - public static FrameCapsule3D nextFrameCapsule3D(Random random, ReferenceFrame referenceFrame, double minLength, double maxLength, double minRadius, + public static FrameCapsule3D nextFrameCapsule3D(Random random, + ReferenceFrame referenceFrame, + double minLength, + double maxLength, + double minRadius, double maxRadius) { return new FrameCapsule3D(referenceFrame, EuclidShapeRandomTools.nextCapsule3D(random, minLength, maxLength, minRadius, maxRadius)); @@ -194,7 +198,11 @@ public static FrameCylinder3D nextFrameCylinder3D(Random random, ReferenceFrame * @return the random cylinder 3D. * @throws RuntimeException if {@code minLength > maxLength} or {@code minRadius > maxRadius}. */ - public static FrameCylinder3D nextFrameCylinder3D(Random random, ReferenceFrame referenceFrame, double minLength, double maxLength, double minRadius, + public static FrameCylinder3D nextFrameCylinder3D(Random random, + ReferenceFrame referenceFrame, + double minLength, + double maxLength, + double minRadius, double maxRadius) { return new FrameCylinder3D(referenceFrame, EuclidShapeRandomTools.nextCylinder3D(random, minLength, maxLength, minRadius, maxRadius)); @@ -382,7 +390,10 @@ public static FrameFace3D nextCircleBasedFrameFace3D(Random random, ReferenceFra * @param numberOfVertices the size of the convex polygon. * @return the random face. */ - public static FrameFace3D nextCircleBasedFrameFace3D(Random random, ReferenceFrame referenceFrame, double centerMinMax, double maxEdgeLength, + public static FrameFace3D nextCircleBasedFrameFace3D(Random random, + ReferenceFrame referenceFrame, + double centerMinMax, + double maxEdgeLength, int numberOfVertices) { return nextCircleBasedFrameFace3D(random, @@ -407,8 +418,12 @@ public static FrameFace3D nextCircleBasedFrameFace3D(Random random, ReferenceFra * modified. * @return the random face. */ - public static FrameFace3D nextCircleBasedFrameFace3D(Random random, ReferenceFrame referenceFrame, double centerMinMax, double maxEdgeLength, - int numberOfVertices, Vector3DReadOnly faceNormal) + public static FrameFace3D nextCircleBasedFrameFace3D(Random random, + ReferenceFrame referenceFrame, + double centerMinMax, + double maxEdgeLength, + int numberOfVertices, + Vector3DReadOnly faceNormal) { List vertices = EuclidShapeRandomTools.nextCircleBasedConvexPolygon3D(random, centerMinMax, maxEdgeLength, numberOfVertices, faceNormal); FrameFace3D face3D = new FrameFace3D(() -> referenceFrame, faceNormal); @@ -499,8 +514,15 @@ public static FrameConvexPolytope3D nextConeFrameConvexPolytope3D(Random random, * @param divisionsMax the maximum number of divisions for discretizing the cone. * @return the random convex polytope 3D. */ - public static FrameConvexPolytope3D nextConeFrameConvexPolytope3D(Random random, ReferenceFrame referenceFrame, double centerMinMax, double heightMin, - double heightMax, double radiusMin, double radiusMax, int divisionsMin, int divisionsMax) + public static FrameConvexPolytope3D nextConeFrameConvexPolytope3D(Random random, + ReferenceFrame referenceFrame, + double centerMinMax, + double heightMin, + double heightMax, + double radiusMin, + double radiusMax, + int divisionsMin, + int divisionsMax) { return new FrameConvexPolytope3D(referenceFrame, EuclidShapeRandomTools.nextConeConvexPolytope3D(random, @@ -550,7 +572,10 @@ public static FrameConvexPolytope3D nextCubeFrameConvexPolytope3D(Random random, * @param edgeLengthMax the maximum value for the cube's edge length. * @return the random convex polytope 3D. */ - public static FrameConvexPolytope3D nextCubeFrameConvexPolytope3D(Random random, ReferenceFrame referenceFrame, double centerMinMax, double edgeLengthMin, + public static FrameConvexPolytope3D nextCubeFrameConvexPolytope3D(Random random, + ReferenceFrame referenceFrame, + double centerMinMax, + double edgeLengthMin, double edgeLengthMax) { return new FrameConvexPolytope3D(referenceFrame, EuclidShapeRandomTools.nextCubeConvexPolytope3D(random, centerMinMax, edgeLengthMin, edgeLengthMax)); @@ -597,8 +622,14 @@ public static FrameConvexPolytope3D nextCylinderFrameConvexPolytope3D(Random ran * @param divisionsMax the maximum number of divisions for discretizing the cylinder. * @return the random convex polytope 3D. */ - public static FrameConvexPolytope3D nextCylinderFrameConvexPolytope3D(Random random, ReferenceFrame referenceFrame, double centerMinMax, double lengthMin, - double lengthMax, double radiusMin, double radiusMax, int divisionsMin, + public static FrameConvexPolytope3D nextCylinderFrameConvexPolytope3D(Random random, + ReferenceFrame referenceFrame, + double centerMinMax, + double lengthMin, + double lengthMax, + double radiusMin, + double radiusMax, + int divisionsMin, int divisionsMax) { return new FrameConvexPolytope3D(referenceFrame, @@ -651,8 +682,11 @@ public static FrameConvexPolytope3D nextIcosahedronBasedFrameConvexPolytope3D(Ra * icosahedron. * @return the random convex polytope 3D. */ - public static FrameConvexPolytope3D nextIcosahedronBasedFrameConvexPolytope3D(Random random, ReferenceFrame referenceFrame, double centerMinMax, - double radiusMin, double radiusMax) + public static FrameConvexPolytope3D nextIcosahedronBasedFrameConvexPolytope3D(Random random, + ReferenceFrame referenceFrame, + double centerMinMax, + double radiusMin, + double radiusMax) { return new FrameConvexPolytope3D(referenceFrame, EuclidShapeRandomTools.nextIcosahedronBasedConvexPolytope3D(random, centerMinMax, radiusMin, radiusMax)); } @@ -697,8 +731,11 @@ public static FrameConvexPolytope3D nextIcoSphereBasedFrameConvexPolytope3D(Rand * @return the random convex polytope 3D. * @see IcoSphereFactory */ - public static FrameConvexPolytope3D nextIcoSphereBasedFrameConvexPolytope3D(Random random, ReferenceFrame referenceFrame, double centerMinMax, - double radiusMin, double radiusMax) + public static FrameConvexPolytope3D nextIcoSphereBasedFrameConvexPolytope3D(Random random, + ReferenceFrame referenceFrame, + double centerMinMax, + double radiusMin, + double radiusMax) { return new FrameConvexPolytope3D(referenceFrame, EuclidShapeRandomTools.nextIcoSphereBasedConvexPolytope3D(random, centerMinMax, radiusMin, radiusMax)); } @@ -716,8 +753,12 @@ public static FrameConvexPolytope3D nextIcoSphereBasedFrameConvexPolytope3D(Rand * @return the random convex polytope 3D. * @see IcoSphereFactory */ - public static FrameConvexPolytope3D nextIcoSphereBasedFrameConvexPolytope3D(Random random, ReferenceFrame referenceFrame, double centerMinMax, - int recursionLevel, double radiusMin, double radiusMax) + public static FrameConvexPolytope3D nextIcoSphereBasedFrameConvexPolytope3D(Random random, + ReferenceFrame referenceFrame, + double centerMinMax, + int recursionLevel, + double radiusMin, + double radiusMax) { return new FrameConvexPolytope3D(referenceFrame, EuclidShapeRandomTools.nextIcoSphereBasedConvexPolytope3D(random, centerMinMax, recursionLevel, radiusMin, radiusMax)); @@ -759,7 +800,9 @@ public static FrameConvexPolytope3D nextPointCloudBasedFrameConvexPolytope3D(Ran * @param minMax the range of the point cloud in the three directions. * @return the random convex polytope 3D. */ - public static FrameConvexPolytope3D nextPointCloudBasedFrameConvexPolytope3D(Random random, ReferenceFrame referenceFrame, double centerMinMax, + public static FrameConvexPolytope3D nextPointCloudBasedFrameConvexPolytope3D(Random random, + ReferenceFrame referenceFrame, + double centerMinMax, double minMax) { return new FrameConvexPolytope3D(referenceFrame, EuclidShapeRandomTools.nextPointCloudBasedConvexPolytope3D(random, centerMinMax, minMax)); @@ -778,8 +821,11 @@ public static FrameConvexPolytope3D nextPointCloudBasedFrameConvexPolytope3D(Ran * polytope will be less than {@code numberOfPossiblePoints}. * @return the random convex polytope 3D. */ - public static FrameConvexPolytope3D nextPointCloudBasedFrameConvexPolytope3D(Random random, ReferenceFrame referenceFrame, double centerMinMax, - double minMax, int numberOfPossiblePoints) + public static FrameConvexPolytope3D nextPointCloudBasedFrameConvexPolytope3D(Random random, + ReferenceFrame referenceFrame, + double centerMinMax, + double minMax, + int numberOfPossiblePoints) { return new FrameConvexPolytope3D(referenceFrame, EuclidShapeRandomTools.nextPointCloudBasedConvexPolytope3D(random, centerMinMax, minMax, numberOfPossiblePoints)); @@ -826,8 +872,14 @@ public static FrameConvexPolytope3D nextPyramidFrameConvexPolytope3D(Random rand * @param baseWidthMax the maximum value for the width of the pyramid's base. * @return the random convex polytope 3D. */ - public static FrameConvexPolytope3D nextPyramidFrameConvexPolytope3D(Random random, ReferenceFrame referenceFrame, double centerMinMax, double heightMin, - double heightMax, double baseLengthMin, double baseLengthMax, double baseWidthMin, + public static FrameConvexPolytope3D nextPyramidFrameConvexPolytope3D(Random random, + ReferenceFrame referenceFrame, + double centerMinMax, + double heightMin, + double heightMax, + double baseLengthMin, + double baseLengthMax, + double baseWidthMin, double baseWidthMax) { return new FrameConvexPolytope3D(referenceFrame, diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameShapeTestTools.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameShapeTestTools.java index 8871e85ca..0c911e096 100644 --- a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameShapeTestTools.java +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameShapeTestTools.java @@ -153,7 +153,10 @@ public static void assertFrameBox3DGeometricallyEquals(String messagePrefix, Fra * @throws AssertionError if the two boxes do not represent the same geometry. If only one of the * arguments is equal to {@code null}. */ - public static void assertFrameBox3DGeometricallyEquals(String messagePrefix, FrameBox3DReadOnly expected, FrameBox3DReadOnly actual, double epsilon, + public static void assertFrameBox3DGeometricallyEquals(String messagePrefix, + FrameBox3DReadOnly expected, + FrameBox3DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -216,7 +219,10 @@ public static void assertFrameCapsule3DEquals(String messagePrefix, FrameCapsule * @throws AssertionError if the two capsules are not equal. If only one of the arguments is equal * to {@code null}. */ - public static void assertFrameCapsule3DEquals(String messagePrefix, FrameCapsule3DReadOnly expected, FrameCapsule3DReadOnly actual, double epsilon, + public static void assertFrameCapsule3DEquals(String messagePrefix, + FrameCapsule3DReadOnly expected, + FrameCapsule3DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -261,7 +267,9 @@ public static void assertFrameCapsule3DGeometricallyEquals(FrameCapsule3DReadOnl * @throws AssertionError if the two capsules do not represent the same geometry. If only one of the * arguments is equal to {@code null}. */ - public static void assertFrameCapsule3DGeometricallyEquals(String messagePrefix, FrameCapsule3DReadOnly expected, FrameCapsule3DReadOnly actual, + public static void assertFrameCapsule3DGeometricallyEquals(String messagePrefix, + FrameCapsule3DReadOnly expected, + FrameCapsule3DReadOnly actual, double epsilon) { assertFrameCapsule3DGeometricallyEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); @@ -283,8 +291,11 @@ public static void assertFrameCapsule3DGeometricallyEquals(String messagePrefix, * @throws AssertionError if the two capsules do not represent the same geometry. If only one of the * arguments is equal to {@code null}. */ - public static void assertFrameCapsule3DGeometricallyEquals(String messagePrefix, FrameCapsule3DReadOnly expected, FrameCapsule3DReadOnly actual, - double epsilon, String format) + public static void assertFrameCapsule3DGeometricallyEquals(String messagePrefix, + FrameCapsule3DReadOnly expected, + FrameCapsule3DReadOnly actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -346,7 +357,10 @@ public static void assertFrameCylinder3DEquals(String messagePrefix, FrameCylind * @throws AssertionError if the two cylinders are not equal. If only one of the arguments is equal * to {@code null}. */ - public static void assertFrameCylinder3DEquals(String messagePrefix, FrameCylinder3DReadOnly expected, FrameCylinder3DReadOnly actual, double epsilon, + public static void assertFrameCylinder3DEquals(String messagePrefix, + FrameCylinder3DReadOnly expected, + FrameCylinder3DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -391,7 +405,9 @@ public static void assertFrameCylinder3DGeometricallyEquals(FrameCylinder3DReadO * @throws AssertionError if the two cylinders do not represent the same geometry. If only one of * the arguments is equal to {@code null}. */ - public static void assertFrameCylinder3DGeometricallyEquals(String messagePrefix, FrameCylinder3DReadOnly expected, FrameCylinder3DReadOnly actual, + public static void assertFrameCylinder3DGeometricallyEquals(String messagePrefix, + FrameCylinder3DReadOnly expected, + FrameCylinder3DReadOnly actual, double epsilon) { assertFrameCylinder3DGeometricallyEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); @@ -413,8 +429,11 @@ public static void assertFrameCylinder3DGeometricallyEquals(String messagePrefix * @throws AssertionError if the two cylinders do not represent the same geometry. If only one of * the arguments is equal to {@code null}. */ - public static void assertFrameCylinder3DGeometricallyEquals(String messagePrefix, FrameCylinder3DReadOnly expected, FrameCylinder3DReadOnly actual, - double epsilon, String format) + public static void assertFrameCylinder3DGeometricallyEquals(String messagePrefix, + FrameCylinder3DReadOnly expected, + FrameCylinder3DReadOnly actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -476,7 +495,10 @@ public static void assertFrameEllipsoid3DEquals(String messagePrefix, FrameEllip * @throws AssertionError if the two ellipsoids are not equal. If only one of the arguments is equal * to {@code null}. */ - public static void assertFrameEllipsoid3DEquals(String messagePrefix, FrameEllipsoid3DReadOnly expected, FrameEllipsoid3DReadOnly actual, double epsilon, + public static void assertFrameEllipsoid3DEquals(String messagePrefix, + FrameEllipsoid3DReadOnly expected, + FrameEllipsoid3DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -521,7 +543,9 @@ public static void assertFrameEllipsoid3DGeometricallyEquals(FrameEllipsoid3DRea * @throws AssertionError if the two ellipsoids do not represent the same geometry. If only one of * the arguments is equal to {@code null}. */ - public static void assertEllipsoid3DGeometricallyEquals(String messagePrefix, FrameEllipsoid3DReadOnly expected, FrameEllipsoid3DReadOnly actual, + public static void assertEllipsoid3DGeometricallyEquals(String messagePrefix, + FrameEllipsoid3DReadOnly expected, + FrameEllipsoid3DReadOnly actual, double epsilon) { assertFrameEllipsoid3DGeometricallyEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); @@ -543,8 +567,11 @@ public static void assertEllipsoid3DGeometricallyEquals(String messagePrefix, Fr * @throws AssertionError if the two ellipsoids do not represent the same geometry. If only one of * the arguments is equal to {@code null}. */ - public static void assertFrameEllipsoid3DGeometricallyEquals(String messagePrefix, FrameEllipsoid3DReadOnly expected, FrameEllipsoid3DReadOnly actual, - double epsilon, String format) + public static void assertFrameEllipsoid3DGeometricallyEquals(String messagePrefix, + FrameEllipsoid3DReadOnly expected, + FrameEllipsoid3DReadOnly actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -606,7 +633,10 @@ public static void assertFramePointShape3DEquals(String messagePrefix, FramePoin * @throws AssertionError if the two point shapes are not equal. If only one of the arguments is * equal to {@code null}. */ - public static void assertFramePointShape3DEquals(String messagePrefix, FramePointShape3DReadOnly expected, FramePointShape3DReadOnly actual, double epsilon, + public static void assertFramePointShape3DEquals(String messagePrefix, + FramePointShape3DReadOnly expected, + FramePointShape3DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -651,7 +681,9 @@ public static void assertFramePointShape3DGeometricallyEquals(FramePointShape3DR * @throws AssertionError if the two point shapes do not represent the same geometry. If only one of * the arguments is equal to {@code null}. */ - public static void assertFramePointShape3DGeometricallyEquals(String messagePrefix, FramePointShape3DReadOnly expected, FramePointShape3DReadOnly actual, + public static void assertFramePointShape3DGeometricallyEquals(String messagePrefix, + FramePointShape3DReadOnly expected, + FramePointShape3DReadOnly actual, double epsilon) { assertFramePointShape3DGeometricallyEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); @@ -673,8 +705,11 @@ public static void assertFramePointShape3DGeometricallyEquals(String messagePref * @throws AssertionError if the two point shapes do not represent the same geometry. If only one of * the arguments is equal to {@code null}. */ - public static void assertFramePointShape3DGeometricallyEquals(String messagePrefix, FramePointShape3DReadOnly expected, FramePointShape3DReadOnly actual, - double epsilon, String format) + public static void assertFramePointShape3DGeometricallyEquals(String messagePrefix, + FramePointShape3DReadOnly expected, + FramePointShape3DReadOnly actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -801,7 +836,10 @@ public static void assertFrameRamp3DGeometricallyEquals(String messagePrefix, Fr * @throws AssertionError if the two ramps do not represent the same geometry. If only one of the * arguments is equal to {@code null}. */ - public static void assertFrameRamp3DGeometricallyEquals(String messagePrefix, FrameRamp3DReadOnly expected, FrameRamp3DReadOnly actual, double epsilon, + public static void assertFrameRamp3DGeometricallyEquals(String messagePrefix, + FrameRamp3DReadOnly expected, + FrameRamp3DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -864,7 +902,10 @@ public static void assertFrameSphere3DEquals(String messagePrefix, FrameSphere3D * @throws AssertionError if the two spheres are not equal. If only one of the arguments is equal to * {@code null}. */ - public static void assertFrameSphere3DEquals(String messagePrefix, FrameSphere3DReadOnly expected, FrameSphere3DReadOnly actual, double epsilon, + public static void assertFrameSphere3DEquals(String messagePrefix, + FrameSphere3DReadOnly expected, + FrameSphere3DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -930,7 +971,10 @@ public static void assertFrameSphere3DGeometricallyEquals(String messagePrefix, * @throws AssertionError if the two spheres do not represent the same geometry. If only one of the * arguments is equal to {@code null}. */ - public static void assertFrameSphere3DGeometricallyEquals(String messagePrefix, FrameSphere3DReadOnly expected, FrameSphere3DReadOnly actual, double epsilon, + public static void assertFrameSphere3DGeometricallyEquals(String messagePrefix, + FrameSphere3DReadOnly expected, + FrameSphere3DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -956,7 +1000,8 @@ public static void assertFrameSphere3DGeometricallyEquals(String messagePrefix, * is equal to {@code null}. */ public static void assertEuclidFrameShape3DCollisionResultEquals(EuclidFrameShape3DCollisionResultReadOnly expected, - EuclidFrameShape3DCollisionResultReadOnly actual, double epsilon) + EuclidFrameShape3DCollisionResultReadOnly actual, + double epsilon) { assertEuclidFrameShape3DCollisionResultEquals(null, expected, actual, epsilon); } @@ -974,8 +1019,10 @@ public static void assertEuclidFrameShape3DCollisionResultEquals(EuclidFrameShap * @throws AssertionError if the two collision results are not equal. If only one of the arguments * is equal to {@code null}. */ - public static void assertEuclidFrameShape3DCollisionResultEquals(String messagePrefix, EuclidFrameShape3DCollisionResultReadOnly expected, - EuclidFrameShape3DCollisionResultReadOnly actual, double epsilon) + public static void assertEuclidFrameShape3DCollisionResultEquals(String messagePrefix, + EuclidFrameShape3DCollisionResultReadOnly expected, + EuclidFrameShape3DCollisionResultReadOnly actual, + double epsilon) { assertEuclidFrameShape3DCollisionResultEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); } @@ -995,8 +1042,11 @@ public static void assertEuclidFrameShape3DCollisionResultEquals(String messageP * @throws AssertionError if the two collision results are not equal. If only one of the arguments * is equal to {@code null}. */ - public static void assertEuclidFrameShape3DCollisionResultEquals(String messagePrefix, EuclidFrameShape3DCollisionResultReadOnly expected, - EuclidFrameShape3DCollisionResultReadOnly actual, double epsilon, String format) + public static void assertEuclidFrameShape3DCollisionResultEquals(String messagePrefix, + EuclidFrameShape3DCollisionResultReadOnly expected, + EuclidFrameShape3DCollisionResultReadOnly actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -1041,7 +1091,8 @@ public static void assertEuclidFrameShape3DCollisionResultEquals(String messageP * one of the arguments is equal to {@code null}. */ public static void assertEuclidFrameShape3DCollisionResultGeometricallyEquals(EuclidFrameShape3DCollisionResultReadOnly expected, - EuclidFrameShape3DCollisionResultReadOnly actual, double epsilon) + EuclidFrameShape3DCollisionResultReadOnly actual, + double epsilon) { assertEuclidFrameShape3DCollisionResultGeometricallyEquals(null, expected, actual, epsilon); } @@ -1066,8 +1117,10 @@ public static void assertEuclidFrameShape3DCollisionResultGeometricallyEquals(Eu * one of the arguments is equal to {@code null}. */ public static void assertEuclidFrameShape3DCollisionResultGeometricallyEquals(EuclidFrameShape3DCollisionResultReadOnly expected, - EuclidFrameShape3DCollisionResultReadOnly actual, double distanceEpsilon, - double pointTangentialEpsilon, double normalEpsilon) + EuclidFrameShape3DCollisionResultReadOnly actual, + double distanceEpsilon, + double pointTangentialEpsilon, + double normalEpsilon) { assertEuclidFrameShape3DCollisionResultGeometricallyEquals(null, expected, @@ -1092,8 +1145,10 @@ public static void assertEuclidFrameShape3DCollisionResultGeometricallyEquals(Eu * @throws AssertionError if the two collision results do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ - public static void assertEuclidFrameShape3DCollisionResultGeometricallyEquals(String messagePrefix, EuclidFrameShape3DCollisionResultReadOnly expected, - EuclidFrameShape3DCollisionResultReadOnly actual, double epsilon) + public static void assertEuclidFrameShape3DCollisionResultGeometricallyEquals(String messagePrefix, + EuclidFrameShape3DCollisionResultReadOnly expected, + EuclidFrameShape3DCollisionResultReadOnly actual, + double epsilon) { assertEuclidFrameShape3DCollisionResultGeometricallyEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); } @@ -1118,9 +1173,12 @@ public static void assertEuclidFrameShape3DCollisionResultGeometricallyEquals(St * @throws AssertionError if the two collision results do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ - public static void assertEuclidFrameShape3DCollisionResultGeometricallyEquals(String messagePrefix, EuclidFrameShape3DCollisionResultReadOnly expected, - EuclidFrameShape3DCollisionResultReadOnly actual, double distanceEpsilon, - double pointTangentialEpsilon, double normalEpsilon) + public static void assertEuclidFrameShape3DCollisionResultGeometricallyEquals(String messagePrefix, + EuclidFrameShape3DCollisionResultReadOnly expected, + EuclidFrameShape3DCollisionResultReadOnly actual, + double distanceEpsilon, + double pointTangentialEpsilon, + double normalEpsilon) { assertEuclidFrameShape3DCollisionResultGeometricallyEquals(messagePrefix, expected, @@ -1147,8 +1205,10 @@ public static void assertEuclidFrameShape3DCollisionResultGeometricallyEquals(St * @throws AssertionError if the two collision results do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ - public static void assertEuclidFrameShape3DCollisionResultGeometricallyEquals(String messagePrefix, EuclidFrameShape3DCollisionResultReadOnly expected, - EuclidFrameShape3DCollisionResultReadOnly actual, double epsilon, + public static void assertEuclidFrameShape3DCollisionResultGeometricallyEquals(String messagePrefix, + EuclidFrameShape3DCollisionResultReadOnly expected, + EuclidFrameShape3DCollisionResultReadOnly actual, + double epsilon, String format) { assertEuclidFrameShape3DCollisionResultGeometricallyEquals(messagePrefix, expected, actual, epsilon, epsilon, epsilon, format); @@ -1176,9 +1236,13 @@ public static void assertEuclidFrameShape3DCollisionResultGeometricallyEquals(St * @throws AssertionError if the two collision results do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ - public static void assertEuclidFrameShape3DCollisionResultGeometricallyEquals(String messagePrefix, EuclidFrameShape3DCollisionResultReadOnly expected, - EuclidFrameShape3DCollisionResultReadOnly actual, double distanceEpsilon, - double pointTangentialEpsilon, double normalEpsilon, String format) + public static void assertEuclidFrameShape3DCollisionResultGeometricallyEquals(String messagePrefix, + EuclidFrameShape3DCollisionResultReadOnly expected, + EuclidFrameShape3DCollisionResultReadOnly actual, + double distanceEpsilon, + double pointTangentialEpsilon, + double normalEpsilon, + String format) { if (expected == null && actual == null) return; @@ -1324,7 +1388,10 @@ public static void assertFrameFace3DGeometricallyEquals(String messagePrefix, Fr * @throws AssertionError if the two faces do not represent the same geometry. If only one of the * arguments is equal to {@code null}. */ - public static void assertFrameFace3DGeometricallyEquals(String messagePrefix, FrameFace3DReadOnly expected, FrameFace3DReadOnly actual, double epsilon, + public static void assertFrameFace3DGeometricallyEquals(String messagePrefix, + FrameFace3DReadOnly expected, + FrameFace3DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -1367,7 +1434,9 @@ public static void assertFrameConvexPolytope3DEquals(FrameConvexPolytope3DReadOn * @throws AssertionError if the two convex polytopes are not equal. If only one of the arguments is * equal to {@code null}. */ - public static void assertFrameConvexPolytope3DEquals(String messagePrefix, FrameConvexPolytope3DReadOnly expected, FrameConvexPolytope3DReadOnly actual, + public static void assertFrameConvexPolytope3DEquals(String messagePrefix, + FrameConvexPolytope3DReadOnly expected, + FrameConvexPolytope3DReadOnly actual, double epsilon) { assertFrameConvexPolytope3DEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); @@ -1388,8 +1457,11 @@ public static void assertFrameConvexPolytope3DEquals(String messagePrefix, Frame * @throws AssertionError if the two convex polytopes are not equal. If only one of the arguments is * equal to {@code null}. */ - public static void assertFrameConvexPolytope3DEquals(String messagePrefix, FrameConvexPolytope3DReadOnly expected, FrameConvexPolytope3DReadOnly actual, - double epsilon, String format) + public static void assertFrameConvexPolytope3DEquals(String messagePrefix, + FrameConvexPolytope3DReadOnly expected, + FrameConvexPolytope3DReadOnly actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -1414,7 +1486,8 @@ public static void assertFrameConvexPolytope3DEquals(String messagePrefix, Frame * @throws AssertionError if the two convex polytopes do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ - public static void assertFrameConvexPolytope3DGeometricallyEquals(FrameConvexPolytope3DReadOnly expected, FrameConvexPolytope3DReadOnly actual, + public static void assertFrameConvexPolytope3DGeometricallyEquals(FrameConvexPolytope3DReadOnly expected, + FrameConvexPolytope3DReadOnly actual, double epsilon) { assertFrameConvexPolytope3DGeometricallyEquals(null, expected, actual, epsilon); @@ -1434,8 +1507,10 @@ public static void assertFrameConvexPolytope3DGeometricallyEquals(FrameConvexPol * @throws AssertionError if the two convex polytopes do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ - public static void assertFrameConvexPolytope3DGeometricallyEquals(String messagePrefix, FrameConvexPolytope3DReadOnly expected, - FrameConvexPolytope3DReadOnly actual, double epsilon) + public static void assertFrameConvexPolytope3DGeometricallyEquals(String messagePrefix, + FrameConvexPolytope3DReadOnly expected, + FrameConvexPolytope3DReadOnly actual, + double epsilon) { assertFrameConvexPolytope3DGeometricallyEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); } @@ -1456,8 +1531,11 @@ public static void assertFrameConvexPolytope3DGeometricallyEquals(String message * @throws AssertionError if the two convex polytopes do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ - public static void assertFrameConvexPolytope3DGeometricallyEquals(String messagePrefix, FrameConvexPolytope3DReadOnly expected, - FrameConvexPolytope3DReadOnly actual, double epsilon, String format) + public static void assertFrameConvexPolytope3DGeometricallyEquals(String messagePrefix, + FrameConvexPolytope3DReadOnly expected, + FrameConvexPolytope3DReadOnly actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -1518,14 +1596,19 @@ private static void throwNotEqualAssertionError(String messagePrefix, FrameSpher EuclidCoreTestTools.throwNotEqualAssertionError(messagePrefix, expectedAsString, actualAsString); } - private static void throwNotEqualAssertionError(String messagePrefix, EuclidFrameShape3DCollisionResultReadOnly expected, - EuclidFrameShape3DCollisionResultReadOnly actual, String format) + private static void throwNotEqualAssertionError(String messagePrefix, + EuclidFrameShape3DCollisionResultReadOnly expected, + EuclidFrameShape3DCollisionResultReadOnly actual, + String format) { throwNotEqualAssertionError(messagePrefix, expected, actual, format, null); } - private static void throwNotEqualAssertionError(String messagePrefix, EuclidFrameShape3DCollisionResultReadOnly expected, - EuclidFrameShape3DCollisionResultReadOnly actual, String format, String differenceAsString) + private static void throwNotEqualAssertionError(String messagePrefix, + EuclidFrameShape3DCollisionResultReadOnly expected, + EuclidFrameShape3DCollisionResultReadOnly actual, + String format, + String differenceAsString) { String expectedAsString = getEuclidFrameShape3DCollisionResultString(format, expected); String actualAsString = getEuclidFrameShape3DCollisionResultString(format, actual); @@ -1539,7 +1622,9 @@ private static void throwNotEqualAssertionError(String messagePrefix, FrameFace3 EuclidCoreTestTools.throwNotEqualAssertionError(messagePrefix, expectedAsString, actualAsString); } - private static void throwNotEqualAssertionError(String messagePrefix, FrameConvexPolytope3DReadOnly expected, FrameConvexPolytope3DReadOnly actual, + private static void throwNotEqualAssertionError(String messagePrefix, + FrameConvexPolytope3DReadOnly expected, + FrameConvexPolytope3DReadOnly actual, String format) { String expectedAsString = getFrameConvexPolytope3DString(format, expected); 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 b7e14f0d4..0148e196d 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 @@ -49,7 +49,16 @@ private static interface BoundingBoxRotationPartCalculator void boundingBoxRotationPartGeneric(RotationMatrixReadOnly rotation1, boolean inverseTransform1, - RotationMatrixReadOnly rotation2, boolean inverseTransform2, - RotationMatrixReadOnly rotation3, boolean inverseTransform3, T shape, + private static void boundingBoxRotationPartGeneric(RotationMatrixReadOnly rotation1, + boolean inverseTransform1, + RotationMatrixReadOnly rotation2, + boolean inverseTransform2, + RotationMatrixReadOnly rotation3, + boolean inverseTransform3, + T shape, BoundingBoxRotationPartCalculator calculator, BoundingBox3DBasics boundingBoxToPack) { @@ -967,8 +1061,11 @@ else if (rotation2.isIdentity()) } } - private static void boundingBoxRotationPartGeneric(RotationMatrixReadOnly rotation1, boolean inverseTransform1, - RotationMatrixReadOnly rotation2, boolean inverseTransform2, T shape, + private static void boundingBoxRotationPartGeneric(RotationMatrixReadOnly rotation1, + boolean inverseTransform1, + RotationMatrixReadOnly rotation2, + boolean inverseTransform2, + T shape, BoundingBoxRotationPartCalculator calculator, BoundingBox3DBasics boundingBoxToPack) { @@ -996,9 +1093,19 @@ private static void boundingBoxRotationPartGeneric(R } } - private static void boundingBoxRotationPartGeneric(double m00, double m01, double m02, double m10, double m11, double m12, - double m20, double m21, double m22, boolean inverseTransform1, - RotationMatrixReadOnly rotation2, boolean inverseTransform2, T shape, + private static void boundingBoxRotationPartGeneric(double m00, + double m01, + double m02, + double m10, + double m11, + double m12, + double m20, + double m21, + double m22, + boolean inverseTransform1, + RotationMatrixReadOnly rotation2, + boolean inverseTransform2, + T shape, BoundingBoxRotationPartCalculator calculator, BoundingBox3DBasics boundingBoxToPack) { @@ -1083,7 +1190,9 @@ private static void boundingBoxRotationPartGeneric(d } } - private static void boundingBoxRotationPartGeneric(RotationMatrixReadOnly rotation, boolean inverseTransform, T shape, + private static void boundingBoxRotationPartGeneric(RotationMatrixReadOnly rotation, + boolean inverseTransform, + T shape, BoundingBoxRotationPartCalculator calculator, BoundingBox3DBasics boundingBoxToPack) { diff --git a/src/frame/java/us/ihmc/euclid/referenceFrame/FrameRotationMatrix.java b/src/frame/java/us/ihmc/euclid/referenceFrame/FrameRotationMatrix.java index 617efdca7..8ee75317f 100644 --- a/src/frame/java/us/ihmc/euclid/referenceFrame/FrameRotationMatrix.java +++ b/src/frame/java/us/ihmc/euclid/referenceFrame/FrameRotationMatrix.java @@ -82,7 +82,15 @@ public FrameRotationMatrix(ReferenceFrame referenceFrame) * @param m22 the 3rd row 3rd column coefficient for this matrix. * @throws NotARotationMatrixException if the resulting matrix is not a rotation matrix. */ - public FrameRotationMatrix(ReferenceFrame referenceFrame, double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, + public FrameRotationMatrix(ReferenceFrame referenceFrame, + double m00, + double m01, + double m02, + double m10, + double m11, + double m12, + double m20, + double m21, double m22) { setIncludingFrame(referenceFrame, m00, m01, m02, m10, m11, m12, m20, m21, m22); diff --git a/src/frame/java/us/ihmc/euclid/referenceFrame/api/EuclidFrameAPITester.java b/src/frame/java/us/ihmc/euclid/referenceFrame/api/EuclidFrameAPITester.java index 637772178..eb45d5bb2 100644 --- a/src/frame/java/us/ihmc/euclid/referenceFrame/api/EuclidFrameAPITester.java +++ b/src/frame/java/us/ihmc/euclid/referenceFrame/api/EuclidFrameAPITester.java @@ -275,7 +275,10 @@ public void registerFrameTypeSmart(Class mutableFrameMutableType) * @param framelessMutableType the geometry with: mutable values and no frame information. * @param framelessReadOnlyType the geometry with: immutable values and no frame information. */ - public void registerFrameType(Class mutableFrameMutableType, Class fixedFrameMutableType, Class frameReadOnlyType, Class framelessMutableType, + public void registerFrameType(Class mutableFrameMutableType, + Class fixedFrameMutableType, + Class frameReadOnlyType, + Class framelessMutableType, Class framelessReadOnlyType) { Objects.requireNonNull(framelessReadOnlyType, "Frameless read-only type cannot be null."); @@ -386,7 +389,9 @@ public void assertOverloadingWithFrameObjects(Class typeWithFrameMethods, Cla * @param minNumberOfFramelessArguments threshold used to filter out methods to assert in * {@code typeWithFramelessMethods}. */ - public void assertOverloadingWithFrameObjects(Class typeWithFrameMethods, Class typeWithFramelessMethods, boolean assertAllCombinations, + public void assertOverloadingWithFrameObjects(Class typeWithFrameMethods, + Class typeWithFramelessMethods, + boolean assertAllCombinations, int minNumberOfFramelessArguments) { assertOverloadingWithFrameObjects(typeWithFrameMethods, typeWithFramelessMethods, assertAllCombinations, minNumberOfFramelessArguments, m -> true); @@ -453,8 +458,11 @@ public static Predicate methodFilterFromSignature(MethodSignature signat * {@code framelessMethodFilter.test(method)} returns * {@code true}. */ - public void assertOverloadingWithFrameObjects(Class typeWithFrameMethods, Class typeWithFramelessMethods, boolean assertAllCombinations, - int minNumberOfFramelessArguments, Predicate framelessMethodFilter) + public void assertOverloadingWithFrameObjects(Class typeWithFrameMethods, + Class typeWithFramelessMethods, + boolean assertAllCombinations, + int minNumberOfFramelessArguments, + Predicate framelessMethodFilter) { // The frame methods are all the methods from 'typeWithFramelessMethods' that have at least one geometry argument. Predicate filter = framelessMethodFilter.and(atLeastNFramelessParameters(minNumberOfFramelessArguments)); @@ -473,8 +481,10 @@ public void assertOverloadingWithFrameObjects(Class typeWithFrameMethods, Cla } } - private void assertMethodOverloadedWithSpecificSignature(Class typeWithOverloadingMethods, Class typeWithOriginalMethods, - MethodSignature originalSignature, MethodSignature overloadingSignature) + private void assertMethodOverloadedWithSpecificSignature(Class typeWithOverloadingMethods, + Class typeWithOriginalMethods, + MethodSignature originalSignature, + MethodSignature overloadingSignature) throws SecurityException { try @@ -1280,7 +1290,8 @@ public void assertStaticMethodsCheckReferenceFrame(Class typeDeclaringStaticM * @param numberOfIterations number of iterations to perform for each method. * @throws Throwable if an unexpected throwable has been thrown by a method at invocation time. */ - public void assertMethodsOfReferenceFrameHolderCheckReferenceFrame(RandomFrameTypeBuilder frameTypeBuilder, Predicate methodFilter, + public void assertMethodsOfReferenceFrameHolderCheckReferenceFrame(RandomFrameTypeBuilder frameTypeBuilder, + Predicate methodFilter, int numberOfIterations) throws Throwable { @@ -1463,13 +1474,17 @@ else if (frameMethod.getName().endsWith(INCLUDING_FRAME)) } } - private void assertSetIncludingFrameChecksFrames(RandomFrameTypeBuilder frameTypeBuilder, Method setIncludingFrameMethod, ReferenceFrame frameA, + private void assertSetIncludingFrameChecksFrames(RandomFrameTypeBuilder frameTypeBuilder, + Method setIncludingFrameMethod, + ReferenceFrame frameA, ReferenceFrame frameB) { // For reference frame check, it should behave the same as setMatchingFrame. assertSetMatchingFrameChecksFrames(frameTypeBuilder, setIncludingFrameMethod, frameA, frameB); } - private void assertSetMatchingFrameChecksFrames(RandomFrameTypeBuilder frameTypeBuilder, Method setMatchingFrameMethod, ReferenceFrame frameA, + private void assertSetMatchingFrameChecksFrames(RandomFrameTypeBuilder frameTypeBuilder, + Method setMatchingFrameMethod, + ReferenceFrame frameA, ReferenceFrame frameB) { ReferenceFrameHolder frameObject = frameTypeBuilder.newInstance(random, frameA); @@ -1580,7 +1595,9 @@ public void assertStaticMethodsPreserveFunctionality(Class typeWithFrameMetho * returns {@code true}. * @param numberOfIterations number of iterations to perform for each method. */ - public void assertStaticMethodsPreserveFunctionality(Class typeWithFrameMethodsToTest, Class typeWithFramelessMethods, Predicate methodFilter, + public void assertStaticMethodsPreserveFunctionality(Class typeWithFrameMethodsToTest, + Class typeWithFramelessMethods, + Predicate methodFilter, int numberOfIterations) { List frameMethods = Stream.of(typeWithFrameMethodsToTest.getMethods()).filter(methodFilter).collect(Collectors.toList()); @@ -1708,8 +1725,10 @@ public void assertStaticMethodsPreserveFunctionality(Class typeWithFrameMetho * {@code true}. * @param numberOfIterations number of iterations to perform for each method. */ - public void assertFrameMethodsOfFrameHolderPreserveFunctionality(FrameTypeCopier frameTypeCopier, RandomFramelessTypeBuilder framelessTypeBuilber, - Predicate methodFilter, int numberOfIterations) + public void assertFrameMethodsOfFrameHolderPreserveFunctionality(FrameTypeCopier frameTypeCopier, + RandomFramelessTypeBuilder framelessTypeBuilber, + Predicate methodFilter, + int numberOfIterations) { assertFrameMethodsOfFrameHolderPreserveFunctionality(frameTypeCopier, framelessTypeBuilber, methodFilter, numberOfIterations, EPSILON); } @@ -1734,8 +1753,11 @@ public void assertFrameMethodsOfFrameHolderPreserveFunctionality(FrameTypeCopier * @param epsilon the tolerance to use when comparing geometries. * @param numberOfIterations number of iterations to perform for each method. */ - public void assertFrameMethodsOfFrameHolderPreserveFunctionality(FrameTypeCopier frameTypeCopier, RandomFramelessTypeBuilder framelessTypeBuilber, - Predicate methodFilter, int numberOfIterations, double epsilon) + public void assertFrameMethodsOfFrameHolderPreserveFunctionality(FrameTypeCopier frameTypeCopier, + RandomFramelessTypeBuilder framelessTypeBuilber, + Predicate methodFilter, + int numberOfIterations, + double epsilon) { Class frameTypeToTest = frameTypeCopier.newInstance(worldFrame, framelessTypeBuilber.newInstance(random)).getClass(); if (frameTypeToTest.isAnonymousClass()) @@ -1891,7 +1913,9 @@ private static void failToThrowReferenceFrameMismatchException(Class typeDecl failToThrowReferenceFrameMismatchException(typeDeclaringMethod, frameMethod, parameters, null); } - private static void failToThrowReferenceFrameMismatchException(Class typeDeclaringMethod, Method frameMethod, Object[] parameters, + private static void failToThrowReferenceFrameMismatchException(Class typeDeclaringMethod, + Method frameMethod, + Object[] parameters, Throwable exceptionThrownInstead) throws AssertionError { @@ -1919,7 +1943,9 @@ private static void debugSecurityException(Class typeWithFramelessMethods, St } } - private static void debugNoSuchMethodException(Class typeWithFrameMethodsToTest, Class typeWithFramelessMethods, Method frameMethod, + private static void debugNoSuchMethodException(Class typeWithFrameMethodsToTest, + Class typeWithFramelessMethods, + Method frameMethod, Class[] framelessMethodParameterTypes) { if (DEBUG) @@ -1957,8 +1983,12 @@ private static void reportInconsistentException(Method frameMethod, Method frame throw new AssertionError(message); } - private void reportInconsistentArguments(Method frameMethod, Method framelessMethod, Object[] frameMethodParameters, Object[] framelessMethodParameters, - Object framelessParameter, Object frameParameter) + private void reportInconsistentArguments(Method frameMethod, + Method framelessMethod, + Object[] frameMethodParameters, + Object[] framelessMethodParameters, + Object framelessParameter, + Object frameParameter) throws AssertionError { String message = ""; diff --git a/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameBoundingBox2DReadOnly.java b/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameBoundingBox2DReadOnly.java index 32962f848..f8a5520d0 100644 --- a/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameBoundingBox2DReadOnly.java +++ b/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameBoundingBox2DReadOnly.java @@ -438,7 +438,8 @@ default int intersectionWithLine2D(FrameLine2DReadOnly line2D, FramePoint2DBasic * @throws ReferenceFrameMismatchException if any of the arguments is not expressed in the same * reference frame as {@code this}. */ - default int intersectionWithLine2D(FrameLine2DReadOnly line2D, FixedFramePoint2DBasics firstIntersectionToPack, + default int intersectionWithLine2D(FrameLine2DReadOnly line2D, + FixedFramePoint2DBasics firstIntersectionToPack, FixedFramePoint2DBasics secondIntersectionToPack) { return intersectionWithLine2D(line2D.getPoint(), line2D.getDirection(), firstIntersectionToPack, secondIntersectionToPack); @@ -465,7 +466,9 @@ default int intersectionWithLine2D(FrameLine2DReadOnly line2D, FixedFramePoint2D * @throws ReferenceFrameMismatchException if any of the frame arguments is not expressed in the * same reference frame as {@code this}. */ - default int intersectionWithLine2D(FramePoint2DReadOnly pointOnLine, FrameVector2DReadOnly lineDirection, Point2DBasics firstIntersectionToPack, + default int intersectionWithLine2D(FramePoint2DReadOnly pointOnLine, + FrameVector2DReadOnly lineDirection, + Point2DBasics firstIntersectionToPack, Point2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(pointOnLine, lineDirection); @@ -493,7 +496,9 @@ default int intersectionWithLine2D(FramePoint2DReadOnly pointOnLine, FrameVector * @throws ReferenceFrameMismatchException if any of the frame arguments is not expressed in the * same reference frame as {@code this}. */ - default int intersectionWithLine2D(Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection, FixedFramePoint2DBasics firstIntersectionToPack, + default int intersectionWithLine2D(Point2DReadOnly pointOnLine, + Vector2DReadOnly lineDirection, + FixedFramePoint2DBasics firstIntersectionToPack, FixedFramePoint2DBasics secondIntersectionToPack) { if (firstIntersectionToPack != null) @@ -521,7 +526,9 @@ default int intersectionWithLine2D(Point2DReadOnly pointOnLine, Vector2DReadOnly * 0 or 2. * @throws RuntimeException if this bounding box is improper according to {@link #checkBounds()}. */ - default int intersectionWithLine2D(Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection, FramePoint2DBasics firstIntersectionToPack, + default int intersectionWithLine2D(Point2DReadOnly pointOnLine, + Vector2DReadOnly lineDirection, + FramePoint2DBasics firstIntersectionToPack, FramePoint2DBasics secondIntersectionToPack) { if (firstIntersectionToPack != null) @@ -552,7 +559,9 @@ default int intersectionWithLine2D(Point2DReadOnly pointOnLine, Vector2DReadOnly * @throws ReferenceFrameMismatchException if any of the arguments is not expressed in the same * reference frame as {@code this}. */ - default int intersectionWithLine2D(FramePoint2DReadOnly pointOnLine, FrameVector2DReadOnly lineDirection, FixedFramePoint2DBasics firstIntersectionToPack, + default int intersectionWithLine2D(FramePoint2DReadOnly pointOnLine, + FrameVector2DReadOnly lineDirection, + FixedFramePoint2DBasics firstIntersectionToPack, FixedFramePoint2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(pointOnLine, lineDirection); @@ -585,7 +594,9 @@ default int intersectionWithLine2D(FramePoint2DReadOnly pointOnLine, FrameVector * not expressed in the same reference frame as * {@code this}. */ - default int intersectionWithLine2D(FramePoint2DReadOnly pointOnLine, FrameVector2DReadOnly lineDirection, FramePoint2DBasics firstIntersectionToPack, + default int intersectionWithLine2D(FramePoint2DReadOnly pointOnLine, + FrameVector2DReadOnly lineDirection, + FramePoint2DBasics firstIntersectionToPack, FramePoint2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(pointOnLine, lineDirection); @@ -625,7 +636,8 @@ default int intersectionWithLine2D(FramePoint2DReadOnly pointOnLine, FrameVector * @throws ReferenceFrameMismatchException if any of the frame arguments is not expressed in the * same reference frame as {@code this}. */ - default int intersectionWithLineSegment2D(FrameLineSegment2DReadOnly lineSegment2D, Point2DBasics firstIntersectionToPack, + default int intersectionWithLineSegment2D(FrameLineSegment2DReadOnly lineSegment2D, + Point2DBasics firstIntersectionToPack, Point2DBasics secondIntersectionToPack) { return intersectionWithLineSegment2D(lineSegment2D.getFirstEndpoint(), @@ -660,7 +672,8 @@ default int intersectionWithLineSegment2D(FrameLineSegment2DReadOnly lineSegment * equal to 0, 1, or 2. * @throws RuntimeException if this bounding box is improper according to {@link #checkBounds()}. */ - default int intersectionWithLineSegment2D(LineSegment2DReadOnly lineSegment2D, FramePoint2DBasics firstIntersectionToPack, + default int intersectionWithLineSegment2D(LineSegment2DReadOnly lineSegment2D, + FramePoint2DBasics firstIntersectionToPack, FramePoint2DBasics secondIntersectionToPack) { return intersectionWithLineSegment2D(lineSegment2D.getFirstEndpoint(), @@ -698,7 +711,8 @@ default int intersectionWithLineSegment2D(LineSegment2DReadOnly lineSegment2D, F * @throws ReferenceFrameMismatchException if any of the frame arguments is not expressed in the * same reference frame as {@code this}. */ - default int intersectionWithLineSegment2D(LineSegment2DReadOnly lineSegment2D, FixedFramePoint2DBasics firstIntersectionToPack, + default int intersectionWithLineSegment2D(LineSegment2DReadOnly lineSegment2D, + FixedFramePoint2DBasics firstIntersectionToPack, FixedFramePoint2DBasics secondIntersectionToPack) { return intersectionWithLineSegment2D(lineSegment2D.getFirstEndpoint(), @@ -736,7 +750,8 @@ default int intersectionWithLineSegment2D(LineSegment2DReadOnly lineSegment2D, F * @throws ReferenceFrameMismatchException if {@code lineSegment2D} is not expressed in the same * reference frame as {@code this}. */ - default int intersectionWithLineSegment2D(FrameLineSegment2DReadOnly lineSegment2D, FramePoint2DBasics firstIntersectionToPack, + default int intersectionWithLineSegment2D(FrameLineSegment2DReadOnly lineSegment2D, + FramePoint2DBasics firstIntersectionToPack, FramePoint2DBasics secondIntersectionToPack) { return intersectionWithLineSegment2D(lineSegment2D.getFirstEndpoint(), @@ -774,7 +789,8 @@ default int intersectionWithLineSegment2D(FrameLineSegment2DReadOnly lineSegment * @throws ReferenceFrameMismatchException if any of the arguments is not expressed in the same * reference frame as {@code this}. */ - default int intersectionWithLineSegment2D(FrameLineSegment2DReadOnly lineSegment2D, FixedFramePoint2DBasics firstIntersectionToPack, + default int intersectionWithLineSegment2D(FrameLineSegment2DReadOnly lineSegment2D, + FixedFramePoint2DBasics firstIntersectionToPack, FixedFramePoint2DBasics secondIntersectionToPack) { return intersectionWithLineSegment2D(lineSegment2D.getFirstEndpoint(), @@ -813,7 +829,9 @@ default int intersectionWithLineSegment2D(FrameLineSegment2DReadOnly lineSegment * @throws ReferenceFrameMismatchException if any of the frame arguments is not expressed in the * same reference frame as {@code this}. */ - default int intersectionWithLineSegment2D(FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd, Point2DBasics firstIntersectionToPack, + default int intersectionWithLineSegment2D(FramePoint2DReadOnly lineSegmentStart, + FramePoint2DReadOnly lineSegmentEnd, + Point2DBasics firstIntersectionToPack, Point2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(lineSegmentStart, lineSegmentEnd); @@ -850,7 +868,9 @@ default int intersectionWithLineSegment2D(FramePoint2DReadOnly lineSegmentStart, * @throws ReferenceFrameMismatchException if any of the frame arguments is not expressed in the * same reference frame as {@code this}. */ - default int intersectionWithLineSegment2D(Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd, FixedFramePoint2DBasics firstIntersectionToPack, + default int intersectionWithLineSegment2D(Point2DReadOnly lineSegmentStart, + Point2DReadOnly lineSegmentEnd, + FixedFramePoint2DBasics firstIntersectionToPack, FixedFramePoint2DBasics secondIntersectionToPack) { if (firstIntersectionToPack != null) @@ -887,7 +907,9 @@ default int intersectionWithLineSegment2D(Point2DReadOnly lineSegmentStart, Poin * equal to 0, 1, or 2. * @throws RuntimeException if this bounding box is improper according to {@link #checkBounds()}. */ - default int intersectionWithLineSegment2D(Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd, FramePoint2DBasics firstIntersectionToPack, + default int intersectionWithLineSegment2D(Point2DReadOnly lineSegmentStart, + Point2DReadOnly lineSegmentEnd, + FramePoint2DBasics firstIntersectionToPack, FramePoint2DBasics secondIntersectionToPack) { if (firstIntersectionToPack != null) @@ -927,8 +949,10 @@ default int intersectionWithLineSegment2D(Point2DReadOnly lineSegmentStart, Poin * @throws ReferenceFrameMismatchException if any of the arguments is not expressed in the same * reference frame as {@code this}. */ - default int intersectionWithLineSegment2D(FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd, - FixedFramePoint2DBasics firstIntersectionToPack, FixedFramePoint2DBasics secondIntersectionToPack) + default int intersectionWithLineSegment2D(FramePoint2DReadOnly lineSegmentStart, + FramePoint2DReadOnly lineSegmentEnd, + FixedFramePoint2DBasics firstIntersectionToPack, + FixedFramePoint2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(lineSegmentStart, lineSegmentEnd); if (firstIntersectionToPack != null) @@ -969,8 +993,10 @@ default int intersectionWithLineSegment2D(FramePoint2DReadOnly lineSegmentStart, * {@code lineSegmentEnd} is not expressed in the same * reference frame as {@code this}. */ - default int intersectionWithLineSegment2D(FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd, - FramePoint2DBasics firstIntersectionToPack, FramePoint2DBasics secondIntersectionToPack) + default int intersectionWithLineSegment2D(FramePoint2DReadOnly lineSegmentStart, + FramePoint2DReadOnly lineSegmentEnd, + FramePoint2DBasics firstIntersectionToPack, + FramePoint2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(lineSegmentStart, lineSegmentEnd); if (firstIntersectionToPack != null) @@ -1009,7 +1035,9 @@ default int intersectionWithLineSegment2D(FramePoint2DReadOnly lineSegmentStart, * @throws ReferenceFrameMismatchException if any of the frame arguments is not expressed in the * same reference frame as {@code this}. */ - default int intersectionWithRay2D(FramePoint2DReadOnly rayOrigin, FrameVector2DReadOnly rayDirection, Point2DBasics firstIntersectionToPack, + default int intersectionWithRay2D(FramePoint2DReadOnly rayOrigin, + FrameVector2DReadOnly rayDirection, + Point2DBasics firstIntersectionToPack, Point2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(rayOrigin, rayDirection); @@ -1042,7 +1070,9 @@ default int intersectionWithRay2D(FramePoint2DReadOnly rayOrigin, FrameVector2DR * 0, 1, or 2. * @throws RuntimeException if this bounding box is improper according to {@link #checkBounds()}. */ - default int intersectionWithRay2D(Point2DReadOnly rayOrigin, Vector2DReadOnly rayDirection, FramePoint2DBasics firstIntersectionToPack, + default int intersectionWithRay2D(Point2DReadOnly rayOrigin, + Vector2DReadOnly rayDirection, + FramePoint2DBasics firstIntersectionToPack, FramePoint2DBasics secondIntersectionToPack) { if (firstIntersectionToPack != null) @@ -1081,7 +1111,9 @@ default int intersectionWithRay2D(Point2DReadOnly rayOrigin, Vector2DReadOnly ra * @throws ReferenceFrameMismatchException if any of the frame arguments is not expressed in the * same reference frame as {@code this}. */ - default int intersectionWithRay2D(Point2DReadOnly rayOrigin, Vector2DReadOnly rayDirection, FixedFramePoint2DBasics firstIntersectionToPack, + default int intersectionWithRay2D(Point2DReadOnly rayOrigin, + Vector2DReadOnly rayDirection, + FixedFramePoint2DBasics firstIntersectionToPack, FixedFramePoint2DBasics secondIntersectionToPack) { if (firstIntersectionToPack != null) @@ -1121,7 +1153,9 @@ default int intersectionWithRay2D(Point2DReadOnly rayOrigin, Vector2DReadOnly ra * not expressed in the same reference frame as * {@code this}. */ - default int intersectionWithRay2D(FramePoint2DReadOnly rayOrigin, FrameVector2DReadOnly rayDirection, FramePoint2DBasics firstIntersectionToPack, + default int intersectionWithRay2D(FramePoint2DReadOnly rayOrigin, + FrameVector2DReadOnly rayDirection, + FramePoint2DBasics firstIntersectionToPack, FramePoint2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(rayOrigin, rayDirection); @@ -1161,7 +1195,9 @@ default int intersectionWithRay2D(FramePoint2DReadOnly rayOrigin, FrameVector2DR * @throws ReferenceFrameMismatchException if any of the arguments is not expressed in the same * reference frame as {@code this}. */ - default int intersectionWithRay2D(FramePoint2DReadOnly rayOrigin, FrameVector2DReadOnly rayDirection, FixedFramePoint2DBasics firstIntersectionToPack, + default int intersectionWithRay2D(FramePoint2DReadOnly rayOrigin, + FrameVector2DReadOnly rayDirection, + FixedFramePoint2DBasics firstIntersectionToPack, FixedFramePoint2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(rayOrigin, rayDirection); diff --git a/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameBoundingBox3DReadOnly.java b/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameBoundingBox3DReadOnly.java index 35ff6ad0b..593518935 100644 --- a/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameBoundingBox3DReadOnly.java +++ b/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameBoundingBox3DReadOnly.java @@ -537,7 +537,8 @@ default int intersectionWithLine3D(Line3DReadOnly line3D, FramePoint3DBasics fir * @throws ReferenceFrameMismatchException if any of the arguments is not expressed in the same * reference frame as {@code this}. */ - default int intersectionWithLine3D(FrameLine3DReadOnly line3D, FixedFramePoint3DBasics firstIntersectionToPack, + default int intersectionWithLine3D(FrameLine3DReadOnly line3D, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(line3D); @@ -593,7 +594,9 @@ default int intersectionWithLine3D(FrameLine3DReadOnly line3D, FramePoint3DBasic * @throws ReferenceFrameMismatchException if any of the frame arguments is not expressed in the * same reference frame as {@code this}. */ - default int intersectionWithLine3D(FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection, Point3DBasics firstIntersectionToPack, + default int intersectionWithLine3D(FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(pointOnLine, lineDirection); @@ -618,7 +621,9 @@ default int intersectionWithLine3D(FramePoint3DReadOnly pointOnLine, FrameVector * @throws ReferenceFrameMismatchException if any of the frame arguments is not expressed in the * same reference frame as {@code this}. */ - default int intersectionWithLine3D(Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection, FixedFramePoint3DBasics firstIntersectionToPack, + default int intersectionWithLine3D(Point3DReadOnly pointOnLine, + Vector3DReadOnly lineDirection, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { if (firstIntersectionToPack != null) @@ -644,7 +649,9 @@ default int intersectionWithLine3D(Point3DReadOnly pointOnLine, Vector3DReadOnly * @return the number of intersections between the line and this bounding box. It is either equal to * 0 or 2. */ - default int intersectionWithLine3D(Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection, FramePoint3DBasics firstIntersectionToPack, + default int intersectionWithLine3D(Point3DReadOnly pointOnLine, + Vector3DReadOnly lineDirection, + FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) { if (firstIntersectionToPack != null) @@ -672,7 +679,9 @@ default int intersectionWithLine3D(Point3DReadOnly pointOnLine, Vector3DReadOnly * @throws ReferenceFrameMismatchException if any of the arguments is not expressed in the same * reference frame as {@code this}. */ - default int intersectionWithLine3D(FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection, FixedFramePoint3DBasics firstIntersectionToPack, + default int intersectionWithLine3D(FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(pointOnLine, lineDirection); @@ -704,7 +713,9 @@ default int intersectionWithLine3D(FramePoint3DReadOnly pointOnLine, FrameVector * not expressed in the same reference frame as * {@code this}. */ - default int intersectionWithLine3D(FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection, FramePoint3DBasics firstIntersectionToPack, + default int intersectionWithLine3D(FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, + FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(pointOnLine, lineDirection); @@ -742,7 +753,8 @@ default int intersectionWithLine3D(FramePoint3DReadOnly pointOnLine, FrameVector * @throws ReferenceFrameMismatchException if the frame argument is not expressed in the same * reference frame as {@code this}. */ - default int intersectionWithLineSegment3D(FrameLineSegment3DReadOnly lineSegment3D, Point3DBasics firstIntersectionToPack, + default int intersectionWithLineSegment3D(FrameLineSegment3DReadOnly lineSegment3D, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(lineSegment3D); @@ -776,7 +788,8 @@ default int intersectionWithLineSegment3D(FrameLineSegment3DReadOnly lineSegment * @throws ReferenceFrameMismatchException if any of the frame arguments is not expressed in the * same reference frame as {@code this}. */ - default int intersectionWithLineSegment3D(LineSegment3DReadOnly lineSegment3D, FixedFramePoint3DBasics firstIntersectionToPack, + default int intersectionWithLineSegment3D(LineSegment3DReadOnly lineSegment3D, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { if (firstIntersectionToPack != null) @@ -811,7 +824,8 @@ default int intersectionWithLineSegment3D(LineSegment3DReadOnly lineSegment3D, F * @return the number of intersections between the line segment and this bounding box. It is either * equal to 0, 1, or 2. */ - default int intersectionWithLineSegment3D(LineSegment3DReadOnly lineSegment3D, FramePoint3DBasics firstIntersectionToPack, + default int intersectionWithLineSegment3D(LineSegment3DReadOnly lineSegment3D, + FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) { if (firstIntersectionToPack != null) @@ -848,7 +862,8 @@ default int intersectionWithLineSegment3D(LineSegment3DReadOnly lineSegment3D, F * @throws ReferenceFrameMismatchException if any of the arguments is not expressed in the same * reference frame as {@code this}. */ - default int intersectionWithLineSegment3D(FrameLineSegment3DReadOnly lineSegment3D, FixedFramePoint3DBasics firstIntersectionToPack, + default int intersectionWithLineSegment3D(FrameLineSegment3DReadOnly lineSegment3D, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(lineSegment3D); @@ -886,7 +901,8 @@ default int intersectionWithLineSegment3D(FrameLineSegment3DReadOnly lineSegment * @throws ReferenceFrameMismatchException if {@code lineSegment3D} is not expressed in the same * reference frame as {@code this}. */ - default int intersectionWithLineSegment3D(FrameLineSegment3DReadOnly lineSegment3D, FramePoint3DBasics firstIntersectionToPack, + default int intersectionWithLineSegment3D(FrameLineSegment3DReadOnly lineSegment3D, + FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(lineSegment3D); @@ -925,7 +941,9 @@ default int intersectionWithLineSegment3D(FrameLineSegment3DReadOnly lineSegment * @throws ReferenceFrameMismatchException if any of the frame arguments is not expressed in the * same reference frame as {@code this}. */ - default int intersectionWithLineSegment3D(FramePoint3DReadOnly lineSegmentStart, FramePoint3DReadOnly lineSegmentEnd, Point3DBasics firstIntersectionToPack, + default int intersectionWithLineSegment3D(FramePoint3DReadOnly lineSegmentStart, + FramePoint3DReadOnly lineSegmentEnd, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(lineSegmentStart, lineSegmentEnd); @@ -960,7 +978,9 @@ default int intersectionWithLineSegment3D(FramePoint3DReadOnly lineSegmentStart, * @throws ReferenceFrameMismatchException if any of the frame arguments is not expressed in the * same reference frame as {@code this}. */ - default int intersectionWithLineSegment3D(Point3DReadOnly lineSegmentStart, Point3DReadOnly lineSegmentEnd, FixedFramePoint3DBasics firstIntersectionToPack, + default int intersectionWithLineSegment3D(Point3DReadOnly lineSegmentStart, + Point3DReadOnly lineSegmentEnd, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { if (firstIntersectionToPack != null) @@ -996,7 +1016,9 @@ default int intersectionWithLineSegment3D(Point3DReadOnly lineSegmentStart, Poin * @return the number of intersections between the line segment and this bounding box. It is either * equal to 0, 1, or 2. */ - default int intersectionWithLineSegment3D(Point3DReadOnly lineSegmentStart, Point3DReadOnly lineSegmentEnd, FramePoint3DBasics firstIntersectionToPack, + default int intersectionWithLineSegment3D(Point3DReadOnly lineSegmentStart, + Point3DReadOnly lineSegmentEnd, + FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) { if (firstIntersectionToPack != null) @@ -1034,8 +1056,10 @@ default int intersectionWithLineSegment3D(Point3DReadOnly lineSegmentStart, Poin * @throws ReferenceFrameMismatchException if any of the arguments is not expressed in the same * reference frame as {@code this}. */ - default int intersectionWithLineSegment3D(FramePoint3DReadOnly lineSegmentStart, FramePoint3DReadOnly lineSegmentEnd, - FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) + default int intersectionWithLineSegment3D(FramePoint3DReadOnly lineSegmentStart, + FramePoint3DReadOnly lineSegmentEnd, + FixedFramePoint3DBasics firstIntersectionToPack, + FixedFramePoint3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(lineSegmentStart, lineSegmentEnd); if (firstIntersectionToPack != null) @@ -1074,8 +1098,10 @@ default int intersectionWithLineSegment3D(FramePoint3DReadOnly lineSegmentStart, * {@code lineSegmentEnd} is not expressed in the same * reference frame as {@code this}. */ - default int intersectionWithLineSegment3D(FramePoint3DReadOnly lineSegmentStart, FramePoint3DReadOnly lineSegmentEnd, - FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) + default int intersectionWithLineSegment3D(FramePoint3DReadOnly lineSegmentStart, + FramePoint3DReadOnly lineSegmentEnd, + FramePoint3DBasics firstIntersectionToPack, + FramePoint3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(lineSegmentStart, lineSegmentEnd); if (firstIntersectionToPack != null) @@ -1112,7 +1138,9 @@ default int intersectionWithLineSegment3D(FramePoint3DReadOnly lineSegmentStart, * @throws ReferenceFrameMismatchException if any of the frame arguments is not expressed in the * same reference frame as {@code this}. */ - default int intersectionWithRay3D(FramePoint3DReadOnly rayOrigin, FrameVector3DReadOnly rayDirection, Point3DBasics firstIntersectionToPack, + default int intersectionWithRay3D(FramePoint3DReadOnly rayOrigin, + FrameVector3DReadOnly rayDirection, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(rayOrigin, rayDirection); @@ -1146,7 +1174,9 @@ default int intersectionWithRay3D(FramePoint3DReadOnly rayOrigin, FrameVector3DR * @throws ReferenceFrameMismatchException if any of the frame arguments is not expressed in the * same reference frame as {@code this}. */ - default int intersectionWithRay3D(Point3DReadOnly rayOrigin, Vector3DReadOnly rayDirection, FixedFramePoint3DBasics firstIntersectionToPack, + default int intersectionWithRay3D(Point3DReadOnly rayOrigin, + Vector3DReadOnly rayDirection, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { if (firstIntersectionToPack != null) @@ -1181,7 +1211,9 @@ default int intersectionWithRay3D(Point3DReadOnly rayOrigin, Vector3DReadOnly ra * @return the number of intersections between the ray and this bounding box. It is either equal to * 0, 1, or 2. */ - default int intersectionWithRay3D(Point3DReadOnly rayOrigin, Vector3DReadOnly rayDirection, FramePoint3DBasics firstIntersectionToPack, + default int intersectionWithRay3D(Point3DReadOnly rayOrigin, + Vector3DReadOnly rayDirection, + FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) { if (firstIntersectionToPack != null) @@ -1218,7 +1250,9 @@ default int intersectionWithRay3D(Point3DReadOnly rayOrigin, Vector3DReadOnly ra * @throws ReferenceFrameMismatchException if any of the arguments is not expressed in the same * reference frame as {@code this}. */ - default int intersectionWithRay3D(FramePoint3DReadOnly rayOrigin, FrameVector3DReadOnly rayDirection, FixedFramePoint3DBasics firstIntersectionToPack, + default int intersectionWithRay3D(FramePoint3DReadOnly rayOrigin, + FrameVector3DReadOnly rayDirection, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(rayOrigin, rayDirection); @@ -1257,7 +1291,9 @@ default int intersectionWithRay3D(FramePoint3DReadOnly rayOrigin, FrameVector3DR * not expressed in the same reference frame as * {@code this}. */ - default int intersectionWithRay3D(FramePoint3DReadOnly rayOrigin, FrameVector3DReadOnly rayDirection, FramePoint3DBasics firstIntersectionToPack, + default int intersectionWithRay3D(FramePoint3DReadOnly rayOrigin, + FrameVector3DReadOnly rayDirection, + FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) { checkReferenceFrameMatch(rayOrigin, rayDirection); diff --git a/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameCommonMatrix3DBasics.java b/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameCommonMatrix3DBasics.java index 9a43285bd..4dcf88761 100644 --- a/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameCommonMatrix3DBasics.java +++ b/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameCommonMatrix3DBasics.java @@ -208,7 +208,15 @@ default void setIncludingFrame(ReferenceFrame referenceFrame, int startRow, int * @param m21 the new 3rd row 2nd column coefficient for this matrix. * @param m22 the new 3rd row 3rd column coefficient for this matrix. */ - default void setIncludingFrame(ReferenceFrame referenceFrame, double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, + default void setIncludingFrame(ReferenceFrame referenceFrame, + double m00, + double m01, + double m02, + double m10, + double m11, + double m12, + double m20, + double m21, double m22) { setReferenceFrame(referenceFrame); diff --git a/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameConvexPolygon2DReadOnly.java b/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameConvexPolygon2DReadOnly.java index e7aaa9250..f8e6cf608 100644 --- a/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameConvexPolygon2DReadOnly.java +++ b/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameConvexPolygon2DReadOnly.java @@ -1300,7 +1300,8 @@ default int intersectionWith(LineSegment2DReadOnly lineSegment2D, FixedFramePoin * not expressed in the same reference frame. * @see #intersectionWith(LineSegment2DReadOnly, Point2DBasics, Point2DBasics) */ - default int intersectionWith(LineSegment2DReadOnly lineSegment2D, FixedFramePoint2DBasics firstIntersectionToPack, + default int intersectionWith(LineSegment2DReadOnly lineSegment2D, + FixedFramePoint2DBasics firstIntersectionToPack, FixedFramePoint2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(firstIntersectionToPack, secondIntersectionToPack); @@ -1361,7 +1362,8 @@ default int intersectionWith(FrameLineSegment2DReadOnly lineSegment2D, Point2DBa * not expressed in the same reference frame. * @see #intersectionWith(LineSegment2DReadOnly, Point2DBasics, Point2DBasics) */ - default int intersectionWith(FrameLineSegment2DReadOnly lineSegment2D, Point2DBasics firstIntersectionToPack, + default int intersectionWith(FrameLineSegment2DReadOnly lineSegment2D, + Point2DBasics firstIntersectionToPack, FixedFramePoint2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(lineSegment2D, secondIntersectionToPack); @@ -1383,7 +1385,8 @@ default int intersectionWith(FrameLineSegment2DReadOnly lineSegment2D, Point2DBa * expressed in the same reference frame. * @see #intersectionWith(LineSegment2DReadOnly, Point2DBasics, Point2DBasics) */ - default int intersectionWith(FrameLineSegment2DReadOnly lineSegment2D, FixedFramePoint2DBasics firstIntersectionToPack, + default int intersectionWith(FrameLineSegment2DReadOnly lineSegment2D, + FixedFramePoint2DBasics firstIntersectionToPack, Point2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(lineSegment2D, firstIntersectionToPack); @@ -1406,7 +1409,8 @@ default int intersectionWith(FrameLineSegment2DReadOnly lineSegment2D, FixedFram * not expressed in the same reference frame. * @see #intersectionWith(LineSegment2DReadOnly, Point2DBasics, Point2DBasics) */ - default int intersectionWith(FrameLineSegment2DReadOnly lineSegment2D, FixedFramePoint2DBasics firstIntersectionToPack, + default int intersectionWith(FrameLineSegment2DReadOnly lineSegment2D, + FixedFramePoint2DBasics firstIntersectionToPack, FixedFramePoint2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(lineSegment2D, firstIntersectionToPack, secondIntersectionToPack); @@ -1427,7 +1431,8 @@ default int intersectionWith(FrameLineSegment2DReadOnly lineSegment2D, FixedFram * expressed in the same reference frame. * @see #intersectionWith(LineSegment2DReadOnly, Point2DBasics, Point2DBasics) */ - default int intersectionWith(FrameLineSegment2DReadOnly lineSegment2D, FramePoint2DBasics firstIntersectionToPack, + default int intersectionWith(FrameLineSegment2DReadOnly lineSegment2D, + FramePoint2DBasics firstIntersectionToPack, FramePoint2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(lineSegment2D); diff --git a/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameLine2DReadOnly.java b/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameLine2DReadOnly.java index 948ca5b25..31b60d078 100644 --- a/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameLine2DReadOnly.java +++ b/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameLine2DReadOnly.java @@ -688,7 +688,8 @@ default int intersectionWith(ConvexPolygon2DReadOnly convexPolygon, FixedFramePo * same reference frame. * @see #intersectionWith(ConvexPolygon2DReadOnly, Point2DBasics, Point2DBasics) */ - default int intersectionWith(FrameConvexPolygon2DReadOnly convexPolygon, FixedFramePoint2DBasics firstIntersectionToPack, + default int intersectionWith(FrameConvexPolygon2DReadOnly convexPolygon, + FixedFramePoint2DBasics firstIntersectionToPack, Point2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(convexPolygon, firstIntersectionToPack); @@ -749,7 +750,8 @@ default int intersectionWith(FrameConvexPolygon2DReadOnly convexPolygon, FramePo * same reference frame. * @see #intersectionWith(ConvexPolygon2DReadOnly, Point2DBasics, Point2DBasics) */ - default int intersectionWith(ConvexPolygon2DReadOnly convexPolygon, FixedFramePoint2DBasics firstIntersectionToPack, + default int intersectionWith(ConvexPolygon2DReadOnly convexPolygon, + FixedFramePoint2DBasics firstIntersectionToPack, FixedFramePoint2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(firstIntersectionToPack, secondIntersectionToPack); @@ -772,7 +774,8 @@ default int intersectionWith(ConvexPolygon2DReadOnly convexPolygon, FixedFramePo * same reference frame. * @see #intersectionWith(ConvexPolygon2DReadOnly, Point2DBasics, Point2DBasics) */ - default int intersectionWith(FrameConvexPolygon2DReadOnly convexPolygon, FixedFramePoint2DBasics firstIntersectionToPack, + default int intersectionWith(FrameConvexPolygon2DReadOnly convexPolygon, + FixedFramePoint2DBasics firstIntersectionToPack, FixedFramePoint2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(convexPolygon, firstIntersectionToPack, secondIntersectionToPack); @@ -812,7 +815,8 @@ default int intersectionWith(ConvexPolygon2DReadOnly convexPolygon, FramePoint2D * expressed in the same reference frame. * @see #intersectionWith(ConvexPolygon2DReadOnly, Point2DBasics, Point2DBasics) */ - default int intersectionWith(FrameConvexPolygon2DReadOnly convexPolygon, FramePoint2DBasics firstIntersectionToPack, + default int intersectionWith(FrameConvexPolygon2DReadOnly convexPolygon, + FramePoint2DBasics firstIntersectionToPack, FramePoint2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(convexPolygon); @@ -856,7 +860,8 @@ default int intersectionWith(ConvexPolygon2DReadOnly convexPolygon, Point2DBasic * same reference frame. * @see #intersectionWith(ConvexPolygon2DReadOnly, Point2DBasics, Point2DBasics) */ - default int intersectionWith(FrameConvexPolygon2DReadOnly convexPolygon, Point2DBasics firstIntersectionToPack, + default int intersectionWith(FrameConvexPolygon2DReadOnly convexPolygon, + Point2DBasics firstIntersectionToPack, FixedFramePoint2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(convexPolygon, secondIntersectionToPack); diff --git a/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameLine3DBasics.java b/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameLine3DBasics.java index b07267587..0abd1d1af 100644 --- a/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameLine3DBasics.java +++ b/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameLine3DBasics.java @@ -77,8 +77,13 @@ default void setToNaN(ReferenceFrame referenceFrame) * @param lineDirectionY the new y-component of the direction of this line. * @param lineDirectionZ the new z-component of the direction of this line. */ - default void setIncludingFrame(ReferenceFrame referenceFrame, double pointOnLineX, double pointOnLineY, double pointOnLineZ, double lineDirectionX, - double lineDirectionY, double lineDirectionZ) + default void setIncludingFrame(ReferenceFrame referenceFrame, + double pointOnLineX, + double pointOnLineY, + double pointOnLineZ, + double lineDirectionX, + double lineDirectionY, + double lineDirectionZ) { setReferenceFrame(referenceFrame); set(pointOnLineX, pointOnLineY, pointOnLineZ, lineDirectionX, lineDirectionY, lineDirectionZ); diff --git a/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameLine3DReadOnly.java b/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameLine3DReadOnly.java index 085983a46..b1a74c152 100644 --- a/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameLine3DReadOnly.java +++ b/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameLine3DReadOnly.java @@ -270,7 +270,8 @@ default double closestPointsWith(Line3DReadOnly otherLine, Point3DBasics closest * {@code closestPointOnThisLineToPack} are not expressed in * the same reference frame. */ - default double closestPointsWith(FrameLine3DReadOnly otherLine, FixedFramePoint3DBasics closestPointOnThisLineToPack, + default double closestPointsWith(FrameLine3DReadOnly otherLine, + FixedFramePoint3DBasics closestPointOnThisLineToPack, Point3DBasics closestPointOnOtherLineToPack) { checkReferenceFrameMatch(otherLine, closestPointOnThisLineToPack); @@ -313,7 +314,8 @@ default double closestPointsWith(FrameLine3DReadOnly otherLine, FramePoint3DBasi * {@code closestPointOnOtherLineToPack} are not expressed * in the same reference frame. */ - default double closestPointsWith(FrameLine3DReadOnly otherLine, Point3DBasics closestPointOnThisLineToPack, + default double closestPointsWith(FrameLine3DReadOnly otherLine, + Point3DBasics closestPointOnThisLineToPack, FixedFramePoint3DBasics closestPointOnOtherLineToPack) { checkReferenceFrameMatch(otherLine, closestPointOnOtherLineToPack); @@ -357,7 +359,8 @@ default double closestPointsWith(FrameLine3DReadOnly otherLine, Point3DBasics cl * {@code closestPointOnOtherLineToPack} are not expressed * in the same reference frame. */ - default double closestPointsWith(FrameLine3DReadOnly otherLine, FixedFramePoint3DBasics closestPointOnThisLineToPack, + default double closestPointsWith(FrameLine3DReadOnly otherLine, + FixedFramePoint3DBasics closestPointOnThisLineToPack, FixedFramePoint3DBasics closestPointOnOtherLineToPack) { checkReferenceFrameMatch(otherLine, closestPointOnThisLineToPack, closestPointOnOtherLineToPack); @@ -378,7 +381,8 @@ default double closestPointsWith(FrameLine3DReadOnly otherLine, FixedFramePoint3 * @throws ReferenceFrameMismatchException if {@code this} and {@code otherLine} are not expressed * in the same reference frame. */ - default double closestPointsWith(FrameLine3DReadOnly otherLine, FramePoint3DBasics closestPointOnThisLineToPack, + default double closestPointsWith(FrameLine3DReadOnly otherLine, + FramePoint3DBasics closestPointOnThisLineToPack, FramePoint3DBasics closestPointOnOtherLineToPack) { checkReferenceFrameMatch(otherLine); @@ -402,7 +406,8 @@ default double closestPointsWith(FrameLine3DReadOnly otherLine, FramePoint3DBasi * and {@code closestPointOnOtherLineToPack} are not * expressed in the same reference frame. */ - default double closestPointsWith(Line3DReadOnly otherLine, FixedFramePoint3DBasics closestPointOnThisLineToPack, + default double closestPointsWith(Line3DReadOnly otherLine, + FixedFramePoint3DBasics closestPointOnThisLineToPack, FixedFramePoint3DBasics closestPointOnOtherLineToPack) { checkReferenceFrameMatch(closestPointOnThisLineToPack, closestPointOnOtherLineToPack); diff --git a/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameLineSegment2DReadOnly.java b/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameLineSegment2DReadOnly.java index 03e9e029c..724d9fc46 100644 --- a/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameLineSegment2DReadOnly.java +++ b/src/frame/java/us/ihmc/euclid/referenceFrame/interfaces/FrameLineSegment2DReadOnly.java @@ -1108,7 +1108,8 @@ default int intersectionWith(ConvexPolygon2DReadOnly convexPolygon, FixedFramePo * same reference frame. * @see #intersectionWith(ConvexPolygon2DReadOnly, Point2DBasics, Point2DBasics) */ - default int intersectionWith(FrameConvexPolygon2DReadOnly convexPolygon, FixedFramePoint2DBasics firstIntersectionToPack, + default int intersectionWith(FrameConvexPolygon2DReadOnly convexPolygon, + FixedFramePoint2DBasics firstIntersectionToPack, Point2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(convexPolygon, firstIntersectionToPack); @@ -1189,7 +1190,8 @@ default int intersectionWith(ConvexPolygon2DReadOnly convexPolygon, Point2DBasic * same reference frame. * @see #intersectionWith(ConvexPolygon2DReadOnly, Point2DBasics, Point2DBasics) */ - default int intersectionWith(FrameConvexPolygon2DReadOnly convexPolygon, Point2DBasics firstIntersectionToPack, + default int intersectionWith(FrameConvexPolygon2DReadOnly convexPolygon, + Point2DBasics firstIntersectionToPack, FixedFramePoint2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(convexPolygon, secondIntersectionToPack); @@ -1250,7 +1252,8 @@ default int intersectionWith(FrameConvexPolygon2DReadOnly convexPolygon, Point2D * same reference frame. * @see #intersectionWith(ConvexPolygon2DReadOnly, Point2DBasics, Point2DBasics) */ - default int intersectionWith(ConvexPolygon2DReadOnly convexPolygon, FixedFramePoint2DBasics firstIntersectionToPack, + default int intersectionWith(ConvexPolygon2DReadOnly convexPolygon, + FixedFramePoint2DBasics firstIntersectionToPack, FixedFramePoint2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(firstIntersectionToPack, secondIntersectionToPack); @@ -1273,7 +1276,8 @@ default int intersectionWith(ConvexPolygon2DReadOnly convexPolygon, FixedFramePo * same reference frame. * @see #intersectionWith(ConvexPolygon2DReadOnly, Point2DBasics, Point2DBasics) */ - default int intersectionWith(FrameConvexPolygon2DReadOnly convexPolygon, FixedFramePoint2DBasics firstIntersectionToPack, + default int intersectionWith(FrameConvexPolygon2DReadOnly convexPolygon, + FixedFramePoint2DBasics firstIntersectionToPack, FixedFramePoint2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(convexPolygon, firstIntersectionToPack, secondIntersectionToPack); @@ -1313,7 +1317,8 @@ default int intersectionWith(ConvexPolygon2DReadOnly convexPolygon, FramePoint2D * expressed in the same reference frame. * @see #intersectionWith(ConvexPolygon2DReadOnly, Point2DBasics, Point2DBasics) */ - default int intersectionWith(FrameConvexPolygon2DReadOnly convexPolygon, FramePoint2DBasics firstIntersectionToPack, + default int intersectionWith(FrameConvexPolygon2DReadOnly convexPolygon, + FramePoint2DBasics firstIntersectionToPack, FramePoint2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(convexPolygon); 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 e6ddd26b1..cc7a6199e 100644 --- a/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameFactories.java +++ b/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameFactories.java @@ -190,7 +190,8 @@ public static FrameVector3DReadOnly newLinkedFrameVector3DReadOnly(DoubleSupplie * @param ySupplier the y-coordinate supplier. * @return the new read-only frame point 2D. */ - public static FramePoint2DReadOnly newLinkedFramePoint2DReadOnly(ReferenceFrameHolder referenceFrameHolder, DoubleSupplier xSupplier, + public static FramePoint2DReadOnly newLinkedFramePoint2DReadOnly(ReferenceFrameHolder referenceFrameHolder, + DoubleSupplier xSupplier, DoubleSupplier ySupplier) { return newLinkedFramePoint2DReadOnly(referenceFrameHolder, newLinkedPoint2DReadOnly(xSupplier, ySupplier)); @@ -205,7 +206,8 @@ public static FramePoint2DReadOnly newLinkedFramePoint2DReadOnly(ReferenceFrameH * @param ySupplier the y-coordinate supplier. * @return the new read-only frame vector 2D. */ - public static FrameVector2DReadOnly newLinkedFrameVector2DReadOnly(ReferenceFrameHolder referenceFrameHolder, DoubleSupplier xSupplier, + public static FrameVector2DReadOnly newLinkedFrameVector2DReadOnly(ReferenceFrameHolder referenceFrameHolder, + DoubleSupplier xSupplier, DoubleSupplier ySupplier) { return newLinkedFrameVector2DReadOnly(referenceFrameHolder, newLinkedVector2DReadOnly(xSupplier, ySupplier)); @@ -221,8 +223,10 @@ public static FrameVector2DReadOnly newLinkedFrameVector2DReadOnly(ReferenceFram * @param zSupplier the z-coordinate supplier. * @return the new read-only frame point 3D. */ - public static FramePoint3DReadOnly newLinkedFramePoint3DReadOnly(ReferenceFrameHolder referenceFrameHolder, DoubleSupplier xSupplier, - DoubleSupplier ySupplier, DoubleSupplier zSupplier) + public static FramePoint3DReadOnly newLinkedFramePoint3DReadOnly(ReferenceFrameHolder referenceFrameHolder, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier zSupplier) { return newLinkedFramePoint3DReadOnly(referenceFrameHolder, newLinkedPoint3DReadOnly(xSupplier, ySupplier, zSupplier)); } @@ -237,8 +241,10 @@ public static FramePoint3DReadOnly newLinkedFramePoint3DReadOnly(ReferenceFrameH * @param zSupplier the z-coordinate supplier. * @return the new read-only frame vector 3D. */ - public static FrameVector3DReadOnly newLinkedFrameVector3DReadOnly(ReferenceFrameHolder referenceFrameHolder, DoubleSupplier xSupplier, - DoubleSupplier ySupplier, DoubleSupplier zSupplier) + public static FrameVector3DReadOnly newLinkedFrameVector3DReadOnly(ReferenceFrameHolder referenceFrameHolder, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier zSupplier) { return newLinkedFrameVector3DReadOnly(referenceFrameHolder, newLinkedVector3DReadOnly(xSupplier, ySupplier, zSupplier)); } @@ -1378,7 +1384,9 @@ public static FixedFrameUnitVector3DBasics newFixedFrameUnitVector3DBasics(Refer * @param initialZ the initial value for the z-component of the new vector. * @return the new unitary vector. */ - public static FixedFrameUnitVector3DBasics newFixedFrameUnitVector3DBasics(ReferenceFrameHolder referenceFrameHolder, double initialX, double initialY, + public static FixedFrameUnitVector3DBasics newFixedFrameUnitVector3DBasics(ReferenceFrameHolder referenceFrameHolder, + double initialX, + double initialY, double initialZ) { return newLinkedFixedFrameUnitVector3DBasics(referenceFrameHolder, new UnitVector3D(initialX, initialY, initialZ)); @@ -2163,7 +2171,8 @@ public static FixedFramePoint2DBasics newObservableFixedFramePoint2DBasics(Refer * @return the observable frame point. */ public static FixedFramePoint2DBasics newObservableFixedFramePoint2DBasics(ObjDoubleConsumer valueChangedListener, - Consumer valueAccessedListener, FixedFramePoint2DBasics source) + Consumer valueAccessedListener, + FixedFramePoint2DBasics source) { return newLinkedFixedFramePoint2DBasics(source, newObservablePoint2DBasics(valueChangedListener, valueAccessedListener, source)); } @@ -2205,7 +2214,8 @@ public static FixedFramePoint3DBasics newObservableFixedFramePoint3DBasics(Refer * @return the observable frame point. */ public static FixedFramePoint3DBasics newObservableFixedFramePoint3DBasics(ObjDoubleConsumer valueChangedListener, - Consumer valueAccessedListener, FixedFramePoint3DBasics source) + Consumer valueAccessedListener, + FixedFramePoint3DBasics source) { return newLinkedFixedFramePoint3DBasics(source, newObservablePoint3DBasics(valueChangedListener, valueAccessedListener, source)); } @@ -2248,7 +2258,8 @@ public static FixedFrameVector2DBasics newObservableFixedFrameVector2DBasics(Ref * @return the observable frame vector. */ public static FixedFrameVector2DBasics newObservableFixedFrameVector2DBasics(ObjDoubleConsumer valueChangedListener, - Consumer valueAccessedListener, FixedFrameVector2DBasics source) + Consumer valueAccessedListener, + FixedFrameVector2DBasics source) { return newLinkedFixedFrameVector2DBasics(source, newObservableVector2DBasics(valueChangedListener, valueAccessedListener, source)); } @@ -2291,7 +2302,8 @@ public static FixedFrameVector3DBasics newObservableFixedFrameVector3DBasics(Ref * @return the observable frame vector. */ public static FixedFrameVector3DBasics newObservableFixedFrameVector3DBasics(ObjDoubleConsumer valueChangedListener, - Consumer valueAccessedListener, FixedFrameVector3DBasics source) + Consumer valueAccessedListener, + FixedFrameVector3DBasics source) { return newLinkedFixedFrameVector3DBasics(source, newObservableVector3DBasics(valueChangedListener, valueAccessedListener, source)); } @@ -2441,7 +2453,8 @@ public static FixedFrameRotationMatrixBasics newObservableFixedFrameRotationMatr * passed. Can be {@code null}. * @return the observable frame quaternion. */ - public static FixedFrameQuaternionBasics newObservableFixedFrameQuaternionBasics(ReferenceFrameHolder referenceFrameHolder, Runnable valueChangedListener, + public static FixedFrameQuaternionBasics newObservableFixedFrameQuaternionBasics(ReferenceFrameHolder referenceFrameHolder, + Runnable valueChangedListener, IntConsumer valueAccessedListener) { return newObservableFixedFrameQuaternionBasics(valueChangedListener, valueAccessedListener, newFixedFrameQuaternionBasics(referenceFrameHolder)); @@ -2460,7 +2473,8 @@ public static FixedFrameQuaternionBasics newObservableFixedFrameQuaternionBasics * linked quaternion interface. * @return the observable frame quaternion. */ - public static FixedFrameQuaternionBasics newObservableFixedFrameQuaternionBasics(Runnable valueChangedListener, IntConsumer valueAccessedListener, + public static FixedFrameQuaternionBasics newObservableFixedFrameQuaternionBasics(Runnable valueChangedListener, + IntConsumer valueAccessedListener, FixedFrameQuaternionBasics source) { return newLinkedFixedFrameQuaternionBasics(source, newObservableQuaternionBasics(valueChangedListener, valueAccessedListener, source)); diff --git a/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameRandomTools.java b/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameRandomTools.java index 0f7ca3cc9..56e2cbf4a 100644 --- a/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameRandomTools.java +++ b/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameRandomTools.java @@ -222,7 +222,10 @@ public static ReferenceFrame[] nextReferenceFrameTree(String frameNamePrefix, Ra * @return the array containing the random reference frames and {@code rootFrame} at the first * index. */ - public static ReferenceFrame[] nextReferenceFrameTree(String frameNamePrefix, Random random, ReferenceFrame rootFrame, int numberOfReferenceFrames, + public static ReferenceFrame[] nextReferenceFrameTree(String frameNamePrefix, + Random random, + ReferenceFrame rootFrame, + int numberOfReferenceFrames, boolean use2DTransforms) { ReferenceFrame[] referenceFrames = new ReferenceFrame[numberOfReferenceFrames + 1]; @@ -333,7 +336,13 @@ public static FramePoint3D nextFramePoint3D(Random random, ReferenceFrame refere * @return the random frame point. * @throws RuntimeException if {@code maxX < minX}, {@code maxY < minY}, {@code maxZ < minZ}. */ - public static FramePoint3D nextFramePoint3D(Random random, ReferenceFrame referenceFrame, double minX, double maxX, double minY, double maxY, double minZ, + public static FramePoint3D nextFramePoint3D(Random random, + ReferenceFrame referenceFrame, + double minX, + double maxX, + double minY, + double maxY, + double minZ, double maxZ) { return new FramePoint3D(referenceFrame, EuclidCoreRandomTools.nextPoint3D(random, minX, maxX, minY, maxY, minZ, maxZ)); @@ -444,7 +453,13 @@ public static FrameVector3D nextFrameVector3D(Random random, ReferenceFrame refe * @return the random vector. * @throws RuntimeException if {@code maxX < minX}, {@code maxY < minY}, {@code maxZ < minZ}. */ - public static FrameVector3D nextFrameVector3D(Random random, ReferenceFrame referenceFrame, double minX, double maxX, double minY, double maxY, double minZ, + public static FrameVector3D nextFrameVector3D(Random random, + ReferenceFrame referenceFrame, + double minX, + double maxX, + double minY, + double maxY, + double minZ, double maxZ) { return new FrameVector3D(referenceFrame, EuclidCoreRandomTools.nextVector3D(random, minX, maxX, minY, maxY, minZ, maxZ)); @@ -485,7 +500,9 @@ public static FrameVector3D nextOrthogonalFrameVector3D(Random random, FrameVect * @param normalize whether to normalize the generated frame vector or not. * @return the random frame vector. */ - public static FrameVector3D nextOrthogonalFrameVector3D(Random random, ReferenceFrame referenceFrame, Vector3DReadOnly vectorToBeOrthogonalTo, + public static FrameVector3D nextOrthogonalFrameVector3D(Random random, + ReferenceFrame referenceFrame, + Vector3DReadOnly vectorToBeOrthogonalTo, boolean normalize) { return new FrameVector3D(referenceFrame, EuclidCoreRandomTools.nextOrthogonalVector3D(random, vectorToBeOrthogonalTo, normalize)); diff --git a/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameTestTools.java b/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameTestTools.java index db549b9a5..9976cb3c9 100644 --- a/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameTestTools.java +++ b/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameTestTools.java @@ -67,7 +67,9 @@ public static void assertRotationFrameVectorGeometricallyEquals(FrameVector3DRea * expressed in the same reference frame. If only one of the arguments is * equal to {@code null}. */ - public static void assertRotationFrameVectorGeometricallyEquals(String messagePrefix, FrameVector3DReadOnly expected, FrameVector3DReadOnly actual, + public static void assertRotationFrameVectorGeometricallyEquals(String messagePrefix, + FrameVector3DReadOnly expected, + FrameVector3DReadOnly actual, double epsilon) { assertRotationFrameVectorGeometricallyEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); @@ -90,8 +92,11 @@ public static void assertRotationFrameVectorGeometricallyEquals(String messagePr * expressed in the same reference frame. If only one of the arguments is * equal to {@code null}. */ - public static void assertRotationFrameVectorGeometricallyEquals(String messagePrefix, FrameVector3DReadOnly expected, FrameVector3DReadOnly actual, - double epsilon, String format) + public static void assertRotationFrameVectorGeometricallyEquals(String messagePrefix, + FrameVector3DReadOnly expected, + FrameVector3DReadOnly actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -171,7 +176,10 @@ public static void assertFrameYawPitchRollEquals(String messagePrefix, FrameYawP * @throws AssertionError if the two yaw-pitch-rolls are not equal or not expressed in the same * reference frame. If only one of the arguments is equal to {@code null}. */ - public static void assertFrameYawPitchRollEquals(String messagePrefix, FrameYawPitchRollReadOnly expected, FrameYawPitchRollReadOnly actual, double epsilon, + public static void assertFrameYawPitchRollEquals(String messagePrefix, + FrameYawPitchRollReadOnly expected, + FrameYawPitchRollReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -219,7 +227,9 @@ public static void assertFrameYawPitchRollGeometricallyEquals(FrameYawPitchRollR * expressed in the same reference frame. If only one of the arguments is * equal to {@code null}. */ - public static void assertFrameYawPitchRollGeometricallyEquals(String messagePrefix, FrameYawPitchRollReadOnly expected, FrameYawPitchRollReadOnly actual, + public static void assertFrameYawPitchRollGeometricallyEquals(String messagePrefix, + FrameYawPitchRollReadOnly expected, + FrameYawPitchRollReadOnly actual, double epsilon) { assertFrameYawPitchRollGeometricallyEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); @@ -241,8 +251,11 @@ public static void assertFrameYawPitchRollGeometricallyEquals(String messagePref * expressed in the same reference frame. If only one of the arguments is * equal to {@code null}. */ - public static void assertFrameYawPitchRollGeometricallyEquals(String messagePrefix, FrameYawPitchRollReadOnly expected, FrameYawPitchRollReadOnly actual, - double epsilon, String format) + public static void assertFrameYawPitchRollGeometricallyEquals(String messagePrefix, + FrameYawPitchRollReadOnly expected, + FrameYawPitchRollReadOnly actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -378,7 +391,10 @@ public static void assertFramePoint2DGeometricallyEquals(String messagePrefix, F * expressed in the reference frame. If only one of the arguments is equal to * {@code null}. */ - public static void assertFramePoint2DGeometricallyEquals(String messagePrefix, FramePoint2DReadOnly expected, FramePoint2DReadOnly actual, double epsilon, + public static void assertFramePoint2DGeometricallyEquals(String messagePrefix, + FramePoint2DReadOnly expected, + FramePoint2DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -450,7 +466,10 @@ public static void assertFrameVector2DGeometricallyEquals(String messagePrefix, * expressed in the reference frame. If only one of the arguments is equal to * {@code null}. */ - public static void assertFrameVector2DGeometricallyEquals(String messagePrefix, FrameVector2DReadOnly expected, FrameVector2DReadOnly actual, double epsilon, + public static void assertFrameVector2DGeometricallyEquals(String messagePrefix, + FrameVector2DReadOnly expected, + FrameVector2DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -587,7 +606,10 @@ public static void assertFramePoint3DGeometricallyEquals(String messagePrefix, F * expressed in the reference frame. If only one of the arguments is equal to * {@code null}. */ - public static void assertFramePoint3DGeometricallyEquals(String messagePrefix, FramePoint3DReadOnly expected, FramePoint3DReadOnly actual, double epsilon, + public static void assertFramePoint3DGeometricallyEquals(String messagePrefix, + FramePoint3DReadOnly expected, + FramePoint3DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -659,7 +681,10 @@ public static void assertFrameVector3DGeometricallyEquals(String messagePrefix, * expressed in the reference frame. If only one of the arguments is equal to * {@code null}. */ - public static void assertFrameVector3DGeometricallyEquals(String messagePrefix, FrameVector3DReadOnly expected, FrameVector3DReadOnly actual, double epsilon, + public static void assertFrameVector3DGeometricallyEquals(String messagePrefix, + FrameVector3DReadOnly expected, + FrameVector3DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -796,7 +821,10 @@ public static void assertFrameVector4DGeometricallyEquals(String messagePrefix, * the reference frame. If only one of the arguments is equal to * {@code null}. */ - public static void assertFrameVector4DGeometricallyEquals(String messagePrefix, FrameVector4DReadOnly expected, FrameVector4DReadOnly actual, double epsilon, + public static void assertFrameVector4DGeometricallyEquals(String messagePrefix, + FrameVector4DReadOnly expected, + FrameVector4DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -846,7 +874,9 @@ public static void assertFrameQuaternionGeometricallyEquals(FrameQuaternionReadO * in the reference frame. If only one of the arguments is equal to * {@code null}. */ - public static void assertFrameQuaternionGeometricallyEquals(String messagePrefix, FrameQuaternionReadOnly expected, FrameQuaternionReadOnly actual, + public static void assertFrameQuaternionGeometricallyEquals(String messagePrefix, + FrameQuaternionReadOnly expected, + FrameQuaternionReadOnly actual, double epsilon) { assertFrameQuaternionGeometricallyEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); @@ -869,8 +899,11 @@ public static void assertFrameQuaternionGeometricallyEquals(String messagePrefix * in the reference frame. If only one of the arguments is equal to * {@code null}. */ - public static void assertFrameQuaternionGeometricallyEquals(String messagePrefix, FrameQuaternionReadOnly expected, FrameQuaternionReadOnly actual, - double epsilon, String format) + public static void assertFrameQuaternionGeometricallyEquals(String messagePrefix, + FrameQuaternionReadOnly expected, + FrameQuaternionReadOnly actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -917,7 +950,9 @@ public static void assertFrameOrientation2DEquals(FrameOrientation2DReadOnly exp * @throws AssertionError if the two orientation 2Ds are not equal or not expressed in the reference * frame. If only one of the arguments is equal to {@code null}. */ - public static void assertFrameOrientation2DEquals(String messagePrefix, FrameOrientation2DReadOnly expected, FrameOrientation2DReadOnly actual, + public static void assertFrameOrientation2DEquals(String messagePrefix, + FrameOrientation2DReadOnly expected, + FrameOrientation2DReadOnly actual, double epsilon) { assertFrameOrientation2DEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); @@ -939,8 +974,11 @@ public static void assertFrameOrientation2DEquals(String messagePrefix, FrameOri * @throws AssertionError if the two orientation 2Ds are not equal or not expressed in the reference * frame. If only one of the arguments is equal to {@code null}. */ - public static void assertFrameOrientation2DEquals(String messagePrefix, FrameOrientation2DReadOnly expected, FrameOrientation2DReadOnly actual, - double epsilon, String format) + public static void assertFrameOrientation2DEquals(String messagePrefix, + FrameOrientation2DReadOnly expected, + FrameOrientation2DReadOnly actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -987,7 +1025,9 @@ public static void assertFrameOrientation2DGeometricallyEquals(FrameOrientation2 * expressed in the reference frame. If only one of the arguments is equal to * {@code null}. */ - public static void assertFrameOrientation2DGeometricallyEquals(String messagePrefix, FrameOrientation2DReadOnly expected, FrameOrientation2DReadOnly actual, + public static void assertFrameOrientation2DGeometricallyEquals(String messagePrefix, + FrameOrientation2DReadOnly expected, + FrameOrientation2DReadOnly actual, double epsilon) { assertFrameOrientation2DGeometricallyEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); @@ -1009,8 +1049,11 @@ public static void assertFrameOrientation2DGeometricallyEquals(String messagePre * expressed in the reference frame. If only one of the arguments is equal to * {@code null}. */ - public static void assertFrameOrientation2DGeometricallyEquals(String messagePrefix, FrameOrientation2DReadOnly expected, FrameOrientation2DReadOnly actual, - double epsilon, String format) + public static void assertFrameOrientation2DGeometricallyEquals(String messagePrefix, + FrameOrientation2DReadOnly expected, + FrameOrientation2DReadOnly actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -1075,7 +1118,10 @@ public static void assertFrameMatrix3DEquals(String messagePrefix, FrameMatrix3D * @throws AssertionError if the two matrices are not equal or not expressed in the reference frame. * If only one of the arguments is equal to {@code null}. */ - public static void assertFrameMatrix3DEquals(String messagePrefix, FrameMatrix3DReadOnly expected, FrameMatrix3DReadOnly actual, double epsilon, + public static void assertFrameMatrix3DEquals(String messagePrefix, + FrameMatrix3DReadOnly expected, + FrameMatrix3DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -1123,8 +1169,10 @@ public static void assertFrameRotationMatrixGeometricallyEquals(FrameRotationMat * expressed in the reference frame. If only one of the arguments is equal to * {@code null}. */ - public static void assertFrameRotationMatrixGeometricallyEquals(String messagePrefix, FrameRotationMatrixReadOnly expected, - FrameRotationMatrixReadOnly actual, double epsilon) + public static void assertFrameRotationMatrixGeometricallyEquals(String messagePrefix, + FrameRotationMatrixReadOnly expected, + FrameRotationMatrixReadOnly actual, + double epsilon) { assertFrameRotationMatrixGeometricallyEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); } @@ -1145,8 +1193,11 @@ public static void assertFrameRotationMatrixGeometricallyEquals(String messagePr * expressed in the reference frame. If only one of the arguments is equal to * {@code null}. */ - public static void assertFrameRotationMatrixGeometricallyEquals(String messagePrefix, FrameRotationMatrixReadOnly expected, - FrameRotationMatrixReadOnly actual, double epsilon, String format) + public static void assertFrameRotationMatrixGeometricallyEquals(String messagePrefix, + FrameRotationMatrixReadOnly expected, + FrameRotationMatrixReadOnly actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -1193,7 +1244,9 @@ public static void assertFrameBoundingBox2DEquals(FrameBoundingBox2DReadOnly exp * @throws AssertionError if the two bounding box 2Ds are not equal or not expressed in the * reference frame. If only one of the arguments is equal to {@code null}. */ - public static void assertFrameBoundingBox2DEquals(String messagePrefix, FrameBoundingBox2DReadOnly expected, FrameBoundingBox2DReadOnly actual, + public static void assertFrameBoundingBox2DEquals(String messagePrefix, + FrameBoundingBox2DReadOnly expected, + FrameBoundingBox2DReadOnly actual, double epsilon) { assertFrameBoundingBox2DEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); @@ -1215,8 +1268,11 @@ public static void assertFrameBoundingBox2DEquals(String messagePrefix, FrameBou * @throws AssertionError if the two bounding box 2Ds are not equal or not expressed in the * reference frame. If only one of the arguments is equal to {@code null}. */ - public static void assertFrameBoundingBox2DEquals(String messagePrefix, FrameBoundingBox2DReadOnly expected, FrameBoundingBox2DReadOnly actual, - double epsilon, String format) + public static void assertFrameBoundingBox2DEquals(String messagePrefix, + FrameBoundingBox2DReadOnly expected, + FrameBoundingBox2DReadOnly actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -1265,7 +1321,9 @@ public static void assertFrameBoundingBox2DGeometricallyEquals(FrameBoundingBox2 * expressed in the reference frame. If only one of the arguments is equal to * {@code null}. */ - public static void assertFrameBoundingBox2DGeometricallyEquals(String messagePrefix, FrameBoundingBox2DReadOnly expected, FrameBoundingBox2DReadOnly actual, + public static void assertFrameBoundingBox2DGeometricallyEquals(String messagePrefix, + FrameBoundingBox2DReadOnly expected, + FrameBoundingBox2DReadOnly actual, double epsilon) { assertFrameBoundingBox2DGeometricallyEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); @@ -1288,8 +1346,11 @@ public static void assertFrameBoundingBox2DGeometricallyEquals(String messagePre * expressed in the reference frame. If only one of the arguments is equal to * {@code null}. */ - public static void assertFrameBoundingBox2DGeometricallyEquals(String messagePrefix, FrameBoundingBox2DReadOnly expected, FrameBoundingBox2DReadOnly actual, - double epsilon, String format) + public static void assertFrameBoundingBox2DGeometricallyEquals(String messagePrefix, + FrameBoundingBox2DReadOnly expected, + FrameBoundingBox2DReadOnly actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -1336,7 +1397,9 @@ public static void assertFrameBoundingBox3DEquals(FrameBoundingBox3DReadOnly exp * @throws AssertionError if the two bounding box 3Ds are not equal or not expressed in the * reference frame. If only one of the arguments is equal to {@code null}. */ - public static void assertFrameBoundingBox3DEquals(String messagePrefix, FrameBoundingBox3DReadOnly expected, FrameBoundingBox3DReadOnly actual, + public static void assertFrameBoundingBox3DEquals(String messagePrefix, + FrameBoundingBox3DReadOnly expected, + FrameBoundingBox3DReadOnly actual, double epsilon) { assertFrameBoundingBox3DEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); @@ -1358,8 +1421,11 @@ public static void assertFrameBoundingBox3DEquals(String messagePrefix, FrameBou * @throws AssertionError if the two bounding box 3Ds are not equal or not expressed in the * reference frame. If only one of the arguments is equal to {@code null}. */ - public static void assertFrameBoundingBox3DEquals(String messagePrefix, FrameBoundingBox3DReadOnly expected, FrameBoundingBox3DReadOnly actual, - double epsilon, String format) + public static void assertFrameBoundingBox3DEquals(String messagePrefix, + FrameBoundingBox3DReadOnly expected, + FrameBoundingBox3DReadOnly actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -1408,7 +1474,9 @@ public static void assertFrameBoundingBox3DGeometricallyEquals(FrameBoundingBox3 * expressed in the reference frame. If only one of the arguments is equal to * {@code null}. */ - public static void assertFrameBoundingBox3DGeometricallyEquals(String messagePrefix, FrameBoundingBox3DReadOnly expected, FrameBoundingBox3DReadOnly actual, + public static void assertFrameBoundingBox3DGeometricallyEquals(String messagePrefix, + FrameBoundingBox3DReadOnly expected, + FrameBoundingBox3DReadOnly actual, double epsilon) { assertFrameBoundingBox3DGeometricallyEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); @@ -1431,8 +1499,11 @@ public static void assertFrameBoundingBox3DGeometricallyEquals(String messagePre * expressed in the reference frame. If only one of the arguments is equal to * {@code null}. */ - public static void assertFrameBoundingBox3DGeometricallyEquals(String messagePrefix, FrameBoundingBox3DReadOnly expected, FrameBoundingBox3DReadOnly actual, - double epsilon, String format) + public static void assertFrameBoundingBox3DGeometricallyEquals(String messagePrefix, + FrameBoundingBox3DReadOnly expected, + FrameBoundingBox3DReadOnly actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -1479,7 +1550,9 @@ public static void assertFrameConvexPolygon2DEquals(FrameConvexPolygon2DReadOnly * @throws AssertionError if the two frame convex polygon 2D are not equal or not expressed in the * reference frame. If only one of the arguments is equal to {@code null}. */ - public static void assertFrameConvexPolygon2DEquals(String messagePrefix, FrameConvexPolygon2DReadOnly expected, FrameConvexPolygon2DReadOnly actual, + public static void assertFrameConvexPolygon2DEquals(String messagePrefix, + FrameConvexPolygon2DReadOnly expected, + FrameConvexPolygon2DReadOnly actual, double epsilon) { assertFrameConvexPolygon2DEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); @@ -1500,8 +1573,11 @@ public static void assertFrameConvexPolygon2DEquals(String messagePrefix, FrameC * @throws AssertionError if the two frame convex polygon 2D are not equal or not expressed in the * reference frame. If only one of the arguments is equal to {@code null}. */ - public static void assertFrameConvexPolygon2DEquals(String messagePrefix, FrameConvexPolygon2DReadOnly expected, FrameConvexPolygon2DReadOnly actual, - double epsilon, String format) + public static void assertFrameConvexPolygon2DEquals(String messagePrefix, + FrameConvexPolygon2DReadOnly expected, + FrameConvexPolygon2DReadOnly actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -1548,8 +1624,10 @@ public static void assertFrameConvexPolygon2DGeometricallyEquals(FrameConvexPoly * not expressed in the reference frame. If only one of the arguments is * equal to {@code null}. */ - public static void assertFrameConvexPolygon2DGeometricallyEquals(String messagePrefix, FrameConvexPolygon2DReadOnly expected, - FrameConvexPolygon2DReadOnly actual, double epsilon) + public static void assertFrameConvexPolygon2DGeometricallyEquals(String messagePrefix, + FrameConvexPolygon2DReadOnly expected, + FrameConvexPolygon2DReadOnly actual, + double epsilon) { assertFrameConvexPolygon2DGeometricallyEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); } @@ -1570,8 +1648,11 @@ public static void assertFrameConvexPolygon2DGeometricallyEquals(String messageP * not expressed in the reference frame. If only one of the arguments is * equal to {@code null}. */ - public static void assertFrameConvexPolygon2DGeometricallyEquals(String messagePrefix, FrameConvexPolygon2DReadOnly expected, - FrameConvexPolygon2DReadOnly actual, double epsilon, String format) + public static void assertFrameConvexPolygon2DGeometricallyEquals(String messagePrefix, + FrameConvexPolygon2DReadOnly expected, + FrameConvexPolygon2DReadOnly actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -1642,7 +1723,9 @@ private static void throwNotEqualAssertionError(String messagePrefix, FrameBound throwNotEqualAssertionError(messagePrefix, expectedAsString, actualAsString); } - private static void throwNotEqualAssertionError(String messagePrefix, FrameConvexPolygon2DReadOnly expected, FrameConvexPolygon2DReadOnly actual, + private static void throwNotEqualAssertionError(String messagePrefix, + FrameConvexPolygon2DReadOnly expected, + FrameConvexPolygon2DReadOnly actual, String format) { String expectedAsString = EuclidFrameIOTools.getFrameConvexPolygon2DString(format, expected); 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 6d1ed5b86..fcbefea8a 100644 --- a/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameTools.java +++ b/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameTools.java @@ -69,8 +69,11 @@ private EuclidFrameTools() * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean areLine2DsCollinear(FramePoint2DReadOnly firstPointOnLine1, FramePoint2DReadOnly secondPointOnLine1, - FramePoint2DReadOnly firstPointOnLine2, FramePoint2DReadOnly secondPointOnLine2, double angleEpsilon, + public static boolean areLine2DsCollinear(FramePoint2DReadOnly firstPointOnLine1, + FramePoint2DReadOnly secondPointOnLine1, + FramePoint2DReadOnly firstPointOnLine2, + FramePoint2DReadOnly secondPointOnLine2, + double angleEpsilon, double distanceEpsilon) { firstPointOnLine1.checkReferenceFrameMatch(secondPointOnLine1, firstPointOnLine2, secondPointOnLine2); @@ -106,8 +109,12 @@ public static boolean areLine2DsCollinear(FramePoint2DReadOnly firstPointOnLine1 * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean areLine2DsCollinear(FramePoint2DReadOnly pointOnLine1, FrameVector2DReadOnly lineDirection1, FramePoint2DReadOnly firstPointOnLine2, - FramePoint2DReadOnly secondPointOnLine2, double angleEpsilon, double distanceEpsilon) + public static boolean areLine2DsCollinear(FramePoint2DReadOnly pointOnLine1, + FrameVector2DReadOnly lineDirection1, + FramePoint2DReadOnly firstPointOnLine2, + FramePoint2DReadOnly secondPointOnLine2, + double angleEpsilon, + double distanceEpsilon) { pointOnLine1.checkReferenceFrameMatch(lineDirection1, firstPointOnLine2, secondPointOnLine2); return EuclidGeometryTools.areLine2DsCollinear(pointOnLine1, lineDirection1, firstPointOnLine2, secondPointOnLine2, angleEpsilon, distanceEpsilon); @@ -137,8 +144,12 @@ public static boolean areLine2DsCollinear(FramePoint2DReadOnly pointOnLine1, Fra * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean areLine2DsCollinear(FramePoint2DReadOnly pointOnLine1, FrameVector2DReadOnly lineDirection1, FramePoint2DReadOnly pointOnLine2, - FrameVector2DReadOnly lineDirection2, double angleEpsilon, double distanceEpsilon) + public static boolean areLine2DsCollinear(FramePoint2DReadOnly pointOnLine1, + FrameVector2DReadOnly lineDirection1, + FramePoint2DReadOnly pointOnLine2, + FrameVector2DReadOnly lineDirection2, + double angleEpsilon, + double distanceEpsilon) { pointOnLine1.checkReferenceFrameMatch(lineDirection1, pointOnLine2, lineDirection2); return EuclidGeometryTools.areLine2DsCollinear(pointOnLine1, lineDirection1, pointOnLine2, lineDirection2, angleEpsilon, distanceEpsilon); @@ -168,8 +179,11 @@ public static boolean areLine2DsCollinear(FramePoint2DReadOnly pointOnLine1, Fra * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean areLine3DsCollinear(FramePoint3DReadOnly firstPointOnLine1, FramePoint3DReadOnly secondPointOnLine1, - FramePoint3DReadOnly firstPointOnLine2, FramePoint3DReadOnly secondPointOnLine2, double angleEpsilon, + public static boolean areLine3DsCollinear(FramePoint3DReadOnly firstPointOnLine1, + FramePoint3DReadOnly secondPointOnLine1, + FramePoint3DReadOnly firstPointOnLine2, + FramePoint3DReadOnly secondPointOnLine2, + double angleEpsilon, double distanceEpsilon) { firstPointOnLine1.checkReferenceFrameMatch(secondPointOnLine1, firstPointOnLine2, secondPointOnLine2); @@ -205,8 +219,12 @@ public static boolean areLine3DsCollinear(FramePoint3DReadOnly firstPointOnLine1 * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean areLine3DsCollinear(FramePoint3DReadOnly pointOnLine1, FrameVector3DReadOnly lineDirection1, FramePoint3DReadOnly pointOnLine2, - FrameVector3DReadOnly lineDirection2, double angleEpsilon, double distanceEpsilon) + public static boolean areLine3DsCollinear(FramePoint3DReadOnly pointOnLine1, + FrameVector3DReadOnly lineDirection1, + FramePoint3DReadOnly pointOnLine2, + FrameVector3DReadOnly lineDirection2, + double angleEpsilon, + double distanceEpsilon) { pointOnLine1.checkReferenceFrameMatch(lineDirection1, pointOnLine2, lineDirection2); return EuclidGeometryTools.areLine3DsCollinear(pointOnLine1, lineDirection1, pointOnLine2, lineDirection2, angleEpsilon, distanceEpsilon); @@ -241,8 +259,12 @@ public static boolean areLine3DsCollinear(FramePoint3DReadOnly pointOnLine1, Fra * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean arePlane3DsCoincident(FramePoint3DReadOnly pointOnPlane1, FrameVector3DReadOnly planeNormal1, FramePoint3DReadOnly pointOnPlane2, - FrameVector3DReadOnly planeNormal2, double angleEpsilon, double distanceEpsilon) + public static boolean arePlane3DsCoincident(FramePoint3DReadOnly pointOnPlane1, + FrameVector3DReadOnly planeNormal1, + FramePoint3DReadOnly pointOnPlane2, + FrameVector3DReadOnly planeNormal2, + double angleEpsilon, + double distanceEpsilon) { pointOnPlane1.checkReferenceFrameMatch(planeNormal1, pointOnPlane2, planeNormal2); return EuclidGeometryTools.arePlane3DsCoincident(pointOnPlane1, planeNormal1, pointOnPlane2, planeNormal2, angleEpsilon, distanceEpsilon); @@ -409,7 +431,8 @@ public static FramePoint3D averagePoint3Ds(FramePoint3DReadOnly a, FramePoint3DR * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static void orientation3DFromFirstToSecondVector3D(FrameVector3DReadOnly firstVector, FrameVector3DReadOnly secondVector, + public static void orientation3DFromFirstToSecondVector3D(FrameVector3DReadOnly firstVector, + FrameVector3DReadOnly secondVector, Orientation3DBasics rotationToPack) { firstVector.checkReferenceFrameMatch(secondVector); @@ -448,7 +471,8 @@ public static void orientation3DFromFirstToSecondVector3D(FrameVector3DReadOnly * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static void orientation3DFromFirstToSecondVector3D(FrameVector3DReadOnly firstVector, FrameVector3DReadOnly secondVector, + public static void orientation3DFromFirstToSecondVector3D(FrameVector3DReadOnly firstVector, + FrameVector3DReadOnly secondVector, FixedFrameOrientation3DBasics rotationToPack) { firstVector.checkReferenceFrameMatch(secondVector, rotationToPack); @@ -487,7 +511,8 @@ public static void orientation3DFromFirstToSecondVector3D(FrameVector3DReadOnly * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static void orientation3DFromFirstToSecondVector3D(FrameVector3DReadOnly firstVector, FrameVector3DReadOnly secondVector, + public static void orientation3DFromFirstToSecondVector3D(FrameVector3DReadOnly firstVector, + FrameVector3DReadOnly secondVector, FrameOrientation3DBasics rotationToPack) { firstVector.checkReferenceFrameMatch(secondVector); @@ -620,9 +645,12 @@ public static void orientation3DFromZUpToVector3D(FrameVector3DReadOnly vector, * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static double closestPoint3DsBetweenTwoLine3Ds(FramePoint3DReadOnly pointOnLine1, FrameVector3DReadOnly lineDirection1, - FramePoint3DReadOnly pointOnLine2, FrameVector3DReadOnly lineDirection2, - FixedFramePoint3DBasics closestPointOnLine1ToPack, FixedFramePoint3DBasics closestPointOnLine2ToPack) + public static double closestPoint3DsBetweenTwoLine3Ds(FramePoint3DReadOnly pointOnLine1, + FrameVector3DReadOnly lineDirection1, + FramePoint3DReadOnly pointOnLine2, + FrameVector3DReadOnly lineDirection2, + FixedFramePoint3DBasics closestPointOnLine1ToPack, + FixedFramePoint3DBasics closestPointOnLine2ToPack) { pointOnLine1.checkReferenceFrameMatch(lineDirection1, pointOnLine2, lineDirection2); if (closestPointOnLine1ToPack != null) @@ -654,9 +682,12 @@ public static double closestPoint3DsBetweenTwoLine3Ds(FramePoint3DReadOnly point * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static double closestPoint3DsBetweenTwoLine3Ds(FramePoint3DReadOnly pointOnLine1, FrameVector3DReadOnly lineDirection1, - FramePoint3DReadOnly pointOnLine2, FrameVector3DReadOnly lineDirection2, - FramePoint3DBasics closestPointOnLine1ToPack, FramePoint3DBasics closestPointOnLine2ToPack) + public static double closestPoint3DsBetweenTwoLine3Ds(FramePoint3DReadOnly pointOnLine1, + FrameVector3DReadOnly lineDirection1, + FramePoint3DReadOnly pointOnLine2, + FrameVector3DReadOnly lineDirection2, + FramePoint3DBasics closestPointOnLine1ToPack, + FramePoint3DBasics closestPointOnLine2ToPack) { pointOnLine1.checkReferenceFrameMatch(lineDirection1, pointOnLine2, lineDirection2); if (closestPointOnLine1ToPack != null) @@ -693,8 +724,10 @@ public static double closestPoint3DsBetweenTwoLine3Ds(FramePoint3DReadOnly point * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static double closestPoint2DsBetweenTwoLineSegment2Ds(FramePoint2DReadOnly lineSegmentStart1, FramePoint2DReadOnly lineSegmentEnd1, - FramePoint2DReadOnly lineSegmentStart2, FramePoint2DReadOnly lineSegmentEnd2, + public static double closestPoint2DsBetweenTwoLineSegment2Ds(FramePoint2DReadOnly lineSegmentStart1, + FramePoint2DReadOnly lineSegmentEnd1, + FramePoint2DReadOnly lineSegmentStart2, + FramePoint2DReadOnly lineSegmentEnd2, FixedFramePoint2DBasics closestPointOnLineSegment1ToPack, FixedFramePoint2DBasics closestPointOnLineSegment2ToPack) { @@ -733,8 +766,10 @@ public static double closestPoint2DsBetweenTwoLineSegment2Ds(FramePoint2DReadOnl * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static double closestPoint2DsBetweenTwoLineSegment2Ds(FramePoint2DReadOnly lineSegmentStart1, FramePoint2DReadOnly lineSegmentEnd1, - FramePoint2DReadOnly lineSegmentStart2, FramePoint2DReadOnly lineSegmentEnd2, + public static double closestPoint2DsBetweenTwoLineSegment2Ds(FramePoint2DReadOnly lineSegmentStart1, + FramePoint2DReadOnly lineSegmentEnd1, + FramePoint2DReadOnly lineSegmentStart2, + FramePoint2DReadOnly lineSegmentEnd2, FramePoint2DBasics closestPointOnLineSegment1ToPack, FramePoint2DBasics closestPointOnLineSegment2ToPack) { @@ -773,8 +808,10 @@ public static double closestPoint2DsBetweenTwoLineSegment2Ds(FramePoint2DReadOnl * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static double closestPoint3DsBetweenTwoLineSegment3Ds(FramePoint3DReadOnly lineSegmentStart1, FramePoint3DReadOnly lineSegmentEnd1, - FramePoint3DReadOnly lineSegmentStart2, FramePoint3DReadOnly lineSegmentEnd2, + public static double closestPoint3DsBetweenTwoLineSegment3Ds(FramePoint3DReadOnly lineSegmentStart1, + FramePoint3DReadOnly lineSegmentEnd1, + FramePoint3DReadOnly lineSegmentStart2, + FramePoint3DReadOnly lineSegmentEnd2, FixedFramePoint3DBasics closestPointOnLineSegment1ToPack, FixedFramePoint3DBasics closestPointOnLineSegment2ToPack) { @@ -813,8 +850,10 @@ public static double closestPoint3DsBetweenTwoLineSegment3Ds(FramePoint3DReadOnl * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static double closestPoint3DsBetweenTwoLineSegment3Ds(FramePoint3DReadOnly lineSegmentStart1, FramePoint3DReadOnly lineSegmentEnd1, - FramePoint3DReadOnly lineSegmentStart2, FramePoint3DReadOnly lineSegmentEnd2, + public static double closestPoint3DsBetweenTwoLineSegment3Ds(FramePoint3DReadOnly lineSegmentStart1, + FramePoint3DReadOnly lineSegmentEnd1, + FramePoint3DReadOnly lineSegmentStart2, + FramePoint3DReadOnly lineSegmentEnd2, FramePoint3DBasics closestPointOnLineSegment1ToPack, FramePoint3DBasics closestPointOnLineSegment2ToPack) { @@ -888,7 +927,9 @@ public static double triangleArea(FramePoint3DReadOnly a, FramePoint3DReadOnly b * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean triangleCircumcenter(FramePoint2DReadOnly A, FramePoint2DReadOnly B, FramePoint2DReadOnly C, + public static boolean triangleCircumcenter(FramePoint2DReadOnly A, + FramePoint2DReadOnly B, + FramePoint2DReadOnly C, FixedFramePoint2DBasics circumcenterToPack) { circumcenterToPack.checkReferenceFrameMatch(A, B, C); @@ -942,7 +983,9 @@ public static boolean triangleCircumcenter(FramePoint2DReadOnly A, FramePoint2DR * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean triangleCircumcenter(FramePoint3DReadOnly A, FramePoint3DReadOnly B, FramePoint3DReadOnly C, + public static boolean triangleCircumcenter(FramePoint3DReadOnly A, + FramePoint3DReadOnly B, + FramePoint3DReadOnly C, FixedFramePoint3DBasics circumcenterToPack) { circumcenterToPack.checkReferenceFrameMatch(A, B, C); @@ -988,7 +1031,9 @@ public static boolean triangleCircumcenter(FramePoint3DReadOnly A, FramePoint3DR * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static double distanceBetweenTwoLine3Ds(FramePoint3DReadOnly pointOnLine1, FrameVector3DReadOnly lineDirection1, FramePoint3DReadOnly pointOnLine2, + public static double distanceBetweenTwoLine3Ds(FramePoint3DReadOnly pointOnLine1, + FrameVector3DReadOnly lineDirection1, + FramePoint3DReadOnly pointOnLine2, FrameVector3DReadOnly lineDirection2) { pointOnLine1.checkReferenceFrameMatch(lineDirection1, pointOnLine2, lineDirection2); @@ -1007,8 +1052,10 @@ public static double distanceBetweenTwoLine3Ds(FramePoint3DReadOnly pointOnLine1 * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static double distanceBetweenTwoLineSegment2Ds(FramePoint2DReadOnly lineSegmentStart1, FramePoint2DReadOnly lineSegmentEnd1, - FramePoint2DReadOnly lineSegmentStart2, FramePoint2DReadOnly lineSegmentEnd2) + public static double distanceBetweenTwoLineSegment2Ds(FramePoint2DReadOnly lineSegmentStart1, + FramePoint2DReadOnly lineSegmentEnd1, + FramePoint2DReadOnly lineSegmentStart2, + FramePoint2DReadOnly lineSegmentEnd2) { lineSegmentStart1.checkReferenceFrameMatch(lineSegmentEnd1, lineSegmentStart2, lineSegmentEnd2); return EuclidGeometryTools.closestPoint2DsBetweenTwoLineSegment2Ds(lineSegmentStart1, lineSegmentEnd1, lineSegmentStart2, lineSegmentEnd2, null, null); @@ -1026,8 +1073,10 @@ public static double distanceBetweenTwoLineSegment2Ds(FramePoint2DReadOnly lineS * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static double distanceBetweenTwoLineSegment3Ds(FramePoint3DReadOnly lineSegmentStart1, FramePoint3DReadOnly lineSegmentEnd1, - FramePoint3DReadOnly lineSegmentStart2, FramePoint3DReadOnly lineSegmentEnd2) + public static double distanceBetweenTwoLineSegment3Ds(FramePoint3DReadOnly lineSegmentStart1, + FramePoint3DReadOnly lineSegmentEnd1, + FramePoint3DReadOnly lineSegmentStart2, + FramePoint3DReadOnly lineSegmentEnd2) { lineSegmentStart1.checkReferenceFrameMatch(lineSegmentEnd1, lineSegmentStart2, lineSegmentEnd2); return EuclidGeometryTools.closestPoint3DsBetweenTwoLineSegment3Ds(lineSegmentStart1, lineSegmentEnd1, lineSegmentStart2, lineSegmentEnd2, null, null); @@ -1152,7 +1201,9 @@ public static double distanceFromPoint2DToLine2D(FramePoint2DReadOnly point, Fra * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static double distanceFromPoint2DToLineSegment2D(double pointX, double pointY, FramePoint2DReadOnly lineSegmentStart, + public static double distanceFromPoint2DToLineSegment2D(double pointX, + double pointY, + FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd) { lineSegmentStart.checkReferenceFrameMatch(lineSegmentEnd); @@ -1177,7 +1228,8 @@ public static double distanceFromPoint2DToLineSegment2D(double pointX, double po * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static double distanceFromPoint2DToLineSegment2D(FramePoint2DReadOnly point, FramePoint2DReadOnly lineSegmentStart, + public static double distanceFromPoint2DToLineSegment2D(FramePoint2DReadOnly point, + FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd) { point.checkReferenceFrameMatch(lineSegmentStart, lineSegmentEnd); @@ -1316,7 +1368,10 @@ public static double distanceFromPoint3DToLine3D(FramePoint3DReadOnly point, Fra * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static double distanceFromPoint3DToLineSegment3D(double pointX, double pointY, double pointZ, FramePoint3DReadOnly lineSegmentStart, + public static double distanceFromPoint3DToLineSegment3D(double pointX, + double pointY, + double pointZ, + FramePoint3DReadOnly lineSegmentStart, FramePoint3DReadOnly lineSegmentEnd) { lineSegmentStart.checkReferenceFrameMatch(lineSegmentEnd); @@ -1341,7 +1396,8 @@ public static double distanceFromPoint3DToLineSegment3D(double pointX, double po * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static double distanceFromPoint3DToLineSegment3D(FramePoint3DReadOnly point, FramePoint3DReadOnly lineSegmentStart, + public static double distanceFromPoint3DToLineSegment3D(FramePoint3DReadOnly point, + FramePoint3DReadOnly lineSegmentStart, FramePoint3DReadOnly lineSegmentEnd) { point.checkReferenceFrameMatch(lineSegmentStart, lineSegmentEnd); @@ -1360,7 +1416,10 @@ public static double distanceFromPoint3DToLineSegment3D(FramePoint3DReadOnly poi * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static double distanceFromPoint3DToPlane3D(double pointX, double pointY, double pointZ, FramePoint3DReadOnly pointOnPlane, + public static double distanceFromPoint3DToPlane3D(double pointX, + double pointY, + double pointZ, + FramePoint3DReadOnly pointOnPlane, FrameVector3DReadOnly planeNormal) { pointOnPlane.checkReferenceFrameMatch(planeNormal); @@ -1398,7 +1457,10 @@ public static double distanceFromPoint3DToPlane3D(FramePoint3DReadOnly point, Fr * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static double signedDistanceFromPoint3DToPlane3D(double pointX, double pointY, double pointZ, FramePoint3DReadOnly pointOnPlane, + public static double signedDistanceFromPoint3DToPlane3D(double pointX, + double pointY, + double pointZ, + FramePoint3DReadOnly pointOnPlane, FrameVector3DReadOnly planeNormal) { pointOnPlane.checkReferenceFrameMatch(planeNormal); @@ -1440,8 +1502,12 @@ public static double signedDistanceFromPoint3DToPlane3D(FramePoint3DReadOnly poi * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static double signedDistanceFromPoint3DToPlane3D(double pointX, double pointY, double pointZ, FramePoint3DReadOnly pointOnPlane, - FrameVector3DReadOnly planeFirstTangent, FrameVector3DReadOnly planeSecondTangent) + public static double signedDistanceFromPoint3DToPlane3D(double pointX, + double pointY, + double pointZ, + FramePoint3DReadOnly pointOnPlane, + FrameVector3DReadOnly planeFirstTangent, + FrameVector3DReadOnly planeSecondTangent) { pointOnPlane.checkReferenceFrameMatch(planeFirstTangent, planeSecondTangent); return EuclidGeometryTools.signedDistanceFromPoint3DToPlane3D(pointX, pointY, pointZ, pointOnPlane, planeFirstTangent, planeSecondTangent); @@ -1461,8 +1527,10 @@ public static double signedDistanceFromPoint3DToPlane3D(double pointX, double po * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static double signedDistanceFromPoint3DToPlane3D(FramePoint3DReadOnly point, FramePoint3DReadOnly pointOnPlane, - FrameVector3DReadOnly planeFirstTangent, FrameVector3DReadOnly planeSecondTangent) + public static double signedDistanceFromPoint3DToPlane3D(FramePoint3DReadOnly point, + FramePoint3DReadOnly pointOnPlane, + FrameVector3DReadOnly planeFirstTangent, + FrameVector3DReadOnly planeSecondTangent) { point.checkReferenceFrameMatch(pointOnPlane, planeFirstTangent, planeSecondTangent); return EuclidGeometryTools.signedDistanceFromPoint3DToPlane3D(point, pointOnPlane, planeFirstTangent, planeSecondTangent); @@ -1487,7 +1555,9 @@ public static double signedDistanceFromPoint3DToPlane3D(FramePoint3DReadOnly poi * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static double distanceSquaredFromPoint2DToLineSegment2D(double pointX, double pointY, FramePoint2DReadOnly lineSegmentStart, + public static double distanceSquaredFromPoint2DToLineSegment2D(double pointX, + double pointY, + FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd) { lineSegmentStart.checkReferenceFrameMatch(lineSegmentEnd); @@ -1512,7 +1582,8 @@ public static double distanceSquaredFromPoint2DToLineSegment2D(double pointX, do * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static double distanceSquaredFromPoint2DToLineSegment2D(FramePoint2DReadOnly point, FramePoint2DReadOnly lineSegmentStart, + public static double distanceSquaredFromPoint2DToLineSegment2D(FramePoint2DReadOnly point, + FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd) { point.checkReferenceFrameMatch(lineSegmentStart, lineSegmentEnd); @@ -1539,7 +1610,10 @@ public static double distanceSquaredFromPoint2DToLineSegment2D(FramePoint2DReadO * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static double distanceSquaredFromPoint3DToLineSegment3D(double pointX, double pointY, double pointZ, FramePoint3DReadOnly lineSegmentStart, + public static double distanceSquaredFromPoint3DToLineSegment3D(double pointX, + double pointY, + double pointZ, + FramePoint3DReadOnly lineSegmentStart, FramePoint3DReadOnly lineSegmentEnd) { lineSegmentStart.checkReferenceFrameMatch(lineSegmentEnd); @@ -1564,7 +1638,8 @@ public static double distanceSquaredFromPoint3DToLineSegment3D(double pointX, do * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static double distanceSquaredFromPoint3DToLineSegment3D(FramePoint3DReadOnly point, FramePoint3DReadOnly lineSegmentStart, + public static double distanceSquaredFromPoint3DToLineSegment3D(FramePoint3DReadOnly point, + FramePoint3DReadOnly lineSegmentStart, FramePoint3DReadOnly lineSegmentEnd) { point.checkReferenceFrameMatch(lineSegmentStart, lineSegmentEnd); @@ -1590,8 +1665,10 @@ public static double distanceSquaredFromPoint3DToLineSegment3D(FramePoint3DReadO * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean doesLineSegment3DIntersectPlane3D(FramePoint3DReadOnly pointOnPlane, FrameVector3DReadOnly planeNormal, - FramePoint3DReadOnly lineSegmentStart, FramePoint3DReadOnly lineSegmentEnd) + public static boolean doesLineSegment3DIntersectPlane3D(FramePoint3DReadOnly pointOnPlane, + FrameVector3DReadOnly planeNormal, + FramePoint3DReadOnly lineSegmentStart, + FramePoint3DReadOnly lineSegmentEnd) { pointOnPlane.checkReferenceFrameMatch(planeNormal, lineSegmentStart, lineSegmentEnd); return EuclidGeometryTools.doesLineSegment3DIntersectPlane3D(pointOnPlane, planeNormal, lineSegmentStart, lineSegmentEnd); @@ -1620,8 +1697,12 @@ public static boolean doesLineSegment3DIntersectPlane3D(FramePoint3DReadOnly poi * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean doLine2DAndLineSegment2DIntersect(double pointOnLineX, double pointOnLineY, double lineDirectionX, double lineDirectionY, - FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd) + public static boolean doLine2DAndLineSegment2DIntersect(double pointOnLineX, + double pointOnLineY, + double lineDirectionX, + double lineDirectionY, + FramePoint2DReadOnly lineSegmentStart, + FramePoint2DReadOnly lineSegmentEnd) { lineSegmentStart.checkReferenceFrameMatch(lineSegmentEnd); return EuclidGeometryTools.doLine2DAndLineSegment2DIntersect(pointOnLineX, @@ -1653,8 +1734,10 @@ public static boolean doLine2DAndLineSegment2DIntersect(double pointOnLineX, dou * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean doLine2DAndLineSegment2DIntersect(FramePoint2DReadOnly pointOnLine, FrameVector2DReadOnly lineDirection, - FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd) + public static boolean doLine2DAndLineSegment2DIntersect(FramePoint2DReadOnly pointOnLine, + FrameVector2DReadOnly lineDirection, + FramePoint2DReadOnly lineSegmentStart, + FramePoint2DReadOnly lineSegmentEnd) { pointOnLine.checkReferenceFrameMatch(lineDirection, lineSegmentStart, lineSegmentEnd); return EuclidGeometryTools.doLine2DAndLineSegment2DIntersect(pointOnLine, lineDirection, lineSegmentStart, lineSegmentEnd); @@ -1680,8 +1763,10 @@ public static boolean doLine2DAndLineSegment2DIntersect(FramePoint2DReadOnly poi * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean doLineSegment2DsIntersect(FramePoint2DReadOnly lineSegmentStart1, FramePoint2DReadOnly lineSegmentEnd1, - FramePoint2DReadOnly lineSegmentStart2, FramePoint2DReadOnly lineSegmentEnd2) + public static boolean doLineSegment2DsIntersect(FramePoint2DReadOnly lineSegmentStart1, + FramePoint2DReadOnly lineSegmentEnd1, + FramePoint2DReadOnly lineSegmentStart2, + FramePoint2DReadOnly lineSegmentEnd2) { lineSegmentStart1.checkReferenceFrameMatch(lineSegmentEnd1, lineSegmentStart2, lineSegmentEnd2); return EuclidGeometryTools.doLineSegment2DsIntersect(lineSegmentStart1, lineSegmentEnd1, lineSegmentStart2, lineSegmentEnd2); @@ -1707,8 +1792,10 @@ public static boolean doLineSegment2DsIntersect(FramePoint2DReadOnly lineSegment * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean doRay2DAndLineSegment2DIntersect(FramePoint2DReadOnly rayOrigin, FrameVector2DReadOnly rayDirection, - FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd) + public static boolean doRay2DAndLineSegment2DIntersect(FramePoint2DReadOnly rayOrigin, + FrameVector2DReadOnly rayDirection, + FramePoint2DReadOnly lineSegmentStart, + FramePoint2DReadOnly lineSegmentEnd) { rayOrigin.checkReferenceFrameMatch(rayDirection, lineSegmentStart, lineSegmentEnd); return EuclidGeometryTools.doRay2DAndLineSegment2DIntersect(rayOrigin, rayDirection, lineSegmentStart, lineSegmentEnd); @@ -1789,8 +1876,10 @@ public static double dotProduct(FramePoint3DReadOnly start1, FramePoint3DReadOnl * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static int intersectionBetweenLine2DAndBoundingBox2D(FramePoint2DReadOnly boundingBoxMin, FramePoint2DReadOnly boundingBoxMax, - FramePoint2DReadOnly firstPointOnLine, FramePoint2DReadOnly secondPointOnLine, + public static int intersectionBetweenLine2DAndBoundingBox2D(FramePoint2DReadOnly boundingBoxMin, + FramePoint2DReadOnly boundingBoxMax, + FramePoint2DReadOnly firstPointOnLine, + FramePoint2DReadOnly secondPointOnLine, FixedFramePoint2DBasics firstIntersectionToPack, FixedFramePoint2DBasics secondIntersectionToPack) { @@ -1842,9 +1931,12 @@ public static int intersectionBetweenLine2DAndBoundingBox2D(FramePoint2DReadOnly * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static int intersectionBetweenLine2DAndBoundingBox2D(FramePoint2DReadOnly boundingBoxMin, FramePoint2DReadOnly boundingBoxMax, - FramePoint2DReadOnly firstPointOnLine, FramePoint2DReadOnly secondPointOnLine, - FramePoint2DBasics firstIntersectionToPack, FramePoint2DBasics secondIntersectionToPack) + public static int intersectionBetweenLine2DAndBoundingBox2D(FramePoint2DReadOnly boundingBoxMin, + FramePoint2DReadOnly boundingBoxMax, + FramePoint2DReadOnly firstPointOnLine, + FramePoint2DReadOnly secondPointOnLine, + FramePoint2DBasics firstIntersectionToPack, + FramePoint2DBasics secondIntersectionToPack) { boundingBoxMin.checkReferenceFrameMatch(boundingBoxMax, firstPointOnLine, secondPointOnLine); int numberOfIntersections = EuclidGeometryTools.intersectionBetweenLine2DAndBoundingBox2D(boundingBoxMin, @@ -1893,8 +1985,10 @@ public static int intersectionBetweenLine2DAndBoundingBox2D(FramePoint2DReadOnly * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static int intersectionBetweenLine2DAndBoundingBox2D(FramePoint2DReadOnly boundingBoxMin, FramePoint2DReadOnly boundingBoxMax, - FramePoint2DReadOnly pointOnLine, FrameVector2DReadOnly lineDirection, + public static int intersectionBetweenLine2DAndBoundingBox2D(FramePoint2DReadOnly boundingBoxMin, + FramePoint2DReadOnly boundingBoxMax, + FramePoint2DReadOnly pointOnLine, + FrameVector2DReadOnly lineDirection, FixedFramePoint2DBasics firstIntersectionToPack, FixedFramePoint2DBasics secondIntersectionToPack) { @@ -1943,9 +2037,12 @@ public static int intersectionBetweenLine2DAndBoundingBox2D(FramePoint2DReadOnly * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static int intersectionBetweenLine2DAndBoundingBox2D(FramePoint2DReadOnly boundingBoxMin, FramePoint2DReadOnly boundingBoxMax, - FramePoint2DReadOnly pointOnLine, FrameVector2DReadOnly lineDirection, - FramePoint2DBasics firstIntersectionToPack, FramePoint2DBasics secondIntersectionToPack) + public static int intersectionBetweenLine2DAndBoundingBox2D(FramePoint2DReadOnly boundingBoxMin, + FramePoint2DReadOnly boundingBoxMax, + FramePoint2DReadOnly pointOnLine, + FrameVector2DReadOnly lineDirection, + FramePoint2DBasics firstIntersectionToPack, + FramePoint2DBasics secondIntersectionToPack) { boundingBoxMin.checkReferenceFrameMatch(boundingBoxMax, pointOnLine, lineDirection); int numberOfIntersections = EuclidGeometryTools.intersectionBetweenLine2DAndBoundingBox2D(boundingBoxMin, @@ -1990,8 +2087,10 @@ public static int intersectionBetweenLine2DAndBoundingBox2D(FramePoint2DReadOnly * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static FramePoint2D intersectionBetweenLine2DAndLineSegment2D(FramePoint2DReadOnly pointOnLine, FrameVector2DReadOnly lineDirection, - FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd) + public static FramePoint2D intersectionBetweenLine2DAndLineSegment2D(FramePoint2DReadOnly pointOnLine, + FrameVector2DReadOnly lineDirection, + FramePoint2DReadOnly lineSegmentStart, + FramePoint2DReadOnly lineSegmentEnd) { pointOnLine.checkReferenceFrameMatch(lineDirection, lineSegmentStart, lineSegmentEnd); Point2D intersection = EuclidGeometryTools.intersectionBetweenLine2DAndLineSegment2D(pointOnLine, lineDirection, lineSegmentStart, lineSegmentEnd); @@ -2027,8 +2126,10 @@ public static FramePoint2D intersectionBetweenLine2DAndLineSegment2D(FramePoint2 * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean intersectionBetweenLine2DAndLineSegment2D(FramePoint2DReadOnly pointOnLine, FrameVector2DReadOnly lineDirection, - FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd, + public static boolean intersectionBetweenLine2DAndLineSegment2D(FramePoint2DReadOnly pointOnLine, + FrameVector2DReadOnly lineDirection, + FramePoint2DReadOnly lineSegmentStart, + FramePoint2DReadOnly lineSegmentEnd, FixedFramePoint2DBasics intersectionToPack) { pointOnLine.checkReferenceFrameMatch(lineDirection, lineSegmentStart, lineSegmentEnd); @@ -2069,8 +2170,10 @@ public static boolean intersectionBetweenLine2DAndLineSegment2D(FramePoint2DRead * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static boolean intersectionBetweenLine2DAndLineSegment2D(FramePoint2DReadOnly pointOnLine, FrameVector2DReadOnly lineDirection, - FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd, + public static boolean intersectionBetweenLine2DAndLineSegment2D(FramePoint2DReadOnly pointOnLine, + FrameVector2DReadOnly lineDirection, + FramePoint2DReadOnly lineSegmentStart, + FramePoint2DReadOnly lineSegmentEnd, FramePoint2DBasics intersectionToPack) { pointOnLine.checkReferenceFrameMatch(lineDirection, lineSegmentStart, lineSegmentEnd); @@ -2119,8 +2222,10 @@ public static boolean intersectionBetweenLine2DAndLineSegment2D(FramePoint2DRead * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static int intersectionBetweenLine3DAndBoundingBox3D(FramePoint3DReadOnly boundingBoxMin, FramePoint3DReadOnly boundingBoxMax, - FramePoint3DReadOnly firstPointOnLine, FramePoint3DReadOnly secondPointOnLine, + public static int intersectionBetweenLine3DAndBoundingBox3D(FramePoint3DReadOnly boundingBoxMin, + FramePoint3DReadOnly boundingBoxMax, + FramePoint3DReadOnly firstPointOnLine, + FramePoint3DReadOnly secondPointOnLine, FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { @@ -2172,9 +2277,12 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(FramePoint3DReadOnly * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static int intersectionBetweenLine3DAndBoundingBox3D(FramePoint3DReadOnly boundingBoxMin, FramePoint3DReadOnly boundingBoxMax, - FramePoint3DReadOnly firstPointOnLine, FramePoint3DReadOnly secondPointOnLine, - FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) + public static int intersectionBetweenLine3DAndBoundingBox3D(FramePoint3DReadOnly boundingBoxMin, + FramePoint3DReadOnly boundingBoxMax, + FramePoint3DReadOnly firstPointOnLine, + FramePoint3DReadOnly secondPointOnLine, + FramePoint3DBasics firstIntersectionToPack, + FramePoint3DBasics secondIntersectionToPack) { boundingBoxMin.checkReferenceFrameMatch(boundingBoxMax, firstPointOnLine, secondPointOnLine); int numberOfIntersections = EuclidGeometryTools.intersectionBetweenLine3DAndBoundingBox3D(boundingBoxMin, @@ -2222,8 +2330,10 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(FramePoint3DReadOnly * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static int intersectionBetweenLine3DAndBoundingBox3D(FramePoint3DReadOnly boundingBoxMin, FramePoint3DReadOnly boundingBoxMax, - FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection, + public static int intersectionBetweenLine3DAndBoundingBox3D(FramePoint3DReadOnly boundingBoxMin, + FramePoint3DReadOnly boundingBoxMax, + FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { @@ -2271,9 +2381,12 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(FramePoint3DReadOnly * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static int intersectionBetweenLine3DAndBoundingBox3D(FramePoint3DReadOnly boundingBoxMin, FramePoint3DReadOnly boundingBoxMax, - FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection, - FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) + public static int intersectionBetweenLine3DAndBoundingBox3D(FramePoint3DReadOnly boundingBoxMin, + FramePoint3DReadOnly boundingBoxMax, + FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, + FramePoint3DBasics firstIntersectionToPack, + FramePoint3DBasics secondIntersectionToPack) { boundingBoxMin.checkReferenceFrameMatch(boundingBoxMax, pointOnLine, lineDirection); int numberOfIntersections = EuclidGeometryTools.intersectionBetweenLine3DAndBoundingBox3D(boundingBoxMin, @@ -2326,9 +2439,15 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(FramePoint3DReadOnly * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static int intersectionBetweenLine3DAndBoundingBox3D(double boundingBoxMinX, double boundingBoxMinY, double boundingBoxMinZ, double boundingBoxMaxX, - double boundingBoxMaxY, double boundingBoxMaxZ, FramePoint3DReadOnly pointOnLine, - FrameVector3DReadOnly lineDirection, FixedFramePoint3DBasics firstIntersectionToPack, + public static int intersectionBetweenLine3DAndBoundingBox3D(double boundingBoxMinX, + double boundingBoxMinY, + double boundingBoxMinZ, + double boundingBoxMaxX, + double boundingBoxMaxY, + double boundingBoxMaxZ, + FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { pointOnLine.checkReferenceFrameMatch(lineDirection); @@ -2384,9 +2503,15 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(double boundingBoxMi * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static int intersectionBetweenLine3DAndBoundingBox3D(double boundingBoxMinX, double boundingBoxMinY, double boundingBoxMinZ, double boundingBoxMaxX, - double boundingBoxMaxY, double boundingBoxMaxZ, FramePoint3DReadOnly pointOnLine, - FrameVector3DReadOnly lineDirection, FramePoint3DBasics firstIntersectionToPack, + public static int intersectionBetweenLine3DAndBoundingBox3D(double boundingBoxMinX, + double boundingBoxMinY, + double boundingBoxMinZ, + double boundingBoxMaxX, + double boundingBoxMaxY, + double boundingBoxMaxZ, + FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, + FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) { pointOnLine.checkReferenceFrameMatch(lineDirection); @@ -2444,9 +2569,13 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(double boundingBoxMi * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, double cylinderRadius, FramePoint3DReadOnly cylinderPosition, - FrameVector3DReadOnly cylinderAxis, FramePoint3DReadOnly firstPointOnLine, - FramePoint3DReadOnly secondPointOnLine, FixedFramePoint3DBasics firstIntersectionToPack, + public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, + double cylinderRadius, + FramePoint3DReadOnly cylinderPosition, + FrameVector3DReadOnly cylinderAxis, + FramePoint3DReadOnly firstPointOnLine, + FramePoint3DReadOnly secondPointOnLine, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { cylinderPosition.checkReferenceFrameMatch(cylinderAxis, firstPointOnLine, secondPointOnLine); @@ -2501,9 +2630,13 @@ public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, double cylinderRadius, FramePoint3DReadOnly cylinderPosition, - FrameVector3DReadOnly cylinderAxis, FramePoint3DReadOnly firstPointOnLine, - FramePoint3DReadOnly secondPointOnLine, FramePoint3DBasics firstIntersectionToPack, + public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, + double cylinderRadius, + FramePoint3DReadOnly cylinderPosition, + FrameVector3DReadOnly cylinderAxis, + FramePoint3DReadOnly firstPointOnLine, + FramePoint3DReadOnly secondPointOnLine, + FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) { cylinderPosition.checkReferenceFrameMatch(cylinderAxis, firstPointOnLine, secondPointOnLine); @@ -2559,9 +2692,13 @@ public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, double cylinderRadius, FramePoint3DReadOnly cylinderPosition, - FrameVector3DReadOnly cylinderAxis, FramePoint3DReadOnly pointOnLine, - FrameVector3DReadOnly lineDirection, FixedFramePoint3DBasics firstIntersectionToPack, + public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, + double cylinderRadius, + FramePoint3DReadOnly cylinderPosition, + FrameVector3DReadOnly cylinderAxis, + FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { cylinderPosition.checkReferenceFrameMatch(cylinderAxis, pointOnLine, lineDirection); @@ -2615,9 +2752,13 @@ public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, double cylinderRadius, FramePoint3DReadOnly cylinderPosition, - FrameVector3DReadOnly cylinderAxis, FramePoint3DReadOnly pointOnLine, - FrameVector3DReadOnly lineDirection, FramePoint3DBasics firstIntersectionToPack, + public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, + double cylinderRadius, + FramePoint3DReadOnly cylinderPosition, + FrameVector3DReadOnly cylinderAxis, + FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, + FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) { cylinderPosition.checkReferenceFrameMatch(cylinderAxis, pointOnLine, lineDirection); @@ -2673,8 +2814,12 @@ public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, double radiusY, double radiusZ, FramePoint3DReadOnly firstPointOnLine, - FramePoint3DReadOnly secondPointOnLine, FixedFramePoint3DBasics firstIntersectionToPack, + public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, + double radiusY, + double radiusZ, + FramePoint3DReadOnly firstPointOnLine, + FramePoint3DReadOnly secondPointOnLine, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { firstPointOnLine.checkReferenceFrameMatch(secondPointOnLine); @@ -2727,8 +2872,12 @@ public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, double * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, double radiusY, double radiusZ, FramePoint3DReadOnly firstPointOnLine, - FramePoint3DReadOnly secondPointOnLine, FramePoint3DBasics firstIntersectionToPack, + public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, + double radiusY, + double radiusZ, + FramePoint3DReadOnly firstPointOnLine, + FramePoint3DReadOnly secondPointOnLine, + FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) { firstPointOnLine.checkReferenceFrameMatch(secondPointOnLine); @@ -2783,8 +2932,12 @@ public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, double * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, double radiusY, double radiusZ, FramePoint3DReadOnly pointOnLine, - FrameVector3DReadOnly lineDirection, FixedFramePoint3DBasics firstIntersectionToPack, + public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, + double radiusY, + double radiusZ, + FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { pointOnLine.checkReferenceFrameMatch(lineDirection); @@ -2837,8 +2990,12 @@ public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, double * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, double radiusY, double radiusZ, FramePoint3DReadOnly pointOnLine, - FrameVector3DReadOnly lineDirection, FramePoint3DBasics firstIntersectionToPack, + public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, + double radiusY, + double radiusZ, + FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, + FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) { pointOnLine.checkReferenceFrameMatch(lineDirection); @@ -2881,8 +3038,10 @@ public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, double * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static FramePoint3D intersectionBetweenLine3DAndPlane3D(FramePoint3DReadOnly pointOnPlane, FrameVector3DReadOnly planeNormal, - FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection) + public static FramePoint3D intersectionBetweenLine3DAndPlane3D(FramePoint3DReadOnly pointOnPlane, + FrameVector3DReadOnly planeNormal, + FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection) { pointOnPlane.checkReferenceFrameMatch(planeNormal, pointOnLine, lineDirection); @@ -2913,8 +3072,10 @@ public static FramePoint3D intersectionBetweenLine3DAndPlane3D(FramePoint3DReadO * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean intersectionBetweenLine3DAndPlane3D(FramePoint3DReadOnly pointOnPlane, FrameVector3DReadOnly planeNormal, - FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection, + public static boolean intersectionBetweenLine3DAndPlane3D(FramePoint3DReadOnly pointOnPlane, + FrameVector3DReadOnly planeNormal, + FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, FixedFramePoint3DBasics intersectionToPack) { pointOnPlane.checkReferenceFrameMatch(planeNormal, pointOnLine, lineDirection); @@ -2942,8 +3103,10 @@ public static boolean intersectionBetweenLine3DAndPlane3D(FramePoint3DReadOnly p * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static boolean intersectionBetweenLine3DAndPlane3D(FramePoint3DReadOnly pointOnPlane, FrameVector3DReadOnly planeNormal, - FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection, + public static boolean intersectionBetweenLine3DAndPlane3D(FramePoint3DReadOnly pointOnPlane, + FrameVector3DReadOnly planeNormal, + FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, FramePoint3DBasics intersectionToPack) { pointOnPlane.checkReferenceFrameMatch(planeNormal, pointOnLine, lineDirection); @@ -2994,8 +3157,10 @@ public static boolean intersectionBetweenLine3DAndPlane3D(FramePoint3DReadOnly p * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static int intersectionBetweenLineSegment2DAndBoundingBox2D(FramePoint2DReadOnly boundingBoxMin, FramePoint2DReadOnly boundingBoxMax, - FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd, + public static int intersectionBetweenLineSegment2DAndBoundingBox2D(FramePoint2DReadOnly boundingBoxMin, + FramePoint2DReadOnly boundingBoxMax, + FramePoint2DReadOnly lineSegmentStart, + FramePoint2DReadOnly lineSegmentEnd, FixedFramePoint2DBasics firstIntersectionToPack, FixedFramePoint2DBasics secondIntersectionToPack) { @@ -3053,9 +3218,12 @@ public static int intersectionBetweenLineSegment2DAndBoundingBox2D(FramePoint2DR * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static int intersectionBetweenLineSegment2DAndBoundingBox2D(FramePoint2DReadOnly boundingBoxMin, FramePoint2DReadOnly boundingBoxMax, - FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd, - FramePoint2DBasics firstIntersectionToPack, FramePoint2DBasics secondIntersectionToPack) + public static int intersectionBetweenLineSegment2DAndBoundingBox2D(FramePoint2DReadOnly boundingBoxMin, + FramePoint2DReadOnly boundingBoxMax, + FramePoint2DReadOnly lineSegmentStart, + FramePoint2DReadOnly lineSegmentEnd, + FramePoint2DBasics firstIntersectionToPack, + FramePoint2DBasics secondIntersectionToPack) { boundingBoxMin.checkReferenceFrameMatch(boundingBoxMax, lineSegmentStart, lineSegmentEnd); int numberOfIntersections = EuclidGeometryTools.intersectionBetweenLineSegment2DAndBoundingBox2D(boundingBoxMin, @@ -3113,8 +3281,10 @@ public static int intersectionBetweenLineSegment2DAndBoundingBox2D(FramePoint2DR * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static int intersectionBetweenLineSegment3DAndBoundingBox3D(FramePoint3DReadOnly boundingBoxMin, FramePoint3DReadOnly boundingBoxMax, - FramePoint3DReadOnly lineSegmentStart, FramePoint3DReadOnly lineSegmentEnd, + public static int intersectionBetweenLineSegment3DAndBoundingBox3D(FramePoint3DReadOnly boundingBoxMin, + FramePoint3DReadOnly boundingBoxMax, + FramePoint3DReadOnly lineSegmentStart, + FramePoint3DReadOnly lineSegmentEnd, FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { @@ -3172,9 +3342,12 @@ public static int intersectionBetweenLineSegment3DAndBoundingBox3D(FramePoint3DR * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static int intersectionBetweenLineSegment3DAndBoundingBox3D(FramePoint3DReadOnly boundingBoxMin, FramePoint3DReadOnly boundingBoxMax, - FramePoint3DReadOnly lineSegmentStart, FramePoint3DReadOnly lineSegmentEnd, - FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) + public static int intersectionBetweenLineSegment3DAndBoundingBox3D(FramePoint3DReadOnly boundingBoxMin, + FramePoint3DReadOnly boundingBoxMax, + FramePoint3DReadOnly lineSegmentStart, + FramePoint3DReadOnly lineSegmentEnd, + FramePoint3DBasics firstIntersectionToPack, + FramePoint3DBasics secondIntersectionToPack) { boundingBoxMin.checkReferenceFrameMatch(boundingBoxMax, lineSegmentStart, lineSegmentEnd); int numberOfIntersections = EuclidGeometryTools.intersectionBetweenLineSegment3DAndBoundingBox3D(boundingBoxMin, @@ -3227,9 +3400,13 @@ public static int intersectionBetweenLineSegment3DAndBoundingBox3D(FramePoint3DR * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static int intersectionBetweenLineSegment3DAndCylinder3D(double cylinderLength, double cylinderRadius, FramePoint3DReadOnly cylinderPosition, - FrameVector3DReadOnly cylinderAxis, FramePoint3DReadOnly lineSegmentStart, - FramePoint3DReadOnly lineSegmentEnd, FixedFramePoint3DBasics firstIntersectionToPack, + public static int intersectionBetweenLineSegment3DAndCylinder3D(double cylinderLength, + double cylinderRadius, + FramePoint3DReadOnly cylinderPosition, + FrameVector3DReadOnly cylinderAxis, + FramePoint3DReadOnly lineSegmentStart, + FramePoint3DReadOnly lineSegmentEnd, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { cylinderPosition.checkReferenceFrameMatch(cylinderAxis, lineSegmentStart, lineSegmentEnd); @@ -3283,9 +3460,13 @@ public static int intersectionBetweenLineSegment3DAndCylinder3D(double cylinderL * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static int intersectionBetweenLineSegment3DAndCylinder3D(double cylinderLength, double cylinderRadius, FramePoint3DReadOnly cylinderPosition, - FrameVector3DReadOnly cylinderAxis, FramePoint3DReadOnly lineSegmentStart, - FramePoint3DReadOnly lineSegmentEnd, FramePoint3DBasics firstIntersectionToPack, + public static int intersectionBetweenLineSegment3DAndCylinder3D(double cylinderLength, + double cylinderRadius, + FramePoint3DReadOnly cylinderPosition, + FrameVector3DReadOnly cylinderAxis, + FramePoint3DReadOnly lineSegmentStart, + FramePoint3DReadOnly lineSegmentEnd, + FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) { cylinderPosition.checkReferenceFrameMatch(cylinderAxis, lineSegmentStart, lineSegmentEnd); @@ -3341,8 +3522,12 @@ public static int intersectionBetweenLineSegment3DAndCylinder3D(double cylinderL * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static int intersectionBetweenLineSegment3DAndEllipsoid3D(double radiusX, double radiusY, double radiusZ, FramePoint3DReadOnly lineSegmentStart, - FramePoint3DReadOnly lineSegmentEnd, FixedFramePoint3DBasics firstIntersectionToPack, + public static int intersectionBetweenLineSegment3DAndEllipsoid3D(double radiusX, + double radiusY, + double radiusZ, + FramePoint3DReadOnly lineSegmentStart, + FramePoint3DReadOnly lineSegmentEnd, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { lineSegmentStart.checkReferenceFrameMatch(lineSegmentEnd); @@ -3395,8 +3580,12 @@ public static int intersectionBetweenLineSegment3DAndEllipsoid3D(double radiusX, * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static int intersectionBetweenLineSegment3DAndEllipsoid3D(double radiusX, double radiusY, double radiusZ, FramePoint3DReadOnly lineSegmentStart, - FramePoint3DReadOnly lineSegmentEnd, FramePoint3DBasics firstIntersectionToPack, + public static int intersectionBetweenLineSegment3DAndEllipsoid3D(double radiusX, + double radiusY, + double radiusZ, + FramePoint3DReadOnly lineSegmentStart, + FramePoint3DReadOnly lineSegmentEnd, + FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) { lineSegmentStart.checkReferenceFrameMatch(lineSegmentEnd); @@ -3443,8 +3632,10 @@ public static int intersectionBetweenLineSegment3DAndEllipsoid3D(double radiusX, * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static FramePoint3D intersectionBetweenLineSegment3DAndPlane3D(FramePoint3DReadOnly pointOnPlane, FrameVector3DReadOnly planeNormal, - FramePoint3DReadOnly lineSegmentStart, FramePoint3DReadOnly lineSegmentEnd) + public static FramePoint3D intersectionBetweenLineSegment3DAndPlane3D(FramePoint3DReadOnly pointOnPlane, + FrameVector3DReadOnly planeNormal, + FramePoint3DReadOnly lineSegmentStart, + FramePoint3DReadOnly lineSegmentEnd) { pointOnPlane.checkReferenceFrameMatch(planeNormal, lineSegmentStart, lineSegmentEnd); @@ -3497,9 +3688,12 @@ public static FramePoint3D intersectionBetweenLineSegment3DAndPlane3D(FramePoint * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static int intersectionBetweenRay2DAndBoundingBox2D(FramePoint2DReadOnly boundingBoxMin, FramePoint2DReadOnly boundingBoxMax, - FramePoint2DReadOnly rayOrigin, FrameVector2DReadOnly rayDirection, - FixedFramePoint2DBasics firstIntersectionToPack, FixedFramePoint2DBasics secondIntersectionToPack) + public static int intersectionBetweenRay2DAndBoundingBox2D(FramePoint2DReadOnly boundingBoxMin, + FramePoint2DReadOnly boundingBoxMax, + FramePoint2DReadOnly rayOrigin, + FrameVector2DReadOnly rayDirection, + FixedFramePoint2DBasics firstIntersectionToPack, + FixedFramePoint2DBasics secondIntersectionToPack) { boundingBoxMin.checkReferenceFrameMatch(boundingBoxMax, rayOrigin, rayDirection); if (firstIntersectionToPack != null) @@ -3554,9 +3748,12 @@ public static int intersectionBetweenRay2DAndBoundingBox2D(FramePoint2DReadOnly * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static int intersectionBetweenRay2DAndBoundingBox2D(FramePoint2DReadOnly boundingBoxMin, FramePoint2DReadOnly boundingBoxMax, - FramePoint2DReadOnly rayOrigin, FrameVector2DReadOnly rayDirection, - FramePoint2DBasics firstIntersectionToPack, FramePoint2DBasics secondIntersectionToPack) + public static int intersectionBetweenRay2DAndBoundingBox2D(FramePoint2DReadOnly boundingBoxMin, + FramePoint2DReadOnly boundingBoxMax, + FramePoint2DReadOnly rayOrigin, + FrameVector2DReadOnly rayDirection, + FramePoint2DBasics firstIntersectionToPack, + FramePoint2DBasics secondIntersectionToPack) { boundingBoxMin.checkReferenceFrameMatch(boundingBoxMax, rayOrigin, rayDirection); int numberOfIntersections = EuclidGeometryTools.intersectionBetweenRay2DAndBoundingBox2D(boundingBoxMin, @@ -3600,8 +3797,10 @@ public static int intersectionBetweenRay2DAndBoundingBox2D(FramePoint2DReadOnly * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static FramePoint2D intersectionBetweenRay2DAndLineSegment2D(FramePoint2DReadOnly rayOrigin, FrameVector2DReadOnly rayDirection, - FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd) + public static FramePoint2D intersectionBetweenRay2DAndLineSegment2D(FramePoint2DReadOnly rayOrigin, + FrameVector2DReadOnly rayDirection, + FramePoint2DReadOnly lineSegmentStart, + FramePoint2DReadOnly lineSegmentEnd) { rayOrigin.checkReferenceFrameMatch(rayDirection, lineSegmentStart, lineSegmentEnd); @@ -3637,8 +3836,10 @@ public static FramePoint2D intersectionBetweenRay2DAndLineSegment2D(FramePoint2D * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean intersectionBetweenRay2DAndLineSegment2D(FramePoint2DReadOnly rayOrigin, FrameVector2DReadOnly rayDirection, - FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd, + public static boolean intersectionBetweenRay2DAndLineSegment2D(FramePoint2DReadOnly rayOrigin, + FrameVector2DReadOnly rayDirection, + FramePoint2DReadOnly lineSegmentStart, + FramePoint2DReadOnly lineSegmentEnd, FixedFramePoint2DBasics intersectionToPack) { rayOrigin.checkReferenceFrameMatch(rayDirection, lineSegmentStart, lineSegmentEnd); @@ -3679,8 +3880,10 @@ public static boolean intersectionBetweenRay2DAndLineSegment2D(FramePoint2DReadO * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static boolean intersectionBetweenRay2DAndLineSegment2D(FramePoint2DReadOnly rayOrigin, FrameVector2DReadOnly rayDirection, - FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd, + public static boolean intersectionBetweenRay2DAndLineSegment2D(FramePoint2DReadOnly rayOrigin, + FrameVector2DReadOnly rayDirection, + FramePoint2DReadOnly lineSegmentStart, + FramePoint2DReadOnly lineSegmentEnd, FramePoint2DBasics intersectionToPack) { rayOrigin.checkReferenceFrameMatch(rayDirection, lineSegmentStart, lineSegmentEnd); @@ -3735,9 +3938,12 @@ public static boolean intersectionBetweenRay2DAndLineSegment2D(FramePoint2DReadO * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static int intersectionBetweenRay3DAndBoundingBox3D(FramePoint3DReadOnly boundingBoxMin, FramePoint3DReadOnly boundingBoxMax, - FramePoint3DReadOnly rayOrigin, FrameVector3DReadOnly rayDirection, - FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) + public static int intersectionBetweenRay3DAndBoundingBox3D(FramePoint3DReadOnly boundingBoxMin, + FramePoint3DReadOnly boundingBoxMax, + FramePoint3DReadOnly rayOrigin, + FrameVector3DReadOnly rayDirection, + FixedFramePoint3DBasics firstIntersectionToPack, + FixedFramePoint3DBasics secondIntersectionToPack) { boundingBoxMin.checkReferenceFrameMatch(boundingBoxMax, rayOrigin, rayDirection); if (firstIntersectionToPack != null) @@ -3792,9 +3998,12 @@ public static int intersectionBetweenRay3DAndBoundingBox3D(FramePoint3DReadOnly * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static int intersectionBetweenRay3DAndBoundingBox3D(FramePoint3DReadOnly boundingBoxMin, FramePoint3DReadOnly boundingBoxMax, - FramePoint3DReadOnly rayOrigin, FrameVector3DReadOnly rayDirection, - FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) + public static int intersectionBetweenRay3DAndBoundingBox3D(FramePoint3DReadOnly boundingBoxMin, + FramePoint3DReadOnly boundingBoxMax, + FramePoint3DReadOnly rayOrigin, + FrameVector3DReadOnly rayDirection, + FramePoint3DBasics firstIntersectionToPack, + FramePoint3DBasics secondIntersectionToPack) { boundingBoxMin.checkReferenceFrameMatch(boundingBoxMax, rayOrigin, rayDirection); int numberOfIntersections = EuclidGeometryTools.intersectionBetweenRay3DAndBoundingBox3D(boundingBoxMin, @@ -3847,9 +4056,13 @@ public static int intersectionBetweenRay3DAndBoundingBox3D(FramePoint3DReadOnly * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static int intersectionBetweenRay3DAndCylinder3D(double cylinderLength, double cylinderRadius, FramePoint3DReadOnly cylinderPosition, - FrameVector3DReadOnly cylinderAxis, FramePoint3DReadOnly rayOrigin, - FrameVector3DReadOnly rayDirection, FixedFramePoint3DBasics firstIntersectionToPack, + public static int intersectionBetweenRay3DAndCylinder3D(double cylinderLength, + double cylinderRadius, + FramePoint3DReadOnly cylinderPosition, + FrameVector3DReadOnly cylinderAxis, + FramePoint3DReadOnly rayOrigin, + FrameVector3DReadOnly rayDirection, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { cylinderPosition.checkReferenceFrameMatch(cylinderAxis, rayOrigin, rayDirection); @@ -3903,9 +4116,13 @@ public static int intersectionBetweenRay3DAndCylinder3D(double cylinderLength, d * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static int intersectionBetweenRay3DAndCylinder3D(double cylinderLength, double cylinderRadius, FramePoint3DReadOnly cylinderPosition, - FrameVector3DReadOnly cylinderAxis, FramePoint3DReadOnly rayOrigin, - FrameVector3DReadOnly rayDirection, FramePoint3DBasics firstIntersectionToPack, + public static int intersectionBetweenRay3DAndCylinder3D(double cylinderLength, + double cylinderRadius, + FramePoint3DReadOnly cylinderPosition, + FrameVector3DReadOnly cylinderAxis, + FramePoint3DReadOnly rayOrigin, + FrameVector3DReadOnly rayDirection, + FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) { cylinderPosition.checkReferenceFrameMatch(cylinderAxis, rayOrigin, rayDirection); @@ -3961,8 +4178,12 @@ public static int intersectionBetweenRay3DAndCylinder3D(double cylinderLength, d * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static int intersectionBetweenRay3DAndEllipsoid3D(double radiusX, double radiusY, double radiusZ, FramePoint3DReadOnly rayOrigin, - FrameVector3DReadOnly rayDirection, FixedFramePoint3DBasics firstIntersectionToPack, + public static int intersectionBetweenRay3DAndEllipsoid3D(double radiusX, + double radiusY, + double radiusZ, + FramePoint3DReadOnly rayOrigin, + FrameVector3DReadOnly rayDirection, + FixedFramePoint3DBasics firstIntersectionToPack, FixedFramePoint3DBasics secondIntersectionToPack) { rayOrigin.checkReferenceFrameMatch(rayDirection); @@ -4015,8 +4236,12 @@ public static int intersectionBetweenRay3DAndEllipsoid3D(double radiusX, double * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static int intersectionBetweenRay3DAndEllipsoid3D(double radiusX, double radiusY, double radiusZ, FramePoint3DReadOnly rayOrigin, - FrameVector3DReadOnly rayDirection, FramePoint3DBasics firstIntersectionToPack, + public static int intersectionBetweenRay3DAndEllipsoid3D(double radiusX, + double radiusY, + double radiusZ, + FramePoint3DReadOnly rayOrigin, + FrameVector3DReadOnly rayDirection, + FramePoint3DBasics firstIntersectionToPack, FramePoint3DBasics secondIntersectionToPack) { rayOrigin.checkReferenceFrameMatch(rayDirection); @@ -4060,8 +4285,10 @@ public static int intersectionBetweenRay3DAndEllipsoid3D(double radiusX, double * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static FramePoint2D intersectionBetweenTwoLine2Ds(FramePoint2DReadOnly firstPointOnLine1, FramePoint2DReadOnly secondPointOnLine1, - FramePoint2DReadOnly firstPointOnLine2, FramePoint2DReadOnly secondPointOnLine2) + public static FramePoint2D intersectionBetweenTwoLine2Ds(FramePoint2DReadOnly firstPointOnLine1, + FramePoint2DReadOnly secondPointOnLine1, + FramePoint2DReadOnly firstPointOnLine2, + FramePoint2DReadOnly secondPointOnLine2) { firstPointOnLine1.checkReferenceFrameMatch(secondPointOnLine1, firstPointOnLine2, secondPointOnLine2); Point2D intersection = EuclidGeometryTools.intersectionBetweenTwoLine2Ds(firstPointOnLine1, secondPointOnLine1, firstPointOnLine2, secondPointOnLine2); @@ -4094,8 +4321,10 @@ public static FramePoint2D intersectionBetweenTwoLine2Ds(FramePoint2DReadOnly fi * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean intersectionBetweenTwoLine2Ds(FramePoint2DReadOnly firstPointOnLine1, FramePoint2DReadOnly secondPointOnLine1, - FramePoint2DReadOnly firstPointOnLine2, FramePoint2DReadOnly secondPointOnLine2, + public static boolean intersectionBetweenTwoLine2Ds(FramePoint2DReadOnly firstPointOnLine1, + FramePoint2DReadOnly secondPointOnLine1, + FramePoint2DReadOnly firstPointOnLine2, + FramePoint2DReadOnly secondPointOnLine2, FixedFramePoint2DBasics intersectionToPack) { firstPointOnLine1.checkReferenceFrameMatch(secondPointOnLine1, firstPointOnLine2, secondPointOnLine2); @@ -4130,8 +4359,10 @@ public static boolean intersectionBetweenTwoLine2Ds(FramePoint2DReadOnly firstPo * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static boolean intersectionBetweenTwoLine2Ds(FramePoint2DReadOnly firstPointOnLine1, FramePoint2DReadOnly secondPointOnLine1, - FramePoint2DReadOnly firstPointOnLine2, FramePoint2DReadOnly secondPointOnLine2, + public static boolean intersectionBetweenTwoLine2Ds(FramePoint2DReadOnly firstPointOnLine1, + FramePoint2DReadOnly secondPointOnLine1, + FramePoint2DReadOnly firstPointOnLine2, + FramePoint2DReadOnly secondPointOnLine2, FramePoint2DBasics intersectionToPack) { firstPointOnLine1.checkReferenceFrameMatch(secondPointOnLine1, firstPointOnLine2, secondPointOnLine2); @@ -4168,8 +4399,10 @@ public static boolean intersectionBetweenTwoLine2Ds(FramePoint2DReadOnly firstPo * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static FramePoint2D intersectionBetweenTwoLine2Ds(FramePoint2DReadOnly pointOnLine1, FrameVector2DReadOnly lineDirection1, - FramePoint2DReadOnly pointOnLine2, FrameVector2DReadOnly lineDirection2) + public static FramePoint2D intersectionBetweenTwoLine2Ds(FramePoint2DReadOnly pointOnLine1, + FrameVector2DReadOnly lineDirection1, + FramePoint2DReadOnly pointOnLine2, + FrameVector2DReadOnly lineDirection2) { pointOnLine1.checkReferenceFrameMatch(lineDirection1, pointOnLine2, lineDirection2); @@ -4203,8 +4436,10 @@ public static FramePoint2D intersectionBetweenTwoLine2Ds(FramePoint2DReadOnly po * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean intersectionBetweenTwoLine2Ds(FramePoint2DReadOnly pointOnLine1, FrameVector2DReadOnly lineDirection1, - FramePoint2DReadOnly pointOnLine2, FrameVector2DReadOnly lineDirection2, + public static boolean intersectionBetweenTwoLine2Ds(FramePoint2DReadOnly pointOnLine1, + FrameVector2DReadOnly lineDirection1, + FramePoint2DReadOnly pointOnLine2, + FrameVector2DReadOnly lineDirection2, FixedFramePoint2DBasics intersectionToPack) { pointOnLine1.checkReferenceFrameMatch(lineDirection1, pointOnLine2, lineDirection2); @@ -4238,8 +4473,10 @@ public static boolean intersectionBetweenTwoLine2Ds(FramePoint2DReadOnly pointOn * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static boolean intersectionBetweenTwoLine2Ds(FramePoint2DReadOnly pointOnLine1, FrameVector2DReadOnly lineDirection1, - FramePoint2DReadOnly pointOnLine2, FrameVector2DReadOnly lineDirection2, + public static boolean intersectionBetweenTwoLine2Ds(FramePoint2DReadOnly pointOnLine1, + FrameVector2DReadOnly lineDirection1, + FramePoint2DReadOnly pointOnLine2, + FrameVector2DReadOnly lineDirection2, FramePoint2DBasics intersectionToPack) { pointOnLine1.checkReferenceFrameMatch(lineDirection1, pointOnLine2, lineDirection2); @@ -4276,8 +4513,10 @@ public static boolean intersectionBetweenTwoLine2Ds(FramePoint2DReadOnly pointOn * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame.s */ - public static FramePoint2D intersectionBetweenTwoLineSegment2Ds(FramePoint2DReadOnly lineSegmentStart1, FramePoint2DReadOnly lineSegmentEnd1, - FramePoint2DReadOnly lineSegmentStart2, FramePoint2DReadOnly lineSegmentEnd2) + public static FramePoint2D intersectionBetweenTwoLineSegment2Ds(FramePoint2DReadOnly lineSegmentStart1, + FramePoint2DReadOnly lineSegmentEnd1, + FramePoint2DReadOnly lineSegmentStart2, + FramePoint2DReadOnly lineSegmentEnd2) { lineSegmentStart1.checkReferenceFrameMatch(lineSegmentEnd1, lineSegmentStart2, lineSegmentEnd2); Point2D intersection = EuclidGeometryTools.intersectionBetweenTwoLineSegment2Ds(lineSegmentStart1, lineSegmentEnd1, lineSegmentStart2, lineSegmentEnd2); @@ -4311,8 +4550,10 @@ public static FramePoint2D intersectionBetweenTwoLineSegment2Ds(FramePoint2DRead * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean intersectionBetweenTwoLineSegment2Ds(FramePoint2DReadOnly lineSegmentStart1, FramePoint2DReadOnly lineSegmentEnd1, - FramePoint2DReadOnly lineSegmentStart2, FramePoint2DReadOnly lineSegmentEnd2, + public static boolean intersectionBetweenTwoLineSegment2Ds(FramePoint2DReadOnly lineSegmentStart1, + FramePoint2DReadOnly lineSegmentEnd1, + FramePoint2DReadOnly lineSegmentStart2, + FramePoint2DReadOnly lineSegmentEnd2, FixedFramePoint2DBasics intersectionToPack) { lineSegmentStart1.checkReferenceFrameMatch(lineSegmentEnd1, lineSegmentStart2, lineSegmentEnd2); @@ -4351,8 +4592,10 @@ public static boolean intersectionBetweenTwoLineSegment2Ds(FramePoint2DReadOnly * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static boolean intersectionBetweenTwoLineSegment2Ds(FramePoint2DReadOnly lineSegmentStart1, FramePoint2DReadOnly lineSegmentEnd1, - FramePoint2DReadOnly lineSegmentStart2, FramePoint2DReadOnly lineSegmentEnd2, + public static boolean intersectionBetweenTwoLineSegment2Ds(FramePoint2DReadOnly lineSegmentStart1, + FramePoint2DReadOnly lineSegmentEnd1, + FramePoint2DReadOnly lineSegmentStart2, + FramePoint2DReadOnly lineSegmentEnd2, FramePoint2DBasics intersectionToPack) { lineSegmentStart1.checkReferenceFrameMatch(lineSegmentEnd1, lineSegmentStart2, lineSegmentEnd2); @@ -4404,9 +4647,13 @@ public static boolean intersectionBetweenTwoLineSegment2Ds(FramePoint2DReadOnly * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean intersectionBetweenTwoPlane3Ds(FramePoint3DReadOnly pointOnPlane1, FrameVector3DReadOnly planeNormal1, - FramePoint3DReadOnly pointOnPlane2, FrameVector3DReadOnly planeNormal2, double angleThreshold, - FixedFramePoint3DBasics pointOnIntersectionToPack, FixedFrameVector3DBasics intersectionDirectionToPack) + public static boolean intersectionBetweenTwoPlane3Ds(FramePoint3DReadOnly pointOnPlane1, + FrameVector3DReadOnly planeNormal1, + FramePoint3DReadOnly pointOnPlane2, + FrameVector3DReadOnly planeNormal2, + double angleThreshold, + FixedFramePoint3DBasics pointOnIntersectionToPack, + FixedFrameVector3DBasics intersectionDirectionToPack) { pointOnPlane1.checkReferenceFrameMatch(planeNormal1, pointOnPlane2, planeNormal2); if (pointOnIntersectionToPack != null) @@ -4460,9 +4707,13 @@ public static boolean intersectionBetweenTwoPlane3Ds(FramePoint3DReadOnly pointO * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static boolean intersectionBetweenTwoPlane3Ds(FramePoint3DReadOnly pointOnPlane1, FrameVector3DReadOnly planeNormal1, - FramePoint3DReadOnly pointOnPlane2, FrameVector3DReadOnly planeNormal2, double angleThreshold, - FramePoint3DBasics pointOnIntersectionToPack, FrameVector3DBasics intersectionDirectionToPack) + public static boolean intersectionBetweenTwoPlane3Ds(FramePoint3DReadOnly pointOnPlane1, + FrameVector3DReadOnly planeNormal1, + FramePoint3DReadOnly pointOnPlane2, + FrameVector3DReadOnly planeNormal2, + double angleThreshold, + FramePoint3DBasics pointOnIntersectionToPack, + FrameVector3DBasics intersectionDirectionToPack) { pointOnPlane1.checkReferenceFrameMatch(planeNormal1, pointOnPlane2, planeNormal2); boolean success = EuclidGeometryTools.intersectionBetweenTwoPlane3Ds(pointOnPlane1, @@ -4511,9 +4762,12 @@ public static boolean intersectionBetweenTwoPlane3Ds(FramePoint3DReadOnly pointO * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean intersectionBetweenTwoPlane3Ds(FramePoint3DReadOnly pointOnPlane1, FrameVector3DReadOnly planeNormal1, - FramePoint3DReadOnly pointOnPlane2, FrameVector3DReadOnly planeNormal2, - FixedFramePoint3DBasics pointOnIntersectionToPack, FixedFrameVector3DBasics intersectionDirectionToPack) + public static boolean intersectionBetweenTwoPlane3Ds(FramePoint3DReadOnly pointOnPlane1, + FrameVector3DReadOnly planeNormal1, + FramePoint3DReadOnly pointOnPlane2, + FrameVector3DReadOnly planeNormal2, + FixedFramePoint3DBasics pointOnIntersectionToPack, + FixedFrameVector3DBasics intersectionDirectionToPack) { pointOnPlane1.checkReferenceFrameMatch(planeNormal1, pointOnPlane2, planeNormal2); if (pointOnIntersectionToPack != null) @@ -4558,9 +4812,12 @@ public static boolean intersectionBetweenTwoPlane3Ds(FramePoint3DReadOnly pointO * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static boolean intersectionBetweenTwoPlane3Ds(FramePoint3DReadOnly pointOnPlane1, FrameVector3DReadOnly planeNormal1, - FramePoint3DReadOnly pointOnPlane2, FrameVector3DReadOnly planeNormal2, - FramePoint3DBasics pointOnIntersectionToPack, FrameVector3DBasics intersectionDirectionToPack) + public static boolean intersectionBetweenTwoPlane3Ds(FramePoint3DReadOnly pointOnPlane1, + FrameVector3DReadOnly planeNormal1, + FramePoint3DReadOnly pointOnPlane2, + FrameVector3DReadOnly planeNormal2, + FramePoint3DBasics pointOnIntersectionToPack, + FrameVector3DBasics intersectionDirectionToPack) { pointOnPlane1.checkReferenceFrameMatch(planeNormal1, pointOnPlane2, planeNormal2); boolean success = EuclidGeometryTools.intersectionBetweenTwoPlane3Ds(pointOnPlane1, @@ -4752,7 +5009,10 @@ public static boolean isPoint2DOnRightSideOfLine2D(FramePoint2DReadOnly point, F * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean isPoint2DOnSideOfLine2D(double pointX, double pointY, FramePoint2DReadOnly firstPointOnLine, FramePoint2DReadOnly secondPointOnLine, + public static boolean isPoint2DOnSideOfLine2D(double pointX, + double pointY, + FramePoint2DReadOnly firstPointOnLine, + FramePoint2DReadOnly secondPointOnLine, boolean testLeftSide) { firstPointOnLine.checkReferenceFrameMatch(secondPointOnLine); @@ -4783,7 +5043,10 @@ public static boolean isPoint2DOnSideOfLine2D(double pointX, double pointY, Fram * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean isPoint2DOnSideOfLine2D(double pointX, double pointY, FramePoint2DReadOnly pointOnLine, FrameVector2DReadOnly lineDirection, + public static boolean isPoint2DOnSideOfLine2D(double pointX, + double pointY, + FramePoint2DReadOnly pointOnLine, + FrameVector2DReadOnly lineDirection, boolean testLeftSide) { pointOnLine.checkReferenceFrameMatch(lineDirection); @@ -4814,7 +5077,9 @@ public static boolean isPoint2DOnSideOfLine2D(double pointX, double pointY, Fram * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean isPoint2DOnSideOfLine2D(FramePoint2DReadOnly point, FramePoint2DReadOnly firstPointOnLine, FramePoint2DReadOnly secondPointOnLine, + public static boolean isPoint2DOnSideOfLine2D(FramePoint2DReadOnly point, + FramePoint2DReadOnly firstPointOnLine, + FramePoint2DReadOnly secondPointOnLine, boolean testLeftSide) { point.checkReferenceFrameMatch(firstPointOnLine, secondPointOnLine); @@ -4844,7 +5109,9 @@ public static boolean isPoint2DOnSideOfLine2D(FramePoint2DReadOnly point, FrameP * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean isPoint2DOnSideOfLine2D(FramePoint2DReadOnly point, FramePoint2DReadOnly pointOnLine, FrameVector2DReadOnly lineDirection, + public static boolean isPoint2DOnSideOfLine2D(FramePoint2DReadOnly point, + FramePoint2DReadOnly pointOnLine, + FrameVector2DReadOnly lineDirection, boolean testLeftSide) { point.checkReferenceFrameMatch(pointOnLine, lineDirection); @@ -4876,8 +5143,12 @@ public static boolean isPoint2DOnSideOfLine2D(FramePoint2DReadOnly point, FrameP * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, double pointY, double pointZ, FramePoint3DReadOnly pointOnPlane, - FrameVector3DReadOnly planeNormal, boolean testForAbove) + public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, + double pointY, + double pointZ, + FramePoint3DReadOnly pointOnPlane, + FrameVector3DReadOnly planeNormal, + boolean testForAbove) { pointOnPlane.checkReferenceFrameMatch(planeNormal); return EuclidGeometryTools.isPoint3DAboveOrBelowPlane3D(pointX, pointY, pointZ, pointOnPlane, planeNormal, testForAbove); @@ -4906,7 +5177,9 @@ public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, double pointY, * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean isPoint3DAboveOrBelowPlane3D(FramePoint3DReadOnly point, FramePoint3DReadOnly pointOnPlane, FrameVector3DReadOnly planeNormal, + public static boolean isPoint3DAboveOrBelowPlane3D(FramePoint3DReadOnly point, + FramePoint3DReadOnly pointOnPlane, + FrameVector3DReadOnly planeNormal, boolean testForAbove) { point.checkReferenceFrameMatch(pointOnPlane, planeNormal); @@ -4936,7 +5209,10 @@ public static boolean isPoint3DAboveOrBelowPlane3D(FramePoint3DReadOnly point, F * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean isPoint3DAbovePlane3D(double pointX, double pointY, double pointZ, FramePoint3DReadOnly pointOnPlane, + public static boolean isPoint3DAbovePlane3D(double pointX, + double pointY, + double pointZ, + FramePoint3DReadOnly pointOnPlane, FrameVector3DReadOnly planeNormal) { return isPoint3DAboveOrBelowPlane3D(pointX, pointY, pointZ, pointOnPlane, planeNormal, true); @@ -4991,7 +5267,10 @@ public static boolean isPoint3DAbovePlane3D(FramePoint3DReadOnly point, FramePoi * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean isPoint3DBelowPlane3D(double pointX, double pointY, double pointZ, FramePoint3DReadOnly pointOnPlane, + public static boolean isPoint3DBelowPlane3D(double pointX, + double pointY, + double pointZ, + FramePoint3DReadOnly pointOnPlane, FrameVector3DReadOnly planeNormal) { return isPoint3DAboveOrBelowPlane3D(pointX, pointY, pointZ, pointOnPlane, planeNormal, false); @@ -5052,8 +5331,13 @@ public static boolean isPoint3DBelowPlane3D(FramePoint3DReadOnly point, FramePoi * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, double pointY, double pointZ, FramePoint3DReadOnly pointOnPlane, - FrameVector3DReadOnly planeFirstTangent, FrameVector3DReadOnly planeSecondTangent, boolean testForAbove) + public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, + double pointY, + double pointZ, + FramePoint3DReadOnly pointOnPlane, + FrameVector3DReadOnly planeFirstTangent, + FrameVector3DReadOnly planeSecondTangent, + boolean testForAbove) { pointOnPlane.checkReferenceFrameMatch(planeFirstTangent, planeSecondTangent); return EuclidGeometryTools.isPoint3DAboveOrBelowPlane3D(pointX, pointY, pointZ, pointOnPlane, planeFirstTangent, planeSecondTangent, testForAbove); @@ -5086,8 +5370,11 @@ public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, double pointY, * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean isPoint3DAboveOrBelowPlane3D(FramePoint3DReadOnly point, FramePoint3DReadOnly pointOnPlane, FrameVector3DReadOnly planeFirstTangent, - FrameVector3DReadOnly planeSecondTangent, boolean testForAbove) + public static boolean isPoint3DAboveOrBelowPlane3D(FramePoint3DReadOnly point, + FramePoint3DReadOnly pointOnPlane, + FrameVector3DReadOnly planeFirstTangent, + FrameVector3DReadOnly planeSecondTangent, + boolean testForAbove) { point.checkReferenceFrameMatch(pointOnPlane, planeFirstTangent, planeSecondTangent); return EuclidGeometryTools.isPoint3DAboveOrBelowPlane3D(point, pointOnPlane, planeFirstTangent, planeSecondTangent, testForAbove); @@ -5120,8 +5407,12 @@ public static boolean isPoint3DAboveOrBelowPlane3D(FramePoint3DReadOnly point, F * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean isPoint3DAbovePlane3D(double pointX, double pointY, double pointZ, FramePoint3DReadOnly pointOnPlane, - FrameVector3DReadOnly planeFirstTangent, FrameVector3DReadOnly planeSecondTangent) + public static boolean isPoint3DAbovePlane3D(double pointX, + double pointY, + double pointZ, + FramePoint3DReadOnly pointOnPlane, + FrameVector3DReadOnly planeFirstTangent, + FrameVector3DReadOnly planeSecondTangent) { return isPoint3DAboveOrBelowPlane3D(pointX, pointY, pointZ, pointOnPlane, planeFirstTangent, planeSecondTangent, true); } @@ -5151,7 +5442,9 @@ public static boolean isPoint3DAbovePlane3D(double pointX, double pointY, double * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean isPoint3DAbovePlane3D(FramePoint3DReadOnly point, FramePoint3DReadOnly pointOnPlane, FrameVector3DReadOnly planeFirstTangent, + public static boolean isPoint3DAbovePlane3D(FramePoint3DReadOnly point, + FramePoint3DReadOnly pointOnPlane, + FrameVector3DReadOnly planeFirstTangent, FrameVector3DReadOnly planeSecondTangent) { return isPoint3DAboveOrBelowPlane3D(point, pointOnPlane, planeFirstTangent, planeSecondTangent, true); @@ -5184,8 +5477,12 @@ public static boolean isPoint3DAbovePlane3D(FramePoint3DReadOnly point, FramePoi * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean isPoint3DBelowPlane3D(double pointX, double pointY, double pointZ, FramePoint3DReadOnly pointOnPlane, - FrameVector3DReadOnly planeFirstTangent, FrameVector3DReadOnly planeSecondTangent) + public static boolean isPoint3DBelowPlane3D(double pointX, + double pointY, + double pointZ, + FramePoint3DReadOnly pointOnPlane, + FrameVector3DReadOnly planeFirstTangent, + FrameVector3DReadOnly planeSecondTangent) { return isPoint3DAboveOrBelowPlane3D(pointX, pointY, pointZ, pointOnPlane, planeFirstTangent, planeSecondTangent, false); } @@ -5215,7 +5512,9 @@ public static boolean isPoint3DBelowPlane3D(double pointX, double pointY, double * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean isPoint3DBelowPlane3D(FramePoint3DReadOnly point, FramePoint3DReadOnly pointOnPlane, FrameVector3DReadOnly planeFirstTangent, + public static boolean isPoint3DBelowPlane3D(FramePoint3DReadOnly point, + FramePoint3DReadOnly pointOnPlane, + FrameVector3DReadOnly planeFirstTangent, FrameVector3DReadOnly planeSecondTangent) { return isPoint3DAboveOrBelowPlane3D(point, pointOnPlane, planeFirstTangent, planeSecondTangent, false); @@ -5244,7 +5543,8 @@ public static boolean isPoint3DBelowPlane3D(FramePoint3DReadOnly point, FramePoi * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static FrameVector3D normal3DFromThreePoint3Ds(FramePoint3DReadOnly firstPointOnPlane, FramePoint3DReadOnly secondPointOnPlane, + public static FrameVector3D normal3DFromThreePoint3Ds(FramePoint3DReadOnly firstPointOnPlane, + FramePoint3DReadOnly secondPointOnPlane, FramePoint3DReadOnly thirdPointOnPlane) { firstPointOnPlane.checkReferenceFrameMatch(secondPointOnPlane, thirdPointOnPlane); @@ -5276,8 +5576,10 @@ public static FrameVector3D normal3DFromThreePoint3Ds(FramePoint3DReadOnly first * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean normal3DFromThreePoint3Ds(FramePoint3DReadOnly firstPointOnPlane, FramePoint3DReadOnly secondPointOnPlane, - FramePoint3DReadOnly thirdPointOnPlane, FixedFrameVector3DBasics normalToPack) + public static boolean normal3DFromThreePoint3Ds(FramePoint3DReadOnly firstPointOnPlane, + FramePoint3DReadOnly secondPointOnPlane, + FramePoint3DReadOnly thirdPointOnPlane, + FixedFrameVector3DBasics normalToPack) { firstPointOnPlane.checkReferenceFrameMatch(secondPointOnPlane, thirdPointOnPlane, normalToPack); return EuclidGeometryTools.normal3DFromThreePoint3Ds(firstPointOnPlane, secondPointOnPlane, thirdPointOnPlane, normalToPack); @@ -5301,8 +5603,10 @@ public static boolean normal3DFromThreePoint3Ds(FramePoint3DReadOnly firstPointO * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static boolean normal3DFromThreePoint3Ds(FramePoint3DReadOnly firstPointOnPlane, FramePoint3DReadOnly secondPointOnPlane, - FramePoint3DReadOnly thirdPointOnPlane, FrameVector3DBasics normalToPack) + public static boolean normal3DFromThreePoint3Ds(FramePoint3DReadOnly firstPointOnPlane, + FramePoint3DReadOnly secondPointOnPlane, + FramePoint3DReadOnly thirdPointOnPlane, + FrameVector3DBasics normalToPack) { firstPointOnPlane.checkReferenceFrameMatch(secondPointOnPlane, thirdPointOnPlane); normalToPack.setReferenceFrame(firstPointOnPlane.getReferenceFrame()); @@ -5331,7 +5635,8 @@ public static boolean normal3DFromThreePoint3Ds(FramePoint3DReadOnly firstPointO * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static FramePoint2D orthogonalProjectionOnLine2D(FramePoint2DReadOnly pointToProject, FramePoint2DReadOnly firstPointOnLine, + public static FramePoint2D orthogonalProjectionOnLine2D(FramePoint2DReadOnly pointToProject, + FramePoint2DReadOnly firstPointOnLine, FramePoint2DReadOnly secondPointOnLine) { pointToProject.checkReferenceFrameMatch(firstPointOnLine, secondPointOnLine); @@ -5363,8 +5668,10 @@ public static FramePoint2D orthogonalProjectionOnLine2D(FramePoint2DReadOnly poi * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean orthogonalProjectionOnLine2D(FramePoint2DReadOnly pointToProject, FramePoint2DReadOnly firstPointOnLine, - FramePoint2DReadOnly secondPointOnLine, FixedFramePoint2DBasics projectionToPack) + public static boolean orthogonalProjectionOnLine2D(FramePoint2DReadOnly pointToProject, + FramePoint2DReadOnly firstPointOnLine, + FramePoint2DReadOnly secondPointOnLine, + FixedFramePoint2DBasics projectionToPack) { pointToProject.checkReferenceFrameMatch(firstPointOnLine, secondPointOnLine, projectionToPack); return EuclidGeometryTools.orthogonalProjectionOnLine2D(pointToProject, firstPointOnLine, secondPointOnLine, projectionToPack); @@ -5391,8 +5698,10 @@ public static boolean orthogonalProjectionOnLine2D(FramePoint2DReadOnly pointToP * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static boolean orthogonalProjectionOnLine2D(FramePoint2DReadOnly pointToProject, FramePoint2DReadOnly firstPointOnLine, - FramePoint2DReadOnly secondPointOnLine, FramePoint2DBasics projectionToPack) + public static boolean orthogonalProjectionOnLine2D(FramePoint2DReadOnly pointToProject, + FramePoint2DReadOnly firstPointOnLine, + FramePoint2DReadOnly secondPointOnLine, + FramePoint2DBasics projectionToPack) { pointToProject.checkReferenceFrameMatch(firstPointOnLine, secondPointOnLine); projectionToPack.setReferenceFrame(pointToProject.getReferenceFrame()); @@ -5421,7 +5730,8 @@ public static boolean orthogonalProjectionOnLine2D(FramePoint2DReadOnly pointToP * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static FramePoint2D orthogonalProjectionOnLine2D(FramePoint2DReadOnly pointToProject, FramePoint2DReadOnly pointOnLine, + public static FramePoint2D orthogonalProjectionOnLine2D(FramePoint2DReadOnly pointToProject, + FramePoint2DReadOnly pointOnLine, FrameVector2DReadOnly lineDirection) { pointToProject.checkReferenceFrameMatch(pointOnLine, lineDirection); @@ -5454,8 +5764,10 @@ public static FramePoint2D orthogonalProjectionOnLine2D(FramePoint2DReadOnly poi * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean orthogonalProjectionOnLine2D(FramePoint2DReadOnly pointToProject, FramePoint2DReadOnly pointOnLine, - FrameVector2DReadOnly lineDirection, FixedFramePoint2DBasics projectionToPack) + public static boolean orthogonalProjectionOnLine2D(FramePoint2DReadOnly pointToProject, + FramePoint2DReadOnly pointOnLine, + FrameVector2DReadOnly lineDirection, + FixedFramePoint2DBasics projectionToPack) { pointToProject.checkReferenceFrameMatch(pointOnLine, lineDirection, projectionToPack); return EuclidGeometryTools.orthogonalProjectionOnLine2D(pointToProject, pointOnLine, lineDirection, projectionToPack); @@ -5482,8 +5794,10 @@ public static boolean orthogonalProjectionOnLine2D(FramePoint2DReadOnly pointToP * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static boolean orthogonalProjectionOnLine2D(FramePoint2DReadOnly pointToProject, FramePoint2DReadOnly pointOnLine, - FrameVector2DReadOnly lineDirection, FramePoint2DBasics projectionToPack) + public static boolean orthogonalProjectionOnLine2D(FramePoint2DReadOnly pointToProject, + FramePoint2DReadOnly pointOnLine, + FrameVector2DReadOnly lineDirection, + FramePoint2DBasics projectionToPack) { pointToProject.checkReferenceFrameMatch(pointOnLine, lineDirection); projectionToPack.setReferenceFrame(pointToProject.getReferenceFrame()); @@ -5512,7 +5826,8 @@ public static boolean orthogonalProjectionOnLine2D(FramePoint2DReadOnly pointToP * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static FramePoint3D orthogonalProjectionOnLine3D(FramePoint3DReadOnly pointToProject, FramePoint3DReadOnly pointOnLine, + public static FramePoint3D orthogonalProjectionOnLine3D(FramePoint3DReadOnly pointToProject, + FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection) { pointToProject.checkReferenceFrameMatch(pointOnLine, lineDirection); @@ -5545,8 +5860,10 @@ public static FramePoint3D orthogonalProjectionOnLine3D(FramePoint3DReadOnly poi * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean orthogonalProjectionOnLine3D(FramePoint3DReadOnly pointToProject, FramePoint3DReadOnly pointOnLine, - FrameVector3DReadOnly lineDirection, FixedFramePoint3DBasics projectionToPack) + public static boolean orthogonalProjectionOnLine3D(FramePoint3DReadOnly pointToProject, + FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, + FixedFramePoint3DBasics projectionToPack) { pointToProject.checkReferenceFrameMatch(pointOnLine, lineDirection, projectionToPack); return EuclidGeometryTools.orthogonalProjectionOnLine3D(pointToProject, pointOnLine, lineDirection, projectionToPack); @@ -5573,8 +5890,10 @@ public static boolean orthogonalProjectionOnLine3D(FramePoint3DReadOnly pointToP * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static boolean orthogonalProjectionOnLine3D(FramePoint3DReadOnly pointToProject, FramePoint3DReadOnly pointOnLine, - FrameVector3DReadOnly lineDirection, FramePoint3DBasics projectionToPack) + public static boolean orthogonalProjectionOnLine3D(FramePoint3DReadOnly pointToProject, + FramePoint3DReadOnly pointOnLine, + FrameVector3DReadOnly lineDirection, + FramePoint3DBasics projectionToPack) { pointToProject.checkReferenceFrameMatch(pointOnLine, lineDirection); projectionToPack.setReferenceFrame(pointToProject.getReferenceFrame()); @@ -5605,7 +5924,8 @@ public static boolean orthogonalProjectionOnLine3D(FramePoint3DReadOnly pointToP * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static FramePoint2D orthogonalProjectionOnLineSegment2D(FramePoint2DReadOnly pointToProject, FramePoint2DReadOnly lineSegmentStart, + public static FramePoint2D orthogonalProjectionOnLineSegment2D(FramePoint2DReadOnly pointToProject, + FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd) { pointToProject.checkReferenceFrameMatch(lineSegmentStart, lineSegmentEnd); @@ -5641,8 +5961,11 @@ public static FramePoint2D orthogonalProjectionOnLineSegment2D(FramePoint2DReadO * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean orthogonalProjectionOnLineSegment2D(double pointToProjectX, double pointToProjectY, FramePoint2DReadOnly lineSegmentStart, - FramePoint2DReadOnly lineSegmentEnd, FixedFramePoint2DBasics projectionToPack) + public static boolean orthogonalProjectionOnLineSegment2D(double pointToProjectX, + double pointToProjectY, + FramePoint2DReadOnly lineSegmentStart, + FramePoint2DReadOnly lineSegmentEnd, + FixedFramePoint2DBasics projectionToPack) { lineSegmentStart.checkReferenceFrameMatch(lineSegmentEnd, projectionToPack); return EuclidGeometryTools.orthogonalProjectionOnLineSegment2D(pointToProjectX, pointToProjectY, lineSegmentStart, lineSegmentEnd, projectionToPack); @@ -5672,8 +5995,11 @@ public static boolean orthogonalProjectionOnLineSegment2D(double pointToProjectX * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static boolean orthogonalProjectionOnLineSegment2D(double pointToProjectX, double pointToProjectY, FramePoint2DReadOnly lineSegmentStart, - FramePoint2DReadOnly lineSegmentEnd, FramePoint2DBasics projectionToPack) + public static boolean orthogonalProjectionOnLineSegment2D(double pointToProjectX, + double pointToProjectY, + FramePoint2DReadOnly lineSegmentStart, + FramePoint2DReadOnly lineSegmentEnd, + FramePoint2DBasics projectionToPack) { lineSegmentStart.checkReferenceFrameMatch(lineSegmentEnd); projectionToPack.setReferenceFrame(lineSegmentStart.getReferenceFrame()); @@ -5703,8 +6029,10 @@ public static boolean orthogonalProjectionOnLineSegment2D(double pointToProjectX * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean orthogonalProjectionOnLineSegment2D(FramePoint2DReadOnly pointToProject, FramePoint2DReadOnly lineSegmentStart, - FramePoint2DReadOnly lineSegmentEnd, FixedFramePoint2DBasics projectionToPack) + public static boolean orthogonalProjectionOnLineSegment2D(FramePoint2DReadOnly pointToProject, + FramePoint2DReadOnly lineSegmentStart, + FramePoint2DReadOnly lineSegmentEnd, + FixedFramePoint2DBasics projectionToPack) { pointToProject.checkReferenceFrameMatch(lineSegmentStart, lineSegmentEnd, projectionToPack); return EuclidGeometryTools.orthogonalProjectionOnLineSegment2D(pointToProject, lineSegmentStart, lineSegmentEnd, projectionToPack); @@ -5733,8 +6061,10 @@ public static boolean orthogonalProjectionOnLineSegment2D(FramePoint2DReadOnly p * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static boolean orthogonalProjectionOnLineSegment2D(FramePoint2DReadOnly pointToProject, FramePoint2DReadOnly lineSegmentStart, - FramePoint2DReadOnly lineSegmentEnd, FramePoint2DBasics projectionToPack) + public static boolean orthogonalProjectionOnLineSegment2D(FramePoint2DReadOnly pointToProject, + FramePoint2DReadOnly lineSegmentStart, + FramePoint2DReadOnly lineSegmentEnd, + FramePoint2DBasics projectionToPack) { pointToProject.checkReferenceFrameMatch(lineSegmentStart, lineSegmentEnd); projectionToPack.setReferenceFrame(pointToProject.getReferenceFrame()); @@ -5765,7 +6095,8 @@ public static boolean orthogonalProjectionOnLineSegment2D(FramePoint2DReadOnly p * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static FramePoint3D orthogonalProjectionOnLineSegment3D(FramePoint3DReadOnly pointToProject, FramePoint3DReadOnly lineSegmentStart, + public static FramePoint3D orthogonalProjectionOnLineSegment3D(FramePoint3DReadOnly pointToProject, + FramePoint3DReadOnly lineSegmentStart, FramePoint3DReadOnly lineSegmentEnd) { pointToProject.checkReferenceFrameMatch(lineSegmentStart, lineSegmentEnd); @@ -5799,8 +6130,10 @@ public static FramePoint3D orthogonalProjectionOnLineSegment3D(FramePoint3DReadO * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean orthogonalProjectionOnLineSegment3D(FramePoint3DReadOnly pointToProject, FramePoint3DReadOnly lineSegmentStart, - FramePoint3DReadOnly lineSegmentEnd, FixedFramePoint3DBasics projectionToPack) + public static boolean orthogonalProjectionOnLineSegment3D(FramePoint3DReadOnly pointToProject, + FramePoint3DReadOnly lineSegmentStart, + FramePoint3DReadOnly lineSegmentEnd, + FixedFramePoint3DBasics projectionToPack) { pointToProject.checkReferenceFrameMatch(lineSegmentStart, lineSegmentEnd, projectionToPack); return EuclidGeometryTools.orthogonalProjectionOnLineSegment3D(pointToProject, lineSegmentStart, lineSegmentEnd, projectionToPack); @@ -5829,8 +6162,10 @@ public static boolean orthogonalProjectionOnLineSegment3D(FramePoint3DReadOnly p * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static boolean orthogonalProjectionOnLineSegment3D(FramePoint3DReadOnly pointToProject, FramePoint3DReadOnly lineSegmentStart, - FramePoint3DReadOnly lineSegmentEnd, FramePoint3DBasics projectionToPack) + public static boolean orthogonalProjectionOnLineSegment3D(FramePoint3DReadOnly pointToProject, + FramePoint3DReadOnly lineSegmentStart, + FramePoint3DReadOnly lineSegmentEnd, + FramePoint3DBasics projectionToPack) { pointToProject.checkReferenceFrameMatch(lineSegmentStart, lineSegmentEnd); projectionToPack.setReferenceFrame(pointToProject.getReferenceFrame()); @@ -5855,7 +6190,8 @@ public static boolean orthogonalProjectionOnLineSegment3D(FramePoint3DReadOnly p * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static FramePoint3D orthogonalProjectionOnPlane3D(FramePoint3DReadOnly pointToProject, FramePoint3DReadOnly pointOnPlane, + public static FramePoint3D orthogonalProjectionOnPlane3D(FramePoint3DReadOnly pointToProject, + FramePoint3DReadOnly pointOnPlane, FrameVector3DReadOnly planeNormal) { pointToProject.checkReferenceFrameMatch(pointOnPlane, planeNormal); @@ -5886,8 +6222,10 @@ public static FramePoint3D orthogonalProjectionOnPlane3D(FramePoint3DReadOnly po * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean orthogonalProjectionOnPlane3D(FramePoint3DReadOnly pointToProject, FramePoint3DReadOnly pointOnPlane, - FrameVector3DReadOnly planeNormal, FixedFramePoint3DBasics projectionToPack) + public static boolean orthogonalProjectionOnPlane3D(FramePoint3DReadOnly pointToProject, + FramePoint3DReadOnly pointOnPlane, + FrameVector3DReadOnly planeNormal, + FixedFramePoint3DBasics projectionToPack) { pointToProject.checkReferenceFrameMatch(pointOnPlane, planeNormal, projectionToPack); return EuclidGeometryTools.orthogonalProjectionOnPlane3D(pointToProject, pointOnPlane, planeNormal, projectionToPack); @@ -5913,8 +6251,10 @@ public static boolean orthogonalProjectionOnPlane3D(FramePoint3DReadOnly pointTo * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static boolean orthogonalProjectionOnPlane3D(FramePoint3DReadOnly pointToProject, FramePoint3DReadOnly pointOnPlane, - FrameVector3DReadOnly planeNormal, FramePoint3DBasics projectionToPack) + public static boolean orthogonalProjectionOnPlane3D(FramePoint3DReadOnly pointToProject, + FramePoint3DReadOnly pointOnPlane, + FrameVector3DReadOnly planeNormal, + FramePoint3DBasics projectionToPack) { pointToProject.checkReferenceFrameMatch(pointOnPlane, planeNormal); projectionToPack.setReferenceFrame(pointToProject.getReferenceFrame()); @@ -5943,7 +6283,11 @@ public static boolean orthogonalProjectionOnPlane3D(FramePoint3DReadOnly pointTo * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean orthogonalProjectionOnPlane3D(double x, double y, double z, FramePoint3DReadOnly pointOnPlane, FrameVector3DReadOnly planeNormal, + public static boolean orthogonalProjectionOnPlane3D(double x, + double y, + double z, + FramePoint3DReadOnly pointOnPlane, + FrameVector3DReadOnly planeNormal, FixedFramePoint3DBasics projectionToPack) { pointOnPlane.checkReferenceFrameMatch(planeNormal, projectionToPack); @@ -5972,7 +6316,11 @@ public static boolean orthogonalProjectionOnPlane3D(double x, double y, double z * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static boolean orthogonalProjectionOnPlane3D(double x, double y, double z, FramePoint3DReadOnly pointOnPlane, FrameVector3DReadOnly planeNormal, + public static boolean orthogonalProjectionOnPlane3D(double x, + double y, + double z, + FramePoint3DReadOnly pointOnPlane, + FrameVector3DReadOnly planeNormal, FramePoint3DBasics projectionToPack) { pointOnPlane.checkReferenceFrameMatch(planeNormal); @@ -6003,8 +6351,10 @@ public static boolean orthogonalProjectionOnPlane3D(double x, double y, double z * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static double percentageOfIntersectionBetweenTwoLine2Ds(FramePoint2DReadOnly pointOnLine1, FrameVector2DReadOnly lineDirection1, - FramePoint2DReadOnly pointOnLine2, FrameVector2DReadOnly lineDirection2) + public static double percentageOfIntersectionBetweenTwoLine2Ds(FramePoint2DReadOnly pointOnLine1, + FrameVector2DReadOnly lineDirection1, + FramePoint2DReadOnly pointOnLine2, + FrameVector2DReadOnly lineDirection2) { pointOnLine1.checkReferenceFrameMatch(lineDirection1, pointOnLine2, lineDirection2); return EuclidGeometryTools.percentageOfIntersectionBetweenTwoLine2Ds(pointOnLine1, lineDirection1, pointOnLine2, lineDirection2); @@ -6037,8 +6387,10 @@ public static double percentageOfIntersectionBetweenTwoLine2Ds(FramePoint2DReadO * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static double percentageOfIntersectionBetweenLineSegment2DAndLine2D(FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd, - FramePoint2DReadOnly pointOnLine, FrameVector2DReadOnly lineDirection) + public static double percentageOfIntersectionBetweenLineSegment2DAndLine2D(FramePoint2DReadOnly lineSegmentStart, + FramePoint2DReadOnly lineSegmentEnd, + FramePoint2DReadOnly pointOnLine, + FrameVector2DReadOnly lineDirection) { lineSegmentStart.checkReferenceFrameMatch(lineSegmentEnd, pointOnLine, lineDirection); return EuclidGeometryTools.percentageOfIntersectionBetweenLineSegment2DAndLine2D(lineSegmentStart, lineSegmentEnd, pointOnLine, lineDirection); @@ -6228,7 +6580,10 @@ public static double percentageAlongLine3D(FramePoint3DReadOnly point, FramePoin * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static double percentageAlongLine3D(double pointX, double pointY, double pointZ, FramePoint3DReadOnly pointOnLine, + public static double percentageAlongLine3D(double pointX, + double pointY, + double pointZ, + FramePoint3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection) { pointOnLine.checkReferenceFrameMatch(lineDirection); @@ -6266,7 +6621,10 @@ public static double percentageAlongLine3D(double pointX, double pointY, double * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static double percentageAlongLineSegment3D(double pointX, double pointY, double pointZ, FramePoint3DReadOnly lineSegmentStart, + public static double percentageAlongLineSegment3D(double pointX, + double pointY, + double pointZ, + FramePoint3DReadOnly lineSegmentStart, FramePoint3DReadOnly lineSegmentEnd) { lineSegmentStart.checkReferenceFrameMatch(lineSegmentEnd); @@ -6331,8 +6689,10 @@ public static double percentageAlongLineSegment3D(FramePoint3DReadOnly point, Fr * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean perpendicularBisector2D(FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd, - FixedFramePoint2DBasics bisectorStartToPack, FixedFrameVector2DBasics bisectorDirectionToPack) + public static boolean perpendicularBisector2D(FramePoint2DReadOnly lineSegmentStart, + FramePoint2DReadOnly lineSegmentEnd, + FixedFramePoint2DBasics bisectorStartToPack, + FixedFrameVector2DBasics bisectorDirectionToPack) { lineSegmentStart.checkReferenceFrameMatch(lineSegmentEnd, bisectorStartToPack, bisectorDirectionToPack); return EuclidGeometryTools.perpendicularBisector2D(lineSegmentStart, lineSegmentEnd, bisectorStartToPack, bisectorDirectionToPack); @@ -6361,8 +6721,10 @@ public static boolean perpendicularBisector2D(FramePoint2DReadOnly lineSegmentSt * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static boolean perpendicularBisector2D(FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd, - FramePoint2DBasics bisectorStartToPack, FrameVector2DBasics bisectorDirectionToPack) + public static boolean perpendicularBisector2D(FramePoint2DReadOnly lineSegmentStart, + FramePoint2DReadOnly lineSegmentEnd, + FramePoint2DBasics bisectorStartToPack, + FrameVector2DBasics bisectorDirectionToPack) { lineSegmentStart.checkReferenceFrameMatch(lineSegmentEnd, bisectorStartToPack, bisectorDirectionToPack); return EuclidGeometryTools.perpendicularBisector2D(lineSegmentStart, lineSegmentEnd, bisectorStartToPack, bisectorDirectionToPack); @@ -6400,7 +6762,8 @@ public static boolean perpendicularBisector2D(FramePoint2DReadOnly lineSegmentSt * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static List perpendicularBisectorSegment2D(FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd, + public static List perpendicularBisectorSegment2D(FramePoint2DReadOnly lineSegmentStart, + FramePoint2DReadOnly lineSegmentEnd, double bisectorSegmentHalfLength) { lineSegmentStart.checkReferenceFrameMatch(lineSegmentEnd); @@ -6440,8 +6803,10 @@ public static List perpendicularBisectorSegment2D(FramePoint2DReadOnly * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean perpendicularBisectorSegment2D(FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd, - double bisectorSegmentHalfLength, FixedFramePoint2DBasics bisectorSegmentStartToPack, + public static boolean perpendicularBisectorSegment2D(FramePoint2DReadOnly lineSegmentStart, + FramePoint2DReadOnly lineSegmentEnd, + double bisectorSegmentHalfLength, + FixedFramePoint2DBasics bisectorSegmentStartToPack, FixedFramePoint2DBasics bisectorSegmentEndToPack) { lineSegmentStart.checkReferenceFrameMatch(lineSegmentEnd, bisectorSegmentStartToPack, bisectorSegmentEndToPack); @@ -6485,8 +6850,10 @@ public static boolean perpendicularBisectorSegment2D(FramePoint2DReadOnly lineSe * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static boolean perpendicularBisectorSegment2D(FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd, - double bisectorSegmentHalfLength, FramePoint2DBasics bisectorSegmentStartToPack, + public static boolean perpendicularBisectorSegment2D(FramePoint2DReadOnly lineSegmentStart, + FramePoint2DReadOnly lineSegmentEnd, + double bisectorSegmentHalfLength, + FramePoint2DBasics bisectorSegmentStartToPack, FramePoint2DBasics bisectorSegmentEndToPack) { lineSegmentStart.checkReferenceFrameMatch(lineSegmentEnd); @@ -6581,7 +6948,8 @@ public static void perpendicularVector2D(FrameVector2DReadOnly vector, FrameVect * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static FrameVector3D perpendicularVector3DFromLine3DToPoint3D(FramePoint3DReadOnly point, FramePoint3DReadOnly firstPointOnLine, + public static FrameVector3D perpendicularVector3DFromLine3DToPoint3D(FramePoint3DReadOnly point, + FramePoint3DReadOnly firstPointOnLine, FramePoint3DReadOnly secondPointOnLine, FixedFramePoint3DBasics orthogonalProjectionToPack) { @@ -6627,8 +6995,10 @@ public static FrameVector3D perpendicularVector3DFromLine3DToPoint3D(FramePoint3 * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static FrameVector3D perpendicularVector3DFromLine3DToPoint3D(FramePoint3DReadOnly point, FramePoint3DReadOnly firstPointOnLine, - FramePoint3DReadOnly secondPointOnLine, FramePoint3DBasics orthogonalProjectionToPack) + public static FrameVector3D perpendicularVector3DFromLine3DToPoint3D(FramePoint3DReadOnly point, + FramePoint3DReadOnly firstPointOnLine, + FramePoint3DReadOnly secondPointOnLine, + FramePoint3DBasics orthogonalProjectionToPack) { point.checkReferenceFrameMatch(firstPointOnLine, secondPointOnLine); if (orthogonalProjectionToPack != null) @@ -6671,8 +7041,10 @@ public static FrameVector3D perpendicularVector3DFromLine3DToPoint3D(FramePoint3 * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean perpendicularVector3DFromLine3DToPoint3D(FramePoint3DReadOnly point, FramePoint3DReadOnly firstPointOnLine, - FramePoint3DReadOnly secondPointOnLine, FixedFramePoint3DBasics orthogonalProjectionToPack, + public static boolean perpendicularVector3DFromLine3DToPoint3D(FramePoint3DReadOnly point, + FramePoint3DReadOnly firstPointOnLine, + FramePoint3DReadOnly secondPointOnLine, + FixedFramePoint3DBasics orthogonalProjectionToPack, FixedFrameVector3DBasics perpendicularVectorToPack) { point.checkReferenceFrameMatch(firstPointOnLine, secondPointOnLine, perpendicularVectorToPack); @@ -6713,8 +7085,10 @@ public static boolean perpendicularVector3DFromLine3DToPoint3D(FramePoint3DReadO * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static boolean perpendicularVector3DFromLine3DToPoint3D(FramePoint3DReadOnly point, FramePoint3DReadOnly firstPointOnLine, - FramePoint3DReadOnly secondPointOnLine, FramePoint3DBasics orthogonalProjectionToPack, + public static boolean perpendicularVector3DFromLine3DToPoint3D(FramePoint3DReadOnly point, + FramePoint3DReadOnly firstPointOnLine, + FramePoint3DReadOnly secondPointOnLine, + FramePoint3DBasics orthogonalProjectionToPack, FrameVector3DBasics perpendicularVectorToPack) { point.checkReferenceFrameMatch(firstPointOnLine, secondPointOnLine); @@ -6752,7 +7126,9 @@ public static boolean perpendicularVector3DFromLine3DToPoint3D(FramePoint3DReadO * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static double signedDistanceFromPoint2DToLine2D(double pointX, double pointY, FramePoint2DReadOnly firstPointOnLine, + public static double signedDistanceFromPoint2DToLine2D(double pointX, + double pointY, + FramePoint2DReadOnly firstPointOnLine, FramePoint2DReadOnly secondPointOnLine) { firstPointOnLine.checkReferenceFrameMatch(secondPointOnLine); @@ -6811,7 +7187,8 @@ public static double signedDistanceFromPoint2DToLine2D(double pointX, double poi * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static double signedDistanceFromPoint2DToLine2D(FramePoint2DReadOnly point, FramePoint2DReadOnly firstPointOnLine, + public static double signedDistanceFromPoint2DToLine2D(FramePoint2DReadOnly point, + FramePoint2DReadOnly firstPointOnLine, FramePoint2DReadOnly secondPointOnLine) { point.checkReferenceFrameMatch(firstPointOnLine, secondPointOnLine); @@ -6872,7 +7249,10 @@ public static double signedDistanceFromPoint2DToLine2D(FramePoint2DReadOnly poin * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static boolean sphere3DPositionFromThreePoints(FramePoint3DReadOnly p1, FramePoint3DReadOnly p2, FramePoint3DReadOnly p3, double sphere3DRadius, + public static boolean sphere3DPositionFromThreePoints(FramePoint3DReadOnly p1, + FramePoint3DReadOnly p2, + FramePoint3DReadOnly p3, + double sphere3DRadius, FixedFramePoint3DBasics sphere3DPositionToPack) { p1.checkReferenceFrameMatch(p2, p3, sphere3DPositionToPack); @@ -6905,7 +7285,10 @@ public static boolean sphere3DPositionFromThreePoints(FramePoint3DReadOnly p1, F * @throws ReferenceFrameMismatchException if the three points are not all expressed in the same * reference frame. */ - public static boolean sphere3DPositionFromThreePoints(FramePoint3DReadOnly p1, FramePoint3DReadOnly p2, FramePoint3DReadOnly p3, double sphere3DRadius, + public static boolean sphere3DPositionFromThreePoints(FramePoint3DReadOnly p1, + FramePoint3DReadOnly p2, + FramePoint3DReadOnly p3, + double sphere3DRadius, FramePoint3DBasics sphere3DPositionToPack) { p1.checkReferenceFrameMatch(p2, p3); @@ -6929,8 +7312,10 @@ public static boolean sphere3DPositionFromThreePoints(FramePoint3DReadOnly p1, F * @throws ReferenceFrameMismatchException if the arguments are not all expressed in the same * reference frame. */ - public static void topVertex3DOfIsoscelesTriangle3D(FramePoint3DReadOnly baseVertexA, FramePoint3DReadOnly baseVertexC, - FrameVector3DReadOnly trianglePlaneNormal, double ccwAngleAboutNormalAtTopVertex, + public static void topVertex3DOfIsoscelesTriangle3D(FramePoint3DReadOnly baseVertexA, + FramePoint3DReadOnly baseVertexC, + FrameVector3DReadOnly trianglePlaneNormal, + double ccwAngleAboutNormalAtTopVertex, FixedFramePoint3DBasics topVertexBToPack) { baseVertexA.checkReferenceFrameMatch(baseVertexC, trianglePlaneNormal, topVertexBToPack); @@ -6953,8 +7338,10 @@ public static void topVertex3DOfIsoscelesTriangle3D(FramePoint3DReadOnly baseVer * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ - public static void topVertex3DOfIsoscelesTriangle3D(FramePoint3DReadOnly baseVertexA, FramePoint3DReadOnly baseVertexC, - FrameVector3DReadOnly trianglePlaneNormal, double ccwAngleAboutNormalAtTopVertex, + public static void topVertex3DOfIsoscelesTriangle3D(FramePoint3DReadOnly baseVertexA, + FramePoint3DReadOnly baseVertexC, + FrameVector3DReadOnly trianglePlaneNormal, + double ccwAngleAboutNormalAtTopVertex, FramePoint3DBasics topVertexBToPack) { baseVertexA.checkReferenceFrameMatch(baseVertexC, trianglePlaneNormal); diff --git a/src/geometry/java/us/ihmc/euclid/geometry/LineSegment3D.java b/src/geometry/java/us/ihmc/euclid/geometry/LineSegment3D.java index 645c87c9b..aa7763a1d 100644 --- a/src/geometry/java/us/ihmc/euclid/geometry/LineSegment3D.java +++ b/src/geometry/java/us/ihmc/euclid/geometry/LineSegment3D.java @@ -57,7 +57,11 @@ public LineSegment3D(Point3DReadOnly firstEndpoint, Point3DReadOnly secondEndpoi * @param secondEndpointY y-coordinate of the second endpoint of this line segment. * @param secondEndpointZ z-coordinate of the second endpoint of this line segment. */ - public LineSegment3D(double firstEndpointX, double firstEndpointY, double firstEndpointZ, double secondEndpointX, double secondEndpointY, + public LineSegment3D(double firstEndpointX, + double firstEndpointY, + double firstEndpointZ, + double secondEndpointX, + double secondEndpointY, double secondEndpointZ) { set(firstEndpointX, firstEndpointY, firstEndpointZ, secondEndpointX, secondEndpointY, secondEndpointZ); diff --git a/src/geometry/java/us/ihmc/euclid/geometry/exceptions/BoundingBoxException.java b/src/geometry/java/us/ihmc/euclid/geometry/exceptions/BoundingBoxException.java index 1d3a0055a..dd51cffc5 100644 --- a/src/geometry/java/us/ihmc/euclid/geometry/exceptions/BoundingBoxException.java +++ b/src/geometry/java/us/ihmc/euclid/geometry/exceptions/BoundingBoxException.java @@ -99,7 +99,11 @@ public BoundingBoxException(Point3DReadOnly boundingBoxMin, Point3DReadOnly boun * @param boundingBoxMaxZ the maximum z-coordinate of the bounding box to be displayed in the detail * message. */ - public BoundingBoxException(double boundingBoxMinX, double boundingBoxMinY, double boundingBoxMinZ, double boundingBoxMaxX, double boundingBoxMaxY, + public BoundingBoxException(double boundingBoxMinX, + double boundingBoxMinY, + double boundingBoxMinZ, + double boundingBoxMaxX, + double boundingBoxMaxY, double boundingBoxMaxZ) { super("Improper bounding box 3D: min = " + EuclidCoreIOTools.getStringOf("(", " )", ", ", boundingBoxMinX, boundingBoxMinY, boundingBoxMinZ) + ", max = " diff --git a/src/geometry/java/us/ihmc/euclid/geometry/interfaces/BoundingBox2DReadOnly.java b/src/geometry/java/us/ihmc/euclid/geometry/interfaces/BoundingBox2DReadOnly.java index d7b9c8af9..11630cd72 100644 --- a/src/geometry/java/us/ihmc/euclid/geometry/interfaces/BoundingBox2DReadOnly.java +++ b/src/geometry/java/us/ihmc/euclid/geometry/interfaces/BoundingBox2DReadOnly.java @@ -470,7 +470,9 @@ default int intersectionWithLine2D(Line2DReadOnly line2D, Point2DBasics firstInt * 0 or 2. * @throws RuntimeException if this bounding box is improper according to {@link #checkBounds()}. */ - default int intersectionWithLine2D(Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection, Point2DBasics firstIntersectionToPack, + default int intersectionWithLine2D(Point2DReadOnly pointOnLine, + Vector2DReadOnly lineDirection, + Point2DBasics firstIntersectionToPack, Point2DBasics secondIntersectionToPack) { checkBounds(); @@ -543,7 +545,9 @@ default int intersectionWithLineSegment2D(LineSegment2DReadOnly lineSegment2D, P * equal to 0, 1, or 2. * @throws RuntimeException if this bounding box is improper according to {@link #checkBounds()}. */ - default int intersectionWithLineSegment2D(Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd, Point2DBasics firstIntersectionToPack, + default int intersectionWithLineSegment2D(Point2DReadOnly lineSegmentStart, + Point2DReadOnly lineSegmentEnd, + Point2DBasics firstIntersectionToPack, Point2DBasics secondIntersectionToPack) { checkBounds(); @@ -581,7 +585,9 @@ default int intersectionWithLineSegment2D(Point2DReadOnly lineSegmentStart, Poin * 0, 1, or 2. * @throws RuntimeException if this bounding box is improper according to {@link #checkBounds()}. */ - default int intersectionWithRay2D(Point2DReadOnly rayOrigin, Vector2DReadOnly rayDirection, Point2DBasics firstIntersectionToPack, + default int intersectionWithRay2D(Point2DReadOnly rayOrigin, + Vector2DReadOnly rayDirection, + Point2DBasics firstIntersectionToPack, Point2DBasics secondIntersectionToPack) { checkBounds(); diff --git a/src/geometry/java/us/ihmc/euclid/geometry/interfaces/BoundingBox3DReadOnly.java b/src/geometry/java/us/ihmc/euclid/geometry/interfaces/BoundingBox3DReadOnly.java index 7bc46a6d7..0c97603ce 100644 --- a/src/geometry/java/us/ihmc/euclid/geometry/interfaces/BoundingBox3DReadOnly.java +++ b/src/geometry/java/us/ihmc/euclid/geometry/interfaces/BoundingBox3DReadOnly.java @@ -717,7 +717,9 @@ default int intersectionWithLine3D(Line3DReadOnly line3D, Point3DBasics firstInt * @return the number of intersections between the line and this bounding box. It is either equal to * 0 or 2. */ - default int intersectionWithLine3D(Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection, Point3DBasics firstIntersectionToPack, + default int intersectionWithLine3D(Point3DReadOnly pointOnLine, + Vector3DReadOnly lineDirection, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { return intersectionBetweenLine3DAndBoundingBox3D(getMinPoint(), @@ -787,7 +789,9 @@ default int intersectionWithLineSegment3D(LineSegment3DReadOnly lineSegment3D, P * @return the number of intersections between the line segment and this bounding box. It is either * equal to 0, 1, or 2. */ - default int intersectionWithLineSegment3D(Point3DReadOnly lineSegmentStart, Point3DReadOnly lineSegmentEnd, Point3DBasics firstIntersectionToPack, + default int intersectionWithLineSegment3D(Point3DReadOnly lineSegmentStart, + Point3DReadOnly lineSegmentEnd, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { return intersectionBetweenLineSegment3DAndBoundingBox3D(getMinPoint(), @@ -823,7 +827,9 @@ default int intersectionWithLineSegment3D(Point3DReadOnly lineSegmentStart, Poin * @return the number of intersections between the ray and this bounding box. It is either equal to * 0, 1, or 2. */ - default int intersectionWithRay3D(Point3DReadOnly rayOrigin, Vector3DReadOnly rayDirection, Point3DBasics firstIntersectionToPack, + default int intersectionWithRay3D(Point3DReadOnly rayOrigin, + Vector3DReadOnly rayDirection, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { return intersectionBetweenRay3DAndBoundingBox3D(getMinPoint(), getMaxPoint(), rayOrigin, rayDirection, firstIntersectionToPack, secondIntersectionToPack); diff --git a/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryFactories.java b/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryFactories.java index 16669a2f8..91d58776b 100644 --- a/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryFactories.java +++ b/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryFactories.java @@ -247,7 +247,8 @@ public static BoundingBox2DBasics newObservableBoundingBox2DBasics(BoundingBoxCh * @return the observable bounding box. */ public static BoundingBox2DBasics newObservableBoundingBox2DBasics(BoundingBoxChangedListener valueChangedListener, - BiConsumer valueAccessedListener, BoundingBox2DBasics source) + BiConsumer valueAccessedListener, + BoundingBox2DBasics source) { return new BoundingBox2DBasics() { @@ -365,7 +366,8 @@ public static BoundingBox3DBasics newObservableBoundingBox3DBasics(BoundingBoxCh * @return the observable bounding box. */ public static BoundingBox3DBasics newObservableBoundingBox3DBasics(BoundingBoxChangedListener valueChangedListener, - BiConsumer valueAccessedListener, BoundingBox3DBasics source) + BiConsumer valueAccessedListener, + BoundingBox3DBasics source) { return new BoundingBox3DBasics() { 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 b7149fd03..5de23e4b8 100644 --- a/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryPolygonTools.java +++ b/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryPolygonTools.java @@ -351,7 +351,9 @@ public static int inPlaceGrahamScanConvexHull2D(List * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static double computeConvexPolygon2DArea(List convexPolygon2D, int numberOfVertices, boolean clockwiseOrdered, + public static double computeConvexPolygon2DArea(List convexPolygon2D, + int numberOfVertices, + boolean clockwiseOrdered, Point2DBasics centroidToPack) { checkNumberOfVertices(convexPolygon2D, numberOfVertices); @@ -449,7 +451,10 @@ else if (numberOfVertices < 3) * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the * size of the given list of vertices. */ - public static boolean edgeNormal(int edgeIndex, List convexPolygon2D, int numberOfVertices, boolean clockwiseOrdered, + public static boolean edgeNormal(int edgeIndex, + List convexPolygon2D, + int numberOfVertices, + boolean clockwiseOrdered, Vector2DBasics normalToPack) { checkNumberOfVertices(convexPolygon2D, numberOfVertices); @@ -502,7 +507,10 @@ public static boolean edgeNormal(int edgeIndex, List * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static boolean isPoint2DInsideConvexPolygon2D(double pointX, double pointY, List convexPolygon2D, int numberOfVertices, + public static boolean isPoint2DInsideConvexPolygon2D(double pointX, + double pointY, + List convexPolygon2D, + int numberOfVertices, boolean clockwiseOrdered) { checkNumberOfVertices(convexPolygon2D, numberOfVertices); @@ -602,7 +610,9 @@ public static boolean isPoint2DInsideConvexPolygon2D(double pointX, double point * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static boolean isPoint2DInsideConvexPolygon2D(Point2DReadOnly point, List convexPolygon2D, int numberOfVertices, + public static boolean isPoint2DInsideConvexPolygon2D(Point2DReadOnly point, + List convexPolygon2D, + int numberOfVertices, boolean clockwiseOrdered) { return isPoint2DInsideConvexPolygon2D(point.getX(), point.getY(), convexPolygon2D, numberOfVertices, clockwiseOrdered); @@ -642,8 +652,12 @@ public static boolean isPoint2DInsideConvexPolygon2D(Point2DReadOnly point, List * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static boolean isPoint2DInsideConvexPolygon2D(double pointX, double pointY, List convexPolygon2D, int numberOfVertices, - boolean clockwiseOrdered, double epsilon) + public static boolean isPoint2DInsideConvexPolygon2D(double pointX, + double pointY, + List convexPolygon2D, + int numberOfVertices, + boolean clockwiseOrdered, + double epsilon) { return signedDistanceFromPoint2DToConvexPolygon2D(pointX, pointY, convexPolygon2D, numberOfVertices, clockwiseOrdered) <= epsilon; } @@ -681,8 +695,11 @@ public static boolean isPoint2DInsideConvexPolygon2D(double pointX, double point * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static boolean isPoint2DInsideConvexPolygon2D(Point2DReadOnly point, List convexPolygon2D, int numberOfVertices, - boolean clockwiseOrdered, double epsilon) + public static boolean isPoint2DInsideConvexPolygon2D(Point2DReadOnly point, + List convexPolygon2D, + int numberOfVertices, + boolean clockwiseOrdered, + double epsilon) { return isPoint2DInsideConvexPolygon2D(point.getX(), point.getY(), convexPolygon2D, numberOfVertices, clockwiseOrdered, epsilon); } @@ -719,9 +736,12 @@ public static boolean isPoint2DInsideConvexPolygon2D(Point2DReadOnly point, List * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static int intersectionBetweenLine2DAndConvexPolygon2D(Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection, - List convexPolygon2D, int numberOfVertices, - boolean clockwiseOrdered, Point2DBasics firstIntersectionToPack, + public static int intersectionBetweenLine2DAndConvexPolygon2D(Point2DReadOnly pointOnLine, + Vector2DReadOnly lineDirection, + List convexPolygon2D, + int numberOfVertices, + boolean clockwiseOrdered, + Point2DBasics firstIntersectionToPack, Point2DBasics secondIntersectionToPack) { double pointOnLineX = pointOnLine.getX(); @@ -774,9 +794,14 @@ public static int intersectionBetweenLine2DAndConvexPolygon2D(Point2DReadOnly po * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static int intersectionBetweenLine2DAndConvexPolygon2D(double pointOnLineX, double pointOnLineY, double lineDirectionX, double lineDirectionY, - List convexPolygon2D, int numberOfVertices, - boolean clockwiseOrdered, Point2DBasics firstIntersectionToPack, + public static int intersectionBetweenLine2DAndConvexPolygon2D(double pointOnLineX, + double pointOnLineY, + double lineDirectionX, + double lineDirectionY, + List convexPolygon2D, + int numberOfVertices, + boolean clockwiseOrdered, + Point2DBasics firstIntersectionToPack, Point2DBasics secondIntersectionToPack) { checkNumberOfVertices(convexPolygon2D, numberOfVertices); @@ -907,8 +932,10 @@ public static int intersectionBetweenLine2DAndConvexPolygon2D(double pointOnLine * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static Point2D[] intersectionBetweenLine2DAndConvexPolygon2D(Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection, - List convexPolygon2D, int numberOfVertices, + public static Point2D[] intersectionBetweenLine2DAndConvexPolygon2D(Point2DReadOnly pointOnLine, + Vector2DReadOnly lineDirection, + List convexPolygon2D, + int numberOfVertices, boolean clockwiseOrdered) { Point2D firstIntersection = new Point2D(); @@ -977,9 +1004,12 @@ public static Point2D[] intersectionBetweenLine2DAndConvexPolygon2D(Point2DReadO * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static int intersectionBetweenLineSegment2DAndConvexPolygon2D(Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd, - List convexPolygon2D, int numberOfVertices, - boolean clockwiseOrdered, Point2DBasics firstIntersectionToPack, + public static int intersectionBetweenLineSegment2DAndConvexPolygon2D(Point2DReadOnly lineSegmentStart, + Point2DReadOnly lineSegmentEnd, + List convexPolygon2D, + int numberOfVertices, + boolean clockwiseOrdered, + Point2DBasics firstIntersectionToPack, Point2DBasics secondIntersectionToPack) { checkNumberOfVertices(convexPolygon2D, numberOfVertices); @@ -1148,8 +1178,10 @@ else if (lambda > 1.0) * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static Point2D[] intersectionBetweenLineSegment2DAndConvexPolygon2D(Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd, - List convexPolygon2D, int numberOfVertices, + public static Point2D[] intersectionBetweenLineSegment2DAndConvexPolygon2D(Point2DReadOnly lineSegmentStart, + Point2DReadOnly lineSegmentEnd, + List convexPolygon2D, + int numberOfVertices, boolean clockwiseOrdered) { Point2D firstIntersection = new Point2D(); @@ -1205,9 +1237,13 @@ public static Point2D[] intersectionBetweenLineSegment2DAndConvexPolygon2D(Point * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static int intersectionBetweenRay2DAndConvexPolygon2D(Point2DReadOnly rayOrigin, Vector2DReadOnly rayDirection, - List convexPolygon2D, int numberOfVertices, boolean clockwiseOrdered, - Point2DBasics firstIntersectionToPack, Point2DBasics secondIntersectionToPack) + public static int intersectionBetweenRay2DAndConvexPolygon2D(Point2DReadOnly rayOrigin, + Vector2DReadOnly rayDirection, + List convexPolygon2D, + int numberOfVertices, + boolean clockwiseOrdered, + Point2DBasics firstIntersectionToPack, + Point2DBasics secondIntersectionToPack) { int numberOfIntersections = intersectionBetweenLine2DAndConvexPolygon2D(rayOrigin, rayDirection, @@ -1259,8 +1295,10 @@ public static int intersectionBetweenRay2DAndConvexPolygon2D(Point2DReadOnly ray * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static Point2D[] intersectionBetweenRay2DAndConvexPolygon2D(Point2DReadOnly rayOrigin, Vector2DReadOnly rayDirection, - List convexPolygon2D, int numberOfVertices, + public static Point2D[] intersectionBetweenRay2DAndConvexPolygon2D(Point2DReadOnly rayOrigin, + Vector2DReadOnly rayDirection, + List convexPolygon2D, + int numberOfVertices, boolean clockwiseOrdered) { Point2D firstIntersection = new Point2D(); @@ -1314,8 +1352,11 @@ public static Point2D[] intersectionBetweenRay2DAndConvexPolygon2D(Point2DReadOn * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static double signedDistanceFromPoint2DToConvexPolygon2D(double pointX, double pointY, List convexPolygon2D, - int numberOfVertices, boolean clockwiseOrdered) + public static double signedDistanceFromPoint2DToConvexPolygon2D(double pointX, + double pointY, + List convexPolygon2D, + int numberOfVertices, + boolean clockwiseOrdered) { checkNumberOfVertices(convexPolygon2D, numberOfVertices); @@ -1376,7 +1417,9 @@ public static double signedDistanceFromPoint2DToConvexPolygon2D(double pointX, d * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static double signedDistanceFromPoint2DToConvexPolygon2D(Point2DReadOnly point, List convexPolygon2D, int numberOfVertices, + public static double signedDistanceFromPoint2DToConvexPolygon2D(Point2DReadOnly point, + List convexPolygon2D, + int numberOfVertices, boolean clockwiseOrdered) { return signedDistanceFromPoint2DToConvexPolygon2D(point.getX(), point.getY(), convexPolygon2D, numberOfVertices, clockwiseOrdered); @@ -1414,8 +1457,11 @@ public static double signedDistanceFromPoint2DToConvexPolygon2D(Point2DReadOnly * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static boolean closestPointToNonInterectingRay2D(Point2DReadOnly rayOrigin, Vector2DReadOnly rayDirection, - List convexPolygon2D, int numberOfVertices, boolean clockwiseOrdered, + public static boolean closestPointToNonInterectingRay2D(Point2DReadOnly rayOrigin, + Vector2DReadOnly rayDirection, + List convexPolygon2D, + int numberOfVertices, + boolean clockwiseOrdered, Point2DBasics closestPointToPack) { int closestVertexIndexToLine = closestVertexIndexToLine2D(rayOrigin, rayDirection, convexPolygon2D, numberOfVertices); @@ -1497,8 +1543,11 @@ public static boolean closestPointToNonInterectingRay2D(Point2DReadOnly rayOrigi * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static Point2D closestPointToNonInterectingRay2D(Point2DReadOnly rayOrigin, Vector2DReadOnly rayDirection, - List convexPolygon2D, int numberOfVertices, boolean clockwiseOrdered) + public static Point2D closestPointToNonInterectingRay2D(Point2DReadOnly rayOrigin, + Vector2DReadOnly rayDirection, + List convexPolygon2D, + int numberOfVertices, + boolean clockwiseOrdered) { Point2D closestPoint = new Point2D(); boolean success = closestPointToNonInterectingRay2D(rayOrigin, rayDirection, convexPolygon2D, numberOfVertices, clockwiseOrdered, closestPoint); @@ -1528,8 +1577,12 @@ public static Point2D closestPointToNonInterectingRay2D(Point2DReadOnly rayOrigi * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static int closestVertexIndexToLine2D(double pointOnLineX, double pointOnLineY, double lineDirectionX, double lineDirectionY, - List convexPolygon2D, int numberOfVertices) + public static int closestVertexIndexToLine2D(double pointOnLineX, + double pointOnLineY, + double lineDirectionX, + double lineDirectionY, + List convexPolygon2D, + int numberOfVertices) { checkNumberOfVertices(convexPolygon2D, numberOfVertices); @@ -1573,8 +1626,10 @@ public static int closestVertexIndexToLine2D(double pointOnLineX, double pointOn * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static int closestVertexIndexToLine2D(Point2DReadOnly firstPointOnLine, Point2DReadOnly secondPointOnLine, - List convexPolygon2D, int numberOfVertices) + public static int closestVertexIndexToLine2D(Point2DReadOnly firstPointOnLine, + Point2DReadOnly secondPointOnLine, + List convexPolygon2D, + int numberOfVertices) { double pointOnLineX = firstPointOnLine.getX(); double pointOnLineY = firstPointOnLine.getY(); @@ -1604,7 +1659,9 @@ public static int closestVertexIndexToLine2D(Point2DReadOnly firstPointOnLine, P * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static int closestVertexIndexToLine2D(Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection, List convexPolygon2D, + public static int closestVertexIndexToLine2D(Point2DReadOnly pointOnLine, + Vector2DReadOnly lineDirection, + List convexPolygon2D, int numberOfVertices) { return closestVertexIndexToLine2D(pointOnLine.getX(), pointOnLine.getY(), lineDirection.getX(), lineDirection.getY(), convexPolygon2D, numberOfVertices); @@ -1632,8 +1689,11 @@ public static int closestVertexIndexToLine2D(Point2DReadOnly pointOnLine, Vector * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static int closestVertexIndexToRay2D(Point2DReadOnly rayOrigin, Vector2DReadOnly rayDirection, List convexPolygon2D, - int numberOfVertices, boolean clockwiseOrdered) + public static int closestVertexIndexToRay2D(Point2DReadOnly rayOrigin, + Vector2DReadOnly rayDirection, + List convexPolygon2D, + int numberOfVertices, + boolean clockwiseOrdered) { checkNumberOfVertices(convexPolygon2D, numberOfVertices); @@ -1745,7 +1805,10 @@ public static int closestVertexIndexToPoint2D(double pointX, double pointY, List * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static int closestEdgeIndexToPoint2D(double pointX, double pointY, List convexPolygon2D, int numberOfVertices, + public static int closestEdgeIndexToPoint2D(double pointX, + double pointY, + List convexPolygon2D, + int numberOfVertices, boolean clockwiseOrdered) { checkNumberOfVertices(convexPolygon2D, numberOfVertices); @@ -1815,7 +1878,9 @@ public static int closestEdgeIndexToPoint2D(double pointX, double pointY, List convexPolygon2D, int numberOfVertices, + public static int closestEdgeIndexToPoint2D(Point2DReadOnly point, + List convexPolygon2D, + int numberOfVertices, boolean clockwiseOrdered) { return closestEdgeIndexToPoint2D(point.getX(), point.getY(), convexPolygon2D, numberOfVertices, clockwiseOrdered); @@ -1854,7 +1919,10 @@ public static int closestEdgeIndexToPoint2D(Point2DReadOnly point, List convexPolygon2D, int numberOfVertices, + public static int lineOfSightStartIndex(double observerX, + double observerY, + List convexPolygon2D, + int numberOfVertices, boolean clockwiseOrdered) { checkNumberOfVertices(convexPolygon2D, numberOfVertices); @@ -1921,7 +1989,9 @@ public static int lineOfSightStartIndex(double observerX, double observerY, List * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static int lineOfSightStartIndex(Point2DReadOnly observer, List convexPolygon2D, int numberOfVertices, + public static int lineOfSightStartIndex(Point2DReadOnly observer, + List convexPolygon2D, + int numberOfVertices, boolean clockwiseOrdered) { return lineOfSightStartIndex(observer.getX(), observer.getY(), convexPolygon2D, numberOfVertices, clockwiseOrdered); @@ -1960,7 +2030,10 @@ public static int lineOfSightStartIndex(Point2DReadOnly observer, List convexPolygon2D, int numberOfVertices, + public static int lineOfSightEndIndex(double observerX, + double observerY, + List convexPolygon2D, + int numberOfVertices, boolean clockwiseOrdered) { checkNumberOfVertices(convexPolygon2D, numberOfVertices); @@ -2027,7 +2100,9 @@ public static int lineOfSightEndIndex(double observerX, double observerY, List convexPolygon2D, int numberOfVertices, + public static int lineOfSightEndIndex(Point2DReadOnly observer, + List convexPolygon2D, + int numberOfVertices, boolean clockwiseOrdered) { return lineOfSightEndIndex(observer.getX(), observer.getY(), convexPolygon2D, numberOfVertices, clockwiseOrdered); @@ -2068,7 +2143,9 @@ public static int lineOfSightEndIndex(Point2DReadOnly observer, List convexPolygon2D, int numberOfVertices, + public static int[] lineOfSightIndices(Point2DReadOnly observer, + List convexPolygon2D, + int numberOfVertices, boolean clockwiseOrdered) { int lineOfSightStartIndex = lineOfSightStartIndex(observer, convexPolygon2D, numberOfVertices, clockwiseOrdered); @@ -2118,8 +2195,11 @@ public static int[] lineOfSightIndices(Point2DReadOnly observer, List convexPolygon2D, int numberOfVertices) + public static int nextEdgeIndexIntersectingWithLine2D(int previousEdgeIndex, + Point2DReadOnly pointOnLine, + Vector2DReadOnly lineDirection, + List convexPolygon2D, + int numberOfVertices) { return nextEdgeIndexIntersectingWithLine2D(previousEdgeIndex, pointOnLine.getX(), @@ -2171,8 +2251,13 @@ public static int nextEdgeIndexIntersectingWithLine2D(int previousEdgeIndex, Poi * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the * size of the given list of vertices. */ - public static int nextEdgeIndexIntersectingWithLine2D(int previousEdgeIndex, double pointOnLineX, double pointOnLineY, double lineDirectionX, - double lineDirectionY, List convexPolygon2D, int numberOfVertices) + public static int nextEdgeIndexIntersectingWithLine2D(int previousEdgeIndex, + double pointOnLineX, + double pointOnLineY, + double lineDirectionX, + double lineDirectionY, + List convexPolygon2D, + int numberOfVertices) { checkNumberOfVertices(convexPolygon2D, numberOfVertices); if (previousEdgeIndex < -2 || previousEdgeIndex >= numberOfVertices) @@ -2236,8 +2321,12 @@ public static int nextEdgeIndexIntersectingWithLine2D(int previousEdgeIndex, dou * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static boolean orthogonalProjectionOnConvexPolygon2D(double pointToProjectX, double pointToProjectY, List convexPolygon2D, - int numberOfVertices, boolean clockwiseOrdered, Point2DBasics projectionToPack) + public static boolean orthogonalProjectionOnConvexPolygon2D(double pointToProjectX, + double pointToProjectY, + List convexPolygon2D, + int numberOfVertices, + boolean clockwiseOrdered, + Point2DBasics projectionToPack) { checkNumberOfVertices(convexPolygon2D, numberOfVertices); @@ -2295,8 +2384,11 @@ public static boolean orthogonalProjectionOnConvexPolygon2D(double pointToProjec * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static boolean orthogonalProjectionOnConvexPolygon2D(Point2DReadOnly pointToProject, List convexPolygon2D, - int numberOfVertices, boolean clockwiseOrdered, Point2DBasics projectionToPack) + public static boolean orthogonalProjectionOnConvexPolygon2D(Point2DReadOnly pointToProject, + List convexPolygon2D, + int numberOfVertices, + boolean clockwiseOrdered, + Point2DBasics projectionToPack) { return orthogonalProjectionOnConvexPolygon2D(pointToProject.getX(), pointToProject.getY(), @@ -2332,8 +2424,10 @@ public static boolean orthogonalProjectionOnConvexPolygon2D(Point2DReadOnly poin * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the size * of the given list of vertices. */ - public static Point2D orthogonalProjectionOnConvexPolygon2D(Point2DReadOnly pointToProject, List convexPolygon2D, - int numberOfVertices, boolean clockwiseOrdered) + public static Point2D orthogonalProjectionOnConvexPolygon2D(Point2DReadOnly pointToProject, + List convexPolygon2D, + int numberOfVertices, + boolean clockwiseOrdered) { Point2D projection = new Point2D(); boolean success = orthogonalProjectionOnConvexPolygon2D(pointToProject.getX(), @@ -2368,7 +2462,10 @@ public static Point2D orthogonalProjectionOnConvexPolygon2D(Point2DReadOnly poin * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the * size of the given list of vertices. */ - public static boolean canObserverSeeEdge(int edgeIndex, Point2DReadOnly observer, List convexPolygon2D, int numberOfVertices, + public static boolean canObserverSeeEdge(int edgeIndex, + Point2DReadOnly observer, + List convexPolygon2D, + int numberOfVertices, boolean clockwiseOrdered) { return canObserverSeeEdge(edgeIndex, observer.getX(), observer.getY(), convexPolygon2D, numberOfVertices, clockwiseOrdered); @@ -2398,8 +2495,12 @@ public static boolean canObserverSeeEdge(int edgeIndex, Point2DReadOnly observer * @throws IllegalArgumentException if {@code numberOfVertices} is negative or greater than the * size of the given list of vertices. */ - public static boolean canObserverSeeEdge(int edgeIndex, double observerX, double observerY, List convexPolygon2D, - int numberOfVertices, boolean clockwiseOrdered) + public static boolean canObserverSeeEdge(int edgeIndex, + double observerX, + double observerY, + List convexPolygon2D, + int numberOfVertices, + boolean clockwiseOrdered) { checkNumberOfVertices(convexPolygon2D, numberOfVertices); checkEdgeIndex(edgeIndex, numberOfVertices); @@ -2551,7 +2652,10 @@ private static void grahamScanAngleSortRecursive(List * pivotIndex * */ - private static int grahamScanAnglePartition(List list, Point2DReadOnly minXMaxYVertex, int startIndex, int endIndex, + private static int grahamScanAnglePartition(List list, + Point2DReadOnly minXMaxYVertex, + int startIndex, + int endIndex, int pivotIndex) { Point2DReadOnly pivot = list.get(pivotIndex); diff --git a/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryTestTools.java b/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryTestTools.java index 9b1f36aff..42681e319 100644 --- a/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryTestTools.java +++ b/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryTestTools.java @@ -474,7 +474,10 @@ public static void assertLineSegment2DEquals(String messagePrefix, LineSegment2D * @throws AssertionError if the two line segment 2Ds are not equal. If only one of the arguments is * equal to {@code null}. */ - public static void assertLineSegment2DEquals(String messagePrefix, LineSegment2DReadOnly expected, LineSegment2DReadOnly actual, double epsilon, + public static void assertLineSegment2DEquals(String messagePrefix, + LineSegment2DReadOnly expected, + LineSegment2DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -539,7 +542,10 @@ public static void assertLineSegment2DGeometricallyEquals(String messagePrefix, * @throws AssertionError if the two line segment 2Ds do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ - public static void assertLineSegment2DGeometricallyEquals(String messagePrefix, LineSegment2DReadOnly expected, LineSegment2DReadOnly actual, double epsilon, + public static void assertLineSegment2DGeometricallyEquals(String messagePrefix, + LineSegment2DReadOnly expected, + LineSegment2DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -602,7 +608,10 @@ public static void assertLineSegment3DEquals(String messagePrefix, LineSegment3D * @throws AssertionError if the two line segment 3Ds are not equal. If only one of the arguments is * equal to {@code null}. */ - public static void assertLineSegment3DEquals(String messagePrefix, LineSegment3DReadOnly expected, LineSegment3DReadOnly actual, double epsilon, + public static void assertLineSegment3DEquals(String messagePrefix, + LineSegment3DReadOnly expected, + LineSegment3DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -667,7 +676,10 @@ public static void assertLineSegment3DGeometricallyEquals(String messagePrefix, * @throws AssertionError if the two line segment 3Ds do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ - public static void assertLineSegment3DGeometricallyEquals(String messagePrefix, LineSegment3DReadOnly expected, LineSegment3DReadOnly actual, double epsilon, + public static void assertLineSegment3DGeometricallyEquals(String messagePrefix, + LineSegment3DReadOnly expected, + LineSegment3DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -730,7 +742,10 @@ public static void assertBoundingBox2DEquals(String messagePrefix, BoundingBox2D * @throws AssertionError if the two bounding box 2Ds are not equal. If only one of the arguments is * equal to {@code null}. */ - public static void assertBoundingBox2DEquals(String messagePrefix, BoundingBox2DReadOnly expected, BoundingBox2DReadOnly actual, double epsilon, + public static void assertBoundingBox2DEquals(String messagePrefix, + BoundingBox2DReadOnly expected, + BoundingBox2DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -798,7 +813,10 @@ public static void assertBoundingBox2DGeometricallyEquals(String messagePrefix, * @throws AssertionError if the two bounding box 2Ds do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ - public static void assertBoundingBox2DGeometricallyEquals(String messagePrefix, BoundingBox2DReadOnly expected, BoundingBox2DReadOnly actual, double epsilon, + public static void assertBoundingBox2DGeometricallyEquals(String messagePrefix, + BoundingBox2DReadOnly expected, + BoundingBox2DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -863,7 +881,10 @@ public static void assertBoundingBox3DEquals(String messagePrefix, BoundingBox3D * @throws AssertionError if the two bounding box 3Ds are not equal. If only one of the arguments is * equal to {@code null}. */ - public static void assertBoundingBox3DEquals(String messagePrefix, BoundingBox3DReadOnly expected, BoundingBox3DReadOnly actual, double epsilon, + public static void assertBoundingBox3DEquals(String messagePrefix, + BoundingBox3DReadOnly expected, + BoundingBox3DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -931,7 +952,10 @@ public static void assertBoundingBox3DGeometricallyEquals(String messagePrefix, * @throws AssertionError if the two bounding box 3Ds do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ - public static void assertBoundingBox3DGeometricallyEquals(String messagePrefix, BoundingBox3DReadOnly expected, BoundingBox3DReadOnly actual, double epsilon, + public static void assertBoundingBox3DGeometricallyEquals(String messagePrefix, + BoundingBox3DReadOnly expected, + BoundingBox3DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -1008,7 +1032,10 @@ public static void assertOrientation2DEquals(String messagePrefix, Orientation2D * instead */ @Deprecated - public static void assertOrientation2DEquals(String messagePrefix, Orientation2DReadOnly expected, Orientation2DReadOnly actual, double epsilon, + public static void assertOrientation2DEquals(String messagePrefix, + Orientation2DReadOnly expected, + Orientation2DReadOnly actual, + double epsilon, String format) { EuclidCoreTestTools.assertOrientation2DEquals(messagePrefix, expected, actual, epsilon, format); @@ -1076,7 +1103,10 @@ public static void assertOrientation2DGeometricallyEquals(String messagePrefix, * instead */ @Deprecated - public static void assertOrientation2DGeometricallyEquals(String messagePrefix, Orientation2DReadOnly expected, Orientation2DReadOnly actual, double epsilon, + public static void assertOrientation2DGeometricallyEquals(String messagePrefix, + Orientation2DReadOnly expected, + Orientation2DReadOnly actual, + double epsilon, String format) { EuclidCoreTestTools.assertOrientation2DGeometricallyEquals(messagePrefix, expected, actual, epsilon, format); @@ -1516,7 +1546,10 @@ public static void assertConvexPolygon2DEquals(String messagePrefix, ConvexPolyg * @throws AssertionError if the two convex polygon 2D are not equal. If only one of the arguments * is equal to {@code null}. */ - public static void assertConvexPolygon2DEquals(String messagePrefix, ConvexPolygon2DReadOnly expected, ConvexPolygon2DReadOnly actual, double epsilon, + public static void assertConvexPolygon2DEquals(String messagePrefix, + ConvexPolygon2DReadOnly expected, + ConvexPolygon2DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -1561,7 +1594,9 @@ public static void assertConvexPolygon2DGeometricallyEquals(ConvexPolygon2DReadO * @throws AssertionError if the two convex polygon 2D do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ - public static void assertConvexPolygon2DGeometricallyEquals(String messagePrefix, ConvexPolygon2DReadOnly expected, ConvexPolygon2DReadOnly actual, + public static void assertConvexPolygon2DGeometricallyEquals(String messagePrefix, + ConvexPolygon2DReadOnly expected, + ConvexPolygon2DReadOnly actual, double epsilon) { assertConvexPolygon2DGeometricallyEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); @@ -1582,8 +1617,11 @@ public static void assertConvexPolygon2DGeometricallyEquals(String messagePrefix * @throws AssertionError if the two convex polygon 2D do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ - public static void assertConvexPolygon2DGeometricallyEquals(String messagePrefix, ConvexPolygon2DReadOnly expected, ConvexPolygon2DReadOnly actual, - double epsilon, String format) + public static void assertConvexPolygon2DGeometricallyEquals(String messagePrefix, + ConvexPolygon2DReadOnly expected, + ConvexPolygon2DReadOnly actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -1711,7 +1749,10 @@ public static void assertTriangle3DGeometricallyEquals(String messagePrefix, Tri * @throws AssertionError if the two triangle 3D do not represent the same geometry. If only one of * the arguments is equal to {@code null}. */ - public static void assertTriangle3DGeometricallyEquals(String messagePrefix, Triangle3DReadOnly expected, Triangle3DReadOnly actual, double epsilon, + public static void assertTriangle3DGeometricallyEquals(String messagePrefix, + Triangle3DReadOnly expected, + Triangle3DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) 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 304097c91..695cc9f9b 100644 --- a/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryTools.java +++ b/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryTools.java @@ -112,14 +112,24 @@ public static double angleFromXForwardToVector2D(double vectorX, double vectorY) * @param secondVectorZ z-component of second the vector. * @return the angle in radians from the first vector to the second vector. */ - public static double angleFromFirstToSecondVector3D(double firstVectorX, double firstVectorY, double firstVectorZ, double secondVectorX, - double secondVectorY, double secondVectorZ) + public static double angleFromFirstToSecondVector3D(double firstVectorX, + double firstVectorY, + double firstVectorZ, + double secondVectorX, + double secondVectorY, + double secondVectorZ) { return angleFromFirstToSecondVector3D(firstVectorX, firstVectorY, firstVectorZ, false, secondVectorX, secondVectorY, secondVectorZ, false); } - private static double angleFromFirstToSecondVector3D(double firstVectorX, double firstVectorY, double firstVectorZ, boolean isFirstVectorUnitary, - double secondVectorX, double secondVectorY, double secondVectorZ, boolean isSecondVectorUnitary) + 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); @@ -161,8 +171,16 @@ else if (dotProduct < -1.0) * @return {@code true} if the two line segments are collinear, {@code false} otherwise. * @throws IllegalArgumentException if angleEpsilon ∉ [0; pi/2] */ - public static boolean areLine2DsCollinear(double pointOnLine1x, double pointOnLine1y, double lineDirection1x, double lineDirection1y, double pointOnLine2x, - double pointOnLine2y, double lineDirection2x, double lineDirection2y, double angleEpsilon, double distanceEpsilon) + public static boolean areLine2DsCollinear(double pointOnLine1x, + double pointOnLine1y, + double lineDirection1x, + double lineDirection1y, + double pointOnLine2x, + double pointOnLine2y, + double lineDirection2x, + double lineDirection2y, + double angleEpsilon, + double distanceEpsilon) { return areLine2DsCollinear(pointOnLine1x, pointOnLine1y, @@ -178,9 +196,18 @@ public static boolean areLine2DsCollinear(double pointOnLine1x, double pointOnLi distanceEpsilon); } - private static boolean areLine2DsCollinear(double pointOnLine1x, double pointOnLine1y, double lineDirection1x, double lineDirection1y, - boolean isDirection1Unitary, double pointOnLine2x, double pointOnLine2y, double lineDirection2x, - double lineDirection2y, boolean isDirection2Unitary, double angleEpsilon, double distanceEpsilon) + private static boolean areLine2DsCollinear(double pointOnLine1x, + double pointOnLine1y, + double lineDirection1x, + double lineDirection1y, + boolean isDirection1Unitary, + double pointOnLine2x, + double pointOnLine2y, + double lineDirection2x, + double lineDirection2y, + boolean isDirection2Unitary, + double angleEpsilon, + double distanceEpsilon) { if (!areVector2DsParallel(lineDirection1x, lineDirection1y, isDirection1Unitary, lineDirection2x, lineDirection2y, isDirection2Unitary, angleEpsilon)) return false; @@ -217,8 +244,12 @@ private static boolean areLine2DsCollinear(double pointOnLine1x, double pointOnL * @return {@code true} if the two line segments are collinear, {@code false} otherwise. * @throws IllegalArgumentException if angleEpsilon ∉ [0; pi/2] */ - public static boolean areLine2DsCollinear(Point2DReadOnly firstPointOnLine1, Point2DReadOnly secondPointOnLine1, Point2DReadOnly firstPointOnLine2, - Point2DReadOnly secondPointOnLine2, double angleEpsilon, double distanceEpsilon) + public static boolean areLine2DsCollinear(Point2DReadOnly firstPointOnLine1, + Point2DReadOnly secondPointOnLine1, + Point2DReadOnly firstPointOnLine2, + Point2DReadOnly secondPointOnLine2, + double angleEpsilon, + double distanceEpsilon) { double pointOnLine1x = firstPointOnLine1.getX(); double pointOnLine1y = firstPointOnLine1.getY(); @@ -262,8 +293,12 @@ public static boolean areLine2DsCollinear(Point2DReadOnly firstPointOnLine1, Poi * @return {@code true} if the two line segments are collinear, {@code false} otherwise. * @throws IllegalArgumentException if angleEpsilon ∉ [0; pi/2] */ - public static boolean areLine2DsCollinear(Point2DReadOnly pointOnLine1, Vector2DReadOnly lineDirection1, Point2DReadOnly firstPointOnLine2, - Point2DReadOnly secondPointOnLine2, double angleEpsilon, double distanceEpsilon) + public static boolean areLine2DsCollinear(Point2DReadOnly pointOnLine1, + Vector2DReadOnly lineDirection1, + Point2DReadOnly firstPointOnLine2, + Point2DReadOnly secondPointOnLine2, + double angleEpsilon, + double distanceEpsilon) { double pointOnLine2x = firstPointOnLine2.getX(); double pointOnLine2y = firstPointOnLine2.getY(); @@ -305,8 +340,12 @@ public static boolean areLine2DsCollinear(Point2DReadOnly pointOnLine1, Vector2D * @return {@code true} if the two line segments are collinear, {@code false} otherwise. * @throws IllegalArgumentException if angleEpsilon ∉ [0; pi/2] */ - public static boolean areLine2DsCollinear(Point2DReadOnly pointOnLine1, Vector2DReadOnly lineDirection1, Point2DReadOnly pointOnLine2, - Vector2DReadOnly lineDirection2, double angleEpsilon, double distanceEpsilon) + public static boolean areLine2DsCollinear(Point2DReadOnly pointOnLine1, + Vector2DReadOnly lineDirection1, + Point2DReadOnly pointOnLine2, + Vector2DReadOnly lineDirection2, + double angleEpsilon, + double distanceEpsilon) { return areLine2DsCollinear(pointOnLine1.getX(), pointOnLine1.getY(), @@ -350,9 +389,20 @@ public static boolean areLine2DsCollinear(Point2DReadOnly pointOnLine1, Vector2D * @return {@code true} if the two line segments are collinear, {@code false} otherwise. * @throws IllegalArgumentException if angleEpsilon ∉ [0; pi/2] */ - public static boolean areLine3DsCollinear(double pointOnLine1x, double pointOnLine1y, double pointOnLine1z, double lineDirection1x, double lineDirection1y, - double lineDirection1z, double pointOnLine2x, double pointOnLine2y, double pointOnLine2z, double lineDirection2x, - double lineDirection2y, double lineDirection2z, double angleEpsilon, double distanceEpsilon) + public static boolean areLine3DsCollinear(double pointOnLine1x, + double pointOnLine1y, + double pointOnLine1z, + double lineDirection1x, + double lineDirection1y, + double lineDirection1z, + double pointOnLine2x, + double pointOnLine2y, + double pointOnLine2z, + double lineDirection2x, + double lineDirection2y, + double lineDirection2z, + double angleEpsilon, + double distanceEpsilon) { return areLine3DsCollinear(pointOnLine1x, pointOnLine1y, @@ -372,10 +422,22 @@ public static boolean areLine3DsCollinear(double pointOnLine1x, double pointOnLi distanceEpsilon); } - private static boolean areLine3DsCollinear(double pointOnLine1x, double pointOnLine1y, double pointOnLine1z, double lineDirection1x, double lineDirection1y, - double lineDirection1z, boolean isDirection1Unitary, double pointOnLine2x, double pointOnLine2y, - double pointOnLine2z, double lineDirection2x, double lineDirection2y, double lineDirection2z, - boolean isDirection2Unitary, double angleEpsilon, double distanceEpsilon) + private static boolean areLine3DsCollinear(double pointOnLine1x, + double pointOnLine1y, + double pointOnLine1z, + double lineDirection1x, + double lineDirection1y, + double lineDirection1z, + boolean isDirection1Unitary, + double pointOnLine2x, + double pointOnLine2y, + double pointOnLine2z, + double lineDirection2x, + double lineDirection2y, + double lineDirection2z, + boolean isDirection2Unitary, + double angleEpsilon, + double distanceEpsilon) { if (!areVector3DsParallel(lineDirection1x, lineDirection1y, @@ -423,8 +485,12 @@ private static boolean areLine3DsCollinear(double pointOnLine1x, double pointOnL * @return {@code true} if the two line segments are collinear, {@code false} otherwise. * @throws IllegalArgumentException if angleEpsilon ∉ [0; pi/2] */ - public static boolean areLine3DsCollinear(Point3DReadOnly firstPointOnLine1, Point3DReadOnly secondPointOnLine1, Point3DReadOnly firstPointOnLine2, - Point3DReadOnly secondPointOnLine2, double angleEpsilon, double distanceEpsilon) + public static boolean areLine3DsCollinear(Point3DReadOnly firstPointOnLine1, + Point3DReadOnly secondPointOnLine1, + Point3DReadOnly firstPointOnLine2, + Point3DReadOnly secondPointOnLine2, + double angleEpsilon, + double distanceEpsilon) { double pointOnLine1x = firstPointOnLine1.getX(); double pointOnLine1y = firstPointOnLine1.getY(); @@ -475,8 +541,12 @@ public static boolean areLine3DsCollinear(Point3DReadOnly firstPointOnLine1, Poi * the first line segment. * @return {@code true} if the two line segments are collinear, {@code false} otherwise. */ - public static boolean areLine3DsCollinear(Point3DReadOnly pointOnLine1, Vector3DReadOnly lineDirection1, Point3DReadOnly pointOnLine2, - Vector3DReadOnly lineDirection2, double angleEpsilon, double distanceEpsilon) + public static boolean areLine3DsCollinear(Point3DReadOnly pointOnLine1, + Vector3DReadOnly lineDirection1, + Point3DReadOnly pointOnLine2, + Vector3DReadOnly lineDirection2, + double angleEpsilon, + double distanceEpsilon) { return areLine3DsCollinear(pointOnLine1.getX(), pointOnLine1.getY(), @@ -523,8 +593,12 @@ public static boolean areLine3DsCollinear(Point3DReadOnly pointOnLine1, Vector3D * @return {@code true} if the two planes are coincident, {@code false} otherwise. * @throws IllegalArgumentException if angleEpsilon ∉ [0; pi/2] */ - public static boolean arePlane3DsCoincident(Point3DReadOnly pointOnPlane1, Vector3DReadOnly planeNormal1, Point3DReadOnly pointOnPlane2, - Vector3DReadOnly planeNormal2, double angleEpsilon, double distanceEpsilon) + public static boolean arePlane3DsCoincident(Point3DReadOnly pointOnPlane1, + Vector3DReadOnly planeNormal1, + Point3DReadOnly pointOnPlane2, + Vector3DReadOnly planeNormal2, + double angleEpsilon, + double distanceEpsilon) { if (!areVector3DsParallel(planeNormal1, planeNormal2, angleEpsilon)) return false; @@ -557,8 +631,13 @@ public static boolean areVector2DsParallel(double firstVectorX, double firstVect return areVector2DsParallel(firstVectorX, firstVectorY, false, secondVectorX, secondVectorY, false, angleEpsilon); } - private static boolean areVector2DsParallel(double firstVectorX, double firstVectorY, boolean isFirstVectorUnitary, double secondVectorX, - double secondVectorY, boolean isSecondVectorUnitary, double angleEpsilon) + private static boolean areVector2DsParallel(double firstVectorX, + double firstVectorY, + boolean isFirstVectorUnitary, + double secondVectorX, + double secondVectorY, + boolean isSecondVectorUnitary, + double angleEpsilon) { if (angleEpsilon < 0.0 || angleEpsilon > HALF_PI) throw new IllegalArgumentException("The angle epsilon has to be inside the interval: [0.0 ; Math.PI / 2.0]"); @@ -624,14 +703,25 @@ public static boolean areVector2DsParallel(Vector2DReadOnly firstVector, Vector2 * @return {@code true} if the two vectors are parallel, {@code false} otherwise. * @throws IllegalArgumentException if angleEpsilon ∉ [0; pi/2] */ - public static boolean areVector3DsParallel(double firstVectorX, double firstVectorY, double firstVectorZ, double secondVectorX, double secondVectorY, - double secondVectorZ, double angleEpsilon) + public static boolean areVector3DsParallel(double firstVectorX, + double firstVectorY, + double firstVectorZ, + double secondVectorX, + double secondVectorY, + double secondVectorZ, + double angleEpsilon) { return areVector3DsParallel(firstVectorX, firstVectorY, firstVectorZ, false, secondVectorX, secondVectorY, secondVectorZ, false, angleEpsilon); } - private static boolean areVector3DsParallel(double firstVectorX, double firstVectorY, double firstVectorZ, boolean isFirstVectorUnitary, - double secondVectorX, double secondVectorY, double secondVectorZ, boolean isSecondVectorUnitary, + private static boolean areVector3DsParallel(double firstVectorX, + double firstVectorY, + double firstVectorZ, + boolean isFirstVectorUnitary, + double secondVectorX, + double secondVectorY, + double secondVectorZ, + boolean isSecondVectorUnitary, double angleEpsilon) { if (angleEpsilon < 0.0 || angleEpsilon > HALF_PI) @@ -785,8 +875,13 @@ public static Point3D averagePoint3Ds(Point3DReadOnly a, Point3DReadOnly b) * @param rotationToPack the minimum rotation from {@code firstVector} to the {@code secondVector}. * Modified. */ - public static void orientation3DFromFirstToSecondVector3D(double firstVectorX, double firstVectorY, double firstVectorZ, double secondVectorX, - double secondVectorY, double secondVectorZ, Orientation3DBasics rotationToPack) + public static void orientation3DFromFirstToSecondVector3D(double firstVectorX, + double firstVectorY, + double firstVectorZ, + double secondVectorX, + double secondVectorY, + double secondVectorZ, + Orientation3DBasics rotationToPack) { orientation3DFromFirstToSecondVector3D(firstVectorX, firstVectorY, @@ -799,8 +894,14 @@ public static void orientation3DFromFirstToSecondVector3D(double firstVectorX, d rotationToPack); } - private static void orientation3DFromFirstToSecondVector3D(double firstVectorX, double firstVectorY, double firstVectorZ, boolean isFirstVectorUnitary, - double secondVectorX, double secondVectorY, double secondVectorZ, boolean isSecondVectorUnitary, + private static void orientation3DFromFirstToSecondVector3D(double firstVectorX, + double firstVectorY, + double firstVectorZ, + boolean isFirstVectorUnitary, + double secondVectorX, + double secondVectorY, + double secondVectorZ, + boolean isSecondVectorUnitary, Orientation3DBasics rotationToPack) { if (!isFirstVectorUnitary) @@ -1019,8 +1120,11 @@ public static void orientation3DFromZUpToVector3D(Vector3DReadOnly vector, Orien * Modified. Can be {@code null}. * @return the minimum distance between the two lines. */ - public static double closestPoint3DsBetweenTwoLine3Ds(Point3DReadOnly pointOnLine1, Vector3DReadOnly lineDirection1, Point3DReadOnly pointOnLine2, - Vector3DReadOnly lineDirection2, Point3DBasics closestPointOnLine1ToPack, + public static double closestPoint3DsBetweenTwoLine3Ds(Point3DReadOnly pointOnLine1, + Vector3DReadOnly lineDirection1, + Point3DReadOnly pointOnLine2, + Vector3DReadOnly lineDirection2, + Point3DBasics closestPointOnLine1ToPack, Point3DBasics closestPointOnLine2ToPack) { // Switching to the notation used in http://geomalgorithms.com/a07-_distance.html. @@ -1105,9 +1209,12 @@ public static double closestPoint3DsBetweenTwoLine3Ds(Point3DReadOnly pointOnLin * 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) + public static double closestPoint2DsBetweenTwoLineSegment2Ds(Point2DReadOnly lineSegmentStart1, + Point2DReadOnly lineSegmentEnd1, + Point2DReadOnly lineSegmentStart2, + Point2DReadOnly lineSegmentEnd2, + Point2DBasics closestPointOnLineSegment1ToPack, + Point2DBasics closestPointOnLineSegment2ToPack) { return closestPoint2DsBetweenTwoLineSegment2Ds(lineSegmentStart1.getX(), lineSegmentStart1.getY(), @@ -1149,10 +1256,16 @@ public static double closestPoint2DsBetweenTwoLineSegment2Ds(Point2DReadOnly lin * point. Modified. Can be {@code null}. * @return the minimum distance between the two line segments. */ - public static double closestPoint2DsBetweenTwoLineSegment2Ds(double lineSegmentStart1X, double lineSegmentStart1Y, double lineSegmentEnd1X, - double lineSegmentEnd1Y, double lineSegmentStart2X, double lineSegmentStart2Y, - double lineSegmentEnd2X, double lineSegmentEnd2Y, - Point2DBasics closestPointOnLineSegment1ToPack, Point2DBasics closestPointOnLineSegment2ToPack) + public static double closestPoint2DsBetweenTwoLineSegment2Ds(double lineSegmentStart1X, + double lineSegmentStart1Y, + double lineSegmentEnd1X, + double lineSegmentEnd1Y, + double lineSegmentStart2X, + double lineSegmentStart2Y, + double lineSegmentEnd2X, + double lineSegmentEnd2Y, + 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). @@ -1277,9 +1390,12 @@ else if (sNumerator > a) * point. Modified. Can be {@code null}. * @return the minimum distance between the two line segments. */ - public static double closestPoint3DsBetweenTwoLineSegment3Ds(Point3DReadOnly lineSegmentStart1, Point3DReadOnly lineSegmentEnd1, - Point3DReadOnly lineSegmentStart2, Point3DReadOnly lineSegmentEnd2, - Point3DBasics closestPointOnLineSegment1ToPack, Point3DBasics closestPointOnLineSegment2ToPack) + public static double closestPoint3DsBetweenTwoLineSegment3Ds(Point3DReadOnly lineSegmentStart1, + Point3DReadOnly lineSegmentEnd1, + Point3DReadOnly lineSegmentStart2, + Point3DReadOnly lineSegmentEnd2, + Point3DBasics closestPointOnLineSegment1ToPack, + Point3DBasics closestPointOnLineSegment2ToPack) { return closestPoint3DsBetweenTwoLineSegment3Ds(lineSegmentStart1.getX(), lineSegmentStart1.getY(), @@ -1333,11 +1449,20 @@ public static double closestPoint3DsBetweenTwoLineSegment3Ds(Point3DReadOnly lin * point. Modified. Can be {@code null}. * @return the minimum distance between the two line segments. */ - public static double closestPoint3DsBetweenTwoLineSegment3Ds(double lineSegmentStart1X, double lineSegmentStart1Y, double lineSegmentStart1Z, - double lineSegmentEnd1X, double lineSegmentEnd1Y, double lineSegmentEnd1Z, - double lineSegmentStart2X, double lineSegmentStart2Y, double lineSegmentStart2Z, - double lineSegmentEnd2X, double lineSegmentEnd2Y, double lineSegmentEnd2Z, - Point3DBasics closestPointOnLineSegment1ToPack, Point3DBasics closestPointOnLineSegment2ToPack) + public static double closestPoint3DsBetweenTwoLineSegment3Ds(double lineSegmentStart1X, + double lineSegmentStart1Y, + double lineSegmentStart1Z, + double lineSegmentEnd1X, + double lineSegmentEnd1Y, + double lineSegmentEnd1Z, + double lineSegmentStart2X, + double lineSegmentStart2Y, + double lineSegmentStart2Z, + double lineSegmentEnd2X, + double lineSegmentEnd2Y, + double lineSegmentEnd2Z, + Point3DBasics closestPointOnLineSegment1ToPack, + Point3DBasics 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). @@ -1704,7 +1829,11 @@ public static double distanceSquaredBetweenPoint2Ds(double firstPointX, double f * @param secondPointZ the z-coordinate of the second point. * @return the distance between the two points. */ - public static double distanceBetweenPoint3Ds(double firstPointX, double firstPointY, double firstPointZ, double secondPointX, double secondPointY, + public static double distanceBetweenPoint3Ds(double firstPointX, + double firstPointY, + double firstPointZ, + double secondPointX, + double secondPointY, double secondPointZ) { return EuclidCoreTools.squareRoot(distanceSquaredBetweenPoint3Ds(firstPointX, firstPointY, firstPointZ, secondPointX, secondPointY, secondPointZ)); @@ -1735,7 +1864,11 @@ public static double distanceBetweenPoint3Ds(double firstPointX, double firstPoi * @param secondPointZ the z-coordinate of the second point. * @return the distance squared between the two points. */ - public static double distanceSquaredBetweenPoint3Ds(double firstPointX, double firstPointY, double firstPointZ, double secondPointX, double secondPointY, + public static double distanceSquaredBetweenPoint3Ds(double firstPointX, + double firstPointY, + double firstPointZ, + double secondPointX, + double secondPointY, double secondPointZ) { double deltaX = secondPointX - firstPointX; @@ -1771,7 +1904,9 @@ public static double distanceSquaredBetweenPoint3Ds(double firstPointX, double f * @param lineDirection2 the 3D direction of the second line. Not modified. * @return the minimum distance between the two lines. */ - public static double distanceBetweenTwoLine3Ds(Point3DReadOnly pointOnLine1, Vector3DReadOnly lineDirection1, Point3DReadOnly pointOnLine2, + public static double distanceBetweenTwoLine3Ds(Point3DReadOnly pointOnLine1, + Vector3DReadOnly lineDirection1, + Point3DReadOnly pointOnLine2, Vector3DReadOnly lineDirection2) { return closestPoint3DsBetweenTwoLine3Ds(pointOnLine1, lineDirection1, pointOnLine2, lineDirection2, null, null); @@ -1787,7 +1922,9 @@ public static double distanceBetweenTwoLine3Ds(Point3DReadOnly pointOnLine1, Vec * @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, + public static double distanceBetweenTwoLineSegment2Ds(Point2DReadOnly lineSegmentStart1, + Point2DReadOnly lineSegmentEnd1, + Point2DReadOnly lineSegmentStart2, Point2DReadOnly lineSegmentEnd2) { return closestPoint2DsBetweenTwoLineSegment2Ds(lineSegmentStart1, lineSegmentEnd1, lineSegmentStart2, lineSegmentEnd2, null, null); @@ -1803,7 +1940,9 @@ public static double distanceBetweenTwoLineSegment2Ds(Point2DReadOnly lineSegmen * @param lineSegmentEnd2 the second endpoint of the second line segment. Not modified. * @return the minimum distance between the two line segments. */ - public static double distanceBetweenTwoLineSegment3Ds(Point3DReadOnly lineSegmentStart1, Point3DReadOnly lineSegmentEnd1, Point3DReadOnly lineSegmentStart2, + public static double distanceBetweenTwoLineSegment3Ds(Point3DReadOnly lineSegmentStart1, + Point3DReadOnly lineSegmentEnd1, + Point3DReadOnly lineSegmentStart2, Point3DReadOnly lineSegmentEnd2) { return closestPoint3DsBetweenTwoLineSegment3Ds(lineSegmentStart1, lineSegmentEnd1, lineSegmentStart2, lineSegmentEnd2, null, null); @@ -1828,14 +1967,23 @@ public static double distanceBetweenTwoLineSegment3Ds(Point3DReadOnly lineSegmen * @param lineDirectionY y-component of the line direction. * @return the minimum distance between the 2D point and the 2D line. */ - public static double distanceFromPoint2DToLine2D(double pointX, double pointY, double pointOnLineX, double pointOnLineY, double lineDirectionX, + public static double distanceFromPoint2DToLine2D(double pointX, + double pointY, + double pointOnLineX, + double pointOnLineY, + double lineDirectionX, double lineDirectionY) { return distanceFromPoint2DToLine2D(pointX, pointY, pointOnLineX, pointOnLineY, lineDirectionX, lineDirectionY, false); } - private static double distanceFromPoint2DToLine2D(double pointX, double pointY, double pointOnLineX, double pointOnLineY, double lineDirectionX, - double lineDirectionY, boolean isDirectionUnitary) + private static double distanceFromPoint2DToLine2D(double pointX, + double pointY, + double pointOnLineX, + double pointOnLineY, + double lineDirectionX, + double lineDirectionY, + boolean isDirectionUnitary) { return Math.abs(signedDistanceFromPoint2DToLine2D(pointX, pointY, pointOnLineX, pointOnLineY, lineDirectionX, lineDirectionY, isDirectionUnitary)); } @@ -1954,8 +2102,12 @@ public static double distanceFromPoint2DToLine2D(Point2DReadOnly point, Point2DR * @param lineSegmentEndY the y-coordinate of the line segment second endpoint. * @return the minimum distance between the 2D point and the 2D line segment. */ - public static double distanceFromPoint2DToLineSegment2D(double pointX, double pointY, double lineSegmentStartX, double lineSegmentStartY, - double lineSegmentEndX, double lineSegmentEndY) + public static double distanceFromPoint2DToLineSegment2D(double pointX, + double pointY, + double lineSegmentStartX, + double lineSegmentStartY, + double lineSegmentEndX, + double lineSegmentEndY) { return EuclidCoreTools.squareRoot(distanceSquaredFromPoint2DToLineSegment2D(pointX, pointY, @@ -2031,14 +2183,23 @@ public static double distanceFromPoint2DToLineSegment2D(Point2DReadOnly point, P * @param rayDirectionY y-component of the ray direction. * @return the minimum distance between the 2D point and the 2D ray. */ - public static double distanceFromPoint2DToRay2D(double pointX, double pointY, double rayOriginX, double rayOriginY, double rayDirectionX, + public static double distanceFromPoint2DToRay2D(double pointX, + double pointY, + double rayOriginX, + double rayOriginY, + double rayDirectionX, double rayDirectionY) { return distanceFromPoint2DToRay2D(pointX, pointY, rayOriginX, rayOriginY, rayDirectionX, rayDirectionY, false); } - private static double distanceFromPoint2DToRay2D(double pointX, double pointY, double rayOriginX, double rayOriginY, double rayDirectionX, - double rayDirectionY, boolean isDirectionUnitary) + private static double distanceFromPoint2DToRay2D(double pointX, + double pointY, + double rayOriginX, + double rayOriginY, + double rayDirectionX, + double rayDirectionY, + boolean isDirectionUnitary) { if (isPoint2DInFrontOfRay2D(pointX, pointY, rayOriginX, rayOriginY, rayDirectionX, rayDirectionY)) return Math.abs(signedDistanceFromPoint2DToLine2D(pointX, pointY, rayOriginX, rayOriginY, rayDirectionX, rayDirectionY, isDirectionUnitary)); @@ -2133,8 +2294,15 @@ public static double distanceFromPoint2DToRay2D(Point2DReadOnly point, Point2DRe * @param lineDirectionZ z-component of the line direction. * @return the minimum distance between the 3D point and the 3D line. */ - public static double distanceFromPoint3DToLine3D(double pointX, double pointY, double pointZ, double pointOnLineX, double pointOnLineY, double pointOnLineZ, - double lineDirectionX, double lineDirectionY, double lineDirectionZ) + public static double distanceFromPoint3DToLine3D(double pointX, + double pointY, + double pointZ, + double pointOnLineX, + double pointOnLineY, + double pointOnLineZ, + double lineDirectionX, + double lineDirectionY, + double lineDirectionZ) { return distanceFromPoint3DToLine3D(pointX, pointY, @@ -2148,8 +2316,16 @@ public static double distanceFromPoint3DToLine3D(double pointX, double pointY, d false); } - private static double distanceFromPoint3DToLine3D(double pointX, double pointY, double pointZ, double pointOnLineX, double pointOnLineY, double pointOnLineZ, - double lineDirectionX, double lineDirectionY, double lineDirectionZ, boolean isDirectionUnitary) + private static double distanceFromPoint3DToLine3D(double pointX, + double pointY, + double pointZ, + double pointOnLineX, + double pointOnLineY, + double pointOnLineZ, + double lineDirectionX, + double lineDirectionY, + double lineDirectionZ, + boolean isDirectionUnitary) { double directionMagnitude = isDirectionUnitary ? 1.0 : EuclidCoreTools.fastNorm(lineDirectionX, lineDirectionY, lineDirectionZ); @@ -2260,8 +2436,15 @@ public static double distanceFromPoint3DToLine3D(Point3DReadOnly point, Point3DR * @param lineSegmentEndZ the z-coordinate of the line segment second endpoint. * @return the minimum distance between the 3D point and the 3D line segment. */ - public static double distanceFromPoint3DToLineSegment3D(double pointX, double pointY, double pointZ, double lineSegmentStartX, double lineSegmentStartY, - double lineSegmentStartZ, double lineSegmentEndX, double lineSegmentEndY, double lineSegmentEndZ) + public static double distanceFromPoint3DToLineSegment3D(double pointX, + double pointY, + double pointZ, + double lineSegmentStartX, + double lineSegmentStartY, + double lineSegmentStartZ, + double lineSegmentEndX, + double lineSegmentEndY, + double lineSegmentEndZ) { return EuclidCoreTools.squareRoot(distanceSquaredFromPoint3DToLineSegment3D(pointX, pointY, @@ -2291,7 +2474,10 @@ public static double distanceFromPoint3DToLineSegment3D(double pointX, double po * @param lineSegmentEnd second endpoint of the line segment. Not modified. * @return the minimum distance between the 3D point and the 3D line segment. */ - public static double distanceFromPoint3DToLineSegment3D(double pointX, double pointY, double pointZ, Point3DReadOnly lineSegmentStart, + public static double distanceFromPoint3DToLineSegment3D(double pointX, + double pointY, + double pointZ, + Point3DReadOnly lineSegmentStart, Point3DReadOnly lineSegmentEnd) { return EuclidCoreTools.squareRoot(distanceSquaredFromPoint3DToLineSegment3D(pointX, pointY, pointZ, lineSegmentStart, lineSegmentEnd)); @@ -2362,8 +2548,15 @@ public static double distanceFromPoint3DToPlane3D(Point3DReadOnly point, Point3D * @param planeNormalZ the z-component of the normal of the plane. Not modified. * @return the signed distance between the point and the plane. */ - public static double signedDistanceFromPoint3DToPlane3D(double pointX, double pointY, double pointZ, double pointOnPlaneX, double pointOnPlaneY, - double pointOnPlaneZ, double planeNormalX, double planeNormalY, double planeNormalZ) + public static double signedDistanceFromPoint3DToPlane3D(double pointX, + double pointY, + double pointZ, + double pointOnPlaneX, + double pointOnPlaneY, + double pointOnPlaneZ, + double planeNormalX, + double planeNormalY, + double planeNormalZ) { return signedDistanceFromPoint3DToPlane3D(pointX, pointY, @@ -2377,8 +2570,15 @@ public static double signedDistanceFromPoint3DToPlane3D(double pointX, double po false); } - private static double signedDistanceFromPoint3DToPlane3D(double pointX, double pointY, double pointZ, double pointOnPlaneX, double pointOnPlaneY, - double pointOnPlaneZ, double planeNormalX, double planeNormalY, double planeNormalZ, + private static double signedDistanceFromPoint3DToPlane3D(double pointX, + double pointY, + double pointZ, + double pointOnPlaneX, + double pointOnPlaneY, + double pointOnPlaneZ, + double planeNormalX, + double planeNormalY, + double planeNormalZ, boolean isNormalUnitary) { double dx = pointX - pointOnPlaneX; @@ -2420,9 +2620,17 @@ private static double signedDistanceFromPoint3DToPlane3D(double pointX, double p * @param planeSecondTangentZ the z-component of a second tangent of the infinite plane. * @return the signed distance between the point and the plane. */ - public static double signedDistanceFromPoint3DToPlane3D(double pointX, double pointY, double pointZ, double pointOnPlaneX, double pointOnPlaneY, - double pointOnPlaneZ, double planeFirstTangentX, double planeFirstTangentY, - double planeFirstTangentZ, double planeSecondTangentX, double planeSecondTangentY, + public static double signedDistanceFromPoint3DToPlane3D(double pointX, + double pointY, + double pointZ, + double pointOnPlaneX, + double pointOnPlaneY, + double pointOnPlaneZ, + double planeFirstTangentX, + double planeFirstTangentY, + double planeFirstTangentZ, + double planeSecondTangentX, + double planeSecondTangentY, double planeSecondTangentZ) { double planeNormalX = planeFirstTangentY * planeSecondTangentZ - planeFirstTangentZ * planeSecondTangentY; @@ -2445,7 +2653,10 @@ public static double signedDistanceFromPoint3DToPlane3D(double pointX, double po * @param planeNormal the normal of the plane. Not modified. * @return the signed distance between the point and the plane. */ - public static double signedDistanceFromPoint3DToPlane3D(double pointX, double pointY, double pointZ, Point3DReadOnly pointOnPlane, + public static double signedDistanceFromPoint3DToPlane3D(double pointX, + double pointY, + double pointZ, + Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal) { return signedDistanceFromPoint3DToPlane3D(pointX, @@ -2490,8 +2701,12 @@ public static double signedDistanceFromPoint3DToPlane3D(Point3DReadOnly point, P * @param planeSecondTangent a second tangent of the infinite plane. Not modified. * @return the signed distance between the point and the plane. */ - public static double signedDistanceFromPoint3DToPlane3D(double pointX, double pointY, double pointZ, Point3DReadOnly pointOnPlane, - Vector3DReadOnly planeFirstTangent, Vector3DReadOnly planeSecondTangent) + public static double signedDistanceFromPoint3DToPlane3D(double pointX, + double pointY, + double pointZ, + Point3DReadOnly pointOnPlane, + Vector3DReadOnly planeFirstTangent, + Vector3DReadOnly planeSecondTangent) { return signedDistanceFromPoint3DToPlane3D(pointX, pointY, @@ -2519,7 +2734,9 @@ public static double signedDistanceFromPoint3DToPlane3D(double pointX, double po * @param planeSecondTangent a second tangent of the infinite plane. Not modified. * @return the signed distance between the point and the plane. */ - public static double signedDistanceFromPoint3DToPlane3D(Point3DReadOnly point, Point3DReadOnly pointOnPlane, Vector3DReadOnly planeFirstTangent, + public static double signedDistanceFromPoint3DToPlane3D(Point3DReadOnly point, + Point3DReadOnly pointOnPlane, + Vector3DReadOnly planeFirstTangent, Vector3DReadOnly planeSecondTangent) { return signedDistanceFromPoint3DToPlane3D(point.getX(), point.getY(), point.getZ(), pointOnPlane, planeFirstTangent, planeSecondTangent); @@ -2543,8 +2760,12 @@ public static double signedDistanceFromPoint3DToPlane3D(Point3DReadOnly point, P * @param lineSegmentEndY the y-coordinate of the line segment second endpoint. * @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) + public static double distanceSquaredFromPoint2DToLineSegment2D(double pointX, + double pointY, + double lineSegmentStartX, + double lineSegmentStartY, + double lineSegmentEndX, + double lineSegmentEndY) { double percentage = percentageAlongLineSegment2D(pointX, pointY, lineSegmentStartX, lineSegmentStartY, lineSegmentEndX, lineSegmentEndY); @@ -2577,7 +2798,9 @@ else if (percentage < 0.0) * @param lineSegmentEnd second endpoint of the line segment. Not 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, Point2DReadOnly lineSegmentStart, + public static double distanceSquaredFromPoint2DToLineSegment2D(double pointX, + double pointY, + Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd) { return distanceSquaredFromPoint2DToLineSegment2D(pointX, @@ -2630,7 +2853,10 @@ public static double distanceSquaredFromPoint2DToLineSegment2D(Point2DReadOnly p * @param lineSegmentEnd second endpoint of the line segment. Not modified. * @return the square of the minimum distance between the 3D point and the 3D line segment. */ - public static double distanceSquaredFromPoint3DToLineSegment3D(double pointX, double pointY, double pointZ, Point3DReadOnly lineSegmentStart, + public static double distanceSquaredFromPoint3DToLineSegment3D(double pointX, + double pointY, + double pointZ, + Point3DReadOnly lineSegmentStart, Point3DReadOnly lineSegmentEnd) { return distanceSquaredFromPoint3DToLineSegment3D(pointX, @@ -2665,9 +2891,15 @@ public static double distanceSquaredFromPoint3DToLineSegment3D(double pointX, do * @param lineSegmentEndZ the z-coordinate of the line segment second endpoint. * @return the square of the minimum distance between the 3D point and the 3D line segment. */ - public static double distanceSquaredFromPoint3DToLineSegment3D(double pointX, double pointY, double pointZ, double lineSegmentStartX, - double lineSegmentStartY, double lineSegmentStartZ, double lineSegmentEndX, - double lineSegmentEndY, double lineSegmentEndZ) + public static double distanceSquaredFromPoint3DToLineSegment3D(double pointX, + double pointY, + double pointZ, + double lineSegmentStartX, + double lineSegmentStartY, + double lineSegmentStartZ, + double lineSegmentEndX, + double lineSegmentEndY, + double lineSegmentEndZ) { double percentage = percentageAlongLineSegment3D(pointX, pointY, @@ -2732,7 +2964,9 @@ public static double distanceSquaredFromPoint3DToLineSegment3D(Point3DReadOnly p * @param lineSegmentEnd second endpoint of the line segment. Not modified. * @return {@code true} if an intersection line segment - plane exists, {@code false} otherwise. */ - public static boolean doesLineSegment3DIntersectPlane3D(Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal, Point3DReadOnly lineSegmentStart, + public static boolean doesLineSegment3DIntersectPlane3D(Point3DReadOnly pointOnPlane, + Vector3DReadOnly planeNormal, + Point3DReadOnly lineSegmentStart, Point3DReadOnly lineSegmentEnd) { double d, ansStart, ansEnd; @@ -2775,8 +3009,12 @@ public static boolean doesLineSegment3DIntersectPlane3D(Point3DReadOnly pointOnP * @param lineSegmentEnd the second endpoint of the line segment. Not modified. * @return {@code true} if the line intersects the line segment, {@code false} otherwise. */ - public static boolean doLine2DAndLineSegment2DIntersect(double pointOnLineX, double pointOnLineY, double lineDirectionX, double lineDirectionY, - Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd) + public static boolean doLine2DAndLineSegment2DIntersect(double pointOnLineX, + double pointOnLineY, + double lineDirectionX, + double lineDirectionY, + Point2DReadOnly lineSegmentStart, + Point2DReadOnly lineSegmentEnd) { return intersectionBetweenLine2DAndLineSegment2D(pointOnLineX, pointOnLineY, @@ -2808,7 +3046,9 @@ public static boolean doLine2DAndLineSegment2DIntersect(double pointOnLineX, dou * @param lineSegmentEnd the second endpoint of the line segment. Not modified. * @return {@code true} if the line intersects the line segment, {@code false} otherwise. */ - public static boolean doLine2DAndLineSegment2DIntersect(Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection, Point2DReadOnly lineSegmentStart, + public static boolean doLine2DAndLineSegment2DIntersect(Point2DReadOnly pointOnLine, + Vector2DReadOnly lineDirection, + Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd) { return intersectionBetweenLine2DAndLineSegment2D(pointOnLine, lineDirection, lineSegmentStart, lineSegmentEnd, null); @@ -2836,8 +3076,14 @@ public static boolean doLine2DAndLineSegment2DIntersect(Point2DReadOnly pointOnL * @param lineSegmentEnd2y y-coordinate of the second endpoint of the second line segment. * @return {@code true} if the two line segments intersect, {@code false} otherwise. */ - public static boolean doLineSegment2DsIntersect(double lineSegmentStart1x, double lineSegmentStart1y, double lineSegmentEnd1x, double lineSegmentEnd1y, - double lineSegmentStart2x, double lineSegmentStart2y, double lineSegmentEnd2x, double lineSegmentEnd2y) + public static boolean doLineSegment2DsIntersect(double lineSegmentStart1x, + double lineSegmentStart1y, + double lineSegmentEnd1x, + double lineSegmentEnd1y, + double lineSegmentStart2x, + double lineSegmentStart2y, + double lineSegmentEnd2x, + double lineSegmentEnd2y) { return intersectionBetweenTwoLineSegment2Ds(lineSegmentStart1x, lineSegmentStart1y, @@ -2868,7 +3114,9 @@ public static boolean doLineSegment2DsIntersect(double lineSegmentStart1x, doubl * @param lineSegmentEnd2 second endpoint of the second line segment. Not modified. * @return {@code true} if the two line segments intersect, {@code false} otherwise. */ - public static boolean doLineSegment2DsIntersect(Point2DReadOnly lineSegmentStart1, Point2DReadOnly lineSegmentEnd1, Point2DReadOnly lineSegmentStart2, + public static boolean doLineSegment2DsIntersect(Point2DReadOnly lineSegmentStart1, + Point2DReadOnly lineSegmentEnd1, + Point2DReadOnly lineSegmentStart2, Point2DReadOnly lineSegmentEnd2) { return doLineSegment2DsIntersect(lineSegmentStart1.getX(), @@ -2903,8 +3151,14 @@ public static boolean doLineSegment2DsIntersect(Point2DReadOnly lineSegmentStart * @param lineSegmentEndY the y-coordinate of the second endpoint of the line segment. * @return {@code true} if the ray and line segment intersect, {@code false} otherwise. */ - public static boolean doRay2DAndLineSegment2DIntersect(double rayOriginX, double rayOriginY, double rayDirectionX, double rayDirectionY, - double lineSegmentStartX, double lineSegmentStartY, double lineSegmentEndX, double lineSegmentEndY) + public static boolean doRay2DAndLineSegment2DIntersect(double rayOriginX, + double rayOriginY, + double rayDirectionX, + double rayDirectionY, + double lineSegmentStartX, + double lineSegmentStartY, + double lineSegmentEndX, + double lineSegmentEndY) { return intersectionBetweenRay2DAndLineSegment2D(rayOriginX, rayOriginY, @@ -2935,7 +3189,9 @@ public static boolean doRay2DAndLineSegment2DIntersect(double rayOriginX, double * @param lineSegmentEnd second endpoint of the line segment. Not modified. * @return {@code true} if the ray and line segment intersect, {@code false} otherwise. */ - public static boolean doRay2DAndLineSegment2DIntersect(Point2DReadOnly rayOrigin, Vector2DReadOnly rayDirection, Point2DReadOnly lineSegmentStart, + public static boolean doRay2DAndLineSegment2DIntersect(Point2DReadOnly rayOrigin, + Vector2DReadOnly rayDirection, + Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd) { return doRay2DAndLineSegment2DIntersect(rayOrigin.getX(), @@ -3027,8 +3283,11 @@ public static double dotProduct(Point3DReadOnly start1, Point3DReadOnly end1, Po * greater than the maximum coordinate of the bounding box on the same * axis. */ - public static int intersectionBetweenLine2DAndBoundingBox2D(Point2DReadOnly boundingBoxMin, Point2DReadOnly boundingBoxMax, Point2DReadOnly firstPointOnLine, - Point2DReadOnly secondPointOnLine, Point2DBasics firstIntersectionToPack, + public static int intersectionBetweenLine2DAndBoundingBox2D(Point2DReadOnly boundingBoxMin, + Point2DReadOnly boundingBoxMax, + Point2DReadOnly firstPointOnLine, + Point2DReadOnly secondPointOnLine, + Point2DBasics firstIntersectionToPack, Point2DBasics secondIntersectionToPack) { return intersectionBetweenLine2DAndBoundingBox2DImpl(boundingBoxMin, @@ -3071,8 +3330,11 @@ public static int intersectionBetweenLine2DAndBoundingBox2D(Point2DReadOnly boun * greater than the maximum coordinate of the bounding box on the same * axis. */ - public static int intersectionBetweenLine2DAndBoundingBox2D(Point2DReadOnly boundingBoxMin, Point2DReadOnly boundingBoxMax, Point2DReadOnly pointOnLine, - Vector2DReadOnly lineDirection, Point2DBasics firstIntersectionToPack, + public static int intersectionBetweenLine2DAndBoundingBox2D(Point2DReadOnly boundingBoxMin, + Point2DReadOnly boundingBoxMax, + Point2DReadOnly pointOnLine, + Vector2DReadOnly lineDirection, + Point2DBasics firstIntersectionToPack, Point2DBasics secondIntersectionToPack) { double firstPointOnLineX = pointOnLine.getX(); @@ -3134,9 +3396,15 @@ public static int intersectionBetweenLine2DAndBoundingBox2D(Point2DReadOnly boun * greater than the maximum coordinate of the bounding box on the same * axis. */ - private static int intersectionBetweenLine2DAndBoundingBox2DImpl(Point2DReadOnly boundingBoxMin, Point2DReadOnly boundingBoxMax, double startX, - double startY, boolean canIntersectionOccurBeforeStart, double endX, double endY, - boolean canIntersectionOccurAfterEnd, Point2DBasics firstIntersectionToPack, + private static int intersectionBetweenLine2DAndBoundingBox2DImpl(Point2DReadOnly boundingBoxMin, + Point2DReadOnly boundingBoxMax, + double startX, + double startY, + boolean canIntersectionOccurBeforeStart, + double endX, + double endY, + boolean canIntersectionOccurAfterEnd, + Point2DBasics firstIntersectionToPack, Point2DBasics secondIntersectionToPack) { if (boundingBoxMin.getX() > boundingBoxMax.getX() || boundingBoxMin.getY() > boundingBoxMax.getY()) @@ -3268,9 +3536,15 @@ private static int intersectionBetweenLine2DAndBoundingBox2DImpl(Point2DReadOnly * Modified. * @return {@code true} if the line intersects the line segment, {@code false} otherwise. */ - public static boolean intersectionBetweenLine2DAndLineSegment2D(double pointOnLineX, double pointOnLineY, double lineDirectionX, double lineDirectionY, - double lineSegmentStartX, double lineSegmentStartY, double lineSegmentEndX, - double lineSegmentEndY, Point2DBasics intersectionToPack) + public static boolean intersectionBetweenLine2DAndLineSegment2D(double pointOnLineX, + double pointOnLineY, + double lineDirectionX, + double lineDirectionY, + double lineSegmentStartX, + double lineSegmentStartY, + double lineSegmentEndX, + double lineSegmentEndY, + Point2DBasics intersectionToPack) { double lineSegmentDirectionX = lineSegmentEndX - lineSegmentStartX; double lineSegmentDirectionY = lineSegmentEndY - lineSegmentStartY; @@ -3342,10 +3616,19 @@ public static boolean intersectionBetweenLine2DAndLineSegment2D(double pointOnLi * Modified. * @return whether there is an intersection or not. */ - 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) + 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 = ONE_TEN_MILLIONTH; @@ -3513,8 +3796,10 @@ private static boolean intersectionBetweenTwoLine2DsImpl(double start1x, double * @param lineSegmentEnd the second endpoint of the line segment. Not modified. * @return the 2D point of intersection if it exist, {@code null} otherwise. */ - public static Point2D intersectionBetweenLine2DAndLineSegment2D(Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection, - Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd) + public static Point2D intersectionBetweenLine2DAndLineSegment2D(Point2DReadOnly pointOnLine, + Vector2DReadOnly lineDirection, + Point2DReadOnly lineSegmentStart, + Point2DReadOnly lineSegmentEnd) { Point2D intersection = new Point2D(); boolean success = intersectionBetweenLine2DAndLineSegment2D(pointOnLine, lineDirection, lineSegmentStart, lineSegmentEnd, intersection); @@ -3548,8 +3833,10 @@ public static Point2D intersectionBetweenLine2DAndLineSegment2D(Point2DReadOnly * Modified. * @return {@code true} if the line intersects the line segment, {@code false} otherwise. */ - public static boolean intersectionBetweenLine2DAndLineSegment2D(Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection, - Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd, + public static boolean intersectionBetweenLine2DAndLineSegment2D(Point2DReadOnly pointOnLine, + Vector2DReadOnly lineDirection, + Point2DReadOnly lineSegmentStart, + Point2DReadOnly lineSegmentEnd, Point2DBasics intersectionToPack) { return intersectionBetweenLine2DAndLineSegment2D(pointOnLine.getX(), @@ -3594,8 +3881,11 @@ public static boolean intersectionBetweenLine2DAndLineSegment2D(Point2DReadOnly * greater than the maximum coordinate of the bounding box on the same * axis. */ - public static int intersectionBetweenLine3DAndBoundingBox3D(Point3DReadOnly boundingBoxMin, Point3DReadOnly boundingBoxMax, Point3DReadOnly firstPointOnLine, - Point3DReadOnly secondPointOnLine, Point3DBasics firstIntersectionToPack, + public static int intersectionBetweenLine3DAndBoundingBox3D(Point3DReadOnly boundingBoxMin, + Point3DReadOnly boundingBoxMax, + Point3DReadOnly firstPointOnLine, + Point3DReadOnly secondPointOnLine, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { return intersectionBetweenLine3DAndBoundingBox3DImpl(boundingBoxMin, @@ -3640,8 +3930,11 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(Point3DReadOnly boun * greater than the maximum coordinate of the bounding box on the same * axis. */ - public static int intersectionBetweenLine3DAndBoundingBox3D(Point3DReadOnly boundingBoxMin, Point3DReadOnly boundingBoxMax, Point3DReadOnly pointOnLine, - Vector3DReadOnly lineDirection, Point3DBasics firstIntersectionToPack, + public static int intersectionBetweenLine3DAndBoundingBox3D(Point3DReadOnly boundingBoxMin, + Point3DReadOnly boundingBoxMax, + Point3DReadOnly pointOnLine, + Vector3DReadOnly lineDirection, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { return intersectionBetweenLine3DAndBoundingBox3D(boundingBoxMin.getX(), @@ -3688,9 +3981,15 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(Point3DReadOnly boun * greater than the maximum coordinate of the bounding box on the same * axis. */ - public static int intersectionBetweenLine3DAndBoundingBox3D(double boundingBoxMinX, double boundingBoxMinY, double boundingBoxMinZ, double boundingBoxMaxX, - double boundingBoxMaxY, double boundingBoxMaxZ, Point3DReadOnly pointOnLine, - Vector3DReadOnly lineDirection, Point3DBasics firstIntersectionToPack, + public static int intersectionBetweenLine3DAndBoundingBox3D(double boundingBoxMinX, + double boundingBoxMinY, + double boundingBoxMinZ, + double boundingBoxMaxX, + double boundingBoxMaxY, + double boundingBoxMaxZ, + Point3DReadOnly pointOnLine, + Vector3DReadOnly lineDirection, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { double firstPointOnLineX = pointOnLine.getX(); @@ -3753,10 +4052,20 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(double boundingBoxMi * greater than the maximum coordinate of the bounding box on the same * axis. */ - public static int intersectionBetweenLine3DAndBoundingBox3D(double boundingBoxMinX, double boundingBoxMinY, double boundingBoxMinZ, double boundingBoxMaxX, - double boundingBoxMaxY, double boundingBoxMaxZ, double pointOnLineX, double pointOnLineY, - double pointOnLineZ, double lineDirectionX, double lineDirectionY, double lineDirectionZ, - Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) + public static int intersectionBetweenLine3DAndBoundingBox3D(double boundingBoxMinX, + double boundingBoxMinY, + double boundingBoxMinZ, + double boundingBoxMaxX, + double boundingBoxMaxY, + double boundingBoxMaxZ, + double pointOnLineX, + double pointOnLineY, + double pointOnLineZ, + double lineDirectionX, + double lineDirectionY, + double lineDirectionZ, + Point3DBasics firstIntersectionToPack, + Point3DBasics secondIntersectionToPack) { double firstPointOnLineX = pointOnLineX; double firstPointOnLineY = pointOnLineY; @@ -3829,10 +4138,18 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(double boundingBoxMi * greater than the maximum coordinate of the bounding box on the same * axis. */ - private static int intersectionBetweenLine3DAndBoundingBox3DImpl(Point3DReadOnly boundingBoxMin, Point3DReadOnly boundingBoxMax, double startX, - double startY, double startZ, boolean canIntersectionOccurBeforeStart, double endX, - double endY, double endZ, boolean canIntersectionOccurAfterEnd, - Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) + private static int intersectionBetweenLine3DAndBoundingBox3DImpl(Point3DReadOnly boundingBoxMin, + Point3DReadOnly boundingBoxMax, + double startX, + double startY, + double startZ, + boolean canIntersectionOccurBeforeStart, + double endX, + double endY, + double endZ, + boolean canIntersectionOccurAfterEnd, + Point3DBasics firstIntersectionToPack, + Point3DBasics secondIntersectionToPack) { return intersectionBetweenLine3DAndBoundingBox3DImpl(boundingBoxMin.getX(), boundingBoxMin.getY(), @@ -3903,11 +4220,22 @@ private static int intersectionBetweenLine3DAndBoundingBox3DImpl(Point3DReadOnly * greater than the maximum coordinate of the bounding box on the same * axis. */ - private static int intersectionBetweenLine3DAndBoundingBox3DImpl(double boundingBoxMinX, double boundingBoxMinY, double boundingBoxMinZ, - double boundingBoxMaxX, double boundingBoxMaxY, double boundingBoxMaxZ, double startX, - double startY, double startZ, boolean canIntersectionOccurBeforeStart, double endX, - double endY, double endZ, boolean canIntersectionOccurAfterEnd, - Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) + private static int intersectionBetweenLine3DAndBoundingBox3DImpl(double boundingBoxMinX, + double boundingBoxMinY, + double boundingBoxMinZ, + double boundingBoxMaxX, + double boundingBoxMaxY, + double boundingBoxMaxZ, + double startX, + double startY, + double startZ, + boolean canIntersectionOccurBeforeStart, + double endX, + double endY, + double endZ, + boolean canIntersectionOccurAfterEnd, + Point3DBasics firstIntersectionToPack, + Point3DBasics secondIntersectionToPack) { if (boundingBoxMinX > boundingBoxMaxX || boundingBoxMinY > boundingBoxMaxY || boundingBoxMinZ > boundingBoxMaxZ) throw new BoundingBoxException(boundingBoxMinX, boundingBoxMinY, boundingBoxMinZ, boundingBoxMaxX, boundingBoxMaxY, boundingBoxMaxZ); @@ -4077,10 +4405,21 @@ private static int intersectionBetweenLine3DAndBoundingBox3DImpl(double bounding * @throws IllegalArgumentException if either {@code cylinderLength < 0} or * {@code cylinderRadius < 0}. */ - public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, double cylinderRadius, double cylinderPositionX, double cylinderPositionY, - double cylinderPositionZ, double cylinderAxisX, double cylinderAxisY, double cylinderAxisZ, - double pointOnLineX, double pointOnLineY, double pointOnLineZ, double lineDirectionX, - double lineDirectionY, double lineDirectionZ, Point3DBasics firstIntersectionToPack, + public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, + double cylinderRadius, + double cylinderPositionX, + double cylinderPositionY, + double cylinderPositionZ, + double cylinderAxisX, + double cylinderAxisY, + double cylinderAxisZ, + double pointOnLineX, + double pointOnLineY, + double pointOnLineZ, + double lineDirectionX, + double lineDirectionY, + double lineDirectionZ, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { double startX = pointOnLineX; @@ -4142,9 +4481,14 @@ public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, * @throws IllegalArgumentException if either {@code cylinderLength < 0} or * {@code cylinderRadius < 0}. */ - public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, double cylinderRadius, Point3DReadOnly cylinderPosition, - Vector3DReadOnly cylinderAxis, Point3DReadOnly firstPointOnLine, Point3DReadOnly secondPointOnLine, - Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) + public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, + double cylinderRadius, + Point3DReadOnly cylinderPosition, + Vector3DReadOnly cylinderAxis, + Point3DReadOnly firstPointOnLine, + Point3DReadOnly secondPointOnLine, + Point3DBasics firstIntersectionToPack, + Point3DBasics secondIntersectionToPack) { double cylinderPositionX = cylinderPosition.getX(); double cylinderPositionY = cylinderPosition.getY(); @@ -4211,9 +4555,14 @@ public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, * @throws IllegalArgumentException if either {@code cylinderLength < 0} or * {@code cylinderRadius < 0}. */ - public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, double cylinderRadius, Point3DReadOnly cylinderPosition, - Vector3DReadOnly cylinderAxis, Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection, - Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) + public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, + double cylinderRadius, + Point3DReadOnly cylinderPosition, + Vector3DReadOnly cylinderAxis, + Point3DReadOnly pointOnLine, + Vector3DReadOnly lineDirection, + Point3DBasics firstIntersectionToPack, + Point3DBasics secondIntersectionToPack) { double cylinderPositionX = cylinderPosition.getX(); double cylinderPositionY = cylinderPosition.getY(); @@ -4311,11 +4660,24 @@ public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, * @throws IllegalArgumentException if either {@code cylinderLength < 0} or * {@code cylinderRadius < 0}. */ - private static int intersectionBetweenLine3DAndCylinder3DImpl(double cylinderLength, double cylinderRadius, double cylinderPositionX, - double cylinderPositionY, double cylinderPositionZ, double cylinderAxisX, double cylinderAxisY, - double cylinderAxisZ, boolean isAxisUnitary, double startX, double startY, double startZ, - boolean canIntersectionOccurBeforeStart, double endX, double endY, double endZ, - boolean canIntersectionOccurAfterEnd, Point3DBasics firstIntersectionToPack, + private static int intersectionBetweenLine3DAndCylinder3DImpl(double cylinderLength, + double cylinderRadius, + double cylinderPositionX, + double cylinderPositionY, + double cylinderPositionZ, + double cylinderAxisX, + double cylinderAxisY, + double cylinderAxisZ, + boolean isAxisUnitary, + double startX, + double startY, + double startZ, + boolean canIntersectionOccurBeforeStart, + double endX, + double endY, + double endZ, + boolean canIntersectionOccurAfterEnd, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { if (cylinderLength < 0.0) @@ -4593,9 +4955,17 @@ else if (dCylinder2 < dIntersection1) * @throws IllegalArgumentException if either {@code radiusX}, {@code radiusY}, or {@code radiusZ} * is negative. */ - public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, double radiusY, double radiusZ, double pointOnLineX, double pointOnLineY, - double pointOnLineZ, double lineDirectionX, double lineDirectionY, double lineDirectionZ, - Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) + public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, + double radiusY, + double radiusZ, + double pointOnLineX, + double pointOnLineY, + double pointOnLineZ, + double lineDirectionX, + double lineDirectionY, + double lineDirectionZ, + Point3DBasics firstIntersectionToPack, + Point3DBasics secondIntersectionToPack) { double startX = pointOnLineX; double startY = pointOnLineY; @@ -4650,8 +5020,12 @@ public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, double * @throws IllegalArgumentException if either {@code radiusX}, {@code radiusY}, or {@code radiusZ} * is negative. */ - public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, double radiusY, double radiusZ, Point3DReadOnly firstPointOnLine, - Point3DReadOnly secondPointOnLine, Point3DBasics firstIntersectionToPack, + public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, + double radiusY, + double radiusZ, + Point3DReadOnly firstPointOnLine, + Point3DReadOnly secondPointOnLine, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { double startX = firstPointOnLine.getX(); @@ -4707,8 +5081,12 @@ public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, double * @throws IllegalArgumentException if either {@code radiusX}, {@code radiusY}, or {@code radiusZ} * is negative. */ - public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, double radiusY, double radiusZ, Point3DReadOnly pointOnLine, - Vector3DReadOnly lineDirection, Point3DBasics firstIntersectionToPack, + public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, + double radiusY, + double radiusZ, + Point3DReadOnly pointOnLine, + Vector3DReadOnly lineDirection, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { double startX = pointOnLine.getX(); @@ -4789,9 +5167,18 @@ public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, double * @throws IllegalArgumentException if either {@code radiusX}, {@code radiusY}, or {@code radiusZ} * is negative. */ - private static int intersectionBetweenLine3DAndEllipsoid3DImpl(double radiusX, double radiusY, double radiusZ, double startX, double startY, double startZ, - boolean canIntersectionOccurBeforeStart, double endX, double endY, double endZ, - boolean canIntersectionOccurAfterEnd, Point3DBasics firstIntersectionToPack, + private static int intersectionBetweenLine3DAndEllipsoid3DImpl(double radiusX, + double radiusY, + double radiusZ, + double startX, + double startY, + double startZ, + boolean canIntersectionOccurBeforeStart, + double endX, + double endY, + double endZ, + boolean canIntersectionOccurAfterEnd, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { if (radiusX < 0.0) @@ -4921,7 +5308,9 @@ private static int intersectionBetweenLine3DAndEllipsoid3DImpl(double radiusX, d * @return the coordinates of the intersection, or {@code null} if the line is parallel to the * plane. */ - public static Point3D intersectionBetweenLine3DAndPlane3D(Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal, Point3DReadOnly pointOnLine, + public static Point3D intersectionBetweenLine3DAndPlane3D(Point3DReadOnly pointOnPlane, + Vector3DReadOnly planeNormal, + Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection) { Point3D intersection = new Point3D(); @@ -4949,8 +5338,11 @@ public static Point3D intersectionBetweenLine3DAndPlane3D(Point3DReadOnly pointO * @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(Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal, Point3DReadOnly pointOnLine, - Vector3DReadOnly lineDirection, Point3DBasics intersectionToPack) + public static boolean intersectionBetweenLine3DAndPlane3D(Point3DReadOnly pointOnPlane, + Vector3DReadOnly planeNormal, + Point3DReadOnly pointOnLine, + Vector3DReadOnly lineDirection, + 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 @@ -5017,9 +5409,12 @@ public static boolean intersectionBetweenLine3DAndPlane3D(Point3DReadOnly pointO * greater than the maximum coordinate of the bounding box on the same * axis. */ - public static int intersectionBetweenLineSegment2DAndBoundingBox2D(Point2DReadOnly boundingBoxMin, Point2DReadOnly boundingBoxMax, - Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd, - Point2DBasics firstIntersectionToPack, Point2DBasics secondIntersectionToPack) + public static int intersectionBetweenLineSegment2DAndBoundingBox2D(Point2DReadOnly boundingBoxMin, + Point2DReadOnly boundingBoxMax, + Point2DReadOnly lineSegmentStart, + Point2DReadOnly lineSegmentEnd, + Point2DBasics firstIntersectionToPack, + Point2DBasics secondIntersectionToPack) { return intersectionBetweenLine2DAndBoundingBox2DImpl(boundingBoxMin, boundingBoxMax, @@ -5070,9 +5465,12 @@ public static int intersectionBetweenLineSegment2DAndBoundingBox2D(Point2DReadOn * greater than the maximum coordinate of the bounding box on the same * axis. */ - public static int intersectionBetweenLineSegment3DAndBoundingBox3D(Point3DReadOnly boundingBoxMin, Point3DReadOnly boundingBoxMax, - Point3DReadOnly lineSegmentStart, Point3DReadOnly lineSegmentEnd, - Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) + public static int intersectionBetweenLineSegment3DAndBoundingBox3D(Point3DReadOnly boundingBoxMin, + Point3DReadOnly boundingBoxMax, + Point3DReadOnly lineSegmentStart, + Point3DReadOnly lineSegmentEnd, + Point3DBasics firstIntersectionToPack, + Point3DBasics secondIntersectionToPack) { return intersectionBetweenLine3DAndBoundingBox3DImpl(boundingBoxMin, boundingBoxMax, @@ -5120,9 +5518,13 @@ public static int intersectionBetweenLineSegment3DAndBoundingBox3D(Point3DReadOn * @throws IllegalArgumentException if either {@code cylinderLength < 0} or * {@code cylinderRadius < 0}. */ - public static int intersectionBetweenLineSegment3DAndCylinder3D(double cylinderLength, double cylinderRadius, Point3DReadOnly cylinderPosition, - Vector3DReadOnly cylinderAxis, Point3DReadOnly lineSegmentStart, - Point3DReadOnly lineSegmentEnd, Point3DBasics firstIntersectionToPack, + public static int intersectionBetweenLineSegment3DAndCylinder3D(double cylinderLength, + double cylinderRadius, + Point3DReadOnly cylinderPosition, + Vector3DReadOnly cylinderAxis, + Point3DReadOnly lineSegmentStart, + Point3DReadOnly lineSegmentEnd, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { double cylinderPositionX = cylinderPosition.getX(); @@ -5190,8 +5592,12 @@ public static int intersectionBetweenLineSegment3DAndCylinder3D(double cylinderL * @throws IllegalArgumentException if either {@code radiusX}, {@code radiusY}, or {@code radiusZ} * is negative. */ - public static int intersectionBetweenLineSegment3DAndEllipsoid3D(double radiusX, double radiusY, double radiusZ, Point3DReadOnly lineSegmentStart, - Point3DReadOnly lineSegmentEnd, Point3DBasics firstIntersectionToPack, + public static int intersectionBetweenLineSegment3DAndEllipsoid3D(double radiusX, + double radiusY, + double radiusZ, + Point3DReadOnly lineSegmentStart, + Point3DReadOnly lineSegmentEnd, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { double startX = lineSegmentStart.getX(); @@ -5239,8 +5645,10 @@ public static int intersectionBetweenLineSegment3DAndEllipsoid3D(double radiusX, * @param lineSegmentEnd second endpoint of the line segment. Not modified. * @return the intersection, or {@code null} if there is no intersection. */ - public static Point3D intersectionBetweenLineSegment3DAndPlane3D(Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal, - Point3DReadOnly lineSegmentStart, Point3DReadOnly lineSegmentEnd) + public static Point3D intersectionBetweenLineSegment3DAndPlane3D(Point3DReadOnly pointOnPlane, + Vector3DReadOnly planeNormal, + Point3DReadOnly lineSegmentStart, + Point3DReadOnly lineSegmentEnd) { if (doesLineSegment3DIntersectPlane3D(pointOnPlane, planeNormal, lineSegmentStart, lineSegmentEnd)) { // Since an intersection exists, it is now the same as computing the intersection line-plane @@ -5290,8 +5698,11 @@ public static Point3D intersectionBetweenLineSegment3DAndPlane3D(Point3DReadOnly * greater than the maximum coordinate of the bounding box on the same * axis. */ - public static int intersectionBetweenRay2DAndBoundingBox2D(Point2DReadOnly boundingBoxMin, Point2DReadOnly boundingBoxMax, Point2DReadOnly rayOrigin, - Vector2DReadOnly rayDirection, Point2DBasics firstIntersectionToPack, + public static int intersectionBetweenRay2DAndBoundingBox2D(Point2DReadOnly boundingBoxMin, + Point2DReadOnly boundingBoxMax, + Point2DReadOnly rayOrigin, + Vector2DReadOnly rayDirection, + Point2DBasics firstIntersectionToPack, Point2DBasics secondIntersectionToPack) { double firstPointOnLineX = rayOrigin.getX(); @@ -5337,9 +5748,15 @@ public static int intersectionBetweenRay2DAndBoundingBox2D(Point2DReadOnly bound * Modified. * @return {@code true} if the ray intersects the line segment, {@code false} otherwise. */ - public static boolean intersectionBetweenRay2DAndLineSegment2D(double rayOriginX, double rayOriginY, double rayDirectionX, double rayDirectionY, - double lineSegmentStartX, double lineSegmentStartY, double lineSegmentEndX, - double lineSegmentEndY, Point2DBasics intersectionToPack) + public static boolean intersectionBetweenRay2DAndLineSegment2D(double rayOriginX, + double rayOriginY, + double rayDirectionX, + double rayDirectionY, + double lineSegmentStartX, + double lineSegmentStartY, + double lineSegmentEndX, + double lineSegmentEndY, + Point2DBasics intersectionToPack) { double start1x = rayOriginX; double start1y = rayOriginY; @@ -5376,7 +5793,9 @@ public static boolean intersectionBetweenRay2DAndLineSegment2D(double rayOriginX * @param lineSegmentEnd the second endpoint of the line segment. Not modified. * @return the 2D point of intersection if it exist, {@code null} otherwise. */ - public static Point2D intersectionBetweenRay2DAndLineSegment2D(Point2DReadOnly rayOrigin, Vector2DReadOnly rayDirection, Point2DReadOnly lineSegmentStart, + public static Point2D intersectionBetweenRay2DAndLineSegment2D(Point2DReadOnly rayOrigin, + Vector2DReadOnly rayDirection, + Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd) { Point2D intersection = new Point2D(); @@ -5418,8 +5837,11 @@ public static Point2D intersectionBetweenRay2DAndLineSegment2D(Point2DReadOnly r * Modified. * @return {@code true} if the ray intersects the line segment, {@code false} otherwise. */ - public static boolean intersectionBetweenRay2DAndLineSegment2D(Point2DReadOnly rayOrigin, Vector2DReadOnly rayDirection, Point2DReadOnly lineSegmentStart, - Point2DReadOnly lineSegmentEnd, Point2DBasics intersectionToPack) + public static boolean intersectionBetweenRay2DAndLineSegment2D(Point2DReadOnly rayOrigin, + Vector2DReadOnly rayDirection, + Point2DReadOnly lineSegmentStart, + Point2DReadOnly lineSegmentEnd, + Point2DBasics intersectionToPack) { return intersectionBetweenRay2DAndLineSegment2D(rayOrigin.getX(), rayOrigin.getY(), @@ -5468,8 +5890,11 @@ public static boolean intersectionBetweenRay2DAndLineSegment2D(Point2DReadOnly r * greater than the maximum coordinate of the bounding box on the same * axis. */ - public static int intersectionBetweenRay3DAndBoundingBox3D(Point3DReadOnly boundingBoxMin, Point3DReadOnly boundingBoxMax, Point3DReadOnly rayOrigin, - Vector3DReadOnly rayDirection, Point3DBasics firstIntersectionToPack, + public static int intersectionBetweenRay3DAndBoundingBox3D(Point3DReadOnly boundingBoxMin, + Point3DReadOnly boundingBoxMax, + Point3DReadOnly rayOrigin, + Vector3DReadOnly rayDirection, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { double firstPointOnLineX = rayOrigin.getX(); @@ -5524,9 +5949,14 @@ public static int intersectionBetweenRay3DAndBoundingBox3D(Point3DReadOnly bound * @throws IllegalArgumentException if either {@code cylinderLength < 0} or * {@code cylinderRadius < 0}. */ - public static int intersectionBetweenRay3DAndCylinder3D(double cylinderLength, double cylinderRadius, Point3DReadOnly cylinderPosition, - Vector3DReadOnly cylinderAxis, Point3DReadOnly rayOrigin, Vector3DReadOnly rayDirection, - Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) + public static int intersectionBetweenRay3DAndCylinder3D(double cylinderLength, + double cylinderRadius, + Point3DReadOnly cylinderPosition, + Vector3DReadOnly cylinderAxis, + Point3DReadOnly rayOrigin, + Vector3DReadOnly rayDirection, + Point3DBasics firstIntersectionToPack, + Point3DBasics secondIntersectionToPack) { double cylinderPositionX = cylinderPosition.getX(); double cylinderPositionY = cylinderPosition.getY(); @@ -5593,8 +6023,12 @@ public static int intersectionBetweenRay3DAndCylinder3D(double cylinderLength, d * @throws IllegalArgumentException if either {@code radiusX}, {@code radiusY}, or {@code radiusZ} * is negative. */ - public static int intersectionBetweenRay3DAndEllipsoid3D(double radiusX, double radiusY, double radiusZ, Point3DReadOnly rayOrigin, - Vector3DReadOnly rayDirection, Point3DBasics firstIntersectionToPack, + public static int intersectionBetweenRay3DAndEllipsoid3D(double radiusX, + double radiusY, + double radiusZ, + Point3DReadOnly rayOrigin, + Vector3DReadOnly rayDirection, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { double startX = rayOrigin.getX(); @@ -5643,8 +6077,14 @@ public static int intersectionBetweenRay3DAndEllipsoid3D(double radiusX, double * @param intersectionToPack 2D point in which the result is stored. Can be {@code null}. Modified. * @return {@code true} if the two lines intersect, {@code false} otherwise. */ - public static boolean intersectionBetweenTwoLine2Ds(double pointOnLine1x, double pointOnLine1y, double lineDirection1x, double lineDirection1y, - double pointOnLine2x, double pointOnLine2y, double lineDirection2x, double lineDirection2y, + public static boolean intersectionBetweenTwoLine2Ds(double pointOnLine1x, + double pointOnLine1y, + double lineDirection1x, + double lineDirection1y, + double pointOnLine2x, + double pointOnLine2y, + double lineDirection2x, + double lineDirection2y, Point2DBasics intersectionToPack) { double alpha = percentageOfIntersectionBetweenTwoLine2Ds(pointOnLine1x, @@ -5696,7 +6136,9 @@ public static boolean intersectionBetweenTwoLine2Ds(double pointOnLine1x, double * @param secondPointOnLine2 a second point located on the second line. Not modified. * @return the 2D point of intersection if the two lines intersect, {@code null} otherwise. */ - public static Point2D intersectionBetweenTwoLine2Ds(Point2DReadOnly firstPointOnLine1, Point2DReadOnly secondPointOnLine1, Point2DReadOnly firstPointOnLine2, + public static Point2D intersectionBetweenTwoLine2Ds(Point2DReadOnly firstPointOnLine1, + Point2DReadOnly secondPointOnLine1, + Point2DReadOnly firstPointOnLine2, Point2DReadOnly secondPointOnLine2) { Point2D intersection = new Point2D(); @@ -5729,8 +6171,11 @@ public static Point2D intersectionBetweenTwoLine2Ds(Point2DReadOnly firstPointOn * @param intersectionToPack 2D point in which the result is stored. Can be {@code null}. Modified. * @return {@code true} if the two lines intersect, {@code false} otherwise. */ - public static boolean intersectionBetweenTwoLine2Ds(Point2DReadOnly firstPointOnLine1, Point2DReadOnly secondPointOnLine1, Point2DReadOnly firstPointOnLine2, - Point2DReadOnly secondPointOnLine2, Point2DBasics intersectionToPack) + public static boolean intersectionBetweenTwoLine2Ds(Point2DReadOnly firstPointOnLine1, + Point2DReadOnly secondPointOnLine1, + Point2DReadOnly firstPointOnLine2, + Point2DReadOnly secondPointOnLine2, + Point2DBasics intersectionToPack) { double pointOnLine1x = firstPointOnLine1.getX(); double pointOnLine1y = firstPointOnLine1.getY(); @@ -5774,7 +6219,9 @@ public static boolean intersectionBetweenTwoLine2Ds(Point2DReadOnly firstPointOn * @param lineDirection2 the second line direction. Not modified. * @return the 2D point of intersection if the two lines intersect, {@code null} otherwise. */ - public static Point2D intersectionBetweenTwoLine2Ds(Point2DReadOnly pointOnLine1, Vector2DReadOnly lineDirection1, Point2DReadOnly pointOnLine2, + public static Point2D intersectionBetweenTwoLine2Ds(Point2DReadOnly pointOnLine1, + Vector2DReadOnly lineDirection1, + Point2DReadOnly pointOnLine2, Vector2DReadOnly lineDirection2) { Point2D intersection = new Point2D(); @@ -5806,8 +6253,11 @@ public static Point2D intersectionBetweenTwoLine2Ds(Point2DReadOnly pointOnLine1 * @param intersectionToPack 2D point in which the result is stored. Can be {@code null}. Modified. * @return {@code true} if the two lines intersect, {@code false} otherwise. */ - public static boolean intersectionBetweenTwoLine2Ds(Point2DReadOnly pointOnLine1, Vector2DReadOnly lineDirection1, Point2DReadOnly pointOnLine2, - Vector2DReadOnly lineDirection2, Point2DBasics intersectionToPack) + public static boolean intersectionBetweenTwoLine2Ds(Point2DReadOnly pointOnLine1, + Vector2DReadOnly lineDirection1, + Point2DReadOnly pointOnLine2, + Vector2DReadOnly lineDirection2, + Point2DBasics intersectionToPack) { return intersectionBetweenTwoLine2Ds(pointOnLine1.getX(), pointOnLine1.getY(), @@ -5846,9 +6296,15 @@ public static boolean intersectionBetweenTwoLine2Ds(Point2DReadOnly pointOnLine1 * Modified. * @return {@code true} if the two line segments intersect, {@code false} otherwise. */ - public static boolean intersectionBetweenTwoLineSegment2Ds(double lineSegmentStart1x, double lineSegmentStart1y, double lineSegmentEnd1x, - double lineSegmentEnd1y, double lineSegmentStart2x, double lineSegmentStart2y, - double lineSegmentEnd2x, double lineSegmentEnd2y, Point2DBasics intersectionToPack) + public static boolean intersectionBetweenTwoLineSegment2Ds(double lineSegmentStart1x, + double lineSegmentStart1y, + double lineSegmentEnd1x, + double lineSegmentEnd1y, + double lineSegmentStart2x, + double lineSegmentStart2y, + double lineSegmentEnd2x, + double lineSegmentEnd2y, + Point2DBasics intersectionToPack) { double start1x = lineSegmentStart1x; double start1y = lineSegmentStart1y; @@ -5884,8 +6340,10 @@ public static boolean intersectionBetweenTwoLineSegment2Ds(double lineSegmentSta * @param lineSegmentEnd2 the second endpoint of the second line segment. Not modified. * @return the intersection point if it exists, {@code null} otherwise. */ - public static Point2D intersectionBetweenTwoLineSegment2Ds(Point2DReadOnly lineSegmentStart1, Point2DReadOnly lineSegmentEnd1, - Point2DReadOnly lineSegmentStart2, Point2DReadOnly lineSegmentEnd2) + public static Point2D intersectionBetweenTwoLineSegment2Ds(Point2DReadOnly lineSegmentStart1, + Point2DReadOnly lineSegmentEnd1, + Point2DReadOnly lineSegmentStart2, + Point2DReadOnly lineSegmentEnd2) { Point2D intersection = new Point2D(); boolean success = intersectionBetweenTwoLineSegment2Ds(lineSegmentStart1, lineSegmentEnd1, lineSegmentStart2, lineSegmentEnd2, intersection); @@ -5917,8 +6375,10 @@ public static Point2D intersectionBetweenTwoLineSegment2Ds(Point2DReadOnly lineS * @param intersectionToPack the 2D point in which the result is stored. Modified. * @return {@code true} if the two line segments intersect, {@code false} otherwise. */ - public static boolean intersectionBetweenTwoLineSegment2Ds(Point2DReadOnly lineSegmentStart1, Point2DReadOnly lineSegmentEnd1, - Point2DReadOnly lineSegmentStart2, Point2DReadOnly lineSegmentEnd2, + public static boolean intersectionBetweenTwoLineSegment2Ds(Point2DReadOnly lineSegmentStart1, + Point2DReadOnly lineSegmentEnd1, + Point2DReadOnly lineSegmentStart2, + Point2DReadOnly lineSegmentEnd2, Point2DBasics intersectionToPack) { return intersectionBetweenTwoLineSegment2Ds(lineSegmentStart1.getX(), @@ -5966,8 +6426,12 @@ public static boolean intersectionBetweenTwoLineSegment2Ds(Point2DReadOnly lineS * @return {@code true} if the intersection was calculated properly, {@code false} otherwise. * @throws IllegalArgumentException if angleThreshold ∉ [0; pi/2] */ - public static boolean intersectionBetweenTwoPlane3Ds(Point3DReadOnly pointOnPlane1, Vector3DReadOnly planeNormal1, Point3DReadOnly pointOnPlane2, - Vector3DReadOnly planeNormal2, double angleThreshold, Point3DBasics pointOnIntersectionToPack, + public static boolean intersectionBetweenTwoPlane3Ds(Point3DReadOnly pointOnPlane1, + Vector3DReadOnly planeNormal1, + Point3DReadOnly pointOnPlane2, + Vector3DReadOnly planeNormal2, + double angleThreshold, + Point3DBasics pointOnIntersectionToPack, Vector3DBasics intersectionDirectionToPack) { if (angleThreshold < 0.0 || angleThreshold > HALF_PI) @@ -6060,8 +6524,11 @@ public static boolean intersectionBetweenTwoPlane3Ds(Point3DReadOnly pointOnPlan * intersection between the two planes. Modified. * @return {@code true} if the intersection was calculated properly, {@code false} otherwise. */ - public static boolean intersectionBetweenTwoPlane3Ds(Point3DReadOnly pointOnPlane1, Vector3DReadOnly planeNormal1, Point3DReadOnly pointOnPlane2, - Vector3DReadOnly planeNormal2, Point3DBasics pointOnIntersectionToPack, + public static boolean intersectionBetweenTwoPlane3Ds(Point3DReadOnly pointOnPlane1, + Vector3DReadOnly planeNormal1, + Point3DReadOnly pointOnPlane2, + Vector3DReadOnly planeNormal2, + Point3DBasics pointOnIntersectionToPack, Vector3DBasics intersectionDirectionToPack) { return intersectionBetweenTwoPlane3Ds(pointOnPlane1, @@ -6311,8 +6778,13 @@ public static boolean isPoint2DOnRightSideOfLine2D(Point2DReadOnly point, Point2 * @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. */ - public static boolean isPoint2DOnSideOfLine2D(double pointX, double pointY, double pointOnLineX, double pointOnLineY, double lineDirectionX, - double lineDirectionY, boolean testLeftSide) + public static boolean isPoint2DOnSideOfLine2D(double pointX, + double pointY, + double pointOnLineX, + double pointOnLineY, + double lineDirectionX, + double lineDirectionY, + boolean testLeftSide) { double dx = pointX - pointOnLineX; double dy = pointY - pointOnLineY; @@ -6346,7 +6818,10 @@ public static boolean isPoint2DOnSideOfLine2D(double pointX, double pointY, doub * @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. */ - public static boolean isPoint2DOnSideOfLine2D(double pointX, double pointY, Point2DReadOnly firstPointOnLine, Point2DReadOnly secondPointOnLine, + public static boolean isPoint2DOnSideOfLine2D(double pointX, + double pointY, + Point2DReadOnly firstPointOnLine, + Point2DReadOnly secondPointOnLine, boolean testLeftSide) { double pointOnLineX = firstPointOnLine.getX(); @@ -6378,7 +6853,10 @@ public static boolean isPoint2DOnSideOfLine2D(double pointX, double pointY, Poin * @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. */ - public static boolean isPoint2DOnSideOfLine2D(double pointX, double pointY, Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection, + public static boolean isPoint2DOnSideOfLine2D(double pointX, + double pointY, + Point2DReadOnly pointOnLine, + Vector2DReadOnly lineDirection, boolean testLeftSide) { double pointOnLineX = pointOnLine.getX(); @@ -6410,7 +6888,9 @@ public static boolean isPoint2DOnSideOfLine2D(double pointX, double pointY, Poin * @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. */ - public static boolean isPoint2DOnSideOfLine2D(Point2DReadOnly point, Point2DReadOnly firstPointOnLine, Point2DReadOnly secondPointOnLine, + public static boolean isPoint2DOnSideOfLine2D(Point2DReadOnly point, + Point2DReadOnly firstPointOnLine, + Point2DReadOnly secondPointOnLine, boolean testLeftSide) { return isPoint2DOnSideOfLine2D(point.getX(), point.getY(), firstPointOnLine, secondPointOnLine, testLeftSide); @@ -6469,8 +6949,16 @@ public static boolean isPoint2DOnSideOfLine2D(Point2DReadOnly point, Point2DRead * @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. */ - public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, double pointY, double pointZ, double pointOnPlaneX, double pointOnPlaneY, - double pointOnPlaneZ, double planeNormalX, double planeNormalY, double planeNormalZ, boolean testForAbove) + public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, + double pointY, + double pointZ, + double pointOnPlaneX, + double pointOnPlaneY, + double pointOnPlaneZ, + double planeNormalX, + double planeNormalY, + double planeNormalZ, + boolean testForAbove) { double dx = (pointX - pointOnPlaneX) * planeNormalX; double dy = (pointY - pointOnPlaneY) * planeNormalY; @@ -6506,7 +6994,11 @@ public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, double pointY, * @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. */ - public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, double pointY, double pointZ, Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal, + public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, + double pointY, + double pointZ, + Point3DReadOnly pointOnPlane, + Vector3DReadOnly planeNormal, boolean testForAbove) { return isPoint3DAboveOrBelowPlane3D(pointX, @@ -6679,9 +7171,19 @@ public static boolean isPoint3DBelowPlane3D(Point3DReadOnly point, Point3DReadOn * @see #isPoint3DAboveOrBelowPlane3D(double, double, double, double, double, double, double, * double, double, boolean) */ - public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, double pointY, double pointZ, double pointOnPlaneX, double pointOnPlaneY, - double pointOnPlaneZ, double planeFirstTangentX, double planeFirstTangentY, double planeFirstTangentZ, - double planeSecondTangentX, double planeSecondTangentY, double planeSecondTangentZ, boolean testForAbove) + public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, + double pointY, + double pointZ, + double pointOnPlaneX, + double pointOnPlaneY, + double pointOnPlaneZ, + double planeFirstTangentX, + double planeFirstTangentY, + double planeFirstTangentZ, + double planeSecondTangentX, + double planeSecondTangentY, + double planeSecondTangentZ, + boolean testForAbove) { double planeNormalX = planeFirstTangentY * planeSecondTangentZ - planeFirstTangentZ * planeSecondTangentY; double planeNormalY = planeFirstTangentZ * planeSecondTangentX - planeFirstTangentX * planeSecondTangentZ; @@ -6725,8 +7227,13 @@ public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, double pointY, * @see #isPoint3DAboveOrBelowPlane3D(double, double, double, double, double, double, double, * double, double, boolean) */ - public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, double pointY, double pointZ, Point3DReadOnly pointOnPlane, - Vector3DReadOnly planeFirstTangent, Vector3DReadOnly planeSecondTangent, boolean testForAbove) + public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, + double pointY, + double pointZ, + Point3DReadOnly pointOnPlane, + Vector3DReadOnly planeFirstTangent, + Vector3DReadOnly planeSecondTangent, + boolean testForAbove) { return isPoint3DAboveOrBelowPlane3D(pointX, pointY, @@ -6768,8 +7275,11 @@ public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, double pointY, * @see #isPoint3DAboveOrBelowPlane3D(double, double, double, double, double, double, double, * double, double, boolean) */ - public static boolean isPoint3DAboveOrBelowPlane3D(Point3DReadOnly point, Point3DReadOnly pointOnPlane, Vector3DReadOnly planeFirstTangent, - Vector3DReadOnly planeSecondTangent, boolean testForAbove) + public static boolean isPoint3DAboveOrBelowPlane3D(Point3DReadOnly point, + Point3DReadOnly pointOnPlane, + Vector3DReadOnly planeFirstTangent, + Vector3DReadOnly planeSecondTangent, + boolean testForAbove) { return isPoint3DAboveOrBelowPlane3D(point.getX(), point.getY(), point.getZ(), pointOnPlane, planeFirstTangent, planeSecondTangent, testForAbove); } @@ -6799,7 +7309,11 @@ public static boolean isPoint3DAboveOrBelowPlane3D(Point3DReadOnly point, Point3 * @see #isPoint3DAboveOrBelowPlane3D(double, double, double, double, double, double, double, * double, double, boolean) */ - public static boolean isPoint3DAbovePlane3D(double pointX, double pointY, double pointZ, Point3DReadOnly pointOnPlane, Vector3DReadOnly planeFirstTangent, + public static boolean isPoint3DAbovePlane3D(double pointX, + double pointY, + double pointZ, + Point3DReadOnly pointOnPlane, + Vector3DReadOnly planeFirstTangent, Vector3DReadOnly planeSecondTangent) { return isPoint3DAboveOrBelowPlane3D(pointX, pointY, pointZ, pointOnPlane, planeFirstTangent, planeSecondTangent, true); @@ -6828,7 +7342,9 @@ public static boolean isPoint3DAbovePlane3D(double pointX, double pointY, double * @see #isPoint3DAboveOrBelowPlane3D(double, double, double, double, double, double, double, * double, double, boolean) */ - public static boolean isPoint3DAbovePlane3D(Point3DReadOnly point, Point3DReadOnly pointOnPlane, Vector3DReadOnly planeFirstTangent, + public static boolean isPoint3DAbovePlane3D(Point3DReadOnly point, + Point3DReadOnly pointOnPlane, + Vector3DReadOnly planeFirstTangent, Vector3DReadOnly planeSecondTangent) { return isPoint3DAboveOrBelowPlane3D(point, pointOnPlane, planeFirstTangent, planeSecondTangent, true); @@ -6859,7 +7375,11 @@ public static boolean isPoint3DAbovePlane3D(Point3DReadOnly point, Point3DReadOn * @see #isPoint3DAboveOrBelowPlane3D(double, double, double, double, double, double, double, * double, double, boolean) */ - public static boolean isPoint3DBelowPlane3D(double pointX, double pointY, double pointZ, Point3DReadOnly pointOnPlane, Vector3DReadOnly planeFirstTangent, + public static boolean isPoint3DBelowPlane3D(double pointX, + double pointY, + double pointZ, + Point3DReadOnly pointOnPlane, + Vector3DReadOnly planeFirstTangent, Vector3DReadOnly planeSecondTangent) { return isPoint3DAboveOrBelowPlane3D(pointX, pointY, pointZ, pointOnPlane, planeFirstTangent, planeSecondTangent, false); @@ -6888,7 +7408,9 @@ public static boolean isPoint3DBelowPlane3D(double pointX, double pointY, double * @see #isPoint3DAboveOrBelowPlane3D(double, double, double, double, double, double, double, * double, double, boolean) */ - public static boolean isPoint3DBelowPlane3D(Point3DReadOnly point, Point3DReadOnly pointOnPlane, Vector3DReadOnly planeFirstTangent, + public static boolean isPoint3DBelowPlane3D(Point3DReadOnly point, + Point3DReadOnly pointOnPlane, + Vector3DReadOnly planeFirstTangent, Vector3DReadOnly planeSecondTangent) { return isPoint3DAboveOrBelowPlane3D(point, pointOnPlane, planeFirstTangent, planeSecondTangent, false); @@ -6944,7 +7466,9 @@ public static Vector3D normal3DFromThreePoint3Ds(Point3DReadOnly firstPointOnPla * @param normalToPack the vector in which the result is stored. Modified. * @return whether the plane normal is properly determined. */ - public static boolean normal3DFromThreePoint3Ds(Point3DReadOnly firstPointOnPlane, Point3DReadOnly secondPointOnPlane, Point3DReadOnly thirdPointOnPlane, + public static boolean normal3DFromThreePoint3Ds(Point3DReadOnly firstPointOnPlane, + Point3DReadOnly secondPointOnPlane, + Point3DReadOnly thirdPointOnPlane, Vector3DBasics normalToPack) { double v1_x = secondPointOnPlane.getX() - firstPointOnPlane.getX(); @@ -6986,8 +7510,12 @@ public static boolean normal3DFromThreePoint3Ds(Point3DReadOnly firstPointOnPlan * Modified. * @return whether the method succeeded or not. */ - public static boolean orthogonalProjectionOnLine2D(Point2DReadOnly pointToProject, double pointOnLineX, double pointOnLineY, double lineDirectionX, - double lineDirectionY, Point2DBasics projectionToPack) + public static boolean orthogonalProjectionOnLine2D(Point2DReadOnly pointToProject, + double pointOnLineX, + double pointOnLineY, + double lineDirectionX, + double lineDirectionY, + Point2DBasics projectionToPack) { double directionLengthSquared = normSquared(lineDirectionX, lineDirectionY); @@ -7055,7 +7583,9 @@ public static Point2D orthogonalProjectionOnLine2D(Point2DReadOnly pointToProjec * Modified. * @return whether the method succeeded or not. */ - public static boolean orthogonalProjectionOnLine2D(Point2DReadOnly pointToProject, Point2DReadOnly firstPointOnLine, Point2DReadOnly secondPointOnLine, + public static boolean orthogonalProjectionOnLine2D(Point2DReadOnly pointToProject, + Point2DReadOnly firstPointOnLine, + Point2DReadOnly secondPointOnLine, Point2DBasics projectionToPack) { double pointOnLineX = firstPointOnLine.getX(); @@ -7114,7 +7644,9 @@ public static Point2D orthogonalProjectionOnLine2D(Point2DReadOnly pointToProjec * Modified. * @return whether the method succeeded or not. */ - public static boolean orthogonalProjectionOnLine2D(Point2DReadOnly pointToProject, Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection, + public static boolean orthogonalProjectionOnLine2D(Point2DReadOnly pointToProject, + Point2DReadOnly pointOnLine, + Vector2DReadOnly lineDirection, Point2DBasics projectionToPack) { return orthogonalProjectionOnLine2D(pointToProject, pointOnLine.getX(), pointOnLine.getY(), lineDirection.getX(), lineDirection.getY(), projectionToPack); @@ -7143,8 +7675,14 @@ public static boolean orthogonalProjectionOnLine2D(Point2DReadOnly pointToProjec * Modified. * @return whether the method succeeded or not. */ - public static boolean orthogonalProjectionOnLine3D(Point3DReadOnly pointToProject, double pointOnLineX, double pointOnLineY, double pointOnLineZ, - double lineDirectionX, double lineDirectionY, double lineDirectionZ, Point3DBasics projectionToPack) + public static boolean orthogonalProjectionOnLine3D(Point3DReadOnly pointToProject, + double pointOnLineX, + double pointOnLineY, + double pointOnLineZ, + double lineDirectionX, + double lineDirectionY, + double lineDirectionZ, + Point3DBasics projectionToPack) { double directionLengthSquared = normSquared(lineDirectionX, lineDirectionY, lineDirectionZ); @@ -7212,7 +7750,9 @@ public static Point3D orthogonalProjectionOnLine3D(Point3DReadOnly pointToProjec * Modified. * @return whether the method succeeded or not. */ - public static boolean orthogonalProjectionOnLine3D(Point3DReadOnly pointToProject, Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection, + public static boolean orthogonalProjectionOnLine3D(Point3DReadOnly pointToProject, + Point3DReadOnly pointOnLine, + Vector3DReadOnly lineDirection, Point3DBasics projectionToPack) { return orthogonalProjectionOnLine3D(pointToProject, @@ -7249,8 +7789,13 @@ public static boolean orthogonalProjectionOnLine3D(Point3DReadOnly pointToProjec * stored. Modified. * @return whether the method succeeded or not. */ - public static boolean orthogonalProjectionOnLineSegment2D(double pointToProjectX, double pointToProjectY, double lineSegmentStartX, double lineSegmentStartY, - double lineSegmentEndX, double lineSegmentEndY, Point2DBasics projectionToPack) + public static boolean orthogonalProjectionOnLineSegment2D(double pointToProjectX, + double pointToProjectY, + double lineSegmentStartX, + double lineSegmentStartY, + double lineSegmentEndX, + double lineSegmentEndY, + Point2DBasics projectionToPack) { double percentage = percentageAlongLineSegment2D(pointToProjectX, pointToProjectY, @@ -7296,8 +7841,12 @@ else if (percentage < 0.0) * stored. Modified. * @return whether the method succeeded or not. */ - public static boolean orthogonalProjectionOnLineSegment2D(Point2DReadOnly pointToProject, double lineSegmentStartX, double lineSegmentStartY, - double lineSegmentEndX, double lineSegmentEndY, Point2DBasics projectionToPack) + public static boolean orthogonalProjectionOnLineSegment2D(Point2DReadOnly pointToProject, + double lineSegmentStartX, + double lineSegmentStartY, + double lineSegmentEndX, + double lineSegmentEndY, + Point2DBasics projectionToPack) { return orthogonalProjectionOnLineSegment2D(pointToProject.getX(), pointToProject.getY(), @@ -7367,8 +7916,11 @@ public static Point2D orthogonalProjectionOnLineSegment2D(Point2DReadOnly pointT * stored. Modified. * @return whether the method succeeded or not. */ - public static boolean orthogonalProjectionOnLineSegment2D(double pointToProjectX, double pointToProjectY, Point2DReadOnly lineSegmentStart, - Point2DReadOnly lineSegmentEnd, Point2DBasics projectionToPack) + public static boolean orthogonalProjectionOnLineSegment2D(double pointToProjectX, + double pointToProjectY, + Point2DReadOnly lineSegmentStart, + Point2DReadOnly lineSegmentEnd, + Point2DBasics projectionToPack) { return orthogonalProjectionOnLineSegment2D(pointToProjectX, pointToProjectY, @@ -7400,7 +7952,9 @@ public static boolean orthogonalProjectionOnLineSegment2D(double pointToProjectX * stored. Modified. * @return whether the method succeeded or not. */ - public static boolean orthogonalProjectionOnLineSegment2D(Point2DReadOnly pointToProject, Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd, + public static boolean orthogonalProjectionOnLineSegment2D(Point2DReadOnly pointToProject, + Point2DReadOnly lineSegmentStart, + Point2DReadOnly lineSegmentEnd, Point2DBasics projectionToPack) { return orthogonalProjectionOnLineSegment2D(pointToProject, @@ -7436,8 +7990,13 @@ public static boolean orthogonalProjectionOnLineSegment2D(Point2DReadOnly pointT * stored. Modified. * @return whether the method succeeded or not. */ - public static boolean orthogonalProjectionOnLineSegment3D(Point3DReadOnly pointToProject, double lineSegmentStartX, double lineSegmentStartY, - double lineSegmentStartZ, double lineSegmentEndX, double lineSegmentEndY, double lineSegmentEndZ, + public static boolean orthogonalProjectionOnLineSegment3D(Point3DReadOnly pointToProject, + double lineSegmentStartX, + double lineSegmentStartY, + double lineSegmentStartZ, + double lineSegmentEndX, + double lineSegmentEndY, + double lineSegmentEndZ, Point3DBasics projectionToPack) { double percentage = percentageAlongLineSegment3D(pointToProject.getX(), @@ -7525,7 +8084,9 @@ public static Point3D orthogonalProjectionOnLineSegment3D(Point3DReadOnly pointT * stored. Modified. * @return whether the method succeeded or not. */ - public static boolean orthogonalProjectionOnLineSegment3D(Point3DReadOnly pointToProject, Point3DReadOnly lineSegmentStart, Point3DReadOnly lineSegmentEnd, + public static boolean orthogonalProjectionOnLineSegment3D(Point3DReadOnly pointToProject, + Point3DReadOnly lineSegmentStart, + Point3DReadOnly lineSegmentEnd, Point3DBasics projectionToPack) { return orthogonalProjectionOnLineSegment3D(pointToProject, @@ -7582,7 +8143,9 @@ public static Point3D orthogonalProjectionOnPlane3D(Point3DReadOnly pointToProje * Modified. * @return whether the method succeeded or not. */ - public static boolean orthogonalProjectionOnPlane3D(Point3DReadOnly pointToProject, Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal, + public static boolean orthogonalProjectionOnPlane3D(Point3DReadOnly pointToProject, + Point3DReadOnly pointOnPlane, + Vector3DReadOnly planeNormal, Point3DBasics projectionToPack) { return orthogonalProjectionOnPlane3D(pointToProject.getX(), pointToProject.getY(), pointToProject.getZ(), pointOnPlane, planeNormal, projectionToPack); @@ -7608,7 +8171,11 @@ public static boolean orthogonalProjectionOnPlane3D(Point3DReadOnly pointToProje * Modified. * @return whether the method succeeded or not. */ - public static boolean orthogonalProjectionOnPlane3D(double x, double y, double z, Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal, + public static boolean orthogonalProjectionOnPlane3D(double x, + double y, + double z, + Point3DReadOnly pointOnPlane, + Vector3DReadOnly planeNormal, Point3DBasics projectionToPack) { double normalMagnitude = planeNormal.length(); @@ -7650,8 +8217,14 @@ public static boolean orthogonalProjectionOnPlane3D(double x, double y, double z * @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 percentageOfIntersectionBetweenTwoLine2Ds(double pointOnLine1x, double pointOnLine1y, double lineDirection1x, double lineDirection1y, - double pointOnLine2x, double pointOnLine2y, double lineDirection2x, double lineDirection2y) + public static double percentageOfIntersectionBetweenTwoLine2Ds(double pointOnLine1x, + double pointOnLine1y, + double lineDirection1x, + double lineDirection1y, + double pointOnLine2x, + double pointOnLine2y, + double lineDirection2x, + double lineDirection2y) { // We solve for x the problem of the form: A * x = b // A * x = b @@ -7715,7 +8288,9 @@ public static double percentageOfIntersectionBetweenTwoLine2Ds(double pointOnLin * @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 percentageOfIntersectionBetweenTwoLine2Ds(Point2DReadOnly pointOnLine1, Vector2DReadOnly lineDirection1, Point2DReadOnly pointOnLine2, + public static double percentageOfIntersectionBetweenTwoLine2Ds(Point2DReadOnly pointOnLine1, + Vector2DReadOnly lineDirection1, + Point2DReadOnly pointOnLine2, Vector2DReadOnly lineDirection2) { return percentageOfIntersectionBetweenTwoLine2Ds(pointOnLine1.getX(), @@ -7753,8 +8328,10 @@ public static double percentageOfIntersectionBetweenTwoLine2Ds(Point2DReadOnly p * @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. */ - public static double percentageOfIntersectionBetweenLineSegment2DAndLine2D(Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd, - Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection) + public static double percentageOfIntersectionBetweenLineSegment2DAndLine2D(Point2DReadOnly lineSegmentStart, + Point2DReadOnly lineSegmentEnd, + Point2DReadOnly pointOnLine, + Vector2DReadOnly lineDirection) { double lineSegmentStartX = lineSegmentStart.getX(); double lineSegmentStartY = lineSegmentStart.getY(); @@ -7857,7 +8434,11 @@ public static double percentageAlongLine2D(double pointX, double pointY, Point2D * @return the computed percentage along the line representing where the point projection is * located. */ - public static double percentageAlongLine2D(double pointX, double pointY, double pointOnLineX, double pointOnLineY, double lineDirectionX, + public static double percentageAlongLine2D(double pointX, + double pointY, + double pointOnLineX, + double pointOnLineY, + double lineDirectionX, double lineDirectionY) { double lengthSquared = normSquared(lineDirectionX, lineDirectionY); @@ -7903,7 +8484,11 @@ public static double percentageAlongLine2D(double pointX, double pointY, double * @return the computed percentage along the line segment representing where the point projection is * located. */ - public static double percentageAlongLineSegment2D(double pointX, double pointY, double lineSegmentStartX, double lineSegmentStartY, double lineSegmentEndX, + public static double percentageAlongLineSegment2D(double pointX, + double pointY, + double lineSegmentStartX, + double lineSegmentStartY, + double lineSegmentEndX, double lineSegmentEndY) { double lineSegmentDx = lineSegmentEndX - lineSegmentStartX; @@ -8083,8 +8668,15 @@ public static double percentageAlongLine3D(double pointX, double pointY, double * @return the computed percentage along the line representing where the point projection is * located. */ - public static double percentageAlongLine3D(double pointX, double pointY, double pointZ, double pointOnLineX, double pointOnLineY, double pointOnLineZ, - double lineDirectionX, double lineDirectionY, double lineDirectionZ) + public static double percentageAlongLine3D(double pointX, + double pointY, + double pointZ, + double pointOnLineX, + double pointOnLineY, + double pointOnLineZ, + double lineDirectionX, + double lineDirectionY, + double lineDirectionZ) { double lengthSquared = normSquared(lineDirectionX, lineDirectionY, lineDirectionZ); @@ -8133,8 +8725,15 @@ public static double percentageAlongLine3D(double pointX, double pointY, double * @return the computed percentage along the line segment representing where the point projection is * located. */ - public static double percentageAlongLineSegment3D(double pointX, double pointY, double pointZ, double lineSegmentStartX, double lineSegmentStartY, - double lineSegmentStartZ, double lineSegmentEndX, double lineSegmentEndY, double lineSegmentEndZ) + public static double percentageAlongLineSegment3D(double pointX, + double pointY, + double pointZ, + double lineSegmentStartX, + double lineSegmentStartY, + double lineSegmentStartZ, + double lineSegmentEndX, + double lineSegmentEndY, + double lineSegmentEndZ) { double lineSegmentDx = lineSegmentEndX - lineSegmentStartX; double lineSegmentDy = lineSegmentEndY - lineSegmentStartY; @@ -8182,7 +8781,10 @@ public static double percentageAlongLineSegment3D(double pointX, double pointY, * @return the computed percentage along the line segment representing where the point projection is * located. */ - public static double percentageAlongLineSegment3D(double pointX, double pointY, double pointZ, Point3DReadOnly lineSegmentStart, + public static double percentageAlongLineSegment3D(double pointX, + double pointY, + double pointZ, + Point3DReadOnly lineSegmentStart, Point3DReadOnly lineSegmentEnd) { return percentageAlongLineSegment3D(pointX, @@ -8249,7 +8851,9 @@ public static double percentageAlongLineSegment3D(Point3DReadOnly point, Point3D * Modified. * @return whether the perpendicular bisector could be determined or not. */ - public static boolean perpendicularBisector2D(Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd, Point2DBasics bisectorStartToPack, + public static boolean perpendicularBisector2D(Point2DReadOnly lineSegmentStart, + Point2DReadOnly lineSegmentEnd, + Point2DBasics bisectorStartToPack, Vector2DBasics bisectorDirectionToPack) { if (lineSegmentStart.distance(lineSegmentEnd) < ONE_TRILLIONTH) @@ -8293,7 +8897,8 @@ public static boolean perpendicularBisector2D(Point2DReadOnly lineSegmentStart, * perpendicular bisector segment will be positioned. * @return a list containing the two endpoints of the perpendicular bisector segment. */ - public static List perpendicularBisectorSegment2D(Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd, + public static List perpendicularBisectorSegment2D(Point2DReadOnly lineSegmentStart, + Point2DReadOnly lineSegmentEnd, double bisectorSegmentHalfLength) { Point2D bisectorSegmentStart = new Point2D(); @@ -8340,8 +8945,11 @@ public static List perpendicularBisectorSegment2D(Point2DReadOnly lineS * computed. Modified. * @return whether the perpendicular bisector could be determined or not. */ - public static boolean perpendicularBisectorSegment2D(Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd, double bisectorSegmentHalfLength, - Point2DBasics bisectorSegmentStartToPack, Point2DBasics bisectorSegmentEndToPack) + public static boolean perpendicularBisectorSegment2D(Point2DReadOnly lineSegmentStart, + Point2DReadOnly lineSegmentEnd, + double bisectorSegmentHalfLength, + Point2DBasics bisectorSegmentStartToPack, + Point2DBasics bisectorSegmentEndToPack) { if (lineSegmentStart.distance(lineSegmentEnd) < ONE_TRILLIONTH) return false; @@ -8423,7 +9031,9 @@ public static void perpendicularVector2D(Vector2DReadOnly vector, Vector2DBasics * @return the vector perpendicular to the line and pointing to the {@code point}, or {@code null} * when the method fails. */ - public static Vector3D perpendicularVector3DFromLine3DToPoint3D(Point3DReadOnly point, Point3DReadOnly firstPointOnLine, Point3DReadOnly secondPointOnLine, + public static Vector3D perpendicularVector3DFromLine3DToPoint3D(Point3DReadOnly point, + Point3DReadOnly firstPointOnLine, + Point3DReadOnly secondPointOnLine, Point3DBasics orthogonalProjectionToPack) { Vector3D perpendicularVector = new Vector3D(); @@ -8460,8 +9070,11 @@ public static Vector3D perpendicularVector3DFromLine3DToPoint3D(Point3DReadOnly * {@code null}. * @return {@code true} if the method succeeded, {@code false} otherwise. */ - public static boolean perpendicularVector3DFromLine3DToPoint3D(Point3DReadOnly point, Point3DReadOnly firstPointOnLine, Point3DReadOnly secondPointOnLine, - Point3DBasics orthogonalProjectionToPack, Vector3DBasics perpendicularVectorToPack) + public static boolean perpendicularVector3DFromLine3DToPoint3D(Point3DReadOnly point, + Point3DReadOnly firstPointOnLine, + Point3DReadOnly secondPointOnLine, + Point3DBasics orthogonalProjectionToPack, + Vector3DBasics perpendicularVectorToPack) { double lineDirectionX = secondPointOnLine.getX() - firstPointOnLine.getX(); double lineDirectionY = secondPointOnLine.getY() - firstPointOnLine.getY(); @@ -8704,14 +9317,23 @@ public static double signedDistanceFromPoint2DToLine2D(Point2DReadOnly point, Po * @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. */ - public static double signedDistanceFromPoint2DToLine2D(double pointX, double pointY, double pointOnLineX, double pointOnLineY, double lineDirectionX, + public static double signedDistanceFromPoint2DToLine2D(double pointX, + double pointY, + double pointOnLineX, + double pointOnLineY, + double lineDirectionX, double lineDirectionY) { return signedDistanceFromPoint2DToLine2D(pointX, pointY, pointOnLineX, pointOnLineY, lineDirectionX, lineDirectionY, false); } - private static double signedDistanceFromPoint2DToLine2D(double pointX, double pointY, double pointOnLineX, double pointOnLineY, double lineDirectionX, - double lineDirectionY, boolean isDirectionUnitary) + private static double signedDistanceFromPoint2DToLine2D(double pointX, + double pointY, + double pointOnLineX, + double pointOnLineY, + double lineDirectionX, + double lineDirectionY, + boolean isDirectionUnitary) { double dx = pointX - pointOnLineX; double dy = pointY - pointOnLineY; @@ -8751,7 +9373,10 @@ private static double signedDistanceFromPoint2DToLine2D(double pointX, double po * @param sphere3DPositionToPack the point used to store the result. Modified. * @return whether the sphere position was successfully or not. */ - public static boolean sphere3DPositionFromThreePoints(Point3DReadOnly p1, Point3DReadOnly p2, Point3DReadOnly p3, double sphere3DRadius, + public static boolean sphere3DPositionFromThreePoints(Point3DReadOnly p1, + Point3DReadOnly p2, + Point3DReadOnly p3, + double sphere3DRadius, Point3DBasics sphere3DPositionToPack) { double ux = p2.getX() - p1.getX(); @@ -8806,8 +9431,11 @@ public static boolean sphere3DPositionFromThreePoints(Point3DReadOnly p1, Point3 * @param ccwAngleAboutNormalAtTopVertex the angle at B from the the leg BA to the leg BC. * @param topVertexBToPack the missing vertex B. Modified. */ - public static void topVertex3DOfIsoscelesTriangle3D(Point3DReadOnly baseVertexA, Point3DReadOnly baseVertexC, Vector3DReadOnly trianglePlaneNormal, - double ccwAngleAboutNormalAtTopVertex, Point3DBasics topVertexBToPack) + public static void topVertex3DOfIsoscelesTriangle3D(Point3DReadOnly baseVertexA, + Point3DReadOnly baseVertexC, + Vector3DReadOnly trianglePlaneNormal, + double ccwAngleAboutNormalAtTopVertex, + Point3DBasics topVertexBToPack) { double baseEdgeACx = baseVertexC.getX() - baseVertexA.getX(); double baseEdgeACy = baseVertexC.getY() - baseVertexA.getY(); diff --git a/src/main/java/us/ihmc/euclid/matrix/Matrix3D.java b/src/main/java/us/ihmc/euclid/matrix/Matrix3D.java index 5cab69f49..0c6422406 100644 --- a/src/main/java/us/ihmc/euclid/matrix/Matrix3D.java +++ b/src/main/java/us/ihmc/euclid/matrix/Matrix3D.java @@ -112,11 +112,11 @@ public void set(double m00, double m01, double m02, double m10, double m11, doub this.m00 = m00; this.m01 = m01; this.m02 = m02; - + this.m10 = m10; this.m11 = m11; this.m12 = m12; - + this.m20 = m20; this.m21 = m21; this.m22 = m22; diff --git a/src/main/java/us/ihmc/euclid/rotationConversion/AxisAngleConversion.java b/src/main/java/us/ihmc/euclid/rotationConversion/AxisAngleConversion.java index 35f2e2aff..9cd058b4c 100644 --- a/src/main/java/us/ihmc/euclid/rotationConversion/AxisAngleConversion.java +++ b/src/main/java/us/ihmc/euclid/rotationConversion/AxisAngleConversion.java @@ -92,7 +92,15 @@ public static void convertMatrixToAxisAngle(RotationMatrixReadOnly rotationMatri * @param m22 the 3rd row 3rd column coefficient of the rotation matrix. * @param axisAngleToPack the axis-angle in which the result is stored. Modified. */ - public static void convertMatrixToAxisAngle(double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, double m22, + public static void convertMatrixToAxisAngle(double m00, + double m01, + double m02, + double m10, + double m11, + double m12, + double m20, + double m21, + double m22, AxisAngleBasics axisAngleToPack) { if (EuclidCoreTools.containsNaN(m00, m01, m02, m10, m11, m12, m20, m21, m22)) diff --git a/src/main/java/us/ihmc/euclid/rotationConversion/QuaternionConversion.java b/src/main/java/us/ihmc/euclid/rotationConversion/QuaternionConversion.java index c1a7737f2..0c99a8b06 100644 --- a/src/main/java/us/ihmc/euclid/rotationConversion/QuaternionConversion.java +++ b/src/main/java/us/ihmc/euclid/rotationConversion/QuaternionConversion.java @@ -194,7 +194,15 @@ public static void convertMatrixToQuaternion(RotationMatrixReadOnly rotationMatr * @param m22 the 3rd row 3rd column coefficient of the rotation matrix. * @param quaternionToPack the quaternion in which the result is stored. */ - public static void convertMatrixToQuaternion(double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, double m22, + public static void convertMatrixToQuaternion(double m00, + double m01, + double m02, + double m10, + double m11, + double m12, + double m20, + double m21, + double m22, QuaternionBasics quaternionToPack) { if (EuclidCoreTools.containsNaN(m00, m01, m02, m10, m11, m12, m20, m21, m22)) diff --git a/src/main/java/us/ihmc/euclid/rotationConversion/RotationVectorConversion.java b/src/main/java/us/ihmc/euclid/rotationConversion/RotationVectorConversion.java index aeb12ea94..2340160d1 100644 --- a/src/main/java/us/ihmc/euclid/rotationConversion/RotationVectorConversion.java +++ b/src/main/java/us/ihmc/euclid/rotationConversion/RotationVectorConversion.java @@ -220,7 +220,15 @@ public static void convertMatrixToRotationVector(RotationMatrixReadOnly rotation * conversion. * @param rotationVectorToPack the vector in which the result is stored. Modified. */ - static void convertMatrixToRotationVectorImpl(double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, double m22, + static void convertMatrixToRotationVectorImpl(double m00, + double m01, + double m02, + double m10, + double m11, + double m12, + double m20, + double m21, + double m22, Vector3DBasics rotationVectorToPack) { if (EuclidCoreTools.containsNaN(m00, m01, m02, m10, m11, m12, m20, m21, m22)) diff --git a/src/main/java/us/ihmc/euclid/rotationConversion/YawPitchRollConversion.java b/src/main/java/us/ihmc/euclid/rotationConversion/YawPitchRollConversion.java index c29b00c56..60c7ffd27 100644 --- a/src/main/java/us/ihmc/euclid/rotationConversion/YawPitchRollConversion.java +++ b/src/main/java/us/ihmc/euclid/rotationConversion/YawPitchRollConversion.java @@ -269,7 +269,15 @@ public static void convertMatrixToYawPitchRoll(RotationMatrixReadOnly rotationMa * @param m22 the 3rd row 3rd column coefficient of the rotation matrix. * @param yawPitchRollToPack the yaw-pitch-roll used to store the orientation. Modified. */ - public static void convertMatrixToYawPitchRoll(double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, double m22, + public static void convertMatrixToYawPitchRoll(double m00, + double m01, + double m02, + double m10, + double m11, + double m12, + double m20, + double m21, + double m22, YawPitchRollBasics yawPitchRollToPack) { double yaw = computeYawImpl(m00, m10); diff --git a/src/main/java/us/ihmc/euclid/tools/AxisAngleTools.java b/src/main/java/us/ihmc/euclid/tools/AxisAngleTools.java index a5512a92d..bbc7cd112 100644 --- a/src/main/java/us/ihmc/euclid/tools/AxisAngleTools.java +++ b/src/main/java/us/ihmc/euclid/tools/AxisAngleTools.java @@ -182,7 +182,9 @@ public static void transform(AxisAngleReadOnly axisAngle, Tuple2DReadOnly tupleO * @throws NotAMatrix2DException if {@code checkIfTransformInXYPlane == true} and the axis-angle * does not represent a transformation in the XY plane. */ - public static void inverseTransform(AxisAngleReadOnly axisAngle, Tuple2DReadOnly tupleOriginal, Tuple2DBasics tupleTransformed, + public static void inverseTransform(AxisAngleReadOnly axisAngle, + Tuple2DReadOnly tupleOriginal, + Tuple2DBasics tupleTransformed, boolean checkIfTransformInXYPlane) { transformImpl(axisAngle, true, tupleOriginal, tupleTransformed, checkIfTransformInXYPlane); @@ -207,7 +209,10 @@ public static void inverseTransform(AxisAngleReadOnly axisAngle, Tuple2DReadOnly * a transformation in the XY plane. * @throws NotAMatrix2DException if {@code checkIfTransformInXYPlane == true} and the axis-angle */ - private static void transformImpl(AxisAngleReadOnly axisAngle, boolean negateAngle, Tuple2DReadOnly tupleOriginal, Tuple2DBasics tupleTransformed, + private static void transformImpl(AxisAngleReadOnly axisAngle, + boolean negateAngle, + Tuple2DReadOnly tupleOriginal, + Tuple2DBasics tupleTransformed, boolean checkIfTransformInXYPlane) { if (checkIfTransformInXYPlane) @@ -488,7 +493,8 @@ public static void transform(AxisAngleReadOnly axisAngle, RotationMatrixReadOnly * @param rotationMatrixOriginal the rotation matrix to transform. Not modified. * @param rotationMatrixTransformed the rotation matrix in which the result is stored. Modified. */ - public static void inverseTransform(AxisAngleReadOnly axisAngle, RotationMatrixReadOnly rotationMatrixOriginal, + public static void inverseTransform(AxisAngleReadOnly axisAngle, + RotationMatrixReadOnly rotationMatrixOriginal, RotationMatrixBasics rotationMatrixTransformed) { RotationMatrixTools.multiply(axisAngle, true, rotationMatrixOriginal, false, rotationMatrixTransformed); @@ -528,7 +534,10 @@ public static void multiply(AxisAngleReadOnly aa1, AxisAngleReadOnly aa2, AxisAn * @param inverse2 whether the second orientation should be inverted in the multiplication. * @param axisAngleToPack the axis-angle in which the result is stored. Modified. */ - public static void multiply(Orientation3DReadOnly orientation1, boolean inverse1, Orientation3DReadOnly orientation2, boolean inverse2, + public static void multiply(Orientation3DReadOnly orientation1, + boolean inverse1, + Orientation3DReadOnly orientation2, + boolean inverse2, AxisAngleBasics axisAngleToPack) { if (orientation1 instanceof AxisAngleReadOnly) @@ -581,7 +590,10 @@ public static void multiply(Orientation3DReadOnly orientation1, boolean inverse1 * @param inverse2 whether the second orientation should be inverted in the multiplication. * @param axisAngleToPack the axis-angle in which the result is stored. Modified. */ - public static void multiply(Orientation3DReadOnly orientation1, boolean inverse1, AxisAngleReadOnly orientation2, boolean inverse2, + public static void multiply(Orientation3DReadOnly orientation1, + boolean inverse1, + AxisAngleReadOnly orientation2, + boolean inverse2, AxisAngleBasics axisAngleToPack) { if (orientation1 instanceof AxisAngleReadOnly) @@ -621,7 +633,10 @@ public static void multiply(Orientation3DReadOnly orientation1, boolean inverse1 * @param inverse2 whether the second orientation should be inverted in the multiplication. * @param axisAngleToPack the axis-angle in which the result is stored. Modified. */ - public static void multiply(AxisAngleReadOnly orientation1, boolean inverse1, Orientation3DReadOnly orientation2, boolean inverse2, + public static void multiply(AxisAngleReadOnly orientation1, + boolean inverse1, + Orientation3DReadOnly orientation2, + boolean inverse2, AxisAngleBasics axisAngleToPack) { if (orientation2 instanceof AxisAngleReadOnly) @@ -730,8 +745,17 @@ private static void multiplyImpl(AxisAngleReadOnly aa1, boolean inverse1, AxisAn multiplyImpl(alpha, u1x, u1y, u1z, inverse1, beta, u2x, u2y, u2z, inverse2, axisAngleToPack); } - private static void multiplyImpl(double alpha, double u1x, double u1y, double u1z, boolean inverse1, double beta, double u2x, double u2y, double u2z, - boolean inverse2, AxisAngleBasics axisAngleToPack) + private static void multiplyImpl(double alpha, + double u1x, + double u1y, + double u1z, + boolean inverse1, + double beta, + double u2x, + double u2y, + double u2z, + boolean inverse2, + AxisAngleBasics axisAngleToPack) { double axisNorm1 = EuclidCoreTools.fastNorm(u1x, u1y, u1z); if (axisNorm1 < EPS) diff --git a/src/main/java/us/ihmc/euclid/tools/EuclidCoreFactories.java b/src/main/java/us/ihmc/euclid/tools/EuclidCoreFactories.java index 0e8b00c22..38030e2e4 100644 --- a/src/main/java/us/ihmc/euclid/tools/EuclidCoreFactories.java +++ b/src/main/java/us/ihmc/euclid/tools/EuclidCoreFactories.java @@ -1324,7 +1324,8 @@ public static Point2DBasics newObservablePoint2DBasics(ObjDoubleConsumer * point interface. * @return the observable point. */ - public static Point2DBasics newObservablePoint2DBasics(ObjDoubleConsumer valueChangedListener, Consumer valueAccessedListener, + public static Point2DBasics newObservablePoint2DBasics(ObjDoubleConsumer valueChangedListener, + Consumer valueAccessedListener, Point2DBasics source) { return new Point2DBasics() @@ -1446,7 +1447,8 @@ public static Point3DBasics newObservablePoint3DBasics(ObjDoubleConsumer * point interface. * @return the observable point. */ - public static Point3DBasics newObservablePoint3DBasics(ObjDoubleConsumer valueChangedListener, Consumer valueAccessedListener, + public static Point3DBasics newObservablePoint3DBasics(ObjDoubleConsumer valueChangedListener, + Consumer valueAccessedListener, Point3DBasics source) { @@ -1586,7 +1588,8 @@ public static Vector2DBasics newObservableVector2DBasics(ObjDoubleConsumer valueChangedListener, Consumer valueAccessedListener, + public static Vector2DBasics newObservableVector2DBasics(ObjDoubleConsumer valueChangedListener, + Consumer valueAccessedListener, Vector2DBasics source) { return new Vector2DBasics() @@ -1708,7 +1711,8 @@ public static Vector3DBasics newObservableVector3DBasics(ObjDoubleConsumer valueChangedListener, Consumer valueAccessedListener, + public static Vector3DBasics newObservableVector3DBasics(ObjDoubleConsumer valueChangedListener, + Consumer valueAccessedListener, Vector3DBasics source) { return new Vector3DBasics() @@ -1847,7 +1851,8 @@ public static UnitVector2DBasics newObservableUnitVector2DBasics(ObjDoubleConsum * vector interface. * @return the observable vector. */ - public static UnitVector2DBasics newObservableUnitVector2DBasics(ObjDoubleConsumer valueChangedListener, Consumer valueAccessedListener, + public static UnitVector2DBasics newObservableUnitVector2DBasics(ObjDoubleConsumer valueChangedListener, + Consumer valueAccessedListener, UnitVector2DBasics source) { return new UnitVector2DBasics() @@ -2044,7 +2049,8 @@ public static UnitVector3DBasics newObservableUnitVector3DBasics(ObjDoubleConsum * vector interface. * @return the observable vector. */ - public static UnitVector3DBasics newObservableUnitVector3DBasics(ObjDoubleConsumer valueChangedListener, Consumer valueAccessedListener, + public static UnitVector3DBasics newObservableUnitVector3DBasics(ObjDoubleConsumer valueChangedListener, + Consumer valueAccessedListener, UnitVector3DBasics source) { return new UnitVector3DBasics() @@ -2269,7 +2275,8 @@ public static RotationMatrixBasics newObservableRotationMatrixBasics(Runnable va * linked rotation matrix interface. * @return the observable rotation matrix. */ - public static RotationMatrixBasics newObservableRotationMatrixBasics(Runnable valueChangedListener, BiConsumer valueAccessedListener, + public static RotationMatrixBasics newObservableRotationMatrixBasics(Runnable valueChangedListener, + BiConsumer valueAccessedListener, RotationMatrixBasics source) { return new RotationMatrixBasics() diff --git a/src/main/java/us/ihmc/euclid/tools/EuclidCoreIOTools.java b/src/main/java/us/ihmc/euclid/tools/EuclidCoreIOTools.java index 8cd134187..fd3c51701 100644 --- a/src/main/java/us/ihmc/euclid/tools/EuclidCoreIOTools.java +++ b/src/main/java/us/ihmc/euclid/tools/EuclidCoreIOTools.java @@ -498,7 +498,15 @@ public static String getMatrix3DString(double m00, double m01, double m02, doubl * @param m22 the 3rd row 3rd column coefficient of the matrix. * @return the representative {@code String}. */ - public static String getMatrix3DString(String format, double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, + public static String getMatrix3DString(String format, + double m00, + double m01, + double m02, + double m10, + double m11, + double m12, + double m20, + double m21, double m22) { String ret = getStringOf("/", " \\\n", ", ", format, m00, m01, m02); @@ -795,7 +803,10 @@ public static String getArrayString(String separator, T[] array, Function String getCollectionString(String prefix, String suffix, String separator, Collection collection, + public static String getCollectionString(String prefix, + String suffix, + String separator, + Collection collection, Function elementToStringFunction) { if (collection == null) diff --git a/src/main/java/us/ihmc/euclid/tools/EuclidCoreTestTools.java b/src/main/java/us/ihmc/euclid/tools/EuclidCoreTestTools.java index 221f56838..05a24e376 100644 --- a/src/main/java/us/ihmc/euclid/tools/EuclidCoreTestTools.java +++ b/src/main/java/us/ihmc/euclid/tools/EuclidCoreTestTools.java @@ -238,7 +238,10 @@ public static void assertYawPitchRollGeometricallyEquals(String messagePrefix, Y * @throws AssertionError if the two yaw-pitch-roll do not represent the same geometry. If only one * of the arguments is equal to {@code null}. */ - public static void assertYawPitchRollGeometricallyEquals(String messagePrefix, YawPitchRollReadOnly expected, YawPitchRollReadOnly actual, double epsilon, + public static void assertYawPitchRollGeometricallyEquals(String messagePrefix, + YawPitchRollReadOnly expected, + YawPitchRollReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -317,7 +320,10 @@ public static void assertRotationVectorGeometricallyEquals(String messagePrefix, * @throws AssertionError if the two rotation vectors do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ - public static void assertRotationVectorGeometricallyEquals(String messagePrefix, Vector3DReadOnly expected, Vector3DReadOnly actual, double epsilon, + public static void assertRotationVectorGeometricallyEquals(String messagePrefix, + Vector3DReadOnly expected, + Vector3DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -961,7 +967,9 @@ public static void assertRotationMatrixGeometricallyEquals(RotationMatrixReadOnl * @throws AssertionError if the two rotation matrices do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ - public static void assertRotationMatrixGeometricallyEquals(String messagePrefix, RotationMatrixReadOnly expected, RotationMatrixReadOnly actual, + public static void assertRotationMatrixGeometricallyEquals(String messagePrefix, + RotationMatrixReadOnly expected, + RotationMatrixReadOnly actual, double epsilon) { assertRotationMatrixGeometricallyEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); @@ -982,8 +990,11 @@ public static void assertRotationMatrixGeometricallyEquals(String messagePrefix, * @throws AssertionError if the two rotation matrices do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ - public static void assertRotationMatrixGeometricallyEquals(String messagePrefix, RotationMatrixReadOnly expected, RotationMatrixReadOnly actual, - double epsilon, String format) + public static void assertRotationMatrixGeometricallyEquals(String messagePrefix, + RotationMatrixReadOnly expected, + RotationMatrixReadOnly actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -1353,7 +1364,10 @@ public static void assertQuaternionGeometricallyEquals(String messagePrefix, Qua * @throws AssertionError if the two quaternions do not represent the same geometry. If only one of * the arguments is equal to {@code null}. */ - public static void assertQuaternionGeometricallyEquals(String messagePrefix, QuaternionReadOnly expected, QuaternionReadOnly actual, double epsilon, + public static void assertQuaternionGeometricallyEquals(String messagePrefix, + QuaternionReadOnly expected, + QuaternionReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -1482,7 +1496,10 @@ public static void assertAxisAngleGeometricallyEquals(String messagePrefix, Axis * @throws AssertionError if the two axis-angles do not represent the same geometry. If only one of * the arguments is equal to {@code null}. */ - public static void assertAxisAngleGeometricallyEquals(String messagePrefix, AxisAngleReadOnly expected, AxisAngleReadOnly actual, double epsilon, + public static void assertAxisAngleGeometricallyEquals(String messagePrefix, + AxisAngleReadOnly expected, + AxisAngleReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -1549,7 +1566,10 @@ public static void assertOrientation2DEquals(String messagePrefix, Orientation2D * @throws AssertionError if the two orientation 2Ds are not equal. If only one of the arguments is * equal to {@code null}. */ - public static void assertOrientation2DEquals(String messagePrefix, Orientation2DReadOnly expected, Orientation2DReadOnly actual, double epsilon, + public static void assertOrientation2DEquals(String messagePrefix, + Orientation2DReadOnly expected, + Orientation2DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -1614,7 +1634,10 @@ public static void assertOrientation2DGeometricallyEquals(String messagePrefix, * @throws AssertionError if the two orientation 2Ds do not represent the same geometry. If only one * of the arguments is equal to {@code null}. */ - public static void assertOrientation2DGeometricallyEquals(String messagePrefix, Orientation2DReadOnly expected, Orientation2DReadOnly actual, double epsilon, + public static void assertOrientation2DGeometricallyEquals(String messagePrefix, + Orientation2DReadOnly expected, + Orientation2DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -2110,7 +2133,10 @@ public static void assertRigidBodyTransformEquals(String messagePrefix, RigidBod * @throws AssertionError if the two rigid-body transforms are not equal. If only one of the * arguments is equal to {@code null}. */ - public static void assertRigidBodyTransformEquals(String messagePrefix, RigidBodyTransform expected, RigidBodyTransform actual, double epsilon, + public static void assertRigidBodyTransformEquals(String messagePrefix, + RigidBodyTransform expected, + RigidBodyTransform actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -2178,7 +2204,10 @@ public static void assertRigidBodyTransformGeometricallyEquals(String messagePre * @throws AssertionError if the two rigid-body transforms do not represent the same geometry. If * only one of the arguments is equal to {@code null}. */ - public static void assertRigidBodyTransformGeometricallyEquals(String messagePrefix, RigidBodyTransform expected, RigidBodyTransform actual, double epsilon, + public static void assertRigidBodyTransformGeometricallyEquals(String messagePrefix, + RigidBodyTransform expected, + RigidBodyTransform actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -2225,7 +2254,9 @@ public static void assertQuaternionBasedTransformEquals(QuaternionBasedTransform * @throws AssertionError if the two quaternion-based transforms are not equal. If only one of the * arguments is equal to {@code null}. */ - public static void assertQuaternionBasedTransformEquals(String messagePrefix, QuaternionBasedTransform expected, QuaternionBasedTransform actual, + public static void assertQuaternionBasedTransformEquals(String messagePrefix, + QuaternionBasedTransform expected, + QuaternionBasedTransform actual, double epsilon) { assertQuaternionBasedTransformEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); @@ -2247,8 +2278,11 @@ public static void assertQuaternionBasedTransformEquals(String messagePrefix, Qu * @throws AssertionError if the two quaternion-based transforms are not equal. If only one of the * arguments is equal to {@code null}. */ - public static void assertQuaternionBasedTransformEquals(String messagePrefix, QuaternionBasedTransform expected, QuaternionBasedTransform actual, - double epsilon, String format) + public static void assertQuaternionBasedTransformEquals(String messagePrefix, + QuaternionBasedTransform expected, + QuaternionBasedTransform actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -2294,8 +2328,10 @@ public static void assertQuaternionBasedTransformGeometricallyEquals(QuaternionB * @throws AssertionError if the two quaternion-based transforms do not represent the same geometry. * If only one of the arguments is equal to {@code null}. */ - public static void assertQuaternionBasedTransformGeometricallyEquals(String messagePrefix, QuaternionBasedTransform expected, - QuaternionBasedTransform actual, double epsilon) + public static void assertQuaternionBasedTransformGeometricallyEquals(String messagePrefix, + QuaternionBasedTransform expected, + QuaternionBasedTransform actual, + double epsilon) { assertQuaternionBasedTransformGeometricallyEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); } @@ -2316,8 +2352,11 @@ public static void assertQuaternionBasedTransformGeometricallyEquals(String mess * @throws AssertionError if the two quaternion-based transforms do not represent the same geometry. * If only one of the arguments is equal to {@code null}. */ - public static void assertQuaternionBasedTransformGeometricallyEquals(String messagePrefix, QuaternionBasedTransform expected, - QuaternionBasedTransform actual, double epsilon, String format) + public static void assertQuaternionBasedTransformGeometricallyEquals(String messagePrefix, + QuaternionBasedTransform expected, + QuaternionBasedTransform actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -2384,7 +2423,10 @@ public static void assertAffineTransformEquals(String messagePrefix, AffineTrans * @throws AssertionError if the two affine transforms are not equal. If only one of the arguments * is equal to {@code null}. */ - public static void assertAffineTransformEquals(String messagePrefix, AffineTransformReadOnly expected, AffineTransformReadOnly actual, double epsilon, + public static void assertAffineTransformEquals(String messagePrefix, + AffineTransformReadOnly expected, + AffineTransformReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -2429,7 +2471,9 @@ public static void assertAffineTransformGeometricallyEquals(AffineTransformReadO * @throws AssertionError if the two affine transforms do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ - public static void assertAffineTransformGeometricallyEquals(String messagePrefix, AffineTransformReadOnly expected, AffineTransformReadOnly actual, + public static void assertAffineTransformGeometricallyEquals(String messagePrefix, + AffineTransformReadOnly expected, + AffineTransformReadOnly actual, double epsilon) { assertAffineTransformGeometricallyEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); @@ -2450,8 +2494,11 @@ public static void assertAffineTransformGeometricallyEquals(String messagePrefix * @throws AssertionError if the two affine transforms do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ - public static void assertAffineTransformGeometricallyEquals(String messagePrefix, AffineTransformReadOnly expected, AffineTransformReadOnly actual, - double epsilon, String format) + public static void assertAffineTransformGeometricallyEquals(String messagePrefix, + AffineTransformReadOnly expected, + AffineTransformReadOnly actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -2580,7 +2627,10 @@ private static void throwNotEqualAssertionError(String messagePrefix, YawPitchRo throwNotEqualAssertionError(messagePrefix, expectedAsString, actualAsString); } - private static void throwNotEqualAssertionError(String messagePrefix, YawPitchRollReadOnly expected, YawPitchRollReadOnly actual, String difference, + private static void throwNotEqualAssertionError(String messagePrefix, + YawPitchRollReadOnly expected, + YawPitchRollReadOnly actual, + String difference, String format) { String expectedAsString = getYawPitchRollString(format, expected); diff --git a/src/main/java/us/ihmc/euclid/tools/Matrix3DFeatures.java b/src/main/java/us/ihmc/euclid/tools/Matrix3DFeatures.java index 787a6ae06..d0085bffb 100644 --- a/src/main/java/us/ihmc/euclid/tools/Matrix3DFeatures.java +++ b/src/main/java/us/ihmc/euclid/tools/Matrix3DFeatures.java @@ -374,7 +374,15 @@ public static boolean isRotationMatrix(double m00, double m01, double m02, doubl * @param epsilon the tolerance as shown above. * @return {@code true} if the given matrix is a rotation matrix, {@code false} otherwise. */ - public static boolean isRotationMatrix(double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, double m22, + public static boolean isRotationMatrix(double m00, + double m01, + double m02, + double m10, + double m11, + double m12, + double m20, + double m21, + double m22, double epsilon) { double xyDot = m00 * m10 + m01 * m11 + m02 * m12; @@ -505,7 +513,15 @@ public static boolean isMatrixSkewSymmetric(double m00, double m01, double m02, * @param epsilon the tolerance used as shown above. * @return {@code true} if the matrix is skew symmetric, {@code false} otherwise. */ - public static boolean isMatrixSkewSymmetric(double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, double m22, + public static boolean isMatrixSkewSymmetric(double m00, + double m01, + double m02, + double m10, + double m11, + double m12, + double m20, + double m21, + double m22, double epsilon) { if (Math.abs(m00) <= epsilon && Math.abs(m11) <= epsilon && Math.abs(m22) <= epsilon) @@ -577,7 +593,15 @@ public static boolean isMatrixSymmetric(double m00, double m01, double m02, doub * @param epsilon the tolerance used as shown above. * @return {@code true} if the matrix is skew symmetric, {@code false} otherwise. */ - public static boolean isMatrixSymmetric(double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, double m22, + public static boolean isMatrixSymmetric(double m00, + double m01, + double m02, + double m10, + double m11, + double m12, + double m20, + double m21, + double m22, double epsilon) { return Math.abs(m01 - m10) <= epsilon && Math.abs(m02 - m20) <= epsilon && Math.abs(m12 - m21) <= epsilon; diff --git a/src/main/java/us/ihmc/euclid/tools/Matrix3DTools.java b/src/main/java/us/ihmc/euclid/tools/Matrix3DTools.java index 1801ad96f..8a342e442 100644 --- a/src/main/java/us/ihmc/euclid/tools/Matrix3DTools.java +++ b/src/main/java/us/ihmc/euclid/tools/Matrix3DTools.java @@ -246,7 +246,12 @@ public static void multiplyInvertRight(Matrix3DReadOnly m1, RotationMatrixReadOn * @param invert2 whether to invert {@code m2} before performing the multiplication. * @param matrixToPack the matrix in which the result is stored. Modified. */ - public static void multiply(Matrix3DReadOnly m1, boolean transpose1, boolean invert1, Matrix3DReadOnly m2, boolean transpose2, boolean invert2, + public static void multiply(Matrix3DReadOnly m1, + boolean transpose1, + boolean invert1, + Matrix3DReadOnly m2, + boolean transpose2, + boolean invert2, CommonMatrix3DBasics matrixToPack) { if (m1.isIdentity()) @@ -463,8 +468,12 @@ public static void multiplyInner(Matrix3DReadOnly matrix, CommonMatrix3DBasics m * @param invertOrientation whether to invert the orientation before performing the operation. * @param matrixToPack the matrix in which the result is stored. Modified. */ - public static void multiply(Matrix3DReadOnly matrix, boolean transposeMatrix, boolean invertMatrix, Orientation3DReadOnly orientation, - boolean invertOrientation, CommonMatrix3DBasics matrixToPack) + public static void multiply(Matrix3DReadOnly matrix, + boolean transposeMatrix, + boolean invertMatrix, + Orientation3DReadOnly orientation, + boolean invertOrientation, + CommonMatrix3DBasics matrixToPack) { if (orientation instanceof RotationMatrixReadOnly) { @@ -602,8 +611,12 @@ else if (orientation.isZeroOrientation()) * @param invertMatrix whether to invert the matrix before performing the operation. * @param matrixToPack the matrix in which the result is stored. Modified. */ - public static void multiply(Orientation3DReadOnly orientation, boolean invertOrientation, Matrix3DReadOnly matrix, boolean transposeMatrix, - boolean invertMatrix, CommonMatrix3DBasics matrixToPack) + public static void multiply(Orientation3DReadOnly orientation, + boolean invertOrientation, + Matrix3DReadOnly matrix, + boolean transposeMatrix, + boolean invertMatrix, + CommonMatrix3DBasics matrixToPack) { if (orientation instanceof RotationMatrixReadOnly) { @@ -996,7 +1009,9 @@ public static void inverseTransform(Matrix3DReadOnly matrix, Tuple3DReadOnly tup * {@code checkIfTransformInXYPlane} is {@code true}. * @throws SingularMatrixException if {@code matrix} is not invertible. */ - public static void inverseTransform(Matrix3DReadOnly matrix, Tuple2DReadOnly tupleOriginal, Tuple2DBasics tupleTransformed, + public static void inverseTransform(Matrix3DReadOnly matrix, + Tuple2DReadOnly tupleOriginal, + Tuple2DBasics tupleTransformed, boolean checkIfTransformInXYPlane) { boolean isMatrix2D = matrix.isMatrix2D(); diff --git a/src/main/java/us/ihmc/euclid/tools/QuaternionTools.java b/src/main/java/us/ihmc/euclid/tools/QuaternionTools.java index e1a5d49ca..379507c0b 100644 --- a/src/main/java/us/ihmc/euclid/tools/QuaternionTools.java +++ b/src/main/java/us/ihmc/euclid/tools/QuaternionTools.java @@ -152,7 +152,10 @@ public static void multiplyConjugateBoth(QuaternionReadOnly q1, QuaternionReadOn * @param inverse2 whether the second orientation should be inverted in the multiplication. * @param quaternionToPack the quaternion in which the result is stored. Modified. */ - public static void multiply(Orientation3DReadOnly orientation1, boolean inverse1, Orientation3DReadOnly orientation2, boolean inverse2, + public static void multiply(Orientation3DReadOnly orientation1, + boolean inverse1, + Orientation3DReadOnly orientation2, + boolean inverse2, QuaternionBasics quaternionToPack) { if (orientation1 instanceof QuaternionReadOnly) @@ -205,7 +208,10 @@ public static void multiply(Orientation3DReadOnly orientation1, boolean inverse1 * @param inverse2 whether the second orientation should be inverted in the multiplication. * @param quaternionToPack the quaternion in which the result is stored. Modified. */ - public static void multiply(Orientation3DReadOnly orientation1, boolean inverse1, QuaternionReadOnly orientation2, boolean inverse2, + public static void multiply(Orientation3DReadOnly orientation1, + boolean inverse1, + QuaternionReadOnly orientation2, + boolean inverse2, QuaternionBasics quaternionToPack) { if (orientation1 instanceof QuaternionReadOnly) @@ -245,7 +251,10 @@ public static void multiply(Orientation3DReadOnly orientation1, boolean inverse1 * @param inverse2 whether the second orientation should be inverted in the multiplication. * @param quaternionToPack the quaternion in which the result is stored. Modified. */ - public static void multiply(QuaternionReadOnly orientation1, boolean inverse1, Orientation3DReadOnly orientation2, boolean inverse2, + public static void multiply(QuaternionReadOnly orientation1, + boolean inverse1, + Orientation3DReadOnly orientation2, + boolean inverse2, QuaternionBasics quaternionToPack) { if (orientation2 instanceof QuaternionReadOnly) @@ -324,8 +333,17 @@ private static void multiplyImpl(QuaternionReadOnly q1, boolean conjugateQ1, Qua * @param conjugateQ2 whether to conjugate {@code q2} or not. * @param orientationToPack the orientation in which the result is stored. Modified. */ - public static void multiplyImpl(double q1x, double q1y, double q1z, double q1s, boolean conjugateQ1, double q2x, double q2y, double q2z, double q2s, - boolean conjugateQ2, Orientation3DBasics orientationToPack) + public static void multiplyImpl(double q1x, + double q1y, + double q1z, + double q1s, + boolean conjugateQ1, + double q2x, + double q2y, + double q2z, + double q2s, + boolean conjugateQ2, + Orientation3DBasics orientationToPack) { if (conjugateQ1) { @@ -459,8 +477,17 @@ private static void multiplyImpl(Tuple4DReadOnly t1, boolean conjugateT1, Tuple4 * @param conjugateT2 whether to conjugate {@code t2} or not. * @param vectorToPack the vector in which the result is stores. Modified. */ - public static void multiplyImpl(double x1, double y1, double z1, double s1, boolean conjugateT1, double x2, double y2, double z2, double s2, - boolean conjugateT2, Vector4DBasics vectorToPack) + public static void multiplyImpl(double x1, + double y1, + double z1, + double s1, + boolean conjugateT1, + double x2, + double y2, + double z2, + double s2, + boolean conjugateT2, + Vector4DBasics vectorToPack) { if (conjugateT1) { @@ -646,7 +673,10 @@ public static void subTransform(QuaternionReadOnly quaternion, Tuple3DReadOnly t * @param tupleOriginal the tuple to transform. Not modified. * @param tupleTransformed the tuple in which the result is stored. Modified. */ - private static void addTransform(boolean subtract, QuaternionReadOnly quaternion, boolean conjugateQuaternion, Tuple3DReadOnly tupleOriginal, + private static void addTransform(boolean subtract, + QuaternionReadOnly quaternion, + boolean conjugateQuaternion, + Tuple3DReadOnly tupleOriginal, Tuple3DBasics tupleTransformed) { double qx = quaternion.getX(); @@ -748,7 +778,9 @@ public static void transform(QuaternionReadOnly quaternion, Tuple2DReadOnly tupl * @throws NotAMatrix2DException if {@code checkIfTransformInXYPlane == true} and the quaternion * does not represent a transformation in the XY plane. */ - public static void inverseTransform(QuaternionReadOnly quaternion, Tuple2DReadOnly tupleOriginal, Tuple2DBasics tupleTransformed, + public static void inverseTransform(QuaternionReadOnly quaternion, + Tuple2DReadOnly tupleOriginal, + Tuple2DBasics tupleTransformed, boolean checkIfTransformInXYPlane) { transformImpl(quaternion, true, tupleOriginal, tupleTransformed, checkIfTransformInXYPlane); @@ -776,7 +808,10 @@ public static void inverseTransform(QuaternionReadOnly quaternion, Tuple2DReadOn * @throws NotAMatrix2DException if {@code checkIfTransformInXYPlane == true} and the quaternion * does not represent a transformation in the XY plane. */ - private static void transformImpl(QuaternionReadOnly quaternion, boolean conjugateQuaternion, Tuple2DReadOnly tupleOriginal, Tuple2DBasics tupleTransformed, + private static void transformImpl(QuaternionReadOnly quaternion, + boolean conjugateQuaternion, + Tuple2DReadOnly tupleOriginal, + Tuple2DBasics tupleTransformed, boolean checkIfTransformInXYPlane) { if (checkIfTransformInXYPlane) @@ -929,7 +964,9 @@ public static void inverseTransform(QuaternionReadOnly quaternion, Vector4DReadO * @param vectorOriginal the vector to transform. Not modified. * @param vectorTransformed the vector in which the result is stored. Modified. */ - private static void transformImpl(QuaternionReadOnly quaternion, boolean conjugateQuaternion, Vector4DReadOnly vectorOriginal, + private static void transformImpl(QuaternionReadOnly quaternion, + boolean conjugateQuaternion, + Vector4DReadOnly vectorOriginal, Vector4DBasics vectorTransformed) { double norm = quaternion.norm(); @@ -1041,7 +1078,9 @@ public static void inverseTransform(QuaternionReadOnly quaternion, Matrix3DReadO * @param matrixOriginal the matrix to transform. Not modified. * @param matrixTransformed the matrix in which the result is stored. Modified. */ - private static void transformImpl(QuaternionReadOnly quaternion, boolean conjugateQuaternion, Matrix3DReadOnly matrixOriginal, + private static void transformImpl(QuaternionReadOnly quaternion, + boolean conjugateQuaternion, + Matrix3DReadOnly matrixOriginal, Matrix3DBasics matrixTransformed) { double qx = quaternion.getX(); @@ -1074,7 +1113,12 @@ private static void transformImpl(QuaternionReadOnly quaternion, boolean conjuga * @param matrixOriginal the matrix to transform. Not modified. * @param matrixTransformed the matrix in which the result is stored. Modified. */ - static void transformImpl(double qx, double qy, double qz, double qs, boolean conjugateQuaternion, Matrix3DReadOnly matrixOriginal, + static void transformImpl(double qx, + double qy, + double qz, + double qs, + boolean conjugateQuaternion, + Matrix3DReadOnly matrixOriginal, Matrix3DBasics matrixTransformed) { double norm = EuclidCoreTools.fastNorm(qx, qy, qz, qs); @@ -1191,7 +1235,8 @@ public static void transform(QuaternionReadOnly quaternion, RotationMatrixReadOn * @param rotationMatrixOriginal the rotation matrix to transform. Not modified. * @param rotationMatrixTransformed the rotation matrix in which the result is stored. Modified. */ - public static void inverseTransform(QuaternionReadOnly quaternion, RotationMatrixReadOnly rotationMatrixOriginal, + public static void inverseTransform(QuaternionReadOnly quaternion, + RotationMatrixReadOnly rotationMatrixOriginal, RotationMatrixBasics rotationMatrixTransformed) { RotationMatrixTools.multiply(quaternion, true, rotationMatrixOriginal, false, rotationMatrixTransformed); diff --git a/src/main/java/us/ihmc/euclid/tools/RotationMatrixTools.java b/src/main/java/us/ihmc/euclid/tools/RotationMatrixTools.java index a6c3eae03..46bdc3e20 100644 --- a/src/main/java/us/ihmc/euclid/tools/RotationMatrixTools.java +++ b/src/main/java/us/ihmc/euclid/tools/RotationMatrixTools.java @@ -107,7 +107,10 @@ public static void multiplyTransposeRight(RotationMatrixReadOnly m1, RotationMat * @param inverse2 whether the second orientation should be inverted in the multiplication. * @param matrixToPack the rotation matrix in which the result is stored. Modified. */ - public static void multiply(Orientation3DReadOnly orientation1, boolean inverse1, Orientation3DReadOnly orientation2, boolean inverse2, + public static void multiply(Orientation3DReadOnly orientation1, + boolean inverse1, + Orientation3DReadOnly orientation2, + boolean inverse2, CommonMatrix3DBasics matrixToPack) { if (orientation1 instanceof RotationMatrixReadOnly) @@ -208,7 +211,10 @@ else if (orientation2.isZeroOrientation()) * @param inverse2 whether the second orientation should be inverted in the multiplication. * @param matrixToPack the rotation matrix in which the result is stored. Modified. */ - public static void multiply(Orientation3DReadOnly orientation1, boolean inverse1, RotationMatrixReadOnly orientation2, boolean inverse2, + public static void multiply(Orientation3DReadOnly orientation1, + boolean inverse1, + RotationMatrixReadOnly orientation2, + boolean inverse2, CommonMatrix3DBasics matrixToPack) { if (orientation1 instanceof RotationMatrixReadOnly) @@ -291,7 +297,10 @@ else if (orientation2.isZeroOrientation()) * @param inverse2 whether the second orientation should be inverted in the multiplication. * @param matrixToPack the rotation matrix in which the result is stored. Modified. */ - public static void multiply(RotationMatrixReadOnly orientation1, boolean inverse1, Orientation3DReadOnly orientation2, boolean inverse2, + public static void multiply(RotationMatrixReadOnly orientation1, + boolean inverse1, + Orientation3DReadOnly orientation2, + boolean inverse2, CommonMatrix3DBasics matrixToPack) { if (orientation2 instanceof RotationMatrixReadOnly) @@ -370,7 +379,10 @@ else if (orientation2.isZeroOrientation()) * @param transposeB whether the second matrix should be transposed in the multiplication. * @param matrixToPack the rotation matrix in which the result is stored. Modified. */ - private static void multiplyImpl(RotationMatrixReadOnly a, boolean transposeA, RotationMatrixReadOnly b, boolean transposeB, + private static void multiplyImpl(RotationMatrixReadOnly a, + boolean transposeA, + RotationMatrixReadOnly b, + boolean transposeB, CommonMatrix3DBasics matrixToPack) { if (a.isZeroOrientation()) @@ -417,9 +429,27 @@ else if (b.isZeroOrientation()) matrixToPack); } - private static void multiplyImpl(double a00, double a01, double a02, double a10, double a11, double a12, double a20, double a21, double a22, - boolean transposeA, double b00, double b01, double b02, double b10, double b11, double b12, double b20, double b21, - double b22, boolean transposeB, CommonMatrix3DBasics matrixToPack) + private static void multiplyImpl(double a00, + double a01, + double a02, + double a10, + double a11, + double a12, + double a20, + double a21, + double a22, + boolean transposeA, + double b00, + double b01, + double b02, + double b10, + double b11, + double b12, + double b20, + double b21, + double b22, + boolean transposeB, + CommonMatrix3DBasics matrixToPack) { double c00, c01, c02, c10, c11, c12, c20, c21, c22; diff --git a/src/main/java/us/ihmc/euclid/tools/SingularValueDecomposition3D.java b/src/main/java/us/ihmc/euclid/tools/SingularValueDecomposition3D.java index 218d42c05..21c5f8d72 100644 --- a/src/main/java/us/ihmc/euclid/tools/SingularValueDecomposition3D.java +++ b/src/main/java/us/ihmc/euclid/tools/SingularValueDecomposition3D.java @@ -332,7 +332,16 @@ private void computeUW(double a00, double a01, double a02, double a10, double a1 * @param a21 element of the matrix to decompose. * @param a22 element of the matrix to decompose. */ - private void computeB(double a00, double a01, double a02, double a10, double a11, double a12, double a20, double a21, double a22, Quaternion V, + private void computeB(double a00, + double a01, + double a02, + double a10, + double a11, + double a12, + double a20, + double a21, + double a22, + Quaternion V, Matrix3D BToPack) { double qx = V.getX(); diff --git a/src/main/java/us/ihmc/euclid/tools/YawPitchRollTools.java b/src/main/java/us/ihmc/euclid/tools/YawPitchRollTools.java index deca86d4b..ab18dd587 100644 --- a/src/main/java/us/ihmc/euclid/tools/YawPitchRollTools.java +++ b/src/main/java/us/ihmc/euclid/tools/YawPitchRollTools.java @@ -270,7 +270,11 @@ public static void inverseTransform(YawPitchRollReadOnly yawPitchRoll, Tuple3DRe inverseTransform(yawPitchRoll.getYaw(), yawPitchRoll.getPitch(), yawPitchRoll.getRoll(), tupleOriginal, tupleTransformed); } - private static void transformImpl(double yaw, double pitch, double roll, boolean inverseTransform, Tuple3DReadOnly tupleOriginal, + private static void transformImpl(double yaw, + double pitch, + double roll, + boolean inverseTransform, + Tuple3DReadOnly tupleOriginal, Tuple3DBasics tupleTransformed) { if (isZero(yaw, pitch, roll, ZERO_EPS)) @@ -376,7 +380,11 @@ public static void addTransform(YawPitchRollReadOnly yawPitchRoll, Tuple3DReadOn * @throws NotAMatrix2DException if {@code checkIfTransformInXYPlane == true} and the yaw-pitch-roll * does not represent a transformation in the XY plane. */ - public static void transform(double yaw, double pitch, double roll, Tuple2DReadOnly tupleOriginal, Tuple2DBasics tupleTransformed, + public static void transform(double yaw, + double pitch, + double roll, + Tuple2DReadOnly tupleOriginal, + Tuple2DBasics tupleTransformed, boolean checkIfTransformInXYPlane) { transformImpl(yaw, pitch, roll, false, tupleOriginal, tupleTransformed, checkIfTransformInXYPlane); @@ -397,7 +405,9 @@ public static void transform(double yaw, double pitch, double roll, Tuple2DReadO * @throws NotAMatrix2DException if {@code checkIfTransformInXYPlane == true} and the yaw-pitch-roll * does not represent a transformation in the XY plane. */ - public static void transform(YawPitchRollReadOnly yawPitchRoll, Tuple2DReadOnly tupleOriginal, Tuple2DBasics tupleTransformed, + public static void transform(YawPitchRollReadOnly yawPitchRoll, + Tuple2DReadOnly tupleOriginal, + Tuple2DBasics tupleTransformed, boolean checkIfTransformInXYPlane) { transform(yawPitchRoll.getYaw(), yawPitchRoll.getPitch(), yawPitchRoll.getRoll(), tupleOriginal, tupleTransformed, checkIfTransformInXYPlane); @@ -425,7 +435,11 @@ public static void transform(YawPitchRollReadOnly yawPitchRoll, Tuple2DReadOnly * @throws NotAMatrix2DException if {@code checkIfTransformInXYPlane == true} and the yaw-pitch-roll * does not represent a transformation in the XY plane. */ - public static void inverseTransform(double yaw, double pitch, double roll, Tuple2DReadOnly tupleOriginal, Tuple2DBasics tupleTransformed, + public static void inverseTransform(double yaw, + double pitch, + double roll, + Tuple2DReadOnly tupleOriginal, + Tuple2DBasics tupleTransformed, boolean checkIfTransformInXYPlane) { transformImpl(yaw, pitch, roll, true, tupleOriginal, tupleTransformed, checkIfTransformInXYPlane); @@ -451,14 +465,21 @@ public static void inverseTransform(double yaw, double pitch, double roll, Tuple * @throws NotAMatrix2DException if {@code checkIfTransformInXYPlane == true} and the yaw-pitch-roll * does not represent a transformation in the XY plane. */ - public static void inverseTransform(YawPitchRollReadOnly yawPitchRoll, Tuple2DReadOnly tupleOriginal, Tuple2DBasics tupleTransformed, + public static void inverseTransform(YawPitchRollReadOnly yawPitchRoll, + Tuple2DReadOnly tupleOriginal, + Tuple2DBasics tupleTransformed, boolean checkIfTransformInXYPlane) { inverseTransform(yawPitchRoll.getYaw(), yawPitchRoll.getPitch(), yawPitchRoll.getRoll(), tupleOriginal, tupleTransformed, checkIfTransformInXYPlane); } - private static void transformImpl(double yaw, double pitch, double roll, boolean inverseTransform, Tuple2DReadOnly tupleOriginal, - Tuple2DBasics tupleTransformed, boolean checkIfTransformInXYPlane) + private static void transformImpl(double yaw, + double pitch, + double roll, + boolean inverseTransform, + Tuple2DReadOnly tupleOriginal, + Tuple2DBasics tupleTransformed, + boolean checkIfTransformInXYPlane) { if (isZero(yaw, pitch, roll, ZERO_EPS)) { @@ -598,7 +619,11 @@ public static void inverseTransform(YawPitchRollReadOnly yawPitchRoll, Matrix3DR inverseTransform(yawPitchRoll.getYaw(), yawPitchRoll.getPitch(), yawPitchRoll.getRoll(), matrixOriginal, matrixTransformed); } - private static void transformImpl(double yaw, double pitch, double roll, boolean inverseTransform, Matrix3DReadOnly matrixOriginal, + private static void transformImpl(double yaw, + double pitch, + double roll, + boolean inverseTransform, + Matrix3DReadOnly matrixOriginal, Matrix3DBasics matrixTransformed) { if (isZero(yaw, pitch, roll, ZERO_EPS)) @@ -693,7 +718,10 @@ private static void transformImpl(double yaw, double pitch, double roll, boolean * @param rotationMatrixOriginal the rotation matrix to transform. Not modified. * @param rotationMatrixTransformed the rotation matrix in which the result is stored. Modified. */ - public static void transform(double yaw, double pitch, double roll, RotationMatrixReadOnly rotationMatrixOriginal, + public static void transform(double yaw, + double pitch, + double roll, + RotationMatrixReadOnly rotationMatrixOriginal, RotationMatrixBasics rotationMatrixTransformed) { transformImpl(yaw, pitch, roll, false, rotationMatrixOriginal, rotationMatrixTransformed); @@ -719,7 +747,8 @@ public static void transform(double yaw, double pitch, double roll, RotationMatr * @param rotationMatrixOriginal the rotation matrix to transform. Not modified. * @param rotationMatrixTransformed the rotation matrix in which the result is stored. Modified. */ - public static void transform(YawPitchRollReadOnly yawPitchRoll, RotationMatrixReadOnly rotationMatrixOriginal, + public static void transform(YawPitchRollReadOnly yawPitchRoll, + RotationMatrixReadOnly rotationMatrixOriginal, RotationMatrixBasics rotationMatrixTransformed) { transform(yawPitchRoll.getYaw(), yawPitchRoll.getPitch(), yawPitchRoll.getRoll(), rotationMatrixOriginal, rotationMatrixTransformed); @@ -751,7 +780,10 @@ public static void transform(YawPitchRollReadOnly yawPitchRoll, RotationMatrixRe * @param rotationMatrixOriginal the rotation matrix to transform. Not modified. * @param rotationMatrixTransformed the rotation matrix in which the result is stored. Modified. */ - public static void inverseTransform(double yaw, double pitch, double roll, RotationMatrixReadOnly rotationMatrixOriginal, + public static void inverseTransform(double yaw, + double pitch, + double roll, + RotationMatrixReadOnly rotationMatrixOriginal, RotationMatrixBasics rotationMatrixTransformed) { transformImpl(yaw, pitch, roll, true, rotationMatrixOriginal, rotationMatrixTransformed); @@ -782,13 +814,18 @@ public static void inverseTransform(double yaw, double pitch, double roll, Rotat * @param rotationMatrixOriginal the rotation matrix to transform. Not modified. * @param rotationMatrixTransformed the rotation matrix in which the result is stored. Modified. */ - public static void inverseTransform(YawPitchRollReadOnly yawPitchRoll, RotationMatrixReadOnly rotationMatrixOriginal, + public static void inverseTransform(YawPitchRollReadOnly yawPitchRoll, + RotationMatrixReadOnly rotationMatrixOriginal, RotationMatrixBasics rotationMatrixTransformed) { inverseTransform(yawPitchRoll.getYaw(), yawPitchRoll.getPitch(), yawPitchRoll.getRoll(), rotationMatrixOriginal, rotationMatrixTransformed); } - private static void transformImpl(double yaw, double pitch, double roll, boolean inverseTransform, RotationMatrixReadOnly rotationMatrixOriginal, + private static void transformImpl(double yaw, + double pitch, + double roll, + boolean inverseTransform, + RotationMatrixReadOnly rotationMatrixOriginal, RotationMatrixBasics rotationMatrixTransformed) { if (isZero(yaw, pitch, roll, ZERO_EPS)) @@ -930,7 +967,11 @@ public static void inverseTransform(YawPitchRollReadOnly yawPitchRoll, Vector4DR inverseTransform(yawPitchRoll.getYaw(), yawPitchRoll.getPitch(), yawPitchRoll.getRoll(), vectorOriginal, vectorTransformed); } - private static void transformImpl(double yaw, double pitch, double roll, boolean inverseTransform, Vector4DReadOnly vectorOriginal, + private static void transformImpl(double yaw, + double pitch, + double roll, + boolean inverseTransform, + Vector4DReadOnly vectorOriginal, Vector4DBasics vectorTransformed) { if (isZero(yaw, pitch, roll, ZERO_EPS)) @@ -1000,7 +1041,10 @@ private static void transformImpl(double yaw, double pitch, double roll, boolean * multiplication. * @param yawPitchRollToPack the yaw-pitch-roll in which the result is stored. Modified. */ - public static void multiply(Orientation3DReadOnly orientation1, boolean inverse1, Orientation3DReadOnly orientation2, boolean inverse2, + public static void multiply(Orientation3DReadOnly orientation1, + boolean inverse1, + Orientation3DReadOnly orientation2, + boolean inverse2, YawPitchRollBasics yawPitchRollToPack) { double q1s, q1x, q1y, q1z; diff --git a/src/main/java/us/ihmc/euclid/transform/RigidBodyTransform.java b/src/main/java/us/ihmc/euclid/transform/RigidBodyTransform.java index da2477df9..263445909 100644 --- a/src/main/java/us/ihmc/euclid/transform/RigidBodyTransform.java +++ b/src/main/java/us/ihmc/euclid/transform/RigidBodyTransform.java @@ -170,7 +170,17 @@ public RigidBodyTransform(Orientation3DReadOnly orientation, Tuple3DReadOnly tra * @throws NotARotationMatrixException if the components for the rotation part do not represent a * rotation matrix. */ - public RigidBodyTransform(double m00, double m01, double m02, double m03, double m10, double m11, double m12, double m13, double m20, double m21, double m22, + public RigidBodyTransform(double m00, + double m01, + double m02, + double m03, + double m10, + double m11, + double m12, + double m13, + double m20, + double m21, + double m22, double m23) { set(m00, m01, m02, m03, m10, m11, m12, m13, m20, m21, m22, m23); @@ -217,7 +227,17 @@ public double determinantRotationPart() * @throws NotARotationMatrixException if the components for the rotation part do not represent a * rotation matrix. */ - public void set(double m00, double m01, double m02, double m03, double m10, double m11, double m12, double m13, double m20, double m21, double m22, + public void set(double m00, + double m01, + double m02, + double m03, + double m10, + double m11, + double m12, + double m13, + double m20, + double m21, + double m22, double m23) { getRotation().set(m00, m01, m02, m10, m11, m12, m20, m21, m22); @@ -245,7 +265,17 @@ public void set(double m00, double m01, double m02, double m03, double m10, doub * @param m22 the 3rd row 3rd column component of the rotation part of this transform. * @param m23 the z-component of the translation part of this transform. */ - public void setUnsafe(double m00, double m01, double m02, double m03, double m10, double m11, double m12, double m13, double m20, double m21, double m22, + public void setUnsafe(double m00, + double m01, + double m02, + double m03, + double m10, + double m11, + double m12, + double m13, + double m20, + double m21, + double m22, double m23) { getRotation().setUnsafe(m00, m01, m02, m10, m11, m12, m20, m21, m22); diff --git a/src/main/java/us/ihmc/euclid/transform/interfaces/AffineTransformBasics.java b/src/main/java/us/ihmc/euclid/transform/interfaces/AffineTransformBasics.java index 7455983b7..c56aa12ef 100644 --- a/src/main/java/us/ihmc/euclid/transform/interfaces/AffineTransformBasics.java +++ b/src/main/java/us/ihmc/euclid/transform/interfaces/AffineTransformBasics.java @@ -141,7 +141,17 @@ default void invert() * @param m22 the 3rd row 3rd column component of the rotation-scale part of this transform. * @param m23 the z-component of the translation part of this transform. */ - default void set(double m00, double m01, double m02, double m03, double m10, double m11, double m12, double m13, double m20, double m21, double m22, + default void set(double m00, + double m01, + double m02, + double m03, + double m10, + double m11, + double m12, + double m13, + double m20, + double m21, + double m22, double m23) { getLinearTransform().set(m00, m01, m02, m10, m11, m12, m20, m21, m22); diff --git a/src/shape/java/us/ihmc/euclid/shape/collision/EuclidShapeCollisionTools.java b/src/shape/java/us/ihmc/euclid/shape/collision/EuclidShapeCollisionTools.java index 044739f76..f42067ee3 100644 --- a/src/shape/java/us/ihmc/euclid/shape/collision/EuclidShapeCollisionTools.java +++ b/src/shape/java/us/ihmc/euclid/shape/collision/EuclidShapeCollisionTools.java @@ -91,7 +91,8 @@ private static void evaluatePoint3DBox3DCollision(Point3DReadOnly point3D, Box3D * @param shapeB the capsule. Not modified. * @param resultToPack the object in which the collision result is stored. Modified. */ - public static void evaluatePointShape3DCapsule3DCollision(PointShape3DReadOnly shapeA, Capsule3DReadOnly shapeB, + public static void evaluatePointShape3DCapsule3DCollision(PointShape3DReadOnly shapeA, + Capsule3DReadOnly shapeB, EuclidShape3DCollisionResultBasics resultToPack) { evaluatePoint3DCapsule3DCollision(shapeA, shapeB, resultToPack); @@ -179,7 +180,8 @@ private static void evaluatePoint3DCapsule3DCollision(Point3DReadOnly point3D, C * @param shapeB the cylinder. Not modified. * @param resultToPack the object in which the collision result is stored. Modified. */ - public static void evaluatePointShape3DCylinder3DCollision(PointShape3DReadOnly shapeA, Cylinder3DReadOnly shapeB, + public static void evaluatePointShape3DCylinder3DCollision(PointShape3DReadOnly shapeA, + Cylinder3DReadOnly shapeB, EuclidShape3DCollisionResultBasics resultToPack) { evaluatePoint3DCylinder3DCollision(shapeA, shapeB, resultToPack); @@ -207,7 +209,8 @@ public static void evaluateSphere3DCylinder3DCollision(Sphere3DReadOnly shapeA, resultToPack.setSignedDistance(distance); } - private static void evaluatePoint3DCylinder3DCollision(Point3DReadOnly point3D, Cylinder3DReadOnly cylinder3D, + private static void evaluatePoint3DCylinder3DCollision(Point3DReadOnly point3D, + Cylinder3DReadOnly cylinder3D, EuclidShape3DCollisionResultBasics resultToPack) { resultToPack.setToNaN(); @@ -233,7 +236,8 @@ private static void evaluatePoint3DCylinder3DCollision(Point3DReadOnly point3D, * @param shapeB the ellipsoid. Not modified. * @param resultToPack the object in which the collision result is stored. Modified. */ - public static void evaluatePointShape3DEllipsoid3DCollision(PointShape3DReadOnly shapeA, Ellipsoid3DReadOnly shapeB, + public static void evaluatePointShape3DEllipsoid3DCollision(PointShape3DReadOnly shapeA, + Ellipsoid3DReadOnly shapeB, EuclidShape3DCollisionResultBasics resultToPack) { evaluatePoint3DEllipsoid3DCollision(shapeA, shapeB, resultToPack); @@ -261,7 +265,8 @@ public static void evaluateSphere3DEllipsoid3DCollision(Sphere3DReadOnly shapeA, resultToPack.setSignedDistance(distance); } - private static void evaluatePoint3DEllipsoid3DCollision(Point3DReadOnly point3D, Ellipsoid3DReadOnly ellipsoid3D, + private static void evaluatePoint3DEllipsoid3DCollision(Point3DReadOnly point3D, + Ellipsoid3DReadOnly ellipsoid3D, EuclidShape3DCollisionResultBasics resultToPack) { resultToPack.setToNaN(); @@ -290,7 +295,8 @@ private static void evaluatePoint3DEllipsoid3DCollision(Point3DReadOnly point3D, * @param shapeB the second point shape. Not modified. * @param resultToPack the object in which the collision result is stored. Modified. */ - public static void evaluatePointShape3DPointShape3DCollision(PointShape3DReadOnly shapeA, PointShape3DReadOnly shapeB, + public static void evaluatePointShape3DPointShape3DCollision(PointShape3DReadOnly shapeA, + PointShape3DReadOnly shapeB, EuclidShape3DCollisionResultBasics resultToPack) { Point3DBasics pointOnA = resultToPack.getPointOnA(); @@ -369,7 +375,8 @@ private static void evaluatePoint3DRamp3DCollision(Point3DReadOnly point3D, Ramp * @param shapeB the sphere. Not modified. * @param resultToPack the object in which the collision result is stored. Modified. */ - public static void evaluatePointShape3DSphere3DCollision(PointShape3DReadOnly shapeA, Sphere3DReadOnly shapeB, + public static void evaluatePointShape3DSphere3DCollision(PointShape3DReadOnly shapeA, + Sphere3DReadOnly shapeB, EuclidShape3DCollisionResultBasics resultToPack) { evaluatePoint3DSphere3DCollision(shapeA, shapeB.getPosition(), shapeB.getRadius(), resultToPack); @@ -398,7 +405,9 @@ public static void evaluateSphere3DSphere3DCollision(Sphere3DReadOnly shapeA, Sp resultToPack.setSignedDistance(distance); } - private static void evaluatePoint3DSphere3DCollision(Point3DReadOnly point3D, Point3DReadOnly sphere3DPosition, double sphere3DRadius, + private static void evaluatePoint3DSphere3DCollision(Point3DReadOnly point3D, + Point3DReadOnly sphere3DPosition, + double sphere3DRadius, EuclidShape3DCollisionResultBasics resultToPack) { resultToPack.setToNaN(); diff --git a/src/shape/java/us/ihmc/euclid/shape/collision/epa/EPATools.java b/src/shape/java/us/ihmc/euclid/shape/collision/epa/EPATools.java index 10024088a..6411ca5b8 100644 --- a/src/shape/java/us/ihmc/euclid/shape/collision/epa/EPATools.java +++ b/src/shape/java/us/ihmc/euclid/shape/collision/epa/EPATools.java @@ -59,7 +59,10 @@ public enum BarycentricCoordinatesOutput * be equal to 3. Modified. * @return additional info about the location of the projection and the sanity of the triangle. */ - public static BarycentricCoordinatesOutput barycentricCoordinatesFrom2Simplex(Point3DReadOnly s1, Point3DReadOnly s2, Point3DReadOnly s3, double epsilon, + public static BarycentricCoordinatesOutput barycentricCoordinatesFrom2Simplex(Point3DReadOnly s1, + Point3DReadOnly s2, + Point3DReadOnly s3, + double epsilon, double[] lambdasToPack) { double s1x = s1.getX(), s1y = s1.getY(), s1z = s1.getZ(); @@ -311,7 +314,9 @@ public static double[] barycentricCoordinatesFrom1Simplex(Point3DReadOnly s1, Po * whether a triangle is affinely dependent or not. * @return a convex polytope usable to initiate the Expanding Polytope algorithm. */ - public static List newEPAPolytopeFromGJKSimplex(SupportingVertexHolder shapeA, SupportingVertexHolder shapeB, GJKVertex3D[] gjkVertices, + public static List newEPAPolytopeFromGJKSimplex(SupportingVertexHolder shapeA, + SupportingVertexHolder shapeB, + GJKVertex3D[] gjkVertices, double epsilon) { List epaPolytope = new ArrayList<>(); diff --git a/src/shape/java/us/ihmc/euclid/shape/collision/epa/ExpandingPolytopeAlgorithm.java b/src/shape/java/us/ihmc/euclid/shape/collision/epa/ExpandingPolytopeAlgorithm.java index 9d7c4e207..ab1039e29 100644 --- a/src/shape/java/us/ihmc/euclid/shape/collision/epa/ExpandingPolytopeAlgorithm.java +++ b/src/shape/java/us/ihmc/euclid/shape/collision/epa/ExpandingPolytopeAlgorithm.java @@ -182,7 +182,9 @@ public boolean evaluateCollision(SupportingVertexHolder shapeA, SupportingVertex * @param resultToPack the object in which the collision result is stored. Modified. * @return {@code true} if the shapes are colliding, {@code false} otherwise. */ - public boolean evaluateCollision(SupportingVertexHolder shapeA, SupportingVertexHolder shapeB, GJKVertex3D[] simplex, + public boolean evaluateCollision(SupportingVertexHolder shapeA, + SupportingVertexHolder shapeB, + GJKVertex3D[] simplex, EuclidShape3DCollisionResultBasics resultToPack) { PriorityQueue queue = new PriorityQueue<>(); diff --git a/src/shape/java/us/ihmc/euclid/shape/collision/gjk/GJKTools.java b/src/shape/java/us/ihmc/euclid/shape/collision/gjk/GJKTools.java index e2eff8118..c5f440a49 100644 --- a/src/shape/java/us/ihmc/euclid/shape/collision/gjk/GJKTools.java +++ b/src/shape/java/us/ihmc/euclid/shape/collision/gjk/GJKTools.java @@ -48,24 +48,36 @@ public static interface ProjectedTriangleSignedAreaCalculator * The value returned by this calculated is actually two times the triangle area. *

*/ - public static final ProjectedTriangleSignedAreaCalculator yzTriangleAreaCalculator = (ax, ay, az, bx, by, bz, cx, cy, - cz) -> triangleSignedArea(ay, az, by, bz, cy, cz); + public static final ProjectedTriangleSignedAreaCalculator yzTriangleAreaCalculator = (ax, ay, az, bx, by, bz, cx, cy, cz) -> triangleSignedArea(ay, + az, + by, + bz, + cy, + cz); /** * Calculator for computing the area of a 3D triangle projected onto the ZX-plane. *

* The value returned by this calculated is actually two times the triangle area. *

*/ - public static final ProjectedTriangleSignedAreaCalculator zxTriangleAreaCalculator = (ax, ay, az, bx, by, bz, cx, cy, - cz) -> triangleSignedArea(az, ax, bz, bx, cz, cx); + public static final ProjectedTriangleSignedAreaCalculator zxTriangleAreaCalculator = (ax, ay, az, bx, by, bz, cx, cy, cz) -> triangleSignedArea(az, + ax, + bz, + bx, + cz, + cx); /** * Calculator for computing the area of a 3D triangle projected onto the XY-plane. *

* The value returned by this calculated is actually two times the triangle area. *

*/ - public static final ProjectedTriangleSignedAreaCalculator xyTriangleAreaCalculator = (ax, ay, az, bx, by, bz, cx, cy, - cz) -> triangleSignedArea(ax, ay, bx, by, cx, cy); + public static final ProjectedTriangleSignedAreaCalculator xyTriangleAreaCalculator = (ax, ay, az, bx, by, bz, cx, cy, cz) -> triangleSignedArea(ax, + ay, + bx, + by, + cx, + cy); /** * Calculates the signed area of a 2D triangle. diff --git a/src/shape/java/us/ihmc/euclid/shape/convexPolytope/tools/EuclidPolytopeConstructionTools.java b/src/shape/java/us/ihmc/euclid/shape/convexPolytope/tools/EuclidPolytopeConstructionTools.java index 67edec36f..a89272545 100644 --- a/src/shape/java/us/ihmc/euclid/shape/convexPolytope/tools/EuclidPolytopeConstructionTools.java +++ b/src/shape/java/us/ihmc/euclid/shape/convexPolytope/tools/EuclidPolytopeConstructionTools.java @@ -287,8 +287,10 @@ public static , Edge extends return newFaces; } - private static boolean filterInPlaneFaces(Vertex3DReadOnly vertex, List silhouetteEdges, - Collection inPlaneFaces, double epsilon) + private static boolean filterInPlaneFaces(Vertex3DReadOnly vertex, + List silhouetteEdges, + Collection inPlaneFaces, + double epsilon) { if (inPlaneFaces.isEmpty()) return true; @@ -630,8 +632,11 @@ public static void computeCovariance3D(List input, Tu * Can be {@code null}. * @return the area of the polygon. */ - public static double computeConvexPolygon3DArea(List convexPolygon3D, Vector3DReadOnly normal, int numberOfVertices, - boolean clockwiseOrdered, Point3DBasics centroidToPack) + public static double computeConvexPolygon3DArea(List convexPolygon3D, + Vector3DReadOnly normal, + int numberOfVertices, + boolean clockwiseOrdered, + Point3DBasics centroidToPack) { checkNumberOfVertices(convexPolygon3D, numberOfVertices); diff --git a/src/shape/java/us/ihmc/euclid/shape/convexPolytope/tools/EuclidPolytopeTools.java b/src/shape/java/us/ihmc/euclid/shape/convexPolytope/tools/EuclidPolytopeTools.java index 86237df06..378f7d301 100644 --- a/src/shape/java/us/ihmc/euclid/shape/convexPolytope/tools/EuclidPolytopeTools.java +++ b/src/shape/java/us/ihmc/euclid/shape/convexPolytope/tools/EuclidPolytopeTools.java @@ -57,7 +57,9 @@ public static int computeConvexPolytopeNumberOfVertices(int numberOfFaces, int n * @param lineSegmentEnd2 the end position of the second line segment. Not modified. * @return the result of the cross product. */ - public static Vector3D crossProductOfLineSegment3Ds(Point3DReadOnly lineSegmentStart1, Point3DReadOnly lineSegmentEnd1, Point3DReadOnly lineSegmentStart2, + public static Vector3D crossProductOfLineSegment3Ds(Point3DReadOnly lineSegmentStart1, + Point3DReadOnly lineSegmentEnd1, + Point3DReadOnly lineSegmentStart2, Point3DReadOnly lineSegmentEnd2) { Vector3D crossProduct = new Vector3D(); @@ -78,8 +80,11 @@ public static Vector3D crossProductOfLineSegment3Ds(Point3DReadOnly lineSegmentS * @param lineSegmentEnd2 the end position of the second line segment. Not modified. * @param crossProductToPack the vector used to store the result of the cross product. Modified. */ - public static void crossProductOfLineSegment3Ds(Point3DReadOnly lineSegmentStart1, Point3DReadOnly lineSegmentEnd1, Point3DReadOnly lineSegmentStart2, - Point3DReadOnly lineSegmentEnd2, Vector3DBasics crossProductToPack) + public static void crossProductOfLineSegment3Ds(Point3DReadOnly lineSegmentStart1, + Point3DReadOnly lineSegmentEnd1, + Point3DReadOnly lineSegmentStart2, + Point3DReadOnly lineSegmentEnd2, + Vector3DBasics crossProductToPack) { double direction1X = lineSegmentEnd1.getX() - lineSegmentStart1.getX(); double direction1Y = lineSegmentEnd1.getY() - lineSegmentStart1.getY(); @@ -110,7 +115,9 @@ public static void crossProductOfLineSegment3Ds(Point3DReadOnly lineSegmentStart * @param planeNormal the normal of the plane the line is lying onto. Not modified. * @return {@code true} if the query is located on the left side of the line. */ - public static boolean isPoint3DOnLeftSideOfLine3D(Point3DReadOnly point, Point3DReadOnly firstPointOnLine, Point3DReadOnly secondPointOnLine, + public static boolean isPoint3DOnLeftSideOfLine3D(Point3DReadOnly point, + Point3DReadOnly firstPointOnLine, + Point3DReadOnly secondPointOnLine, Vector3DReadOnly planeNormal) { return isPoint3DOnSideOfLine3D(point, firstPointOnLine, secondPointOnLine, planeNormal, true); @@ -131,7 +138,9 @@ public static boolean isPoint3DOnLeftSideOfLine3D(Point3DReadOnly point, Point3D * @param planeNormal the normal of the plane the line is lying onto. Not modified. * @return {@code true} if the query is located on the left side of the line. */ - public static boolean isPoint3DOnLeftSideOfLine3D(Point3DReadOnly point, Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection, + public static boolean isPoint3DOnLeftSideOfLine3D(Point3DReadOnly point, + Point3DReadOnly pointOnLine, + Vector3DReadOnly lineDirection, Vector3DReadOnly planeNormal) { return isPoint3DOnSideOfLine3D(point, pointOnLine, lineDirection, planeNormal, true); @@ -152,7 +161,9 @@ public static boolean isPoint3DOnLeftSideOfLine3D(Point3DReadOnly point, Point3D * @param planeNormal the normal of the plane the line is lying onto. Not modified. * @return {@code true} if the query is located on the right side of the line. */ - public static boolean isPoint3DOnRightSideOfLine3D(Point3DReadOnly point, Point3DReadOnly firstPointOnLine, Point3DReadOnly secondPointOnLine, + public static boolean isPoint3DOnRightSideOfLine3D(Point3DReadOnly point, + Point3DReadOnly firstPointOnLine, + Point3DReadOnly secondPointOnLine, Vector3DReadOnly planeNormal) { return isPoint3DOnSideOfLine3D(point, firstPointOnLine, secondPointOnLine, planeNormal, false); @@ -173,7 +184,9 @@ public static boolean isPoint3DOnRightSideOfLine3D(Point3DReadOnly point, Point3 * @param planeNormal the normal of the plane the line is lying onto. Not modified. * @return {@code true} if the query is located on the right side of the line. */ - public static boolean isPoint3DOnRightSideOfLine3D(Point3DReadOnly point, Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection, + public static boolean isPoint3DOnRightSideOfLine3D(Point3DReadOnly point, + Point3DReadOnly pointOnLine, + Vector3DReadOnly lineDirection, Vector3DReadOnly planeNormal) { return isPoint3DOnSideOfLine3D(point, pointOnLine, lineDirection, planeNormal, false); @@ -205,9 +218,19 @@ public static boolean isPoint3DOnRightSideOfLine3D(Point3DReadOnly point, Point3 * @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. */ - public static boolean isPoint3DOnSideOfLine3D(double pointX, double pointY, double pointZ, double pointOnLineX, double pointOnLineY, double pointOnLineZ, - double lineDirectionX, double lineDirectionY, double lineDirectionZ, double planeNormalX, double planeNormalY, - double planeNormalZ, boolean testLeftSide) + public static boolean isPoint3DOnSideOfLine3D(double pointX, + double pointY, + double pointZ, + double pointOnLineX, + double pointOnLineY, + double pointOnLineZ, + double lineDirectionX, + double lineDirectionY, + double lineDirectionZ, + double planeNormalX, + double planeNormalY, + double planeNormalZ, + boolean testLeftSide) { double dx = pointX - pointOnLineX; double dy = pointY - pointOnLineY; @@ -243,8 +266,11 @@ public static boolean isPoint3DOnSideOfLine3D(double pointX, double pointY, doub * @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. */ - public static boolean isPoint3DOnSideOfLine3D(Point3DReadOnly point, Point3DReadOnly firstPointOnLine, Point3DReadOnly secondPointOnLine, - Vector3DReadOnly planeNormal, boolean testLeftSide) + public static boolean isPoint3DOnSideOfLine3D(Point3DReadOnly point, + Point3DReadOnly firstPointOnLine, + Point3DReadOnly secondPointOnLine, + Vector3DReadOnly planeNormal, + boolean testLeftSide) { double pointOnLineX = firstPointOnLine.getX(); double pointOnLineY = firstPointOnLine.getY(); @@ -285,8 +311,11 @@ public static boolean isPoint3DOnSideOfLine3D(Point3DReadOnly point, Point3DRead * @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. */ - public static boolean isPoint3DOnSideOfLine3D(Point3DReadOnly point, Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection, - Vector3DReadOnly planeNormal, boolean testLeftSide) + public static boolean isPoint3DOnSideOfLine3D(Point3DReadOnly point, + Point3DReadOnly pointOnLine, + Vector3DReadOnly lineDirection, + Vector3DReadOnly planeNormal, + boolean testLeftSide) { return isPoint3DOnSideOfLine3D(point.getX(), point.getY(), @@ -382,7 +411,9 @@ public static List c * any face or if the observer is inside one of the faces. */ @SuppressWarnings("unchecked") - public static List computeSilhouette(List faces, Point3DReadOnly observer, double epsilon, + public static List computeSilhouette(List faces, + Point3DReadOnly observer, + double epsilon, Collection visibleFacesToPack) { if (faces.isEmpty()) @@ -499,9 +530,8 @@ public static List c * WARNING: This method generates garbage. *

* - * - * @param the type of face. - * @param the type half-edge. + * @param the type of face. + * @param the type half-edge. * @param query the coordinates of the query. Not modified. * @param silhouette the list of the half-edges to navigate. Not modified. * @param epsilon the tolerance used for determining whether the query is close to a face support @@ -510,7 +540,8 @@ public static List c */ @SuppressWarnings("unchecked") public static List computeInPlaneFacesAroundSilhouette(Point3DReadOnly query, - Collection silhouette, double epsilon) + Collection silhouette, + double epsilon) { List inPlaneFacesToPack = new ArrayList<>(); diff --git a/src/shape/java/us/ihmc/euclid/shape/primitives/interfaces/Box3DReadOnly.java b/src/shape/java/us/ihmc/euclid/shape/primitives/interfaces/Box3DReadOnly.java index 00eff055e..bae3a89a8 100644 --- a/src/shape/java/us/ihmc/euclid/shape/primitives/interfaces/Box3DReadOnly.java +++ b/src/shape/java/us/ihmc/euclid/shape/primitives/interfaces/Box3DReadOnly.java @@ -241,7 +241,9 @@ default int intersectionWith(Line3DReadOnly line, Point3DBasics firstIntersectio * @return the number of intersections between the line and this box. It is either equal to 0, 1, or * 2. */ - default int intersectionWith(Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection, Point3DBasics firstIntersectionToPack, + default int intersectionWith(Point3DReadOnly pointOnLine, + Vector3DReadOnly lineDirection, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { double maxX = 0.5 * getSizeX(); diff --git a/src/shape/java/us/ihmc/euclid/shape/primitives/interfaces/Cylinder3DReadOnly.java b/src/shape/java/us/ihmc/euclid/shape/primitives/interfaces/Cylinder3DReadOnly.java index 9634a61be..cccb9963f 100644 --- a/src/shape/java/us/ihmc/euclid/shape/primitives/interfaces/Cylinder3DReadOnly.java +++ b/src/shape/java/us/ihmc/euclid/shape/primitives/interfaces/Cylinder3DReadOnly.java @@ -197,7 +197,9 @@ default int intersectionWith(Line3DReadOnly line, Point3DBasics firstIntersectio * @return the number of intersections between the line and this cylinder. It is either equal to 0, * 1, or 2. */ - default int intersectionWith(Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection, Point3DBasics firstIntersectionToPack, + default int intersectionWith(Point3DReadOnly pointOnLine, + Vector3DReadOnly lineDirection, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { return EuclidGeometryTools.intersectionBetweenLine3DAndCylinder3D(getLength(), diff --git a/src/shape/java/us/ihmc/euclid/shape/primitives/interfaces/Ellipsoid3DReadOnly.java b/src/shape/java/us/ihmc/euclid/shape/primitives/interfaces/Ellipsoid3DReadOnly.java index f3c165340..37080a809 100644 --- a/src/shape/java/us/ihmc/euclid/shape/primitives/interfaces/Ellipsoid3DReadOnly.java +++ b/src/shape/java/us/ihmc/euclid/shape/primitives/interfaces/Ellipsoid3DReadOnly.java @@ -237,7 +237,9 @@ default int intersectionWith(Line3DReadOnly line, Point3DBasics firstIntersectio * @return the number of intersections between the line and this ellipsoid. It is either equal to 0, * 1, or 2. */ - default int intersectionWith(Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection, Point3DBasics firstIntersectionToPack, + default int intersectionWith(Point3DReadOnly pointOnLine, + Vector3DReadOnly lineDirection, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { Point3DBasics pointOnLineInLocal = getIntermediateVariableSupplier().requestPoint3D(); diff --git a/src/shape/java/us/ihmc/euclid/shape/primitives/interfaces/Sphere3DReadOnly.java b/src/shape/java/us/ihmc/euclid/shape/primitives/interfaces/Sphere3DReadOnly.java index 07a82463a..5066072bb 100644 --- a/src/shape/java/us/ihmc/euclid/shape/primitives/interfaces/Sphere3DReadOnly.java +++ b/src/shape/java/us/ihmc/euclid/shape/primitives/interfaces/Sphere3DReadOnly.java @@ -139,7 +139,9 @@ default int intersectionWith(Line3DReadOnly line, Point3DBasics firstIntersectio * @return the number of intersections between the line and this sphere. It is either equal to 0, 1, * or 2. */ - default int intersectionWith(Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection, Point3DBasics firstIntersectionToPack, + default int intersectionWith(Point3DReadOnly pointOnLine, + Vector3DReadOnly lineDirection, + Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack) { diff --git a/src/shape/java/us/ihmc/euclid/shape/tools/EuclidEllipsoid3DTools.java b/src/shape/java/us/ihmc/euclid/shape/tools/EuclidEllipsoid3DTools.java index fd3967ab9..4ae8bd186 100644 --- a/src/shape/java/us/ihmc/euclid/shape/tools/EuclidEllipsoid3DTools.java +++ b/src/shape/java/us/ihmc/euclid/shape/tools/EuclidEllipsoid3DTools.java @@ -75,7 +75,10 @@ public static double distancePoint3DEllipsoid3D(Vector3DReadOnly radii, Point3DR * stored. Modified. * @return the smallest distance between the point and the ellipsoid's surface. */ - public static double distancePoint3DEllipsoid3D(Vector3DReadOnly radii, Point3DReadOnly query, int maxIterations, double epsilon, + public static double distancePoint3DEllipsoid3D(Vector3DReadOnly radii, + Point3DReadOnly query, + int maxIterations, + double epsilon, Point3DBasics closestPointToPack) { // Enforcing: e0 > e1 > e2 @@ -213,7 +216,12 @@ public static double distancePoint3DEllipsoid3D(Vector3DReadOnly radii, Point3DR return distance; } - private static double distancePoint2DEllipse2D(double e0, double e1, double y0, double y1, int maxIterations, double epsilon, + private static double distancePoint2DEllipse2D(double e0, + double e1, + double y0, + double y1, + int maxIterations, + double epsilon, Point3DBasics closestPointToPack) { double distance; @@ -342,7 +350,9 @@ private enum CoordinateOrder implements Tuple3DUpdater private final CoordinateReader coordinateReader1; private final CoordinateReader coordinateReader2; - private CoordinateOrder(Tuple3DUpdater tuple3DUpdater, CoordinateReader coordinateReader0, CoordinateReader coordinateReader1, + private CoordinateOrder(Tuple3DUpdater tuple3DUpdater, + CoordinateReader coordinateReader0, + CoordinateReader coordinateReader1, CoordinateReader coordinateReader2) { this.tuple3DUpdater = tuple3DUpdater; diff --git a/src/shape/java/us/ihmc/euclid/shape/tools/EuclidShapeRandomTools.java b/src/shape/java/us/ihmc/euclid/shape/tools/EuclidShapeRandomTools.java index 43fb81c71..fa60b4da1 100644 --- a/src/shape/java/us/ihmc/euclid/shape/tools/EuclidShapeRandomTools.java +++ b/src/shape/java/us/ihmc/euclid/shape/tools/EuclidShapeRandomTools.java @@ -449,7 +449,10 @@ public static Face3D nextCircleBasedFace3D(Random random, double centerMinMax, d * @param planeNormal defines the normal of the plane onto which the vertices are positioned. * @return the random convex polygon 3D. */ - public static List nextCircleBasedConvexPolygon3D(Random random, double centerMinMax, double maxEdgeLength, int numberOfVertices, + public static List nextCircleBasedConvexPolygon3D(Random random, + double centerMinMax, + double maxEdgeLength, + int numberOfVertices, Vector3DReadOnly planeNormal) { return nextCircleBasedConvexPolygon3D(random, EuclidCoreRandomTools.nextPoint3D(random, centerMinMax), maxEdgeLength, numberOfVertices, planeNormal); @@ -467,7 +470,10 @@ public static List nextCircleBasedConvexPolygon3D(Random random, double * @param planeNormal defines the normal of the plane onto which the vertices are positioned. * @return the random convex polygon 3D. */ - public static List nextCircleBasedConvexPolygon3D(Random random, Point3DReadOnly center, double maxEdgeLength, int numberOfVertices, + public static List nextCircleBasedConvexPolygon3D(Random random, + Point3DReadOnly center, + double maxEdgeLength, + int numberOfVertices, Vector3DReadOnly planeNormal) { List circleBasedConvexPolygon2D = EuclidGeometryRandomTools.nextCircleBasedConvexPolygon2D(random, @@ -617,8 +623,14 @@ public static ConvexPolytope3D nextConeConvexPolytope3D(Random random, double ce * @param divisionsMax the maximum number of divisions for discretizing the cone. * @return the random convex polytope 3D. */ - public static ConvexPolytope3D nextConeConvexPolytope3D(Random random, double centerMinMax, double heightMin, double heightMax, double radiusMin, - double radiusMax, int divisionsMin, int divisionsMax) + public static ConvexPolytope3D nextConeConvexPolytope3D(Random random, + double centerMinMax, + double heightMin, + double heightMax, + double radiusMin, + double radiusMax, + int divisionsMin, + int divisionsMax) { List coneVertices = EuclidPolytopeFactories.newConeVertices(EuclidCoreRandomTools.nextDouble(random, heightMin, heightMax), EuclidCoreRandomTools.nextDouble(random, radiusMin, radiusMax), @@ -710,8 +722,14 @@ public static ConvexPolytope3D nextCylinderConvexPolytope3D(Random random, doubl * @param divisionsMax the maximum number of divisions for discretizing the cylinder. * @return the random convex polytope 3D. */ - public static ConvexPolytope3D nextCylinderConvexPolytope3D(Random random, double centerMinMax, double lengthMin, double lengthMax, double radiusMin, - double radiusMax, int divisionsMin, int divisionsMax) + public static ConvexPolytope3D nextCylinderConvexPolytope3D(Random random, + double centerMinMax, + double lengthMin, + double lengthMax, + double radiusMin, + double radiusMax, + int divisionsMin, + int divisionsMax) { List cylinderVertices = EuclidPolytopeFactories.newCylinderVertices(EuclidCoreRandomTools.nextDouble(random, lengthMin, lengthMax), EuclidCoreRandomTools.nextDouble(random, radiusMin, radiusMax), @@ -922,8 +940,14 @@ public static ConvexPolytope3D nextPyramidConvexPolytope3D(Random random, double * @param baseWidthMax the maximum value for the width of the pyramid's base. * @return the random convex polytope 3D. */ - public static ConvexPolytope3D nextPyramidConvexPolytope3D(Random random, double centerMinMax, double heightMin, double heightMax, double baseLengthMin, - double baseLengthMax, double baseWidthMin, double baseWidthMax) + public static ConvexPolytope3D nextPyramidConvexPolytope3D(Random random, + double centerMinMax, + double heightMin, + double heightMax, + double baseLengthMin, + double baseLengthMax, + double baseWidthMin, + double baseWidthMax) { List pyramidVertices = EuclidPolytopeFactories.newPyramidVertices(EuclidCoreRandomTools.nextDouble(random, heightMin, heightMax), EuclidCoreRandomTools.nextDouble(random, baseLengthMin, baseLengthMax), diff --git a/src/shape/java/us/ihmc/euclid/shape/tools/EuclidShapeTestTools.java b/src/shape/java/us/ihmc/euclid/shape/tools/EuclidShapeTestTools.java index 33d44448b..0aa920ead 100644 --- a/src/shape/java/us/ihmc/euclid/shape/tools/EuclidShapeTestTools.java +++ b/src/shape/java/us/ihmc/euclid/shape/tools/EuclidShapeTestTools.java @@ -293,7 +293,10 @@ public static void assertCapsule3DGeometricallyEquals(String messagePrefix, Caps * @throws AssertionError if the two capsules do not represent the same geometry. If only one of the * arguments is equal to {@code null}. */ - public static void assertCapsule3DGeometricallyEquals(String messagePrefix, Capsule3DReadOnly expected, Capsule3DReadOnly actual, double epsilon, + public static void assertCapsule3DGeometricallyEquals(String messagePrefix, + Capsule3DReadOnly expected, + Capsule3DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -421,7 +424,10 @@ public static void assertCylinder3DGeometricallyEquals(String messagePrefix, Cyl * @throws AssertionError if the two cylinders do not represent the same geometry. If only one of * the arguments is equal to {@code null}. */ - public static void assertCylinder3DGeometricallyEquals(String messagePrefix, Cylinder3DReadOnly expected, Cylinder3DReadOnly actual, double epsilon, + public static void assertCylinder3DGeometricallyEquals(String messagePrefix, + Cylinder3DReadOnly expected, + Cylinder3DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -549,7 +555,10 @@ public static void assertEllipsoid3DGeometricallyEquals(String messagePrefix, El * @throws AssertionError if the two ellipsoids do not represent the same geometry. If only one of * the arguments is equal to {@code null}. */ - public static void assertEllipsoid3DGeometricallyEquals(String messagePrefix, Ellipsoid3DReadOnly expected, Ellipsoid3DReadOnly actual, double epsilon, + public static void assertEllipsoid3DGeometricallyEquals(String messagePrefix, + Ellipsoid3DReadOnly expected, + Ellipsoid3DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -677,7 +686,10 @@ public static void assertPointShape3DGeometricallyEquals(String messagePrefix, P * @throws AssertionError if the two point shapes do not represent the same geometry. If only one of * the arguments is equal to {@code null}. */ - public static void assertPointShape3DGeometricallyEquals(String messagePrefix, PointShape3DReadOnly expected, PointShape3DReadOnly actual, double epsilon, + public static void assertPointShape3DGeometricallyEquals(String messagePrefix, + PointShape3DReadOnly expected, + PointShape3DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -1083,7 +1095,8 @@ public static void assertTorus3DGeometricallyEquals(String messagePrefix, Torus3 * @throws AssertionError if the two collision results are not equal. If only one of the arguments * is equal to {@code null}. */ - public static void assertEuclidShape3DCollisionResultEquals(EuclidShape3DCollisionResultReadOnly expected, EuclidShape3DCollisionResultReadOnly actual, + public static void assertEuclidShape3DCollisionResultEquals(EuclidShape3DCollisionResultReadOnly expected, + EuclidShape3DCollisionResultReadOnly actual, double epsilon) { assertEuclidShape3DCollisionResultEquals(null, expected, actual, epsilon); @@ -1102,8 +1115,10 @@ public static void assertEuclidShape3DCollisionResultEquals(EuclidShape3DCollisi * @throws AssertionError if the two collision results are not equal. If only one of the arguments * is equal to {@code null}. */ - public static void assertEuclidShape3DCollisionResultEquals(String messagePrefix, EuclidShape3DCollisionResultReadOnly expected, - EuclidShape3DCollisionResultReadOnly actual, double epsilon) + public static void assertEuclidShape3DCollisionResultEquals(String messagePrefix, + EuclidShape3DCollisionResultReadOnly expected, + EuclidShape3DCollisionResultReadOnly actual, + double epsilon) { assertEuclidShape3DCollisionResultEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); } @@ -1123,8 +1138,11 @@ public static void assertEuclidShape3DCollisionResultEquals(String messagePrefix * @throws AssertionError if the two collision results are not equal. If only one of the arguments * is equal to {@code null}. */ - public static void assertEuclidShape3DCollisionResultEquals(String messagePrefix, EuclidShape3DCollisionResultReadOnly expected, - EuclidShape3DCollisionResultReadOnly actual, double epsilon, String format) + public static void assertEuclidShape3DCollisionResultEquals(String messagePrefix, + EuclidShape3DCollisionResultReadOnly expected, + EuclidShape3DCollisionResultReadOnly actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -1169,7 +1187,8 @@ public static void assertEuclidShape3DCollisionResultEquals(String messagePrefix * one of the arguments is equal to {@code null}. */ public static void assertEuclidShape3DCollisionResultGeometricallyEquals(EuclidShape3DCollisionResultReadOnly expected, - EuclidShape3DCollisionResultReadOnly actual, double epsilon) + EuclidShape3DCollisionResultReadOnly actual, + double epsilon) { assertEuclidShape3DCollisionResultGeometricallyEquals(null, expected, actual, epsilon); } @@ -1194,8 +1213,10 @@ public static void assertEuclidShape3DCollisionResultGeometricallyEquals(EuclidS * one of the arguments is equal to {@code null}. */ public static void assertEuclidShape3DCollisionResultGeometricallyEquals(EuclidShape3DCollisionResultReadOnly expected, - EuclidShape3DCollisionResultReadOnly actual, double distanceEpsilon, - double pointTangentialEpsilon, double normalEpsilon) + EuclidShape3DCollisionResultReadOnly actual, + double distanceEpsilon, + double pointTangentialEpsilon, + double normalEpsilon) { assertEuclidShape3DCollisionResultGeometricallyEquals(null, expected, actual, distanceEpsilon, pointTangentialEpsilon, normalEpsilon, DEFAULT_FORMAT); } @@ -1214,8 +1235,10 @@ public static void assertEuclidShape3DCollisionResultGeometricallyEquals(EuclidS * @throws AssertionError if the two collision results do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ - public static void assertEuclidShape3DCollisionResultGeometricallyEquals(String messagePrefix, EuclidShape3DCollisionResultReadOnly expected, - EuclidShape3DCollisionResultReadOnly actual, double epsilon) + public static void assertEuclidShape3DCollisionResultGeometricallyEquals(String messagePrefix, + EuclidShape3DCollisionResultReadOnly expected, + EuclidShape3DCollisionResultReadOnly actual, + double epsilon) { assertEuclidShape3DCollisionResultGeometricallyEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); } @@ -1240,9 +1263,12 @@ public static void assertEuclidShape3DCollisionResultGeometricallyEquals(String * @throws AssertionError if the two collision results do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ - public static void assertEuclidShape3DCollisionResultGeometricallyEquals(String messagePrefix, EuclidShape3DCollisionResultReadOnly expected, - EuclidShape3DCollisionResultReadOnly actual, double distanceEpsilon, - double pointTangentialEpsilon, double normalEpsilon) + public static void assertEuclidShape3DCollisionResultGeometricallyEquals(String messagePrefix, + EuclidShape3DCollisionResultReadOnly expected, + EuclidShape3DCollisionResultReadOnly actual, + double distanceEpsilon, + double pointTangentialEpsilon, + double normalEpsilon) { assertEuclidShape3DCollisionResultGeometricallyEquals(messagePrefix, expected, @@ -1269,8 +1295,11 @@ public static void assertEuclidShape3DCollisionResultGeometricallyEquals(String * @throws AssertionError if the two collision results do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ - public static void assertEuclidShape3DCollisionResultGeometricallyEquals(String messagePrefix, EuclidShape3DCollisionResultReadOnly expected, - EuclidShape3DCollisionResultReadOnly actual, double epsilon, String format) + public static void assertEuclidShape3DCollisionResultGeometricallyEquals(String messagePrefix, + EuclidShape3DCollisionResultReadOnly expected, + EuclidShape3DCollisionResultReadOnly actual, + double epsilon, + String format) { assertEuclidShape3DCollisionResultGeometricallyEquals(messagePrefix, expected, actual, epsilon, epsilon, epsilon, format); } @@ -1297,9 +1326,13 @@ public static void assertEuclidShape3DCollisionResultGeometricallyEquals(String * @throws AssertionError if the two collision results do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ - public static void assertEuclidShape3DCollisionResultGeometricallyEquals(String messagePrefix, EuclidShape3DCollisionResultReadOnly expected, - EuclidShape3DCollisionResultReadOnly actual, double distanceEpsilon, - double pointTangentialEpsilon, double normalEpsilon, String format) + public static void assertEuclidShape3DCollisionResultGeometricallyEquals(String messagePrefix, + EuclidShape3DCollisionResultReadOnly expected, + EuclidShape3DCollisionResultReadOnly actual, + double distanceEpsilon, + double pointTangentialEpsilon, + double normalEpsilon, + String format) { if (expected == null && actual == null) return; @@ -1507,7 +1540,10 @@ public static void assertConvexPolytope3DEquals(String messagePrefix, ConvexPoly * @throws AssertionError if the two convex polytopes are not equal. If only one of the arguments is * equal to {@code null}. */ - public static void assertConvexPolytope3DEquals(String messagePrefix, ConvexPolytope3DReadOnly expected, ConvexPolytope3DReadOnly actual, double epsilon, + public static void assertConvexPolytope3DEquals(String messagePrefix, + ConvexPolytope3DReadOnly expected, + ConvexPolytope3DReadOnly actual, + double epsilon, String format) { if (expected == null && actual == null) @@ -1552,7 +1588,9 @@ public static void assertConvexPolytope3DGeometricallyEquals(ConvexPolytope3DRea * @throws AssertionError if the two convex polytopes do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ - public static void assertConvexPolytope3DGeometricallyEquals(String messagePrefix, ConvexPolytope3DReadOnly expected, ConvexPolytope3DReadOnly actual, + public static void assertConvexPolytope3DGeometricallyEquals(String messagePrefix, + ConvexPolytope3DReadOnly expected, + ConvexPolytope3DReadOnly actual, double epsilon) { assertConvexPolytope3DGeometricallyEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); @@ -1574,8 +1612,11 @@ public static void assertConvexPolytope3DGeometricallyEquals(String messagePrefi * @throws AssertionError if the two convex polytopes do not represent the same geometry. If only * one of the arguments is equal to {@code null}. */ - public static void assertConvexPolytope3DGeometricallyEquals(String messagePrefix, ConvexPolytope3DReadOnly expected, ConvexPolytope3DReadOnly actual, - double epsilon, String format) + public static void assertConvexPolytope3DGeometricallyEquals(String messagePrefix, + ConvexPolytope3DReadOnly expected, + ConvexPolytope3DReadOnly actual, + double epsilon, + String format) { if (expected == null && actual == null) return; @@ -1951,14 +1992,19 @@ private static void throwNotEqualAssertionError(String messagePrefix, Torus3DRea EuclidCoreTestTools.throwNotEqualAssertionError(messagePrefix, expectedAsString, actualAsString); } - private static void throwNotEqualAssertionError(String messagePrefix, EuclidShape3DCollisionResultReadOnly expected, - EuclidShape3DCollisionResultReadOnly actual, String format) + private static void throwNotEqualAssertionError(String messagePrefix, + EuclidShape3DCollisionResultReadOnly expected, + EuclidShape3DCollisionResultReadOnly actual, + String format) { throwNotEqualAssertionError(messagePrefix, expected, actual, format, null); } - private static void throwNotEqualAssertionError(String messagePrefix, EuclidShape3DCollisionResultReadOnly expected, - EuclidShape3DCollisionResultReadOnly actual, String format, String differenceAsString) + private static void throwNotEqualAssertionError(String messagePrefix, + EuclidShape3DCollisionResultReadOnly expected, + EuclidShape3DCollisionResultReadOnly actual, + String format, + String differenceAsString) { String expectedAsString = getEuclidShape3DCollisionResultString(format, expected); String actualAsString = getEuclidShape3DCollisionResultString(format, actual); diff --git a/src/shape/java/us/ihmc/euclid/shape/tools/EuclidShapeTools.java b/src/shape/java/us/ihmc/euclid/shape/tools/EuclidShapeTools.java index bf9c95ecc..2020ad2e3 100644 --- a/src/shape/java/us/ihmc/euclid/shape/tools/EuclidShapeTools.java +++ b/src/shape/java/us/ihmc/euclid/shape/tools/EuclidShapeTools.java @@ -179,7 +179,9 @@ public static void supportingVertexBox3D(Vector3DReadOnly supportDirection, Vect * @param box3DSize the size of the box. Not modified. * @param boundingBoxToPack the bounding box in which the result is stored. Modified. */ - public static void boundingBoxBox3D(Point3DReadOnly box3DPosition, RotationMatrixReadOnly box3DOrientation, Vector3DReadOnly box3DSize, + public static void boundingBoxBox3D(Point3DReadOnly box3DPosition, + RotationMatrixReadOnly box3DOrientation, + Vector3DReadOnly box3DSize, BoundingBox3DBasics boundingBoxToPack) { double halfSizeX = 0.5 * box3DSize.getX(); @@ -224,7 +226,9 @@ public static void boundingBoxBox3D(Point3DReadOnly box3DPosition, RotationMatri * @return the signed distance between the query and the box. It is negative when the query is * inside, positive otherwise. */ - public static double evaluatePoint3DBox3DCollision(Point3DReadOnly query, Vector3DReadOnly box3DSize, Point3DBasics closestPointOnSurfaceToPack, + public static double evaluatePoint3DBox3DCollision(Point3DReadOnly query, + Vector3DReadOnly box3DSize, + Point3DBasics closestPointOnSurfaceToPack, Vector3DBasics normalToPack) { double halfSizeX = 0.5 * box3DSize.getX(); @@ -313,8 +317,12 @@ public static double evaluatePoint3DBox3DCollision(Point3DReadOnly query, Vector * shrinking it. * @return {@code true} if the query is inside or on the capsule's surface, {@code false} otherwise. */ - public static boolean isPoint3DInsideCapsule3D(Point3DReadOnly query, Point3DReadOnly capsule3DPosition, Vector3DReadOnly capsule3DAxis, - double capsule3DLength, double capsule3DRadius, double epsilon) + public static boolean isPoint3DInsideCapsule3D(Point3DReadOnly query, + Point3DReadOnly capsule3DPosition, + Vector3DReadOnly capsule3DAxis, + double capsule3DLength, + double capsule3DRadius, + double epsilon) { double capsule3DHalfLength = 0.5 * capsule3DLength; double topCenterX = capsule3DPosition.getX() + capsule3DHalfLength * capsule3DAxis.getX(); @@ -357,8 +365,11 @@ public static boolean isPoint3DInsideCapsule3D(Point3DReadOnly query, Point3DRea * @param capsule3DRadius the radius of the capsule. * @return the signed distance between the query and the capsule. */ - public static double signedDistanceBetweenPoint3DAndCapsule3D(Point3DReadOnly query, Point3DReadOnly capsule3DPosition, Vector3DReadOnly capsule3DAxis, - double capsule3DLength, double capsule3DRadius) + public static double signedDistanceBetweenPoint3DAndCapsule3D(Point3DReadOnly query, + Point3DReadOnly capsule3DPosition, + Vector3DReadOnly capsule3DAxis, + double capsule3DLength, + double capsule3DRadius) { double capsuleHalfLength = 0.5 * capsule3DLength; double topCenterX = capsule3DPosition.getX() + capsuleHalfLength * capsule3DAxis.getX(); @@ -397,8 +408,12 @@ public static double signedDistanceBetweenPoint3DAndCapsule3D(Point3DReadOnly qu * @param projectionToPack point in which the projection is stored. Modified. * @return whether the projection has succeeded or not. */ - public static boolean orthogonalProjectionOntoCapsule3D(Point3DReadOnly pointToProject, Point3DReadOnly capsule3DPosition, Vector3DReadOnly capsule3DAxis, - double capsule3DLength, double capsule3DRadius, Point3DBasics projectionToPack) + public static boolean orthogonalProjectionOntoCapsule3D(Point3DReadOnly pointToProject, + Point3DReadOnly capsule3DPosition, + Vector3DReadOnly capsule3DAxis, + double capsule3DLength, + double capsule3DRadius, + Point3DBasics projectionToPack) { if (capsule3DRadius <= 0.0) { @@ -497,8 +512,12 @@ else if (percentageOnAxis > 0.0) * @param capsule3DRadius the radius of the capsule. * @param supportingVertexToPack point in which the supporting vertex is stored. Modified. */ - public static void supportingVertexCapsule3D(Vector3DReadOnly supportDirection, Point3DReadOnly capsule3DPosition, Vector3DReadOnly capsule3DAxis, - double capsule3DLength, double capsule3DRadius, Point3DBasics supportingVertexToPack) + public static void supportingVertexCapsule3D(Vector3DReadOnly supportDirection, + Point3DReadOnly capsule3DPosition, + Vector3DReadOnly capsule3DAxis, + double capsule3DLength, + double capsule3DRadius, + Point3DBasics supportingVertexToPack) { double dot = supportDirection.dot(capsule3DAxis); double capsule3DHalfLength = dot > 0.0 ? 0.5 * capsule3DLength : -0.5 * capsule3DLength; @@ -518,7 +537,10 @@ public static void supportingVertexCapsule3D(Vector3DReadOnly supportDirection, * @param capsule3DRadius the radius of the capsule. * @param boundingBoxToPack the bounding box in which the result is stored. Modified. */ - public static void boundingBoxCapsule3D(Point3DReadOnly capsule3DPosition, Vector3DReadOnly capsule3DAxis, double capsule3DLength, double capsule3DRadius, + public static void boundingBoxCapsule3D(Point3DReadOnly capsule3DPosition, + Vector3DReadOnly capsule3DAxis, + double capsule3DLength, + double capsule3DRadius, BoundingBox3DBasics boundingBoxToPack) { double capsule3DHalfLength = 0.5 * capsule3DLength; @@ -552,8 +574,12 @@ public static void boundingBoxCapsule3D(Point3DReadOnly capsule3DPosition, Vecto * @return the signed distance between the query and the capsule. It is negative when the query is * inside, positive otherwise. */ - public static double evaluatePoint3DCapsule3DCollision(Point3DReadOnly query, Point3DReadOnly capsule3DPosition, Vector3DReadOnly capsule3DAxis, - double capsule3DLength, double capsule3DRadius, Point3DBasics closestPointOnSurfaceToPack, + public static double evaluatePoint3DCapsule3DCollision(Point3DReadOnly query, + Point3DReadOnly capsule3DPosition, + Vector3DReadOnly capsule3DAxis, + double capsule3DLength, + double capsule3DRadius, + Point3DBasics closestPointOnSurfaceToPack, Vector3DBasics normalToPack) { if (capsule3DRadius <= 0.0) @@ -632,8 +658,12 @@ else if (percentageOnAxis < -capsule3DHalfLength) * @return {@code true} if the query is inside or on the cylinder's surface, {@code false} * otherwise. */ - public static boolean isPoint3DInsideCylinder3D(Point3DReadOnly query, Point3DReadOnly cylinder3DPosition, Vector3DReadOnly cylinder3DAxis, - double cylinder3DLength, double cylinder3DRadius, double epsilon) + public static boolean isPoint3DInsideCylinder3D(Point3DReadOnly query, + Point3DReadOnly cylinder3DPosition, + Vector3DReadOnly cylinder3DAxis, + double cylinder3DLength, + double cylinder3DRadius, + double epsilon) { double positionOnAxis = EuclidGeometryTools.percentageAlongLine3D(query, cylinder3DPosition, cylinder3DAxis); @@ -668,8 +698,11 @@ public static boolean isPoint3DInsideCylinder3D(Point3DReadOnly query, Point3DRe * @param cylinder3DRadius the radius of the cylinder. * @return the signed distance between the query and the cylinder. */ - public static double signedDistanceBetweenPoint3DAndCylinder3D(Point3DReadOnly query, Point3DReadOnly cylinder3DPosition, Vector3DReadOnly cylinder3DAxis, - double cylinder3DLength, double cylinder3DRadius) + public static double signedDistanceBetweenPoint3DAndCylinder3D(Point3DReadOnly query, + Point3DReadOnly cylinder3DPosition, + Vector3DReadOnly cylinder3DAxis, + double cylinder3DLength, + double cylinder3DRadius) { if (cylinder3DRadius <= 0.0 || cylinder3DLength <= 0.0) { @@ -763,8 +796,12 @@ else if (positionOnAxisClamped > halfLength) * @param projectionToPack point in which the projection is stored. Modified. * @return whether the projection has succeeded or not. */ - public static boolean orthogonalProjectionOntoCylinder3D(Point3DReadOnly pointToProject, Point3DReadOnly cylinder3DPosition, Vector3DReadOnly cylinder3DAxis, - double cylinder3DLength, double cylinder3DRadius, Point3DBasics projectionToPack) + public static boolean orthogonalProjectionOntoCylinder3D(Point3DReadOnly pointToProject, + Point3DReadOnly cylinder3DPosition, + Vector3DReadOnly cylinder3DAxis, + double cylinder3DLength, + double cylinder3DRadius, + Point3DBasics projectionToPack) { if (cylinder3DRadius <= 0.0 || cylinder3DLength <= 0.0) { @@ -849,8 +886,12 @@ else if (positionOnAxisClamped > halfLength) * @param cylinder3DRadius the radius of the cylinder. * @param supportingVertexToPack point in which the supporting vertex is stored. Modified. */ - public static void supportingVertexCylinder3D(Vector3DReadOnly supportDirection, Point3DReadOnly cylinder3DPosition, Vector3DReadOnly cylinder3DAxis, - double cylinder3DLength, double cylinder3DRadius, Point3DBasics supportingVertexToPack) + public static void supportingVertexCylinder3D(Vector3DReadOnly supportDirection, + Point3DReadOnly cylinder3DPosition, + Vector3DReadOnly cylinder3DAxis, + double cylinder3DLength, + double cylinder3DRadius, + Point3DBasics supportingVertexToPack) { supportingVertexToPack.set(supportDirection); @@ -883,8 +924,11 @@ public static void supportingVertexCylinder3D(Vector3DReadOnly supportDirection, * @param cylinder3DRadius the radius of the cylinder. * @param boundingBoxToPack the bounding box in which the result is stored. Modified. */ - public static void boundingBoxCylinder3D(Point3DReadOnly cylinder3DPosition, Vector3DReadOnly cylinder3DAxis, double cylinder3DLength, - double cylinder3DRadius, BoundingBox3DBasics boundingBoxToPack) + public static void boundingBoxCylinder3D(Point3DReadOnly cylinder3DPosition, + Vector3DReadOnly cylinder3DAxis, + double cylinder3DLength, + double cylinder3DRadius, + BoundingBox3DBasics boundingBoxToPack) { // From https://iquilezles.org/www/articles/diskbbox/diskbbox.htm double cylinder3DHalfLength = 0.5 * cylinder3DLength; @@ -922,8 +966,12 @@ public static void boundingBoxCylinder3D(Point3DReadOnly cylinder3DPosition, Vec * @return the signed distance between the query and the cylinder. It is negative when the query is * inside, positive otherwise. */ - public static double evaluatePoint3DCylinder3DCollision(Point3DReadOnly query, Point3DReadOnly cylinder3DPosition, Vector3DReadOnly cylinder3DAxis, - double cylinder3DLength, double cylinder3DRadius, Point3DBasics closestPointOnSurfaceToPack, + public static double evaluatePoint3DCylinder3DCollision(Point3DReadOnly query, + Point3DReadOnly cylinder3DPosition, + Vector3DReadOnly cylinder3DAxis, + double cylinder3DLength, + double cylinder3DRadius, + Point3DBasics closestPointOnSurfaceToPack, Vector3DBasics normalToPack) { if (cylinder3DRadius <= 0.0 || cylinder3DLength <= 0.0) @@ -1166,8 +1214,10 @@ public static void supportingVertexEllipsoid3D(Vector3DReadOnly supportDirection * @param ellipsoid3DRadii the radii of the ellipsoid. Not modified. * @param boundingBoxToPack the bounding box in which the result is stored. Modified. */ - public static void boundingBoxEllipsoid3D(Point3DReadOnly ellipsoid3DPosition, RotationMatrixReadOnly ellipsoid3DOrientation, - Vector3DReadOnly ellipsoid3DRadii, BoundingBox3DBasics boundingBoxToPack) + public static void boundingBoxEllipsoid3D(Point3DReadOnly ellipsoid3DPosition, + RotationMatrixReadOnly ellipsoid3DOrientation, + Vector3DReadOnly ellipsoid3DRadii, + BoundingBox3DBasics boundingBoxToPack) { double xRange, yRange, zRange; @@ -1219,7 +1269,9 @@ public static void boundingBoxEllipsoid3D(Point3DReadOnly ellipsoid3DPosition, R * @return the signed distance between the query and the ellipsoid. It is negative when the query is * inside, positive otherwise. */ - public static double evaluatePoint3DEllipsoid3DCollision(Point3DReadOnly query, Vector3DReadOnly ellipsoid3DRadii, Point3DBasics closestPointOnSurfaceToPack, + public static double evaluatePoint3DEllipsoid3DCollision(Point3DReadOnly query, + Vector3DReadOnly ellipsoid3DRadii, + Point3DBasics closestPointOnSurfaceToPack, Vector3DBasics normalToPack) { double distance = EuclidEllipsoid3DTools.distancePoint3DEllipsoid3D(ellipsoid3DRadii, query, closestPointOnSurfaceToPack); @@ -1607,7 +1659,9 @@ else if (supportDirection.getX() > 0.0) * @return the signed distance between the query and the box. It is negative when the query is * inside, positive otherwise. */ - public static double evaluatePoint3DRamp3DCollision(Point3DReadOnly query, Vector3DReadOnly ramp3DSize, Point3DBasics closestPointOnSurfaceToPack, + public static double evaluatePoint3DRamp3DCollision(Point3DReadOnly query, + Vector3DReadOnly ramp3DSize, + Point3DBasics closestPointOnSurfaceToPack, Vector3DBasics normalToPack) { double rampLength = computeRamp3DLength(ramp3DSize); @@ -1754,8 +1808,13 @@ else if (distanceToBottomFace <= distanceToSlopeFace) } } - private static double computeNormalAndDistanceFromClosestPoint(double xLocalQuery, double yLocalQuery, double zLocalQuery, double xLocalClosest, - double yLocalClosest, double zLocalClosest, Vector3DBasics normalToPack) + private static double computeNormalAndDistanceFromClosestPoint(double xLocalQuery, + double yLocalQuery, + double zLocalQuery, + double xLocalClosest, + double yLocalClosest, + double zLocalClosest, + Vector3DBasics normalToPack) { double dx = xLocalQuery - xLocalClosest; double dy = yLocalQuery - yLocalClosest; @@ -1904,7 +1963,9 @@ public static double signedDistanceBetweenPoint3DAndSphere3D(Point3DReadOnly que * @param projectionToPack point in which the projection is stored. Modified. * @return whether the projection has succeeded or not. */ - public static boolean orthogonalProjectionOntoSphere3D(Point3DReadOnly pointToProject, Point3DReadOnly sphere3DPosition, double sphere3DRadius, + public static boolean orthogonalProjectionOntoSphere3D(Point3DReadOnly pointToProject, + Point3DReadOnly sphere3DPosition, + double sphere3DRadius, Point3DBasics projectionToPack) { double distanceSquared = sphere3DPosition.distanceSquared(pointToProject); @@ -1929,7 +1990,9 @@ public static boolean orthogonalProjectionOntoSphere3D(Point3DReadOnly pointToPr * @param sphere3DRadius the radius of the sphere. * @param supportingVertexToPack point in which the supporting vertex is stored. Modified. */ - public static void supportingVertexSphere3D(Vector3DReadOnly supportDirection, Point3DReadOnly sphere3DPosition, double sphere3DRadius, + public static void supportingVertexSphere3D(Vector3DReadOnly supportDirection, + Point3DReadOnly sphere3DPosition, + double sphere3DRadius, Point3DBasics supportingVertexToPack) { supportingVertexToPack.scaleAdd(sphere3DRadius / supportDirection.length(), supportDirection, sphere3DPosition); @@ -1947,8 +2010,11 @@ public static void supportingVertexSphere3D(Vector3DReadOnly supportDirection, P * @return the signed distance between the query and the sphere. It is negative when the query is * inside, positive otherwise. */ - public static double evaluatePoint3DSphere3DCollision(Point3DReadOnly query, Point3DReadOnly sphere3DPosition, double sphere3DRadius, - Point3DBasics closestPointOnSurfaceToPack, Vector3DBasics normalToPack) + public static double evaluatePoint3DSphere3DCollision(Point3DReadOnly query, + Point3DReadOnly sphere3DPosition, + double sphere3DRadius, + Point3DBasics closestPointOnSurfaceToPack, + Vector3DBasics normalToPack) { double distance = sphere3DPosition.distance(query); @@ -1982,8 +2048,12 @@ public static double evaluatePoint3DSphere3DCollision(Point3DReadOnly query, Poi * shrinking it. * @return {@code true} if the query is inside or on the capsule's surface, {@code false} otherwise. */ - public static boolean isPoint3DInsideTorus3D(Point3DReadOnly query, Point3DReadOnly torus3DPosition, Vector3DReadOnly torus3DAxis, double torus3DRadius, - double torus3DTubeRadius, double epsilon) + public static boolean isPoint3DInsideTorus3D(Point3DReadOnly query, + Point3DReadOnly torus3DPosition, + Vector3DReadOnly torus3DAxis, + double torus3DRadius, + double torus3DTubeRadius, + double epsilon) { double positionOnAxis = EuclidGeometryTools.percentageAlongLine3D(query, torus3DPosition, torus3DAxis); double projectionOnAxisX = torus3DPosition.getX() + positionOnAxis * torus3DAxis.getX(); @@ -2020,8 +2090,11 @@ public static boolean isPoint3DInsideTorus3D(Point3DReadOnly query, Point3DReadO * @param torus3DTubeRadius the radius of the tube. * @return the signed distance between the query and the torus. */ - public static double signedDistanceBetweenPoint3DAndTorus3D(Point3DReadOnly query, Point3DReadOnly torus3DPosition, Vector3DReadOnly torus3DAxis, - double torus3DRadius, double torus3DTubeRadius) + public static double signedDistanceBetweenPoint3DAndTorus3D(Point3DReadOnly query, + Point3DReadOnly torus3DPosition, + Vector3DReadOnly torus3DAxis, + double torus3DRadius, + double torus3DTubeRadius) { double torus3DPositionX = torus3DPosition.getX(); double torus3DPositionY = torus3DPosition.getY(); @@ -2075,8 +2148,12 @@ public static double signedDistanceBetweenPoint3DAndTorus3D(Point3DReadOnly quer * @param projectionToPack point in which the projection is stored. Modified. * @return whether the projection has succeeded or not. */ - public static boolean orthogonalProjectionOntoTorus3D(Point3DReadOnly pointToProject, Point3DReadOnly torus3DPosition, Vector3DReadOnly torus3DAxis, - double torus3DRadius, double torus3DTubeRadius, Point3DBasics projectionToPack) + public static boolean orthogonalProjectionOntoTorus3D(Point3DReadOnly pointToProject, + Point3DReadOnly torus3DPosition, + Vector3DReadOnly torus3DAxis, + double torus3DRadius, + double torus3DTubeRadius, + Point3DBasics projectionToPack) { double x = pointToProject.getX() - torus3DPosition.getX(); double y = pointToProject.getY() - torus3DPosition.getY(); @@ -2163,8 +2240,12 @@ public static boolean orthogonalProjectionOntoTorus3D(Point3DReadOnly pointToPro * @return the signed distance between the query and the torus. It is negative when the query is * inside, positive otherwise. */ - public static double evaluatePoint3DTorus3DCollision(Point3DReadOnly query, Point3DReadOnly torus3DPosition, Vector3DReadOnly torus3DAxis, - double torus3DRadius, double torus3DTubeRadius, Point3DBasics closestPointOnSurfaceToPack, + public static double evaluatePoint3DTorus3DCollision(Point3DReadOnly query, + Point3DReadOnly torus3DPosition, + Vector3DReadOnly torus3DAxis, + double torus3DRadius, + double torus3DTubeRadius, + Point3DBasics closestPointOnSurfaceToPack, Vector3DBasics normalToPack) { double torus3DPositionX = torus3DPosition.getX(); @@ -2274,8 +2355,11 @@ public static double evaluatePoint3DTorus3DCollision(Point3DReadOnly query, Poin * @param circle3DRadius the radius of the circle. * @param supportingVertexToPack point in which the supporting vertex is stored. Modified. */ - public static void supportingVertexCircle3D(Vector3DReadOnly supportDirection, Point3DReadOnly circle3DPosition, Vector3DReadOnly circle3DAxis, - double circle3DRadius, Point3DBasics supportingVertexToPack) + public static void supportingVertexCircle3D(Vector3DReadOnly supportDirection, + Point3DReadOnly circle3DPosition, + Vector3DReadOnly circle3DAxis, + double circle3DRadius, + Point3DBasics supportingVertexToPack) { supportingVertexToPack.set(supportDirection); double dot = supportDirection.dot(circle3DAxis); @@ -2327,8 +2411,12 @@ public static void supportingVertexCircle3D(Vector3DReadOnly supportDirection, P * @param torus3DTubeRadius the radius of the tube. * @param supportingVertexToPack point in which the supporting vertex is stored. Modified. */ - public static void innerSupportingVertexTorus3D(Vector3DReadOnly supportDirection, Point3DReadOnly torus3DPosition, Vector3DReadOnly torus3DAxis, - double torus3DRadius, double torus3DTubeRadius, Point3DBasics supportingVertexToPack) + public static void innerSupportingVertexTorus3D(Vector3DReadOnly supportDirection, + Point3DReadOnly torus3DPosition, + Vector3DReadOnly torus3DAxis, + double torus3DRadius, + double torus3DTubeRadius, + Point3DBasics supportingVertexToPack) { double torus3DAxisX = torus3DAxis.getX(); double torus3DAxisY = torus3DAxis.getY(); @@ -2590,7 +2678,10 @@ public static double icosahedronRadius(double edgeLength) * @return {@code true} if the two points are considered to represent the same geometry, * {@code false} otherwise. */ - public static boolean geometricallyEquals(Point3DReadOnly expected, Point3DReadOnly actual, Vector3DReadOnly normal, double normalEpsilon, + public static boolean geometricallyEquals(Point3DReadOnly expected, + Point3DReadOnly actual, + Vector3DReadOnly normal, + double normalEpsilon, double tangentialEpsilon) { double normalX = normal.getX(); @@ -2617,8 +2708,13 @@ public static boolean geometricallyEquals(Point3DReadOnly expected, Point3DReadO * @return {@code true} if the two points are considered to represent the same geometry, * {@code false} otherwise. */ - public static boolean geometricallyEquals(Point3DReadOnly expected, Point3DReadOnly actual, double normalX, double normalY, double normalZ, - double normalEpsilon, double tangentialEpsilon) + public static boolean geometricallyEquals(Point3DReadOnly expected, + Point3DReadOnly actual, + double normalX, + double normalY, + double normalZ, + double normalEpsilon, + double tangentialEpsilon) { double normalLengthSquared = EuclidCoreTools.normSquared(normalX, normalY, normalZ); diff --git a/src/test/java/us/ihmc/euclid/geometry/ConvexPolygon2DBasicsTest.java b/src/test/java/us/ihmc/euclid/geometry/ConvexPolygon2DBasicsTest.java index 7a0ff994c..1d3f72837 100644 --- a/src/test/java/us/ihmc/euclid/geometry/ConvexPolygon2DBasicsTest.java +++ b/src/test/java/us/ihmc/euclid/geometry/ConvexPolygon2DBasicsTest.java @@ -591,7 +591,7 @@ public void testIsInside() for (int i = 0; i < ITERATIONS; i++) { // Testing that querying for one of the vertices it returns true ConvexPolygon2D convexPolygon2D = EuclidGeometryRandomTools.nextConvexPolygon2D(random); - + for (Point2DReadOnly vertex : convexPolygon2D.getPolygonVerticesView()) { assertTrue(convexPolygon2D.isPointInside(vertex)); diff --git a/src/test/java/us/ihmc/euclid/geometry/LineSegment2DTest.java b/src/test/java/us/ihmc/euclid/geometry/LineSegment2DTest.java index 87f4e54bd..a209344e5 100644 --- a/src/test/java/us/ihmc/euclid/geometry/LineSegment2DTest.java +++ b/src/test/java/us/ihmc/euclid/geometry/LineSegment2DTest.java @@ -438,8 +438,11 @@ public void testIntersectionWithLineSegment2d2() } } - private void assertOnlyExistenceOfIntersectionAllCombinations(boolean intersectionExist, Point2D lineSegmentStart1, Point2D lineSegmentEnd1, - Point2D lineSegmentStart2, Point2D lineSegmentEnd2) + private void assertOnlyExistenceOfIntersectionAllCombinations(boolean intersectionExist, + Point2D lineSegmentStart1, + Point2D lineSegmentEnd1, + Point2D lineSegmentStart2, + Point2D lineSegmentEnd2) { LineSegment2D lineSegment1 = new LineSegment2D(lineSegmentStart1, lineSegmentEnd1); LineSegment2D reverseLineSegment1 = new LineSegment2D(lineSegmentEnd1, lineSegmentStart1); @@ -472,8 +475,11 @@ private void assertOnlyExistenceOfIntersectionAllCombinations(boolean intersecti } } - private void assertAllCombinationsOfIntersectionWith(Point2D expectedIntersection, Point2D lineSegmentStart1, Point2D lineSegmentEnd1, - Point2D lineSegmentStart2, Point2D lineSegmentEnd2) + private void assertAllCombinationsOfIntersectionWith(Point2D expectedIntersection, + Point2D lineSegmentStart1, + Point2D lineSegmentEnd1, + Point2D lineSegmentStart2, + Point2D lineSegmentEnd2) { LineSegment2D lineSegment1 = new LineSegment2D(lineSegmentStart1, lineSegmentEnd1); diff --git a/src/test/java/us/ihmc/euclid/geometry/Triangle3DBasicsTest.java b/src/test/java/us/ihmc/euclid/geometry/Triangle3DBasicsTest.java index ac23b76ba..51c3db497 100644 --- a/src/test/java/us/ihmc/euclid/geometry/Triangle3DBasicsTest.java +++ b/src/test/java/us/ihmc/euclid/geometry/Triangle3DBasicsTest.java @@ -22,10 +22,9 @@ public abstract class Triangle3DBasicsTest public abstract T newRandomTriangle3D(Random random); public abstract T newTriangle3D(Point3DReadOnly A, Point3DReadOnly B, Point3DReadOnly C); - + public abstract double getEpsilon(); - @Test public void testSetter() { @@ -41,14 +40,14 @@ public void testSetter() assertFalse(firstTriangle.equals(secondTriangle)); firstTriangle.set(secondTriangle); assertEquals(firstTriangle, secondTriangle); - + //Test set by 3 points assertFalse(firstTriangle.equals(thirdTriangle)); - thirdTriangle.set(firstTriangle.getA(),firstTriangle.getB(),firstTriangle.getC()); + thirdTriangle.set(firstTriangle.getA(), firstTriangle.getB(), firstTriangle.getC()); assertEquals(firstTriangle, thirdTriangle); } } - + @Test public void testSetToZero() throws Exception { @@ -68,7 +67,7 @@ public void testSetToZero() throws Exception assertTrue(0 == triangle.getB().getZ()); assertTrue(0 == triangle.getC().getX()); assertTrue(0 == triangle.getC().getY()); - assertTrue(0 == triangle.getC().getZ()); + assertTrue(0 == triangle.getC().getZ()); } @Test @@ -92,17 +91,17 @@ public void testContainsNaN() throws Exception Point3D pointA = new Point3D(1.0, 2.0, 3.0); Point3D pointB = new Point3D(4.0, 5.0, 6.0); Point3D pointC = new Point3D(7.0, 8.0, 9.0); - + T triangle = newEmptyTriangle3D(); assertFalse(triangle.containsNaN()); triangle.set(pointA, pointB, pointC); assertFalse(triangle.containsNaN()); - + //Test containsNan method pointA.setX(Double.NaN); triangle.set(pointA, pointB, pointC); assertTrue(triangle.containsNaN()); - + pointA.setX(0.0); pointA.setY(Double.NaN); triangle.set(pointA, pointB, pointC); @@ -112,12 +111,12 @@ public void testContainsNaN() throws Exception pointA.setZ(Double.NaN); triangle.set(pointA, pointB, pointC); assertTrue(triangle.containsNaN()); - + pointA.setZ(1.0); pointB.setX(Double.NaN); triangle.set(pointA, pointB, pointC); assertTrue(triangle.containsNaN()); - + pointB.setX(0.0); pointB.setY(Double.NaN); triangle.set(pointA, pointB, pointC); @@ -127,27 +126,27 @@ public void testContainsNaN() throws Exception pointB.setZ(Double.NaN); triangle.set(pointA, pointB, pointC); assertTrue(triangle.containsNaN()); - + pointB.setZ(1.0); pointC.setX(Double.NaN); triangle.set(pointA, pointB, pointC); - assertTrue(triangle.containsNaN()); + assertTrue(triangle.containsNaN()); pointC.setX(0.0); pointC.setY(Double.NaN); triangle.set(pointA, pointB, pointC); - assertTrue(triangle.containsNaN()); - + assertTrue(triangle.containsNaN()); + pointC.setY(0.0); pointC.setZ(Double.NaN); triangle.set(pointA, pointB, pointC); - assertTrue(triangle.containsNaN()); - + assertTrue(triangle.containsNaN()); + pointC.setZ(1.0); triangle.set(pointA, pointB, pointC); - assertFalse(triangle.containsNaN()); + assertFalse(triangle.containsNaN()); } - + @Test public void testApplyTransform() throws Exception { @@ -158,7 +157,7 @@ public void testApplyTransform() throws Exception RigidBodyTransform transform = EuclidCoreRandomTools.nextRigidBodyTransform(random); T original = newRandomTriangle3D(random); T expected = newEmptyTriangle3D(); - + expected.set(original); assertEquals(expected, original); @@ -170,13 +169,13 @@ public void testApplyTransform() throws Exception original.applyTransform(transform); assertEquals(expected, original); } - + for (int i = 0; i < ITERATIONS; i++) { QuaternionBasedTransform transform = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); T original = newRandomTriangle3D(random); T expected = newEmptyTriangle3D(); - + expected.set(original); assertEquals(expected, original); @@ -188,13 +187,13 @@ public void testApplyTransform() throws Exception original.applyTransform(transform); assertEquals(expected, original); } - + for (int i = 0; i < ITERATIONS; i++) { AffineTransform transform = EuclidCoreRandomTools.nextAffineTransform(random); T original = newRandomTriangle3D(random); T expected = newEmptyTriangle3D(); - + expected.set(original); assertEquals(expected, original); @@ -207,7 +206,7 @@ public void testApplyTransform() throws Exception assertEquals(expected, original); } } - + @Test public void testApplyInverseTransform() throws Exception { @@ -218,45 +217,45 @@ public void testApplyInverseTransform() throws Exception RigidBodyTransform transform = EuclidCoreRandomTools.nextRigidBodyTransform(random); T original = newRandomTriangle3D(random); T expected = newEmptyTriangle3D(); - + expected.set(original); assertEquals(original, expected); - + expected.applyTransform(transform); assertFalse(expected.equals(original)); expected.applyInverseTransform(transform); assertTrue(expected.epsilonEquals(original, getEpsilon())); } - + for (int i = 0; i < ITERATIONS; i++) { QuaternionBasedTransform transform = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); T original = newRandomTriangle3D(random); T expected = newEmptyTriangle3D(); - + expected.set(original); assertEquals(original, expected); - + expected.applyTransform(transform); assertFalse(expected.equals(original)); expected.applyInverseTransform(transform); assertTrue(expected.epsilonEquals(original, getEpsilon())); } - + for (int i = 0; i < ITERATIONS; i++) { AffineTransform transform = EuclidCoreRandomTools.nextAffineTransform(random); T original = newRandomTriangle3D(random); T expected = newEmptyTriangle3D(); - + expected.set(original); assertEquals(original, expected); - + expected.applyTransform(transform); assertFalse(expected.equals(original)); expected.applyInverseTransform(transform); assertTrue(expected.epsilonEquals(original, getEpsilon())); } - } + } } diff --git a/src/test/java/us/ihmc/euclid/geometry/Triangle3DTest.java b/src/test/java/us/ihmc/euclid/geometry/Triangle3DTest.java index 6d4a86bd5..baf88f911 100644 --- a/src/test/java/us/ihmc/euclid/geometry/Triangle3DTest.java +++ b/src/test/java/us/ihmc/euclid/geometry/Triangle3DTest.java @@ -27,7 +27,7 @@ public Triangle3D newRandomTriangle3D(Random random) Point3D pointA = EuclidCoreRandomTools.nextPoint3D(random); Point3D pointB = EuclidCoreRandomTools.nextPoint3D(random); Point3D pointC = EuclidCoreRandomTools.nextPoint3D(random); - + return new Triangle3D(pointA, pointB, pointC); } @@ -37,9 +37,9 @@ public Triangle3D newTriangle3D(Point3DReadOnly A, Point3DReadOnly B, Point3DRea // Return new Triangle3D with points return new Triangle3D(A, B, C); } - + @Override - public double getEpsilon() + public double getEpsilon() { return 1.0e-5; } @@ -48,7 +48,7 @@ public double getEpsilon() public void testConstructors() { Random random = new Random(1000L); - + // Test Triangle3D() Triangle3D triangle = new Triangle3D(); @@ -67,14 +67,14 @@ public void testConstructors() Point3D pointA = EuclidCoreRandomTools.nextPoint3D(random); Point3D pointB = EuclidCoreRandomTools.nextPoint3D(random); Point3D pointC = EuclidCoreRandomTools.nextPoint3D(random); - + // Test Triangle3D(Point3DReadOnly a, Point3DReadOnly b, Point3DReadOnly c) Triangle3D triangle3DbyPointPointPoint = new Triangle3D(pointA, pointB, pointC); assertEquals(triangle3DbyPointPointPoint.getA(), pointA); assertEquals(triangle3DbyPointPointPoint.getB(), pointB); assertEquals(triangle3DbyPointPointPoint.getC(), pointC); - + // Test Triangle3D(Triangle3DReadOnly other) - copy of triangle3DbyPointPointPoint Triangle3D triangle3DbyCopy = new Triangle3D(triangle3DbyPointPointPoint); assertFalse(triangle3DbyCopy == triangle3DbyPointPointPoint); @@ -84,7 +84,7 @@ public void testConstructors() } } - + @Test public void testEpsilonEquals() throws Exception { @@ -96,236 +96,236 @@ public void testEpsilonEquals() throws Exception Triangle3D original = new Triangle3D(pointA, pointB, pointC); Triangle3D same = new Triangle3D(original); assertTrue(original.epsilonEquals(same, getEpsilon())); - + for (int i = 0; i < ITERATIONS; i++) { pointA = EuclidCoreRandomTools.nextPoint3D(random); pointB = EuclidCoreRandomTools.nextPoint3D(random); pointC = EuclidCoreRandomTools.nextPoint3D(random); - + original = new Triangle3D(pointA, pointB, pointC); - + Point3D pointAChanged = new Point3D(pointA); Point3D pointBChanged = new Point3D(pointB); Point3D pointCChanged = new Point3D(pointC); - + //Point A above epsilon pointAChanged.setElement(i % 3, pointAChanged.getElement(i % 3) - 1.01 * getEpsilon()); Triangle3D changedA = new Triangle3D(pointAChanged, pointB, pointC); assertFalse(changedA.epsilonEquals(original, getEpsilon())); - + //Point A below epsilon pointAChanged.set(pointA); pointAChanged.setElement(i % 3, pointAChanged.getElement(i % 3) - 0.99 * getEpsilon()); changedA = new Triangle3D(pointAChanged, pointB, pointC); assertTrue(changedA.epsilonEquals(original, getEpsilon())); - + //Point B above epsilon pointBChanged.setElement(i % 3, pointBChanged.getElement(i % 3) - 1.01 * getEpsilon()); Triangle3D changedB = new Triangle3D(pointA, pointBChanged, pointC); assertFalse(changedB.epsilonEquals(original, getEpsilon())); - + //Point B below epsilon pointBChanged.set(pointB); pointBChanged.setElement(i % 3, pointBChanged.getElement(i % 3) - 0.99 * getEpsilon()); changedB = new Triangle3D(pointA, pointBChanged, pointC); - assertTrue(changedB.epsilonEquals(original, getEpsilon())); - + assertTrue(changedB.epsilonEquals(original, getEpsilon())); + //Point C above epsilon pointCChanged.setElement(i % 3, pointCChanged.getElement(i % 3) - 1.01 * getEpsilon()); Triangle3D changedC = new Triangle3D(pointA, pointB, pointCChanged); assertFalse(changedC.epsilonEquals(original, getEpsilon())); - + //Point C below epsilon pointCChanged.set(pointC); pointCChanged.setElement(i % 3, pointCChanged.getElement(i % 3) - 0.99 * getEpsilon()); changedC = new Triangle3D(pointA, pointB, pointCChanged); - assertTrue(changedC.epsilonEquals(original, getEpsilon())); - } + assertTrue(changedC.epsilonEquals(original, getEpsilon())); + } } - + @Test public void testGeometricallyEquals() throws Exception { Random random = new Random(87452L); - + for (int i = 0; i < ITERATIONS; i++) { Point3D pointA = EuclidCoreRandomTools.nextPoint3D(random); Point3D pointB = EuclidCoreRandomTools.nextPoint3D(random); Point3D pointC = EuclidCoreRandomTools.nextPoint3D(random); - + Triangle3D original = new Triangle3D(pointA, pointB, pointC); Triangle3D expected = new Triangle3D(original); assertTrue(original.geometricallyEquals(expected, getEpsilon())); - + expected = new Triangle3D(pointA, pointC, pointB); assertTrue(original.geometricallyEquals(expected, getEpsilon())); - + expected = new Triangle3D(pointB, pointA, pointC); assertTrue(original.geometricallyEquals(expected, getEpsilon())); - + expected = new Triangle3D(pointB, pointC, pointA); assertTrue(original.geometricallyEquals(expected, getEpsilon())); - + expected = new Triangle3D(pointC, pointB, pointA); assertTrue(original.geometricallyEquals(expected, getEpsilon())); - + expected = new Triangle3D(pointC, pointA, pointB); - assertTrue(original.geometricallyEquals(expected, getEpsilon())); - + assertTrue(original.geometricallyEquals(expected, getEpsilon())); + expected = new Triangle3D(pointA, pointA, pointA); assertFalse(original.geometricallyEquals(expected, getEpsilon())); - + expected = new Triangle3D(pointB, pointB, pointB); assertFalse(original.geometricallyEquals(expected, getEpsilon())); - + expected = new Triangle3D(pointC, pointC, pointC); assertFalse(original.geometricallyEquals(expected, getEpsilon())); - + expected = new Triangle3D(pointA, pointA, pointB); assertFalse(original.geometricallyEquals(expected, getEpsilon())); - + expected = new Triangle3D(pointA, pointA, pointC); assertFalse(original.geometricallyEquals(expected, getEpsilon())); - + expected = new Triangle3D(pointB, pointB, pointA); assertFalse(original.geometricallyEquals(expected, getEpsilon())); - + expected = new Triangle3D(pointB, pointB, pointC); assertFalse(original.geometricallyEquals(expected, getEpsilon())); - + expected = new Triangle3D(pointC, pointC, pointA); assertFalse(original.geometricallyEquals(expected, getEpsilon())); - + expected = new Triangle3D(pointC, pointC, pointB); assertFalse(original.geometricallyEquals(expected, getEpsilon())); - + expected = new Triangle3D(pointA, pointB, pointB); assertFalse(original.geometricallyEquals(expected, getEpsilon())); - + expected = new Triangle3D(pointA, pointC, pointC); assertFalse(original.geometricallyEquals(expected, getEpsilon())); - + expected = new Triangle3D(pointB, pointA, pointA); assertFalse(original.geometricallyEquals(expected, getEpsilon())); - + expected = new Triangle3D(pointB, pointC, pointC); assertFalse(original.geometricallyEquals(expected, getEpsilon())); - + expected = new Triangle3D(pointC, pointA, pointA); assertFalse(original.geometricallyEquals(expected, getEpsilon())); - + expected = new Triangle3D(pointC, pointB, pointB); assertFalse(original.geometricallyEquals(expected, getEpsilon())); } - + //Test Epsilon is working correctly for geometricallyEquals method for (int i = 0; i < ITERATIONS; i++) { Point3D pointA = EuclidCoreRandomTools.nextPoint3D(random); Point3D pointB = EuclidCoreRandomTools.nextPoint3D(random); Point3D pointC = EuclidCoreRandomTools.nextPoint3D(random); - + Triangle3D original = new Triangle3D(pointA, pointB, pointC); - + Point3D pointAChanged = new Point3D(pointA); Point3D pointBChanged = new Point3D(pointB); Point3D pointCChanged = new Point3D(pointC); - + //Point A above epsilon pointAChanged.setElement(i % 3, pointAChanged.getElement(i % 3) - 1.01 * getEpsilon()); Triangle3D changedA = new Triangle3D(pointAChanged, pointB, pointC); assertFalse(changedA.geometricallyEquals(original, getEpsilon())); - + //Point A below epsilon pointAChanged.set(pointA); pointAChanged.setElement(i % 3, pointAChanged.getElement(i % 3) - 0.99 * getEpsilon()); changedA = new Triangle3D(pointAChanged, pointB, pointC); assertTrue(changedA.geometricallyEquals(original, getEpsilon())); - + //Point B above epsilon pointBChanged.setElement(i % 3, pointBChanged.getElement(i % 3) - 1.01 * getEpsilon()); Triangle3D changedB = new Triangle3D(pointA, pointBChanged, pointC); assertFalse(changedB.geometricallyEquals(original, getEpsilon())); - + //Point B below epsilon pointBChanged.set(pointB); pointBChanged.setElement(i % 3, pointBChanged.getElement(i % 3) - 0.99 * getEpsilon()); changedB = new Triangle3D(pointA, pointBChanged, pointC); - assertTrue(changedB.geometricallyEquals(original, getEpsilon())); - + assertTrue(changedB.geometricallyEquals(original, getEpsilon())); + //Point C above epsilon pointCChanged.setElement(i % 3, pointCChanged.getElement(i % 3) - 1.01 * getEpsilon()); Triangle3D changedC = new Triangle3D(pointA, pointB, pointCChanged); assertFalse(changedC.geometricallyEquals(original, getEpsilon())); - + //Point C below epsilon pointCChanged.set(pointC); pointCChanged.setElement(i % 3, pointCChanged.getElement(i % 3) - 0.99 * getEpsilon()); changedC = new Triangle3D(pointA, pointB, pointCChanged); - assertTrue(changedC.geometricallyEquals(original, getEpsilon())); - } + assertTrue(changedC.geometricallyEquals(original, getEpsilon())); + } } @Test public void testHashCode() throws Exception { Random random = new Random(62691L); - + for (int i = 0; i < ITERATIONS; i++) { Point3D pointA = EuclidCoreRandomTools.nextPoint3D(random); Point3D pointB = EuclidCoreRandomTools.nextPoint3D(random); Point3D pointC = EuclidCoreRandomTools.nextPoint3D(random); - + Point3D pointAChanged = new Point3D(pointA); Point3D pointBChanged = new Point3D(pointB); Point3D pointCChanged = new Point3D(pointC); - + //Verify hash of the points is equal to hash of the Triangle int hashOfPointAPointBPointC = EuclidHashCodeTools.toIntHashCode(pointA, pointB, pointC); Triangle3D original = new Triangle3D(pointA, pointB, pointC); - + int hashOfTriangle = original.hashCode(); assertEquals(hashOfPointAPointBPointC, hashOfTriangle); - + //Change one of the points in PointA to verify the hash is no longer equal pointAChanged.setElement(i % 3, random.nextDouble()); - Triangle3D changed = new Triangle3D(pointAChanged, pointB, pointC); + Triangle3D changed = new Triangle3D(pointAChanged, pointB, pointC); hashOfTriangle = changed.hashCode(); assertFalse(hashOfPointAPointBPointC == hashOfTriangle); - + //Change one of the points in PointB to verify the hash is no longer equal pointBChanged.setElement(i % 3, random.nextDouble()); - changed = new Triangle3D(pointA, pointBChanged, pointC); + changed = new Triangle3D(pointA, pointBChanged, pointC); hashOfTriangle = changed.hashCode(); assertFalse(hashOfPointAPointBPointC == hashOfTriangle); - + //Change one of the points in PointC to verify the hash is no longer equal pointCChanged.setElement(i % 3, random.nextDouble()); - changed = new Triangle3D(pointA, pointB, pointCChanged); + changed = new Triangle3D(pointA, pointB, pointCChanged); hashOfTriangle = changed.hashCode(); assertFalse(hashOfPointAPointBPointC == hashOfTriangle); - - } - } - + + } + } + @Test public void testToString() throws Exception { Random random = new Random(62691L); - + for (int i = 0; i < ITERATIONS; i++) { Point3D pointA = EuclidCoreRandomTools.nextPoint3D(random); Point3D pointB = EuclidCoreRandomTools.nextPoint3D(random); Point3D pointC = EuclidCoreRandomTools.nextPoint3D(random); - + Triangle3D triangle = new Triangle3D(pointA, pointB, pointC); - + assertEquals(triangle.toString(), EuclidGeometryIOTools.getTriangle3DString(triangle)); } - + } } \ No newline at end of file diff --git a/src/test/java/us/ihmc/euclid/geometry/tools/EuclidGeometryFactoriesTest.java b/src/test/java/us/ihmc/euclid/geometry/tools/EuclidGeometryFactoriesTest.java index 80de53885..ba20f804d 100644 --- a/src/test/java/us/ihmc/euclid/geometry/tools/EuclidGeometryFactoriesTest.java +++ b/src/test/java/us/ihmc/euclid/geometry/tools/EuclidGeometryFactoriesTest.java @@ -127,7 +127,8 @@ public void testNewObservableBoundingBox2DBasics() accessTraceMap.put(bound, new boolean[] {false, false}); } BoundingBox2DBasics source = new BoundingBox2D(); - BoundingBox2DBasics observable = EuclidGeometryFactories.newObservableBoundingBox2DBasics((axis, bound, + BoundingBox2DBasics observable = EuclidGeometryFactories.newObservableBoundingBox2DBasics((axis, + bound, newValue) -> changeTraceMap.get(bound)[axis.ordinal()] = true, (axis, bound) -> accessTraceMap.get(bound)[axis.ordinal()] = true, @@ -195,7 +196,8 @@ public void testNewObservableBoundingBox3DBasics() accessTraceMap.put(bound, new boolean[] {false, false, false}); } BoundingBox3DBasics source = new BoundingBox3D(); - BoundingBox3DBasics observable = EuclidGeometryFactories.newObservableBoundingBox3DBasics((axis, bound, + BoundingBox3DBasics observable = EuclidGeometryFactories.newObservableBoundingBox3DBasics((axis, + bound, newValue) -> changeTraceMap.get(bound)[axis.ordinal()] = true, (axis, bound) -> accessTraceMap.get(bound)[axis.ordinal()] = true, diff --git a/src/test/java/us/ihmc/euclid/geometry/tools/EuclidGeometryTestToolsTest.java b/src/test/java/us/ihmc/euclid/geometry/tools/EuclidGeometryTestToolsTest.java index 08796f06e..822f0ff23 100644 --- a/src/test/java/us/ihmc/euclid/geometry/tools/EuclidGeometryTestToolsTest.java +++ b/src/test/java/us/ihmc/euclid/geometry/tools/EuclidGeometryTestToolsTest.java @@ -710,7 +710,11 @@ public void testAssertPose2DGeometricallyEquals() throws Throwable } } - private static void assertAssertionMethodsBehaveProperly(boolean failExpected, String methodName, Class argumentsClass, Object expected, Object actual, + private static void assertAssertionMethodsBehaveProperly(boolean failExpected, + String methodName, + Class argumentsClass, + Object expected, + Object actual, double epsilon) throws Throwable { 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 631b0a187..58a9ffb21 100644 --- a/src/test/java/us/ihmc/euclid/geometry/tools/EuclidGeometryToolsTest.java +++ b/src/test/java/us/ihmc/euclid/geometry/tools/EuclidGeometryToolsTest.java @@ -2124,8 +2124,13 @@ public void testDistanceBetweenTwoLineSegment2Ds() throws Exception } } - private void assertDistanceBetweenTwoLineSegment2Ds(int iteration, double expectedDistance, Point2D lineSegmentStart1, Point2D lineSegmentEnd1, - Point2D lineSegmentStart2, Point2D lineSegmentEnd2, double epsilon) + private void assertDistanceBetweenTwoLineSegment2Ds(int iteration, + double expectedDistance, + Point2D lineSegmentStart1, + Point2D lineSegmentEnd1, + Point2D lineSegmentStart2, + Point2D lineSegmentEnd2, + double epsilon) { assertEquals(expectedDistance, EuclidGeometryTools.distanceBetweenTwoLineSegment2Ds(lineSegmentStart1, lineSegmentEnd1, lineSegmentStart2, lineSegmentEnd2), @@ -8006,8 +8011,10 @@ public void testIntersectionBetweenRay2DAndLineSegment2D() throws Exception } } - private void assertOnlyExistenceOfIntersectionBetweenRay2DAndAllCombinationsOfLineSegment(boolean intersectionExist, Point2D rayOrigin, - Vector2D rayDirection, Point2D lineSegmentStart, + private void assertOnlyExistenceOfIntersectionBetweenRay2DAndAllCombinationsOfLineSegment(boolean intersectionExist, + Point2D rayOrigin, + Vector2D rayDirection, + Point2D lineSegmentStart, Point2D lineSegmentEnd) { boolean success; @@ -8043,8 +8050,11 @@ private void assertOnlyExistenceOfIntersectionBetweenRay2DAndAllCombinationsOfLi assertTrue(actualIntersection != null == intersectionExist); } - private void assertAllCombinationsOfTwoLineSegmentsIntersection(Point2D expectedIntersection, Point2D rayOrigin, Vector2D rayDirection, - Point2D lineSegmentStart, Point2D lineSegmentEnd) + private void assertAllCombinationsOfTwoLineSegmentsIntersection(Point2D expectedIntersection, + Point2D rayOrigin, + Vector2D rayDirection, + Point2D lineSegmentStart, + Point2D lineSegmentEnd) { double epsilon = EuclidGeometryTools.ONE_TRILLIONTH; @@ -9613,8 +9623,11 @@ public void testIntersectionBetweenTwoLineSegment2Ds() throws Exception } } - private void assertOnlyExistenceOfTwoLineSegmentsIntersectionAllCombinations(boolean intersectionExist, Point2D lineSegmentStart1, Point2D lineSegmentEnd1, - Point2D lineSegmentStart2, Point2D lineSegmentEnd2) + private void assertOnlyExistenceOfTwoLineSegmentsIntersectionAllCombinations(boolean intersectionExist, + Point2D lineSegmentStart1, + Point2D lineSegmentEnd1, + Point2D lineSegmentStart2, + Point2D lineSegmentEnd2) { boolean success; Point2D intersectionThatMayContainOnlyNaNs = new Point2D(); @@ -9700,8 +9713,11 @@ private void assertOnlyExistenceOfTwoLineSegmentsIntersectionAllCombinations(boo assertTrue(actualIntersection != null == intersectionExist); } - private void assertAllCombinationsOfTwoLineSegmentsIntersection(Point2D expectedIntersection, Point2D lineSegmentStart1, Point2D lineSegmentEnd1, - Point2D lineSegmentStart2, Point2D lineSegmentEnd2) + private void assertAllCombinationsOfTwoLineSegmentsIntersection(Point2D expectedIntersection, + Point2D lineSegmentStart1, + Point2D lineSegmentEnd1, + Point2D lineSegmentStart2, + Point2D lineSegmentEnd2) { double epsilon = EuclidGeometryTools.ONE_TRILLIONTH; @@ -10249,8 +10265,11 @@ private enum Plane3DSide EXACTLY_ON, ABOVE, BELOW } - private static void performAssertionsForPoint3DOnSideOfPlane3D(Random random, Point3DReadOnly query, Point3DReadOnly pointOnPlane, - Vector3DReadOnly planeNormal, Plane3DSide expected) + private static void performAssertionsForPoint3DOnSideOfPlane3D(Random random, + Point3DReadOnly query, + Point3DReadOnly pointOnPlane, + Vector3DReadOnly planeNormal, + Plane3DSide expected) { double queryX = query.getX(); double queryY = query.getY(); diff --git a/src/test/java/us/ihmc/euclid/matrix/Matrix3DTest.java b/src/test/java/us/ihmc/euclid/matrix/Matrix3DTest.java index 245456ec3..378a044fb 100644 --- a/src/test/java/us/ihmc/euclid/matrix/Matrix3DTest.java +++ b/src/test/java/us/ihmc/euclid/matrix/Matrix3DTest.java @@ -120,7 +120,7 @@ public void testGeometricallyEquals() throws Exception { Random random = new Random(621541L); double epsilon = random.nextDouble(); - + Matrix3D matrix = createRandomMatrix(random); double m00 = matrix.getM00(); double m01 = matrix.getM01(); @@ -131,10 +131,10 @@ public void testGeometricallyEquals() throws Exception double m20 = matrix.getM20(); double m21 = matrix.getM21(); double m22 = matrix.getM22(); - + double small = 0.999 * epsilon; double big = 1.001 * epsilon; - + assertTrue(matrix.geometricallyEquals(createMatrix(m00 + small, m01, m02, m10, m11, m12, m20, m21, m22), epsilon)); assertTrue(matrix.geometricallyEquals(createMatrix(m00, m01 + small, m02, m10, m11, m12, m20, m21, m22), epsilon)); assertTrue(matrix.geometricallyEquals(createMatrix(m00, m01, m02 + small, m10, m11, m12, m20, m21, m22), epsilon)); @@ -144,7 +144,7 @@ public void testGeometricallyEquals() throws Exception assertTrue(matrix.geometricallyEquals(createMatrix(m00, m01, m02, m10, m11, m12, m20 + small, m21, m22), epsilon)); assertTrue(matrix.geometricallyEquals(createMatrix(m00, m01, m02, m10, m11, m12, m20, m21 + small, m22), epsilon)); assertTrue(matrix.geometricallyEquals(createMatrix(m00, m01, m02, m10, m11, m12, m20, m21, m22 + small), epsilon)); - + assertTrue(matrix.geometricallyEquals(createMatrix(m00 - small, m01, m02, m10, m11, m12, m20, m21, m22), epsilon)); assertTrue(matrix.geometricallyEquals(createMatrix(m00, m01 - small, m02, m10, m11, m12, m20, m21, m22), epsilon)); assertTrue(matrix.geometricallyEquals(createMatrix(m00, m01, m02 - small, m10, m11, m12, m20, m21, m22), epsilon)); @@ -154,7 +154,7 @@ public void testGeometricallyEquals() throws Exception assertTrue(matrix.geometricallyEquals(createMatrix(m00, m01, m02, m10, m11, m12, m20 - small, m21, m22), epsilon)); assertTrue(matrix.geometricallyEquals(createMatrix(m00, m01, m02, m10, m11, m12, m20, m21 - small, m22), epsilon)); assertTrue(matrix.geometricallyEquals(createMatrix(m00, m01, m02, m10, m11, m12, m20, m21, m22 - small), epsilon)); - + assertFalse(matrix.geometricallyEquals(createMatrix(m00 + big, m01, m02, m10, m11, m12, m20, m21, m22), epsilon)); assertFalse(matrix.geometricallyEquals(createMatrix(m00, m01 + big, m02, m10, m11, m12, m20, m21, m22), epsilon)); assertFalse(matrix.geometricallyEquals(createMatrix(m00, m01, m02 + big, m10, m11, m12, m20, m21, m22), epsilon)); @@ -164,7 +164,7 @@ public void testGeometricallyEquals() throws Exception assertFalse(matrix.geometricallyEquals(createMatrix(m00, m01, m02, m10, m11, m12, m20 + big, m21, m22), epsilon)); assertFalse(matrix.geometricallyEquals(createMatrix(m00, m01, m02, m10, m11, m12, m20, m21 + big, m22), epsilon)); assertFalse(matrix.geometricallyEquals(createMatrix(m00, m01, m02, m10, m11, m12, m20, m21, m22 + big), epsilon)); - + assertFalse(matrix.geometricallyEquals(createMatrix(m00 - big, m01, m02, m10, m11, m12, m20, m21, m22), epsilon)); assertFalse(matrix.geometricallyEquals(createMatrix(m00, m01 - big, m02, m10, m11, m12, m20, m21, m22), epsilon)); assertFalse(matrix.geometricallyEquals(createMatrix(m00, m01, m02 - big, m10, m11, m12, m20, m21, m22), epsilon)); diff --git a/src/test/java/us/ihmc/euclid/referenceFrame/collision/EuclidFrameShapeCollisionToolsTest.java b/src/test/java/us/ihmc/euclid/referenceFrame/collision/EuclidFrameShapeCollisionToolsTest.java index c8ad75f40..bb0e481dc 100644 --- a/src/test/java/us/ihmc/euclid/referenceFrame/collision/EuclidFrameShapeCollisionToolsTest.java +++ b/src/test/java/us/ihmc/euclid/referenceFrame/collision/EuclidFrameShapeCollisionToolsTest.java @@ -54,7 +54,8 @@ private void pe Shape3DCollisionEvaluator referenceForTesting, BiFunction shapeARandomGenerator, BiFunction shapeBRandomGenerator, - boolean testNormal, double epsilon) + boolean testNormal, + double epsilon) { for (int i = 0; i < ITERATIONS; i++) { // Test in world frame @@ -106,7 +107,9 @@ private void pe } } - private static void assertCollisionResultsEqual(EuclidShape3DCollisionResult expected, EuclidFrameShape3DCollisionResult actual, boolean testNormal, + private static void assertCollisionResultsEqual(EuclidShape3DCollisionResult expected, + EuclidFrameShape3DCollisionResult actual, + boolean testNormal, double epsilon) { if (!testNormal) diff --git a/src/test/java/us/ihmc/euclid/referenceFrame/collision/epa/FrameExpandingPolytopeAlgorithmTest.java b/src/test/java/us/ihmc/euclid/referenceFrame/collision/epa/FrameExpandingPolytopeAlgorithmTest.java index 58487a99a..1d118bab5 100644 --- a/src/test/java/us/ihmc/euclid/referenceFrame/collision/epa/FrameExpandingPolytopeAlgorithmTest.java +++ b/src/test/java/us/ihmc/euclid/referenceFrame/collision/epa/FrameExpandingPolytopeAlgorithmTest.java @@ -173,7 +173,9 @@ public void testCompareAgainstFramelessEPA() } } - private static void computeResults(FrameShape3DBasics shapeA, FrameShape3DBasics shapeB, EuclidShape3DCollisionResult framelessResultToPack, + private static void computeResults(FrameShape3DBasics shapeA, + FrameShape3DBasics shapeB, + EuclidShape3DCollisionResult framelessResultToPack, EuclidFrameShape3DCollisionResult frameResultToPack) { ReferenceFrame frameA = shapeA.getReferenceFrame(); diff --git a/src/test/java/us/ihmc/euclid/referenceFrame/collision/gjk/FrameGilbertJohnsonKeerthiCollisionDetectorTest.java b/src/test/java/us/ihmc/euclid/referenceFrame/collision/gjk/FrameGilbertJohnsonKeerthiCollisionDetectorTest.java index cb814f5c2..78c13ca41 100644 --- a/src/test/java/us/ihmc/euclid/referenceFrame/collision/gjk/FrameGilbertJohnsonKeerthiCollisionDetectorTest.java +++ b/src/test/java/us/ihmc/euclid/referenceFrame/collision/gjk/FrameGilbertJohnsonKeerthiCollisionDetectorTest.java @@ -283,7 +283,9 @@ public void testCompareAgainstFramelessGJK() } } - private static void computeResults(FrameShape3DBasics shapeA, FrameShape3DBasics shapeB, EuclidShape3DCollisionResult framelessResultToPack, + private static void computeResults(FrameShape3DBasics shapeA, + FrameShape3DBasics shapeB, + EuclidShape3DCollisionResult framelessResultToPack, EuclidFrameShape3DCollisionResult frameResultToPack) { ReferenceFrame frameA = shapeA.getReferenceFrame(); diff --git a/src/test/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameFactoriesTest.java b/src/test/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameFactoriesTest.java index b16950d3b..16a019926 100644 --- a/src/test/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameFactoriesTest.java +++ b/src/test/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameFactoriesTest.java @@ -1779,7 +1779,8 @@ public void testNewObservableFixedFrameBoundingBox2DBasics() accessTraceMap.put(bound, new boolean[] {false, false}); } FrameBoundingBox2D source = new FrameBoundingBox2D(); - FixedFrameBoundingBox2DBasics observable = EuclidFrameFactories.newObservableFixedFrameBoundingBox2DBasics((axis, bound, + FixedFrameBoundingBox2DBasics observable = EuclidFrameFactories.newObservableFixedFrameBoundingBox2DBasics((axis, + bound, newValue) -> changeTraceMap.get(bound)[axis.ordinal()] = true, (axis, bound) -> accessTraceMap.get(bound)[axis.ordinal()] = true, @@ -1848,7 +1849,8 @@ public void testNewObservableFixedFrameBoundingBox3DBasics() accessTraceMap.put(bound, new boolean[] {false, false, false}); } FrameBoundingBox3D source = new FrameBoundingBox3D(); - FixedFrameBoundingBox3DBasics observable = EuclidFrameFactories.newObservableFixedFrameBoundingBox3DBasics((axis, bound, + FixedFrameBoundingBox3DBasics observable = EuclidFrameFactories.newObservableFixedFrameBoundingBox3DBasics((axis, + bound, newValue) -> changeTraceMap.get(bound)[axis.ordinal()] = true, (axis, bound) -> accessTraceMap.get(bound)[axis.ordinal()] = true, diff --git a/src/test/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameTestToolsTest.java b/src/test/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameTestToolsTest.java index 636a9ad97..efb56e35b 100644 --- a/src/test/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameTestToolsTest.java +++ b/src/test/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameTestToolsTest.java @@ -497,7 +497,11 @@ public void testAssertFrameQuaternionGeometricallyEquals() throws Throwable } } - private static void assertAssertionMethodsBehaveProperly(boolean failExpected, String methodName, Class argumentsClass, Object expected, Object actual, + private static void assertAssertionMethodsBehaveProperly(boolean failExpected, + String methodName, + Class argumentsClass, + Object expected, + Object actual, double epsilon) throws Throwable { diff --git a/src/test/java/us/ihmc/euclid/shape/collision/EuclidShapeCollisionToolsTest.java b/src/test/java/us/ihmc/euclid/shape/collision/EuclidShapeCollisionToolsTest.java index 146bde610..760bfedaf 100644 --- a/src/test/java/us/ihmc/euclid/shape/collision/EuclidShapeCollisionToolsTest.java +++ b/src/test/java/us/ihmc/euclid/shape/collision/EuclidShapeCollisionToolsTest.java @@ -1222,7 +1222,10 @@ public void testPointShape3DRamp3D() throws Exception } } - private static void buildPointOutsideAndPerformAssertion(Random random, int iteration, Ramp3DReadOnly ramp3D, Point3DReadOnly pointOnShape, + private static void buildPointOutsideAndPerformAssertion(Random random, + int iteration, + Ramp3DReadOnly ramp3D, + Point3DReadOnly pointOnShape, Vector3DReadOnly normal) { double distance = EuclidCoreRandomTools.nextDouble(random, 0.0, 1.0); diff --git a/src/test/java/us/ihmc/euclid/shape/collision/ExpandingPolytopeAlgorithmTest.java b/src/test/java/us/ihmc/euclid/shape/collision/ExpandingPolytopeAlgorithmTest.java index 603458189..62c281406 100644 --- a/src/test/java/us/ihmc/euclid/shape/collision/ExpandingPolytopeAlgorithmTest.java +++ b/src/test/java/us/ihmc/euclid/shape/collision/ExpandingPolytopeAlgorithmTest.java @@ -513,8 +513,11 @@ else if (convexPolytope3D.getNumberOfFaces() == 1) } } - public static void performAssertionsOnEPA(Random random, ConvexPolytope3DReadOnly polytopeA, ConvexPolytope3DReadOnly polytopeB, - Point3DReadOnly expectedPointOnA, Point3DReadOnly expectedPointOnB) + public static void performAssertionsOnEPA(Random random, + ConvexPolytope3DReadOnly polytopeA, + ConvexPolytope3DReadOnly polytopeB, + Point3DReadOnly expectedPointOnA, + Point3DReadOnly expectedPointOnB) { ExpandingPolytopeAlgorithm epa = new ExpandingPolytopeAlgorithm(); EuclidShape3DCollisionResult result = epa.evaluateCollision(polytopeA, polytopeB); diff --git a/src/test/java/us/ihmc/euclid/shape/collision/GilbertJohnsonKeerthiCollisionDetectorTest.java b/src/test/java/us/ihmc/euclid/shape/collision/GilbertJohnsonKeerthiCollisionDetectorTest.java index ca3bbaa1d..9fa0aa05a 100644 --- a/src/test/java/us/ihmc/euclid/shape/collision/GilbertJohnsonKeerthiCollisionDetectorTest.java +++ b/src/test/java/us/ihmc/euclid/shape/collision/GilbertJohnsonKeerthiCollisionDetectorTest.java @@ -668,15 +668,23 @@ public static ConvexPolytope3D newTetrahedron(Random random, Point3DReadOnly top return new ConvexPolytope3D(Vertex3DSupplier.asVertex3DSupplier(vertices)); } - public static void performAssertionsTwoCombinations(String messagePrefix, ConvexPolytope3DReadOnly polytopeA, ConvexPolytope3DReadOnly polytopeB, - boolean expectedCollisionTestResult, Point3DReadOnly expectedPointOnA, Point3DReadOnly expectedPointOnB) + public static void performAssertionsTwoCombinations(String messagePrefix, + ConvexPolytope3DReadOnly polytopeA, + ConvexPolytope3DReadOnly polytopeB, + boolean expectedCollisionTestResult, + Point3DReadOnly expectedPointOnA, + Point3DReadOnly expectedPointOnB) { performAssertions(messagePrefix, polytopeA, polytopeB, expectedCollisionTestResult, expectedPointOnA, expectedPointOnB); performAssertions(messagePrefix, polytopeB, polytopeA, expectedCollisionTestResult, expectedPointOnB, expectedPointOnA); } - public static void performAssertions(String messagePrefix, ConvexPolytope3DReadOnly polytopeA, ConvexPolytope3DReadOnly polytopeB, - boolean expectedCollisionTestResult, Point3DReadOnly expectedPointOnA, Point3DReadOnly expectedPointOnB) + public static void performAssertions(String messagePrefix, + ConvexPolytope3DReadOnly polytopeA, + ConvexPolytope3DReadOnly polytopeB, + boolean expectedCollisionTestResult, + Point3DReadOnly expectedPointOnA, + Point3DReadOnly expectedPointOnB) { GilbertJohnsonKeerthiCollisionDetector collisionDetector = new GilbertJohnsonKeerthiCollisionDetector(); EuclidShape3DCollisionResult result = collisionDetector.evaluateCollision(polytopeA, polytopeB); diff --git a/src/test/java/us/ihmc/euclid/shape/tools/EuclidShapeToolsTest.java b/src/test/java/us/ihmc/euclid/shape/tools/EuclidShapeToolsTest.java index 3ece199d7..b07669daa 100644 --- a/src/test/java/us/ihmc/euclid/shape/tools/EuclidShapeToolsTest.java +++ b/src/test/java/us/ihmc/euclid/shape/tools/EuclidShapeToolsTest.java @@ -255,19 +255,19 @@ public void testIsFirstValueMinimum() assertEquals(isMinimumValue, actualResult); } } - + for (int i = 0; i < ITERATIONS; i++) { double possibleMin = EuclidCoreRandomTools.nextDouble(random, 10.0); double[] values = new double[3]; - + int numberOfCombinations = (int) Math.pow(2, values.length); - + for (int j = 0; j < numberOfCombinations; j++) { int currentByte = 0; boolean isMinimumValue = j == 0; - + for (int k = 0; k < values.length; k++) { values[k] = EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0); @@ -276,24 +276,24 @@ public void testIsFirstValueMinimum() values[k] = possibleMin + (isLesser ? -values[k] : +values[k]); currentByte++; } - + boolean actualResult = EuclidShapeTools.isFirstValueMinimum(possibleMin, values[0], values[1], values[2]); assertEquals(isMinimumValue, actualResult); } } - + for (int i = 0; i < ITERATIONS; i++) { double possibleMin = EuclidCoreRandomTools.nextDouble(random, 10.0); double[] values = new double[2]; - + int numberOfCombinations = (int) Math.pow(2, values.length); - + for (int j = 0; j < numberOfCombinations; j++) { int currentByte = 0; boolean isMinimumValue = j == 0; - + for (int k = 0; k < values.length; k++) { values[k] = EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0); @@ -302,7 +302,7 @@ public void testIsFirstValueMinimum() values[k] = possibleMin + (isLesser ? -values[k] : +values[k]); currentByte++; } - + boolean actualResult = EuclidShapeTools.isFirstValueMinimum(possibleMin, values[0], values[1]); assertEquals(isMinimumValue, actualResult); } @@ -339,19 +339,19 @@ public void testIsFirstValueMaximum() assertEquals(isMinimumValue, actualResult); } } - + for (int i = 0; i < ITERATIONS; i++) { double possibleMax = EuclidCoreRandomTools.nextDouble(random, 10.0); double[] values = new double[3]; - + int numberOfCombinations = (int) Math.pow(2, values.length); - + for (int j = 0; j < numberOfCombinations; j++) { int currentByte = 0; boolean isMinimumValue = j == 0; - + for (int k = 0; k < values.length; k++) { values[k] = EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0); @@ -360,24 +360,24 @@ public void testIsFirstValueMaximum() values[k] = possibleMax + (isGreater ? +values[k] : -values[k]); currentByte++; } - + boolean actualResult = EuclidShapeTools.isFirstValueMaximum(possibleMax, values[0], values[1], values[2]); assertEquals(isMinimumValue, actualResult); } } - + for (int i = 0; i < ITERATIONS; i++) { double possibleMax = EuclidCoreRandomTools.nextDouble(random, 10.0); double[] values = new double[2]; - + int numberOfCombinations = (int) Math.pow(2, values.length); - + for (int j = 0; j < numberOfCombinations; j++) { int currentByte = 0; boolean isMinimumValue = j == 0; - + for (int k = 0; k < values.length; k++) { values[k] = EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0); @@ -386,7 +386,7 @@ public void testIsFirstValueMaximum() values[k] = possibleMax + (isGreater ? +values[k] : -values[k]); currentByte++; } - + boolean actualResult = EuclidShapeTools.isFirstValueMaximum(possibleMax, values[0], values[1]); assertEquals(isMinimumValue, actualResult); } diff --git a/src/test/java/us/ihmc/euclid/tools/EuclidCoreTestToolsTest.java b/src/test/java/us/ihmc/euclid/tools/EuclidCoreTestToolsTest.java index bc131ffc3..85fe35ddb 100644 --- a/src/test/java/us/ihmc/euclid/tools/EuclidCoreTestToolsTest.java +++ b/src/test/java/us/ihmc/euclid/tools/EuclidCoreTestToolsTest.java @@ -995,15 +995,24 @@ public void testAssertAffineTransformGeometricallyEquals() throws Throwable } } - private static void assertAssertionMethodsBehaveProperly(boolean failExpected, String methodName, Class argumentsClass, Object expected, Object actual, + private static void assertAssertionMethodsBehaveProperly(boolean failExpected, + String methodName, + Class argumentsClass, + Object expected, + Object actual, double epsilon) throws Throwable { assertAssertionMethodsBehaveProperly(EuclidCoreTestTools.class, failExpected, methodName, argumentsClass, expected, actual, epsilon); } - public static void assertAssertionMethodsBehaveProperly(Class assertionClassHolder, boolean failExpected, String methodName, Class argumentsClass, - Object expected, Object actual, double epsilon) + public static void assertAssertionMethodsBehaveProperly(Class assertionClassHolder, + boolean failExpected, + String methodName, + Class argumentsClass, + Object expected, + Object actual, + double epsilon) throws Throwable { Method assertionMethod1 = assertionClassHolder.getMethod(methodName, argumentsClass, argumentsClass, Double.TYPE); diff --git a/src/test/java/us/ihmc/euclid/tools/Matrix3DToolsTest.java b/src/test/java/us/ihmc/euclid/tools/Matrix3DToolsTest.java index f80fe9461..ed3f4461e 100644 --- a/src/test/java/us/ihmc/euclid/tools/Matrix3DToolsTest.java +++ b/src/test/java/us/ihmc/euclid/tools/Matrix3DToolsTest.java @@ -634,12 +634,12 @@ public void testMultiplyInner() EuclidCoreTestTools.assertMatrix3DEquals(expected, actual, EPS); } } - + @Test public void testMultiplyOuter() { Random random = new Random(3245); - + for (int i = 0; i < ITERATIONS; i++) { Matrix3D original = EuclidCoreRandomTools.nextMatrix3D(random, 10.0); @@ -647,7 +647,7 @@ public void testMultiplyOuter() Matrix3DTools.multiplyTransposeRight(original, original, expected); Matrix3D actual = new Matrix3D(); Matrix3DTools.multiplyOuter(original, actual); - + EuclidCoreTestTools.assertMatrix3DEquals(expected, actual, EPS); } } diff --git a/src/test/java/us/ihmc/euclid/tools/SymmetricEigenDecomposition3DTest.java b/src/test/java/us/ihmc/euclid/tools/SymmetricEigenDecomposition3DTest.java index 329bb30d6..d97e2de95 100644 --- a/src/test/java/us/ihmc/euclid/tools/SymmetricEigenDecomposition3DTest.java +++ b/src/test/java/us/ihmc/euclid/tools/SymmetricEigenDecomposition3DTest.java @@ -110,8 +110,13 @@ public void testBug2() performAssertions(0, A, Qeuclid, lambdaeuclid, Qejml, lambdaejml, EPSILON); } - private void performAssertions(int i, Matrix3DReadOnly A, Matrix3DBasics Qeuclid, Vector3DBasics lambdaeuclid, Matrix3DBasics Qejml, - Vector3DBasics lambdaejml, double epsilon) + private void performAssertions(int i, + Matrix3DReadOnly A, + Matrix3DBasics Qeuclid, + Vector3DBasics lambdaeuclid, + Matrix3DBasics Qejml, + Vector3DBasics lambdaejml, + double epsilon) { for (int col = 0; col < 3; col++) { diff --git a/src/test/java/us/ihmc/euclid/transform/AffineTransformTest.java b/src/test/java/us/ihmc/euclid/transform/AffineTransformTest.java index b28f41846..1918c2225 100644 --- a/src/test/java/us/ihmc/euclid/transform/AffineTransformTest.java +++ b/src/test/java/us/ihmc/euclid/transform/AffineTransformTest.java @@ -1794,8 +1794,18 @@ public void testEpsilonEquals() throws Exception assertFalse(t.epsilonEquals(createTransform(m00, m01, m02, m10, m11, m12, m20, m21, m22, tx, ty, tz - big), EPS)); } - private static AffineTransform createTransform(double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, double m22, - double tx, double ty, double tz) + private static AffineTransform createTransform(double m00, + double m01, + double m02, + double m10, + double m11, + double m12, + double m20, + double m21, + double m22, + double tx, + double ty, + double tz) { return new AffineTransform(new Matrix3D(m00, m01, m02, m10, m11, m12, m20, m21, m22), new Vector3D(tx, ty, tz)); } From c03f06dac09b8fd8c7a6b02dfb37414e37787d88 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Mon, 14 Mar 2022 15:39:36 -0500 Subject: [PATCH 2/7] IO tools: cleanup array/collection and handle null. --- .../ihmc/euclid/tools/EuclidCoreIOTools.java | 117 ++++++++++++------ 1 file changed, 80 insertions(+), 37 deletions(-) diff --git a/src/main/java/us/ihmc/euclid/tools/EuclidCoreIOTools.java b/src/main/java/us/ihmc/euclid/tools/EuclidCoreIOTools.java index fd3c51701..795fa542c 100644 --- a/src/main/java/us/ihmc/euclid/tools/EuclidCoreIOTools.java +++ b/src/main/java/us/ihmc/euclid/tools/EuclidCoreIOTools.java @@ -742,26 +742,20 @@ public static String getStringOf(String separator, String format, double... valu * Gets a representative {@code String} of the elements contained in the given array. *

* This provides an alternative to {@link Arrays#toString(Object[])} where the format of the output - * can be controlled by defining a custom {@code separator}. For instance, with + * can be controller by defining a custom {@code separator}. For instance, with * {@code separator = \n} the resulting {@code String} is composed of one element per line as * opposed to {@link Arrays#toString(Object[])} which outputs all the elements in one line. *

* - * @param the type of the array elements. - * @param prefix the {@code String} to prepend to the result. - * @param suffix the {@code String} to append to the result. * @param separator the {@code String} used to separate elements of the array. * @param array the array of elements to get the {@code String} of. * @param elementToStringFunction the {@code Function} used to generate a representative * {@code String} for each element. * @return the representative {@code String}. */ - public static String getArrayString(String prefix, String suffix, String separator, T[] array, Function elementToStringFunction) + public static String getArrayString(String separator, Object[] array) { - if (array == null) - return "null"; - else - return getCollectionString(prefix, suffix, separator, Arrays.asList(array), elementToStringFunction); + return getArrayString(separator, array, Object::toString); } /** @@ -782,7 +776,53 @@ public static String getArrayString(String prefix, String suffix, String sep */ public static String getArrayString(String separator, T[] array, Function elementToStringFunction) { - return getCollectionString(separator, Arrays.asList(array), elementToStringFunction); + return getArrayString(null, null, separator, array, elementToStringFunction); + } + + /** + * Gets a representative {@code String} of the elements contained in the given array. + *

+ * This provides an alternative to {@link Arrays#toString(Object[])} where the format of the output + * can be controlled by defining a custom {@code separator}. For instance, with + * {@code separator = \n} the resulting {@code String} is composed of one element per line as + * opposed to {@link Arrays#toString(Object[])} which outputs all the elements in one line. + *

+ * + * @param prefix the {@code String} to prepend to the result. + * @param suffix the {@code String} to append to the result. + * @param separator the {@code String} used to separate elements of the array. + * @param array the array of elements to get the {@code String} of. + * @return the representative {@code String}. + */ + public static String getArrayString(String prefix, String suffix, String separator, Object[] array) + { + return getArrayString(prefix, suffix, separator, array, Object::toString); + } + + /** + * Gets a representative {@code String} of the elements contained in the given array. + *

+ * This provides an alternative to {@link Arrays#toString(Object[])} where the format of the output + * can be controlled by defining a custom {@code separator}. For instance, with + * {@code separator = \n} the resulting {@code String} is composed of one element per line as + * opposed to {@link Arrays#toString(Object[])} which outputs all the elements in one line. + *

+ * + * @param the type of the array elements. + * @param prefix the {@code String} to prepend to the result. + * @param suffix the {@code String} to append to the result. + * @param separator the {@code String} used to separate elements of the array. + * @param array the array of elements to get the {@code String} of. + * @param elementToStringFunction the {@code Function} used to generate a representative + * {@code String} for each element. + * @return the representative {@code String}. + */ + public static String getArrayString(String prefix, String suffix, String separator, T[] array, Function elementToStringFunction) + { + if (array == null) + return "null"; + else + return getCollectionString(prefix, suffix, separator, Arrays.asList(array), elementToStringFunction); } /** @@ -795,32 +835,15 @@ public static String getArrayString(String separator, T[] array, Function * * @param the type of the collection elements. - * @param prefix the {@code String} to prepend to the result. - * @param suffix the {@code String} to append to the result. * @param separator the {@code String} used to separate elements of the collection. * @param collection the series of elements to get the {@code String} of. * @param elementToStringFunction the {@code Function} used to generate a representative * {@code String} for each element. * @return the representative {@code String}. */ - public static String getCollectionString(String prefix, - String suffix, - String separator, - Collection collection, - Function elementToStringFunction) + public static String getCollectionString(String separator, Collection collection, Function elementToStringFunction) { - if (collection == null) - return "null"; - - String ret = getCollectionString(separator, collection, elementToStringFunction); - - if (prefix != null) - ret = prefix + ret; - - if (suffix != null) - ret += suffix; - - return ret; + return getCollectionString(null, null, separator, collection, elementToStringFunction); } /** @@ -833,24 +856,44 @@ public static String getCollectionString(String prefix, *

* * @param the type of the collection elements. + * @param prefix the {@code String} to prepend to the result. + * @param suffix the {@code String} to append to the result. * @param separator the {@code String} used to separate elements of the collection. * @param collection the series of elements to get the {@code String} of. * @param elementToStringFunction the {@code Function} used to generate a representative * {@code String} for each element. * @return the representative {@code String}. */ - public static String getCollectionString(String separator, Collection collection, Function elementToStringFunction) + public static String getCollectionString(String prefix, + String suffix, + String separator, + Collection collection, + Function elementToStringFunction) { if (collection == null) return "null"; - if (collection.isEmpty()) - return ""; - Iterator iterator = collection.iterator(); - String ret = elementToStringFunction.apply(iterator.next()); - while (iterator.hasNext()) - ret += separator + elementToStringFunction.apply(iterator.next()); - return ret; + StringBuilder sb = new StringBuilder(); + if (prefix != null) + sb.append(prefix); + + if (!collection.isEmpty()) + { + Iterator iterator = collection.iterator(); + sb.append(elementToStringFunction.apply(iterator.next())); + + while (iterator.hasNext()) + { + T next = iterator.next(); + sb.append(separator); + sb.append(next == null ? "null" : elementToStringFunction.apply(next)); + } + } + + if (suffix != null) + sb.append(suffix); + + return sb.toString(); } /** From 143bc1662a9a320799e57afa702688b983fe4a65 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Mon, 14 Mar 2022 16:39:57 -0500 Subject: [PATCH 3/7] IO tools: Some cleanup and added regression tests. --- .../ihmc/euclid/tools/EuclidCoreIOTools.java | 199 ++++++---- .../euclid/tools/EuclidCoreIOToolsTest.java | 374 ++++++++++++++++++ 2 files changed, 505 insertions(+), 68 deletions(-) create mode 100644 src/test/java/us/ihmc/euclid/tools/EuclidCoreIOToolsTest.java diff --git a/src/main/java/us/ihmc/euclid/tools/EuclidCoreIOTools.java b/src/main/java/us/ihmc/euclid/tools/EuclidCoreIOTools.java index 795fa542c..89bf6b0e4 100644 --- a/src/main/java/us/ihmc/euclid/tools/EuclidCoreIOTools.java +++ b/src/main/java/us/ihmc/euclid/tools/EuclidCoreIOTools.java @@ -132,11 +132,11 @@ private static String getHomogeneousTransformString(String format, Matrix3DReadO for (int i = 0; i < 3; i++) { ret += getStringOf(null, " ", " ", format, matrix.getElement(i, 0), matrix.getElement(i, 1), matrix.getElement(i, 2)); - ret += "| " + String.format(format, translation.getElement(i)) + "\n"; + ret += "| " + toString(format, translation.getElement(i)) + "\n"; } ret += getStringOf(null, " ", " ", format, 0.0, 0.0, 0.0); - ret += "| " + String.format(format, 1.0); + ret += "| " + toString(format, 1.0); return ret; } @@ -632,110 +632,112 @@ public static String getYawPitchRollString(String format, double yaw, double pit } /** - * Gets a representative {@code String} of a series of doubles given specific prefix, suffix, and - * separator. + * Gets a representative {@code String} of a series of doubles given a specific separator. *

- * Using {@code prefix = "("}, {@code suffix = ")"}, and {@code separator = ", "}, this provides a - * {@code String} as follows: + * Using {@code separator = ", "}, this provides a {@code String} as follows: * * 

-    * ( 0.123, -0.480,  1.457)
+    *  0.123, -0.480,  1.457
     *
*

* - * @param prefix the {@code String} to prepend to the result. - * @param suffix the {@code String} to append to the result. * @param separator the {@code String} to insert between two values. * @param values the values to get the {@code String} of. * @return the representative {@code String}. */ - public static String getStringOf(String prefix, String suffix, String separator, double... values) + public static String getStringOf(String separator, double... values) { - return getStringOf(prefix, suffix, separator, DEFAULT_FORMAT, values); + return getStringOf(separator, DEFAULT_FORMAT, values); } /** - * Gets a representative {@code String} of a series of doubles given specific prefix, suffix, - * separator, and format to use. + * Gets a representative {@code String} of a series of doubles given a specific separator, and + * format to use. *

- * Using the default format {@link #DEFAULT_FORMAT}, {@code prefix = "("}, {@code suffix = ")"}, and - * {@code separator = ", "}, this provides a {@code String} as follows: + * Using the default format {@link #DEFAULT_FORMAT} and {@code separator = ", "}, this provides a + * {@code String} as follows: * * 

-    * ( 0.123, -0.480,  1.457)
+    *  0.123, -0.480,  1.457
     *
*

* - * @param prefix the {@code String} to prepend to the result. - * @param suffix the {@code String} to append to the result. * @param separator the {@code String} to insert between two values. * @param format the format to use for each number. * @param values the values to get the {@code String} of. * @return the representative {@code String}. */ - public static String getStringOf(String prefix, String suffix, String separator, String format, double... values) + public static String getStringOf(String separator, String format, double... values) { - if (values == null) - return "null"; - - String ret = getStringOf(separator, format, values); - - if (prefix != null) - ret = prefix + ret; - - if (suffix != null) - ret += suffix; - - return ret; + return getStringOf(null, null, separator, format, values); } /** - * Gets a representative {@code String} of a series of doubles given a specific separator. + * Gets a representative {@code String} of a series of doubles given specific prefix, suffix, and + * separator. *

- * Using {@code separator = ", "}, this provides a {@code String} as follows: + * Using {@code prefix = "("}, {@code suffix = ")"}, and {@code separator = ", "}, this provides a + * {@code String} as follows: * * 

-    *  0.123, -0.480,  1.457
+    * ( 0.123, -0.480,  1.457)
     *
*

* + * @param prefix the {@code String} to prepend to the result. + * @param suffix the {@code String} to append to the result. * @param separator the {@code String} to insert between two values. * @param values the values to get the {@code String} of. * @return the representative {@code String}. */ - public static String getStringOf(String separator, double... values) + public static String getStringOf(String prefix, String suffix, String separator, double... values) { - return getStringOf(separator, DEFAULT_FORMAT, values); + return getStringOf(prefix, suffix, separator, DEFAULT_FORMAT, values); } /** - * Gets a representative {@code String} of a series of doubles given a specific separator, and - * format to use. + * Gets a representative {@code String} of a series of doubles given specific prefix, suffix, + * separator, and format to use. *

- * Using the default format {@link #DEFAULT_FORMAT} and {@code separator = ", "}, this provides a - * {@code String} as follows: + * Using the default format {@link #DEFAULT_FORMAT}, {@code prefix = "("}, {@code suffix = ")"}, and + * {@code separator = ", "}, this provides a {@code String} as follows: * * 

-    *  0.123, -0.480,  1.457
+    * ( 0.123, -0.480,  1.457)
     *
*

* + * @param prefix the {@code String} to prepend to the result. + * @param suffix the {@code String} to append to the result. * @param separator the {@code String} to insert between two values. * @param format the format to use for each number. * @param values the values to get the {@code String} of. * @return the representative {@code String}. */ - public static String getStringOf(String separator, String format, double... values) + public static String getStringOf(String prefix, String suffix, String separator, String format, double... values) { if (values == null) return "null"; - if (values.length == 0) - return ""; - String ret = String.format(format, values[0]); - for (int i = 1; i < values.length; i++) - ret += separator + String.format(format, values[i]); - return ret; + StringBuilder sb = new StringBuilder(); + + if (prefix != null) + sb.append(prefix); + + if (values.length > 0) + { + sb.append(toString(format, values[0])); + for (int i = 1; i < values.length; i++) + { + sb.append(separator); + sb.append(toString(format, values[i])); + } + } + + if (suffix != null) + sb.append(suffix); + + return sb.toString(); } /** @@ -747,56 +749,54 @@ public static String getStringOf(String separator, String format, double... valu * opposed to {@link Arrays#toString(Object[])} which outputs all the elements in one line. *

* - * @param separator the {@code String} used to separate elements of the array. - * @param array the array of elements to get the {@code String} of. - * @param elementToStringFunction the {@code Function} used to generate a representative - * {@code String} for each element. + * @param separator the {@code String} used to separate elements of the array. + * @param array the array of elements to get the {@code String} of. * @return the representative {@code String}. */ public static String getArrayString(String separator, Object[] array) { - return getArrayString(separator, array, Object::toString); + return getArrayString(null, null, separator, array); } /** * Gets a representative {@code String} of the elements contained in the given array. *

* This provides an alternative to {@link Arrays#toString(Object[])} where the format of the output - * can be controller by defining a custom {@code separator}. For instance, with + * can be controlled by defining a custom {@code separator}. For instance, with * {@code separator = \n} the resulting {@code String} is composed of one element per line as * opposed to {@link Arrays#toString(Object[])} which outputs all the elements in one line. *

* - * @param the type of the array elements. - * @param separator the {@code String} used to separate elements of the array. - * @param array the array of elements to get the {@code String} of. - * @param elementToStringFunction the {@code Function} used to generate a representative - * {@code String} for each element. + * @param prefix the {@code String} to prepend to the result. + * @param suffix the {@code String} to append to the result. + * @param separator the {@code String} used to separate elements of the array. + * @param array the array of elements to get the {@code String} of. * @return the representative {@code String}. */ - public static String getArrayString(String separator, T[] array, Function elementToStringFunction) + public static String getArrayString(String prefix, String suffix, String separator, Object[] array) { - return getArrayString(null, null, separator, array, elementToStringFunction); + return getArrayString(prefix, suffix, separator, array, Object::toString); } /** * Gets a representative {@code String} of the elements contained in the given array. *

* This provides an alternative to {@link Arrays#toString(Object[])} where the format of the output - * can be controlled by defining a custom {@code separator}. For instance, with + * can be controller by defining a custom {@code separator}. For instance, with * {@code separator = \n} the resulting {@code String} is composed of one element per line as * opposed to {@link Arrays#toString(Object[])} which outputs all the elements in one line. *

* - * @param prefix the {@code String} to prepend to the result. - * @param suffix the {@code String} to append to the result. - * @param separator the {@code String} used to separate elements of the array. - * @param array the array of elements to get the {@code String} of. + * @param the type of the array elements. + * @param separator the {@code String} used to separate elements of the array. + * @param array the array of elements to get the {@code String} of. + * @param elementToStringFunction the {@code Function} used to generate a representative + * {@code String} for each element. * @return the representative {@code String}. */ - public static String getArrayString(String prefix, String suffix, String separator, Object[] array) + public static String getArrayString(String separator, T[] array, Function elementToStringFunction) { - return getArrayString(prefix, suffix, separator, array, Object::toString); + return getArrayString(null, null, separator, array, elementToStringFunction); } /** @@ -825,6 +825,44 @@ public static String getArrayString(String prefix, String suffix, String sep return getCollectionString(prefix, suffix, separator, Arrays.asList(array), elementToStringFunction); } + /** + * Gets a representative {@code String} of the elements contained in the given {@code Collection}. + *

+ * This provides an alternative to {@link Collection#toString()} where the format of the output can + * be controller by defining a custom {@code separator}. For instance, with {@code separator = \n} + * the resulting {@code String} is composed of one element per line as opposed to + * {@link Collection#toString()} which outputs all the elements in one line. + *

+ * + * @param separator the {@code String} used to separate elements of the collection. + * @param collection the series of elements to get the {@code String} of. + * @return the representative {@code String}. + */ + public static String getCollectionString(String separator, Collection collection) + { + return getCollectionString(null, null, separator, collection); + } + + /** + * Gets a representative {@code String} of the elements contained in the given {@code Collection}. + *

+ * This provides an alternative to {@link Collection#toString()} where the format of the output can + * be controller by defining a custom {@code separator}. For instance, with {@code separator = \n} + * the resulting {@code String} is composed of one element per line as opposed to + * {@link Collection#toString()} which outputs all the elements in one line. + *

+ * + * @param prefix the {@code String} to prepend to the result. + * @param suffix the {@code String} to append to the result. + * @param separator the {@code String} used to separate elements of the collection. + * @param collection the series of elements to get the {@code String} of. + * @return the representative {@code String}. + */ + public static String getCollectionString(String prefix, String suffix, String separator, Collection collection) + { + return getCollectionString(prefix, suffix, separator, collection, Object::toString); + } + /** * Gets a representative {@code String} of the elements contained in the given {@code Collection}. *

@@ -896,6 +934,31 @@ public static String getCollectionString(String prefix, return sb.toString(); } + public static String toString(String format, double value) + { + return format != null ? String.format(format, value) : Double.toString(value); + } + + public static String toString(String format, float value) + { + return format != null ? String.format(format, value) : Float.toString(value); + } + + public static String toString(String format, boolean value) + { + return format != null ? String.format(format, value) : Boolean.toString(value); + } + + public static String toString(String format, int value) + { + return format != null ? String.format(format, value) : Integer.toString(value); + } + + public static String toString(String format, long value) + { + return format != null ? String.format(format, value) : Long.toString(value); + } + /** * Gets the {@code String} for formatting decimal numbers. * diff --git a/src/test/java/us/ihmc/euclid/tools/EuclidCoreIOToolsTest.java b/src/test/java/us/ihmc/euclid/tools/EuclidCoreIOToolsTest.java new file mode 100644 index 000000000..7543f4845 --- /dev/null +++ b/src/test/java/us/ihmc/euclid/tools/EuclidCoreIOToolsTest.java @@ -0,0 +1,374 @@ +package us.ihmc.euclid.tools; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import java.util.Arrays; +import java.util.List; +import java.util.Random; +import java.util.stream.IntStream; + +import org.junit.jupiter.api.Test; + +import us.ihmc.euclid.axisAngle.AxisAngle; +import us.ihmc.euclid.matrix.Matrix3D; +import us.ihmc.euclid.orientation.Orientation2D; +import us.ihmc.euclid.transform.AffineTransform; +import us.ihmc.euclid.transform.QuaternionBasedTransform; +import us.ihmc.euclid.transform.RigidBodyTransform; +import us.ihmc.euclid.tuple2D.Point2D; +import us.ihmc.euclid.tuple3D.Point3D; +import us.ihmc.euclid.tuple4D.Vector4D; +import us.ihmc.euclid.yawPitchRoll.YawPitchRoll; + +public class EuclidCoreIOToolsTest +{ + @Test + public void testGetRigidBodyTransformStringRegression() + { + Random random = new Random(345345); + String expected; + String randomFormat = EuclidCoreIOTools.getStringFormat(8, 7); + + RigidBodyTransform t = EuclidCoreRandomTools.nextRigidBodyTransform(random); + + expected = "-0.146 0.372 0.917 | 0.969\n" // + + " 0.050 0.928 -0.369 | 0.028\n" // + + "-0.988 -0.008 -0.154 | -0.619\n" // + + " 0.000 0.000 0.000 | 1.000"; + assertEquals(expected, EuclidCoreIOTools.getRigidBodyTransformString(t)); + assertEquals(expected, EuclidCoreIOTools.getRigidBodyTransformString(EuclidCoreIOTools.DEFAULT_FORMAT, t)); + assertEquals("null", EuclidCoreIOTools.getRigidBodyTransformString(null)); + expected = "-0.1460544 0.3719639 0.9166848 | 0.9690608\n" // + + "0.0497947 0.9282109 -0.3687071 | 0.0275034\n" // + + "-0.9880226 -0.0082053 -0.1540911 | -0.6188257\n"// + + "0.0000000 0.0000000 0.0000000 | 1.0000000"; + assertEquals(expected, EuclidCoreIOTools.getRigidBodyTransformString(randomFormat, t)); + expected = "-0.14605438133050594 0.3719638705161967 0.916684785913224 | 0.9690607506715772\n" // + + "0.04979465293823307 0.9282109416799752 -0.36870711992628336 | 0.02750342450772747\n" // + + "-0.9880225757708799 -0.008205289524678472 -0.154091086669191 | -0.6188257166341338\n"// + + "0.0 0.0 0.0 | 1.0"; + assertEquals(expected, EuclidCoreIOTools.getRigidBodyTransformString(null, t)); + } + + @Test + public void testGetAffineTransformStringRegression() + { + Random random = new Random(345345); + String expected; + String randomFormat = EuclidCoreIOTools.getStringFormat(8, 7); + + AffineTransform t = EuclidCoreRandomTools.nextAffineTransform(random); + + expected = " 1.639 -5.595 9.691 | -0.618\n" // + + " 0.275 -6.188 7.012 | -0.556\n" // + + "-0.728 6.787 -4.461 | 0.498\n" // + + " 0.000 0.000 0.000 | 1.000"; + assertEquals(expected, EuclidCoreIOTools.getAffineTransformString(t)); + assertEquals(expected, EuclidCoreIOTools.getAffineTransformString(EuclidCoreIOTools.DEFAULT_FORMAT, t)); + assertEquals("null", EuclidCoreIOTools.getAffineTransformString(null)); + + expected = "1.6394447 -5.5954184 9.6906075 | -0.6176655\n" // + + "0.2750342 -6.1882572 7.0120891 | -0.5556910\n" // + + "-0.7281610 6.7873417 -4.4606236 | 0.4979491\n"// + + "0.0000000 0.0000000 0.0000000 | 1.0000000"; + assertEquals(expected, EuclidCoreIOTools.getAffineTransformString(randomFormat, t)); + + expected = "1.6394446573547388 -5.59541839804184 9.690607506715772 | -0.6176654552449496\n" // + + "0.2750342450772756 -6.188257166341337 7.012089076126404 | -0.5556910209492063\n" // + + "-0.7281610246766981 6.787341699515743 -4.460623648325692 | 0.4979490960610431\n"// + + "0.0 0.0 0.0 | 1.0"; + assertEquals(expected, EuclidCoreIOTools.getAffineTransformString(null, t)); + } + + @Test + public void testGetQuaternionBasedTransformStringRegression() + { + Random random = new Random(345345); + String expected; + String randomFormat = EuclidCoreIOTools.getStringFormat(8, 7); + + QuaternionBasedTransform t = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); + + expected = "Quaternion: ( 0.141, 0.746, -0.126, 0.638 )\nTranslation: ( 0.969, 0.028, -0.619 )"; + assertEquals(expected, EuclidCoreIOTools.getQuaternionBasedTransformString(t)); + assertEquals(expected, EuclidCoreIOTools.getQuaternionBasedTransformString(EuclidCoreIOTools.DEFAULT_FORMAT, t)); + assertEquals("null", EuclidCoreIOTools.getQuaternionBasedTransformString(null)); + + expected = "Quaternion: (0.1412673, 0.7463840, -0.1262461, 0.6379783 )\nTranslation: (0.9690608, 0.0275034, -0.6188257 )"; + assertEquals(expected, EuclidCoreIOTools.getQuaternionBasedTransformString(randomFormat, t)); + + expected = "Quaternion: (0.14126726766904454, 0.7463840180630331, -0.12624614150672073, 0.6379783447892801 )\n" + + "Translation: (0.9690607506715772, 0.02750342450772747, -0.6188257166341338 )"; + assertEquals(expected, EuclidCoreIOTools.getQuaternionBasedTransformString(null, t)); + } + + @Test + public void testGetTuple2DStringRegression() + { + Random random = new Random(345345); + String expected; + String randomFormat = EuclidCoreIOTools.getStringFormat(8, 7); + + Point2D t = EuclidCoreRandomTools.nextPoint2D(random); + + expected = "( 0.164, -0.560 )"; + assertEquals(expected, EuclidCoreIOTools.getTuple2DString(t)); + assertEquals(expected, EuclidCoreIOTools.getTuple2DString(EuclidCoreIOTools.DEFAULT_FORMAT, t)); + assertEquals("null", EuclidCoreIOTools.getTuple2DString(null)); + + expected = "(0.1639445, -0.5595418 )"; + assertEquals(expected, EuclidCoreIOTools.getTuple2DString(randomFormat, t)); + + expected = "(0.16394446573547383, -0.559541839804184 )"; + assertEquals(expected, EuclidCoreIOTools.getTuple2DString(null, t)); + } + + @Test + public void testGetTuple3DStringRegression() + { + Random random = new Random(345345); + String expected; + String randomFormat = EuclidCoreIOTools.getStringFormat(8, 7); + + Point3D t = EuclidCoreRandomTools.nextPoint3D(random); + + expected = "( 0.164, -0.560, 0.969 )"; + assertEquals(expected, EuclidCoreIOTools.getTuple3DString(t)); + assertEquals(expected, EuclidCoreIOTools.getTuple3DString(EuclidCoreIOTools.DEFAULT_FORMAT, t)); + assertEquals("null", EuclidCoreIOTools.getTuple3DString(null)); + + expected = "(0.1639445, -0.5595418, 0.9690608 )"; + assertEquals(expected, EuclidCoreIOTools.getTuple3DString(randomFormat, t)); + + expected = "(0.16394446573547383, -0.559541839804184, 0.9690607506715772 )"; + assertEquals(expected, EuclidCoreIOTools.getTuple3DString(null, t)); + } + + @Test + public void testGetTuple4DStringRegression() + { + Random random = new Random(345345); + String expected; + String randomFormat = EuclidCoreIOTools.getStringFormat(8, 7); + + Vector4D t = EuclidCoreRandomTools.nextVector4D(random); + + expected = "( 0.164, -0.560, 0.969, 0.028 )"; + assertEquals(expected, EuclidCoreIOTools.getTuple4DString(t)); + assertEquals(expected, EuclidCoreIOTools.getTuple4DString(EuclidCoreIOTools.DEFAULT_FORMAT, t)); + assertEquals("null", EuclidCoreIOTools.getTuple4DString(null)); + + expected = "(0.1639445, -0.5595418, 0.9690608, 0.0275034 )"; + assertEquals(expected, EuclidCoreIOTools.getTuple4DString(randomFormat, t)); + + expected = "(0.16394446573547383, -0.559541839804184, 0.9690607506715772, 0.02750342450772747 )"; + assertEquals(expected, EuclidCoreIOTools.getTuple4DString(null, t)); + } + + @Test + public void testGetAxisAngleStringRegression() + { + Random random = new Random(345345); + String expected; + String randomFormat = EuclidCoreIOTools.getStringFormat(8, 7); + + AxisAngle t = EuclidCoreRandomTools.nextAxisAngle(random); + + expected = "(-0.183, -0.969, 0.164, -1.758 )"; + assertEquals(expected, EuclidCoreIOTools.getAxisAngleString(t)); + assertEquals(expected, EuclidCoreIOTools.getAxisAngleString(EuclidCoreIOTools.DEFAULT_FORMAT, t)); + assertEquals("null", EuclidCoreIOTools.getAxisAngleString(null)); + expected = "(-0.1834510, -0.9692615, 0.1639445, -1.7578525 )"; + assertEquals(expected, EuclidCoreIOTools.getAxisAngleString(randomFormat, t)); + expected = "(-0.18345104608744625, -0.9692615363482303, 0.16394446573547383, -1.7578525333049415 )"; + assertEquals(expected, EuclidCoreIOTools.getAxisAngleString(null, t)); + } + + @Test + public void testGetOrientation2DStringRegression() + { + Random random = new Random(345345); + String expected; + String randomFormat = EuclidCoreIOTools.getStringFormat(8, 7); + + Orientation2D t = EuclidCoreRandomTools.nextOrientation2D(random); + + expected = "( 0.515 )"; + assertEquals(expected, EuclidCoreIOTools.getOrientation2DString(t)); + assertEquals(expected, EuclidCoreIOTools.getOrientation2DString(EuclidCoreIOTools.DEFAULT_FORMAT, t)); + assertEquals("null", EuclidCoreIOTools.getOrientation2DString(null)); + expected = "(0.5150467 )"; + assertEquals(expected, EuclidCoreIOTools.getOrientation2DString(randomFormat, t)); + expected = "(0.5150467291512681 )"; + assertEquals(expected, EuclidCoreIOTools.getOrientation2DString(null, t)); + } + + @Test + public void testGetMatrix3DStringRegression() + { + Random random = new Random(345345); + String expected; + String randomFormat = EuclidCoreIOTools.getStringFormat(8, 7); + + Matrix3D t = EuclidCoreRandomTools.nextMatrix3D(random); + + expected = "/ 0.164, -0.560, 0.969 \\\n"// + + "| 0.028, -0.619, 0.701 |\n"// + + "\\-0.073, 0.679, -0.446 /"; + assertEquals(expected, EuclidCoreIOTools.getMatrix3DString(t)); + assertEquals(expected, EuclidCoreIOTools.getMatrix3DString(EuclidCoreIOTools.DEFAULT_FORMAT, t)); + assertEquals("null", EuclidCoreIOTools.getMatrix3DString(null)); + expected = "/0.1639445, -0.5595418, 0.9690608 \\\n"// + + "|0.0275034, -0.6188257, 0.7012089 |\n"// + + "\\-0.0728161, 0.6787342, -0.4460624 /"; + assertEquals(expected, EuclidCoreIOTools.getMatrix3DString(randomFormat, t)); + expected = "/0.16394446573547383, -0.559541839804184, 0.9690607506715772 \\\n"// + + "|0.02750342450772747, -0.6188257166341338, 0.7012089076126404 |\n"// + + "\\-0.0728161024676699, 0.6787341699515741, -0.4460623648325692 /"; + assertEquals(expected, EuclidCoreIOTools.getMatrix3DString(null, t)); + } + + @Test + public void testGetYawPitchRollStringRegression() + { + Random random = new Random(345345); + String expected; + String randomFormat = EuclidCoreIOTools.getStringFormat(8, 7); + + YawPitchRoll t = EuclidCoreRandomTools.nextYawPitchRoll(random); + + expected = "yaw-pitch-roll: ( 0.515, -0.861, 3.044)"; + assertEquals(expected, EuclidCoreIOTools.getYawPitchRollString(t)); + assertEquals(expected, EuclidCoreIOTools.getYawPitchRollString(EuclidCoreIOTools.DEFAULT_FORMAT, t)); + assertEquals("null", EuclidCoreIOTools.getYawPitchRollString(null)); + expected = "yaw-pitch-roll: (0.5150467, -0.8611524, 3.0443941)"; + assertEquals(expected, EuclidCoreIOTools.getYawPitchRollString(randomFormat, t)); + expected = "yaw-pitch-roll: (0.5150467291512681, -0.8611524243712763, 3.0443941351920367)"; + assertEquals(expected, EuclidCoreIOTools.getYawPitchRollString(null, t)); + } + + @Test + public void testGetStringOf() + { + Random random = new Random(345345); + String expected; + String separator = "-p-"; + String randomFormat = EuclidCoreIOTools.getStringFormat(8, 7); + + double[] values = random.doubles(3).toArray(); + expected = " 0.582-p- 0.220-p- 0.985"; + assertEquals(expected, EuclidCoreIOTools.getStringOf(separator, values)); + assertEquals(expected, EuclidCoreIOTools.getStringOf(separator, EuclidCoreIOTools.DEFAULT_FORMAT, values)); + assertEquals(expected, EuclidCoreIOTools.getStringOf(null, null, separator, values)); + assertEquals(expected, EuclidCoreIOTools.getStringOf(null, null, separator, EuclidCoreIOTools.DEFAULT_FORMAT, values)); + + assertEquals("null", EuclidCoreIOTools.getStringOf(separator, null)); + assertEquals("null", EuclidCoreIOTools.getStringOf(separator, EuclidCoreIOTools.DEFAULT_FORMAT, null)); + assertEquals("null", EuclidCoreIOTools.getStringOf(null, null, separator, null)); + assertEquals("null", EuclidCoreIOTools.getStringOf(null, null, separator, EuclidCoreIOTools.DEFAULT_FORMAT, null)); + + assertEquals("", EuclidCoreIOTools.getStringOf(separator, new double[0])); + assertEquals("", EuclidCoreIOTools.getStringOf(separator, EuclidCoreIOTools.DEFAULT_FORMAT, new double[0])); + assertEquals("", EuclidCoreIOTools.getStringOf(null, null, separator, new double[0])); + assertEquals("", EuclidCoreIOTools.getStringOf(null, null, separator, EuclidCoreIOTools.DEFAULT_FORMAT, new double[0])); + + expected = "0.5819722-p-0.2202291-p-0.9845304"; + assertEquals(expected, EuclidCoreIOTools.getStringOf(separator, randomFormat, values)); + assertEquals(expected, EuclidCoreIOTools.getStringOf(null, null, separator, randomFormat, values)); + + expected = "0.5819722328677369-p-0.22022908009790798-p-0.9845303753357886"; + assertEquals(expected, EuclidCoreIOTools.getStringOf(separator, null, values)); + assertEquals(expected, EuclidCoreIOTools.getStringOf(null, null, separator, null, values)); + + String prefix = "!prefix!"; + String suffix = "?suffix?"; + expected = "!prefix! 0.582-p- 0.220-p- 0.985?suffix?"; + assertEquals(expected, EuclidCoreIOTools.getStringOf(prefix, suffix, separator, values)); + assertEquals(expected, EuclidCoreIOTools.getStringOf(prefix, suffix, separator, EuclidCoreIOTools.DEFAULT_FORMAT, values)); + + assertEquals("null", EuclidCoreIOTools.getStringOf(prefix, suffix, separator, null)); + assertEquals("null", EuclidCoreIOTools.getStringOf(prefix, suffix, separator, EuclidCoreIOTools.DEFAULT_FORMAT, null)); + + assertEquals("!prefix!?suffix?", EuclidCoreIOTools.getStringOf(prefix, suffix, separator, new double[0])); + assertEquals("!prefix!?suffix?", EuclidCoreIOTools.getStringOf(prefix, suffix, separator, EuclidCoreIOTools.DEFAULT_FORMAT, new double[0])); + + expected = "!prefix!0.5819722-p-0.2202291-p-0.9845304?suffix?"; + assertEquals(expected, EuclidCoreIOTools.getStringOf(prefix, suffix, separator, randomFormat, values)); + + expected = "!prefix!0.5819722328677369-p-0.22022908009790798-p-0.9845303753357886?suffix?"; + assertEquals(expected, EuclidCoreIOTools.getStringOf(prefix, suffix, separator, null, values)); + } + + @Test + public void testGetArrayString() + { + Random random = new Random(345345); + String expected; + String separator = "-p-"; + TestObject[] array = nextTestObjectArray(random, 3); + + expected = "[blop=4, choux=-0.0727027174796302]-p-[blop=-4, choux=0.4845303753357886]-p-[blop=44, choux=0.2874116776971093]"; + assertEquals(expected, EuclidCoreIOTools.getArrayString(separator, array)); + assertEquals(expected, EuclidCoreIOTools.getArrayString(null, null, separator, array)); + assertEquals(expected, EuclidCoreIOTools.getArrayString(separator, array, Object::toString)); + assertEquals(expected, EuclidCoreIOTools.getArrayString(null, null, separator, array, Object::toString)); + + String prefix = "!prefix!"; + String suffix = "?suffix?"; + expected = "!prefix![blop=4, choux=-0.0727027174796302]-p-[blop=-4, choux=0.4845303753357886]-p-[blop=44, choux=0.2874116776971093]?suffix?"; + assertEquals(expected, EuclidCoreIOTools.getArrayString(prefix, suffix, separator, array)); + assertEquals(expected, EuclidCoreIOTools.getArrayString(prefix, suffix, separator, array, Object::toString)); + + expected = "blop=4-p-blop=-4-p-blop=44"; + assertEquals(expected, EuclidCoreIOTools.getArrayString(separator, array, e -> "blop=" + e.blop)); + assertEquals(expected, EuclidCoreIOTools.getArrayString(null, null, separator, array, e -> "blop=" + e.blop)); + } + + @Test + public void testGetCollectionString() + { + Random random = new Random(345345); + String expected; + String separator = "-p-"; + List array = Arrays.asList(nextTestObjectArray(random, 3)); + + expected = "[blop=4, choux=-0.0727027174796302]-p-[blop=-4, choux=0.4845303753357886]-p-[blop=44, choux=0.2874116776971093]"; + assertEquals(expected, EuclidCoreIOTools.getCollectionString(separator, array)); + assertEquals(expected, EuclidCoreIOTools.getCollectionString(null, null, separator, array)); + assertEquals(expected, EuclidCoreIOTools.getCollectionString(separator, array, Object::toString)); + assertEquals(expected, EuclidCoreIOTools.getCollectionString(null, null, separator, array, Object::toString)); + + String prefix = "!prefix!"; + String suffix = "?suffix?"; + expected = "!prefix![blop=4, choux=-0.0727027174796302]-p-[blop=-4, choux=0.4845303753357886]-p-[blop=44, choux=0.2874116776971093]?suffix?"; + assertEquals(expected, EuclidCoreIOTools.getCollectionString(prefix, suffix, separator, array)); + assertEquals(expected, EuclidCoreIOTools.getCollectionString(prefix, suffix, separator, array, Object::toString)); + + expected = "blop=4-p-blop=-4-p-blop=44"; + assertEquals(expected, EuclidCoreIOTools.getCollectionString(separator, array, e -> "blop=" + e.blop)); + assertEquals(expected, EuclidCoreIOTools.getCollectionString(null, null, separator, array, e -> "blop=" + e.blop)); + } + + private static TestObject[] nextTestObjectArray(Random random, int length) + { + return IntStream.range(0, length).mapToObj(i -> new TestObject(random)).toArray(TestObject[]::new); + } + + private static class TestObject + { + private int blop; + private double choux; + + public TestObject(Random random) + { + blop = random.nextInt(100) - 50; + choux = random.nextDouble() - 0.5; + } + + @Override + public String toString() + { + return "[blop=" + blop + ", choux=" + choux + "]"; + } + } +} From 5e4ddb9f86d4e201910a65ee52d081b734b3fff7 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Mon, 14 Mar 2022 17:31:33 -0500 Subject: [PATCH 4/7] Added some more String generators. --- .../ihmc/euclid/tools/EuclidCoreIOTools.java | 298 ++++++++++++++++++ .../euclid/tools/EuclidCoreIOToolsTest.java | 187 ++++++++--- 2 files changed, 448 insertions(+), 37 deletions(-) diff --git a/src/main/java/us/ihmc/euclid/tools/EuclidCoreIOTools.java b/src/main/java/us/ihmc/euclid/tools/EuclidCoreIOTools.java index 89bf6b0e4..ce2b51445 100644 --- a/src/main/java/us/ihmc/euclid/tools/EuclidCoreIOTools.java +++ b/src/main/java/us/ihmc/euclid/tools/EuclidCoreIOTools.java @@ -740,6 +740,304 @@ public static String getStringOf(String prefix, String suffix, String separator, return sb.toString(); } + /** + * Gets a representative {@code String} of a series of floats given a specific separator. + *

+ * Using {@code separator = ", "}, this provides a {@code String} as follows: + * + * 

+    *  0.123, -0.480,  1.457
+    *
+ *

+ * + * @param separator the {@code String} to insert between two values. + * @param values the values to get the {@code String} of. + * @return the representative {@code String}. + */ + public static String getStringOf(String separator, float... values) + { + return getStringOf(separator, DEFAULT_FORMAT, values); + } + + /** + * Gets a representative {@code String} of a series of floats given a specific separator, and format + * to use. + *

+ * Using the default format {@link #DEFAULT_FORMAT} and {@code separator = ", "}, this provides a + * {@code String} as follows: + * + * 

+    *  0.123, -0.480,  1.457
+    *
+ *

+ * + * @param separator the {@code String} to insert between two values. + * @param format the format to use for each number. + * @param values the values to get the {@code String} of. + * @return the representative {@code String}. + */ + public static String getStringOf(String separator, String format, float... values) + { + return getStringOf(null, null, separator, format, values); + } + + /** + * Gets a representative {@code String} of a series of floats given specific prefix, suffix, and + * separator. + *

+ * Using {@code prefix = "("}, {@code suffix = ")"}, and {@code separator = ", "}, this provides a + * {@code String} as follows: + * + * 

+    * ( 0.123, -0.480,  1.457)
+    *
+ *

+ * + * @param prefix the {@code String} to prepend to the result. + * @param suffix the {@code String} to append to the result. + * @param separator the {@code String} to insert between two values. + * @param values the values to get the {@code String} of. + * @return the representative {@code String}. + */ + public static String getStringOf(String prefix, String suffix, String separator, float... values) + { + return getStringOf(prefix, suffix, separator, DEFAULT_FORMAT, values); + } + + /** + * Gets a representative {@code String} of a series of floats given specific prefix, suffix, + * separator, and format to use. + *

+ * Using the default format {@link #DEFAULT_FORMAT}, {@code prefix = "("}, {@code suffix = ")"}, and + * {@code separator = ", "}, this provides a {@code String} as follows: + * + * 

+    * ( 0.123, -0.480,  1.457)
+    *
+ *

+ * + * @param prefix the {@code String} to prepend to the result. + * @param suffix the {@code String} to append to the result. + * @param separator the {@code String} to insert between two values. + * @param format the format to use for each number. + * @param values the values to get the {@code String} of. + * @return the representative {@code String}. + */ + public static String getStringOf(String prefix, String suffix, String separator, String format, float... values) + { + if (values == null) + return "null"; + + StringBuilder sb = new StringBuilder(); + + if (prefix != null) + sb.append(prefix); + + if (values.length > 0) + { + sb.append(toString(format, values[0])); + for (int i = 1; i < values.length; i++) + { + sb.append(separator); + sb.append(toString(format, values[i])); + } + } + + if (suffix != null) + sb.append(suffix); + + return sb.toString(); + } + + /** + * Gets a representative {@code String} of a series of booleans given a specific separator. + *

+ * Using {@code separator = ", "}, this provides a {@code String} as follows: + * + * 

+    * true, false, false
+    *
+ *

+ * + * @param separator the {@code String} to insert between two values. + * @param values the values to get the {@code String} of. + * @return the representative {@code String}. + */ + public static String getStringOf(String separator, boolean... values) + { + return getStringOf(null, null, separator, values); + } + + /** + * Gets a representative {@code String} of a series of booleans given specific prefix, suffix, and + * separator. + *

+ * Using {@code prefix = "("}, {@code suffix = ")"}, and {@code separator = ", "}, this provides a + * {@code String} as follows: + * + * 

+    * (true, false, false)
+    *
+ *

+ * + * @param prefix the {@code String} to prepend to the result. + * @param suffix the {@code String} to append to the result. + * @param separator the {@code String} to insert between two values. + * @param values the values to get the {@code String} of. + * @return the representative {@code String}. + */ + public static String getStringOf(String prefix, String suffix, String separator, boolean... values) + { + if (values == null) + return "null"; + + StringBuilder sb = new StringBuilder(); + + if (prefix != null) + sb.append(prefix); + + if (values.length > 0) + { + sb.append(Boolean.toString(values[0])); + for (int i = 1; i < values.length; i++) + { + sb.append(separator); + sb.append(Boolean.toString(values[i])); + } + } + + if (suffix != null) + sb.append(suffix); + + return sb.toString(); + } + + /** + * Gets a representative {@code String} of a series of integers given a specific separator. + *

+ * Using {@code separator = ", "}, this provides a {@code String} as follows: + * + * 

+    * 47, -52200, 9874
+    *
+ *

+ * + * @param separator the {@code String} to insert between two values. + * @param values the values to get the {@code String} of. + * @return the representative {@code String}. + */ + public static String getStringOf(String separator, int... values) + { + return getStringOf(null, null, separator, values); + } + + /** + * Gets a representative {@code String} of a series of integers given specific prefix, suffix, and + * separator. + *

+ * Using {@code prefix = "("}, {@code suffix = ")"}, and {@code separator = ", "}, this provides a + * {@code String} as follows: + * + * 

+    * (47, -52200, 9874)
+    *
+ *

+ * + * @param prefix the {@code String} to prepend to the result. + * @param suffix the {@code String} to append to the result. + * @param separator the {@code String} to insert between two values. + * @param values the values to get the {@code String} of. + * @return the representative {@code String}. + */ + public static String getStringOf(String prefix, String suffix, String separator, int... values) + { + if (values == null) + return "null"; + + StringBuilder sb = new StringBuilder(); + + if (prefix != null) + sb.append(prefix); + + if (values.length > 0) + { + sb.append(Integer.toString(values[0])); + for (int i = 1; i < values.length; i++) + { + sb.append(separator); + sb.append(Integer.toString(values[i])); + } + } + + if (suffix != null) + sb.append(suffix); + + return sb.toString(); + } + + /** + * Gets a representative {@code String} of a series of longs given a specific separator. + *

+ * Using {@code separator = ", "}, this provides a {@code String} as follows: + * + * 

+    * 47, -52200, 9874
+    *
+ *

+ * + * @param separator the {@code String} to insert between two values. + * @param values the values to get the {@code String} of. + * @return the representative {@code String}. + */ + public static String getStringOf(String separator, long... values) + { + return getStringOf(null, null, separator, values); + } + + /** + * Gets a representative {@code String} of a series of longs given specific prefix, suffix, and + * separator. + *

+ * Using {@code prefix = "("}, {@code suffix = ")"}, and {@code separator = ", "}, this provides a + * {@code String} as follows: + * + * 

+    * (47, -52200, 9874)
+    *
+ *

+ * + * @param prefix the {@code String} to prepend to the result. + * @param suffix the {@code String} to append to the result. + * @param separator the {@code String} to insert between two values. + * @param values the values to get the {@code String} of. + * @return the representative {@code String}. + */ + public static String getStringOf(String prefix, String suffix, String separator, long... values) + { + if (values == null) + return "null"; + + StringBuilder sb = new StringBuilder(); + + if (prefix != null) + sb.append(prefix); + + if (values.length > 0) + { + sb.append(Long.toString(values[0])); + for (int i = 1; i < values.length; i++) + { + sb.append(separator); + sb.append(Long.toString(values[i])); + } + } + + if (suffix != null) + sb.append(suffix); + + return sb.toString(); + } + /** * Gets a representative {@code String} of the elements contained in the given array. *

diff --git a/src/test/java/us/ihmc/euclid/tools/EuclidCoreIOToolsTest.java b/src/test/java/us/ihmc/euclid/tools/EuclidCoreIOToolsTest.java index 7543f4845..c5e858c89 100644 --- a/src/test/java/us/ihmc/euclid/tools/EuclidCoreIOToolsTest.java +++ b/src/test/java/us/ihmc/euclid/tools/EuclidCoreIOToolsTest.java @@ -255,48 +255,161 @@ public void testGetStringOf() String separator = "-p-"; String randomFormat = EuclidCoreIOTools.getStringFormat(8, 7); - double[] values = random.doubles(3).toArray(); - expected = " 0.582-p- 0.220-p- 0.985"; - assertEquals(expected, EuclidCoreIOTools.getStringOf(separator, values)); - assertEquals(expected, EuclidCoreIOTools.getStringOf(separator, EuclidCoreIOTools.DEFAULT_FORMAT, values)); - assertEquals(expected, EuclidCoreIOTools.getStringOf(null, null, separator, values)); - assertEquals(expected, EuclidCoreIOTools.getStringOf(null, null, separator, EuclidCoreIOTools.DEFAULT_FORMAT, values)); - - assertEquals("null", EuclidCoreIOTools.getStringOf(separator, null)); - assertEquals("null", EuclidCoreIOTools.getStringOf(separator, EuclidCoreIOTools.DEFAULT_FORMAT, null)); - assertEquals("null", EuclidCoreIOTools.getStringOf(null, null, separator, null)); - assertEquals("null", EuclidCoreIOTools.getStringOf(null, null, separator, EuclidCoreIOTools.DEFAULT_FORMAT, null)); - - assertEquals("", EuclidCoreIOTools.getStringOf(separator, new double[0])); - assertEquals("", EuclidCoreIOTools.getStringOf(separator, EuclidCoreIOTools.DEFAULT_FORMAT, new double[0])); - assertEquals("", EuclidCoreIOTools.getStringOf(null, null, separator, new double[0])); - assertEquals("", EuclidCoreIOTools.getStringOf(null, null, separator, EuclidCoreIOTools.DEFAULT_FORMAT, new double[0])); - - expected = "0.5819722-p-0.2202291-p-0.9845304"; - assertEquals(expected, EuclidCoreIOTools.getStringOf(separator, randomFormat, values)); - assertEquals(expected, EuclidCoreIOTools.getStringOf(null, null, separator, randomFormat, values)); - - expected = "0.5819722328677369-p-0.22022908009790798-p-0.9845303753357886"; - assertEquals(expected, EuclidCoreIOTools.getStringOf(separator, null, values)); - assertEquals(expected, EuclidCoreIOTools.getStringOf(null, null, separator, null, values)); + { // Test generators for double values + double[] values = random.doubles(3).map(d -> d - 0.5).toArray(); + expected = " 0.082-p--0.280-p- 0.485"; + assertEquals(expected, EuclidCoreIOTools.getStringOf(separator, values)); + assertEquals(expected, EuclidCoreIOTools.getStringOf(separator, EuclidCoreIOTools.DEFAULT_FORMAT, values)); + assertEquals(expected, EuclidCoreIOTools.getStringOf(null, null, separator, values)); + assertEquals(expected, EuclidCoreIOTools.getStringOf(null, null, separator, EuclidCoreIOTools.DEFAULT_FORMAT, values)); + + assertEquals("null", EuclidCoreIOTools.getStringOf(separator, (double[]) null)); + assertEquals("null", EuclidCoreIOTools.getStringOf(separator, EuclidCoreIOTools.DEFAULT_FORMAT, (double[]) null)); + assertEquals("null", EuclidCoreIOTools.getStringOf(null, null, separator, (double[]) null)); + assertEquals("null", EuclidCoreIOTools.getStringOf(null, null, separator, EuclidCoreIOTools.DEFAULT_FORMAT, (double[]) null)); + + assertEquals("", EuclidCoreIOTools.getStringOf(separator, new double[0])); + assertEquals("", EuclidCoreIOTools.getStringOf(separator, EuclidCoreIOTools.DEFAULT_FORMAT, new double[0])); + assertEquals("", EuclidCoreIOTools.getStringOf(null, null, separator, new double[0])); + assertEquals("", EuclidCoreIOTools.getStringOf(null, null, separator, EuclidCoreIOTools.DEFAULT_FORMAT, new double[0])); + + expected = "0.0819722-p--0.2797709-p-0.4845304"; + assertEquals(expected, EuclidCoreIOTools.getStringOf(separator, randomFormat, values)); + assertEquals(expected, EuclidCoreIOTools.getStringOf(null, null, separator, randomFormat, values)); + + expected = "0.08197223286773692-p--0.279770919902092-p-0.4845303753357886"; + assertEquals(expected, EuclidCoreIOTools.getStringOf(separator, null, values)); + assertEquals(expected, EuclidCoreIOTools.getStringOf(null, null, separator, null, values)); + + String prefix = "!prefix!"; + String suffix = "?suffix?"; + expected = "!prefix! 0.082-p--0.280-p- 0.485?suffix?"; + assertEquals(expected, EuclidCoreIOTools.getStringOf(prefix, suffix, separator, values)); + assertEquals(expected, EuclidCoreIOTools.getStringOf(prefix, suffix, separator, EuclidCoreIOTools.DEFAULT_FORMAT, values)); + + assertEquals("null", EuclidCoreIOTools.getStringOf(prefix, suffix, separator, (double[]) null)); + assertEquals("null", EuclidCoreIOTools.getStringOf(prefix, suffix, separator, EuclidCoreIOTools.DEFAULT_FORMAT, (double[]) null)); + + assertEquals("!prefix!?suffix?", EuclidCoreIOTools.getStringOf(prefix, suffix, separator, new double[0])); + assertEquals("!prefix!?suffix?", EuclidCoreIOTools.getStringOf(prefix, suffix, separator, EuclidCoreIOTools.DEFAULT_FORMAT, new double[0])); + + expected = "!prefix!0.0819722-p--0.2797709-p-0.4845304?suffix?"; + assertEquals(expected, EuclidCoreIOTools.getStringOf(prefix, suffix, separator, randomFormat, values)); + + expected = "!prefix!0.08197223286773692-p--0.279770919902092-p-0.4845303753357886?suffix?"; + assertEquals(expected, EuclidCoreIOTools.getStringOf(prefix, suffix, separator, null, values)); + } - String prefix = "!prefix!"; - String suffix = "?suffix?"; - expected = "!prefix! 0.582-p- 0.220-p- 0.985?suffix?"; - assertEquals(expected, EuclidCoreIOTools.getStringOf(prefix, suffix, separator, values)); - assertEquals(expected, EuclidCoreIOTools.getStringOf(prefix, suffix, separator, EuclidCoreIOTools.DEFAULT_FORMAT, values)); + { // Test generators for float values + float[] values = {random.nextFloat() - 0.5f, random.nextFloat() - 0.5f, random.nextFloat() - 0.5f}; + expected = " 0.014-p- 0.287-p--0.309"; + assertEquals(expected, EuclidCoreIOTools.getStringOf(separator, values)); + assertEquals(expected, EuclidCoreIOTools.getStringOf(separator, EuclidCoreIOTools.DEFAULT_FORMAT, values)); + assertEquals(expected, EuclidCoreIOTools.getStringOf(null, null, separator, values)); + assertEquals(expected, EuclidCoreIOTools.getStringOf(null, null, separator, EuclidCoreIOTools.DEFAULT_FORMAT, values)); + + assertEquals("null", EuclidCoreIOTools.getStringOf(separator, (float[]) null)); + assertEquals("null", EuclidCoreIOTools.getStringOf(separator, EuclidCoreIOTools.DEFAULT_FORMAT, (float[]) null)); + assertEquals("null", EuclidCoreIOTools.getStringOf(null, null, separator, (float[]) null)); + assertEquals("null", EuclidCoreIOTools.getStringOf(null, null, separator, EuclidCoreIOTools.DEFAULT_FORMAT, (float[]) null)); + + assertEquals("", EuclidCoreIOTools.getStringOf(separator, new float[0])); + assertEquals("", EuclidCoreIOTools.getStringOf(separator, EuclidCoreIOTools.DEFAULT_FORMAT, new float[0])); + assertEquals("", EuclidCoreIOTools.getStringOf(null, null, separator, new float[0])); + assertEquals("", EuclidCoreIOTools.getStringOf(null, null, separator, EuclidCoreIOTools.DEFAULT_FORMAT, new float[0])); + + expected = "0.0137517-p-0.2874116-p--0.3094129"; + assertEquals(expected, EuclidCoreIOTools.getStringOf(separator, randomFormat, values)); + assertEquals(expected, EuclidCoreIOTools.getStringOf(null, null, separator, randomFormat, values)); + + expected = "0.013751686-p-0.28741163-p--0.3094129"; + assertEquals(expected, EuclidCoreIOTools.getStringOf(separator, null, values)); + assertEquals(expected, EuclidCoreIOTools.getStringOf(null, null, separator, null, values)); + + String prefix = "!prefix!"; + String suffix = "?suffix?"; + expected = "!prefix! 0.014-p- 0.287-p--0.309?suffix?"; + assertEquals(expected, EuclidCoreIOTools.getStringOf(prefix, suffix, separator, values)); + assertEquals(expected, EuclidCoreIOTools.getStringOf(prefix, suffix, separator, EuclidCoreIOTools.DEFAULT_FORMAT, values)); + + assertEquals("null", EuclidCoreIOTools.getStringOf(prefix, suffix, separator, (float[]) null)); + assertEquals("null", EuclidCoreIOTools.getStringOf(prefix, suffix, separator, EuclidCoreIOTools.DEFAULT_FORMAT, (float[]) null)); + + assertEquals("!prefix!?suffix?", EuclidCoreIOTools.getStringOf(prefix, suffix, separator, new float[0])); + assertEquals("!prefix!?suffix?", EuclidCoreIOTools.getStringOf(prefix, suffix, separator, EuclidCoreIOTools.DEFAULT_FORMAT, new float[0])); + + expected = "!prefix!0.0137517-p-0.2874116-p--0.3094129?suffix?"; + assertEquals(expected, EuclidCoreIOTools.getStringOf(prefix, suffix, separator, randomFormat, values)); + + expected = "!prefix!0.013751686-p-0.28741163-p--0.3094129?suffix?"; + assertEquals(expected, EuclidCoreIOTools.getStringOf(prefix, suffix, separator, null, values)); + } + + { // Test generators for boolean values + boolean[] values = {random.nextBoolean(), random.nextBoolean(), random.nextBoolean(), random.nextBoolean()}; + expected = "true-p-true-p-true-p-false"; + assertEquals(expected, EuclidCoreIOTools.getStringOf(separator, values)); + assertEquals(expected, EuclidCoreIOTools.getStringOf(null, null, separator, values)); + + assertEquals("null", EuclidCoreIOTools.getStringOf(separator, (boolean[]) null)); + assertEquals("null", EuclidCoreIOTools.getStringOf(null, null, separator, (boolean[]) null)); + + assertEquals("", EuclidCoreIOTools.getStringOf(separator, new boolean[0])); + assertEquals("", EuclidCoreIOTools.getStringOf(null, null, separator, new boolean[0])); + + String prefix = "!prefix!"; + String suffix = "?suffix?"; + expected = "!prefix!true-p-true-p-true-p-false?suffix?"; + assertEquals(expected, EuclidCoreIOTools.getStringOf(prefix, suffix, separator, values)); + + assertEquals("null", EuclidCoreIOTools.getStringOf(prefix, suffix, separator, (boolean[]) null)); + + assertEquals("!prefix!?suffix?", EuclidCoreIOTools.getStringOf(prefix, suffix, separator, new boolean[0])); + } - assertEquals("null", EuclidCoreIOTools.getStringOf(prefix, suffix, separator, null)); - assertEquals("null", EuclidCoreIOTools.getStringOf(prefix, suffix, separator, EuclidCoreIOTools.DEFAULT_FORMAT, null)); + { // Test generators for int values + int[] values = {random.nextInt(1000) - 500, random.nextInt(1000) - 500, random.nextInt(1000) - 500, random.nextInt(1000) - 500}; + expected = "325-p--400-p--255-p--494"; + assertEquals(expected, EuclidCoreIOTools.getStringOf(separator, values)); + assertEquals(expected, EuclidCoreIOTools.getStringOf(null, null, separator, values)); - assertEquals("!prefix!?suffix?", EuclidCoreIOTools.getStringOf(prefix, suffix, separator, new double[0])); - assertEquals("!prefix!?suffix?", EuclidCoreIOTools.getStringOf(prefix, suffix, separator, EuclidCoreIOTools.DEFAULT_FORMAT, new double[0])); + assertEquals("null", EuclidCoreIOTools.getStringOf(separator, (int[]) null)); + assertEquals("null", EuclidCoreIOTools.getStringOf(null, null, separator, (int[]) null)); - expected = "!prefix!0.5819722-p-0.2202291-p-0.9845304?suffix?"; - assertEquals(expected, EuclidCoreIOTools.getStringOf(prefix, suffix, separator, randomFormat, values)); + assertEquals("", EuclidCoreIOTools.getStringOf(separator, new int[0])); + assertEquals("", EuclidCoreIOTools.getStringOf(null, null, separator, new int[0])); - expected = "!prefix!0.5819722328677369-p-0.22022908009790798-p-0.9845303753357886?suffix?"; - assertEquals(expected, EuclidCoreIOTools.getStringOf(prefix, suffix, separator, null, values)); + String prefix = "!prefix!"; + String suffix = "?suffix?"; + expected = "!prefix!325-p--400-p--255-p--494?suffix?"; + assertEquals(expected, EuclidCoreIOTools.getStringOf(prefix, suffix, separator, values)); + + assertEquals("null", EuclidCoreIOTools.getStringOf(prefix, suffix, separator, (int[]) null)); + + assertEquals("!prefix!?suffix?", EuclidCoreIOTools.getStringOf(prefix, suffix, separator, new int[0])); + } + + { // Test generators for int values + long[] values = {random.nextInt(1000) - 500, random.nextInt(1000) - 500, random.nextInt(1000) - 500, random.nextInt(1000) - 500}; + expected = "-107-p-104-p-246-p--373"; + assertEquals(expected, EuclidCoreIOTools.getStringOf(separator, values)); + assertEquals(expected, EuclidCoreIOTools.getStringOf(null, null, separator, values)); + + assertEquals("null", EuclidCoreIOTools.getStringOf(separator, (long[]) null)); + assertEquals("null", EuclidCoreIOTools.getStringOf(null, null, separator, (long[]) null)); + + assertEquals("", EuclidCoreIOTools.getStringOf(separator, new long[0])); + assertEquals("", EuclidCoreIOTools.getStringOf(null, null, separator, new long[0])); + + String prefix = "!prefix!"; + String suffix = "?suffix?"; + expected = "!prefix!-107-p-104-p-246-p--373?suffix?"; + assertEquals(expected, EuclidCoreIOTools.getStringOf(prefix, suffix, separator, values)); + + assertEquals("null", EuclidCoreIOTools.getStringOf(prefix, suffix, separator, (long[]) null)); + + assertEquals("!prefix!?suffix?", EuclidCoreIOTools.getStringOf(prefix, suffix, separator, new long[0])); + } } @Test From f86a826cfbcc2a8a9a64fff7aba6010fc0f046d3 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Mon, 14 Mar 2022 17:42:54 -0500 Subject: [PATCH 5/7] IO tools: Added test for null value in array & collections. --- .../euclid/tools/EuclidCoreIOToolsTest.java | 106 ++++++++++++------ 1 file changed, 74 insertions(+), 32 deletions(-) diff --git a/src/test/java/us/ihmc/euclid/tools/EuclidCoreIOToolsTest.java b/src/test/java/us/ihmc/euclid/tools/EuclidCoreIOToolsTest.java index c5e858c89..e33372a7c 100644 --- a/src/test/java/us/ihmc/euclid/tools/EuclidCoreIOToolsTest.java +++ b/src/test/java/us/ihmc/euclid/tools/EuclidCoreIOToolsTest.java @@ -420,21 +420,42 @@ public void testGetArrayString() String separator = "-p-"; TestObject[] array = nextTestObjectArray(random, 3); - expected = "[blop=4, choux=-0.0727027174796302]-p-[blop=-4, choux=0.4845303753357886]-p-[blop=44, choux=0.2874116776971093]"; - assertEquals(expected, EuclidCoreIOTools.getArrayString(separator, array)); - assertEquals(expected, EuclidCoreIOTools.getArrayString(null, null, separator, array)); - assertEquals(expected, EuclidCoreIOTools.getArrayString(separator, array, Object::toString)); - assertEquals(expected, EuclidCoreIOTools.getArrayString(null, null, separator, array, Object::toString)); - - String prefix = "!prefix!"; - String suffix = "?suffix?"; - expected = "!prefix![blop=4, choux=-0.0727027174796302]-p-[blop=-4, choux=0.4845303753357886]-p-[blop=44, choux=0.2874116776971093]?suffix?"; - assertEquals(expected, EuclidCoreIOTools.getArrayString(prefix, suffix, separator, array)); - assertEquals(expected, EuclidCoreIOTools.getArrayString(prefix, suffix, separator, array, Object::toString)); - - expected = "blop=4-p-blop=-4-p-blop=44"; - assertEquals(expected, EuclidCoreIOTools.getArrayString(separator, array, e -> "blop=" + e.blop)); - assertEquals(expected, EuclidCoreIOTools.getArrayString(null, null, separator, array, e -> "blop=" + e.blop)); + { // Testing without any null value + expected = "[blop=4, choux=-0.0727027174796302]-p-[blop=-4, choux=0.4845303753357886]-p-[blop=44, choux=0.2874116776971093]"; + assertEquals(expected, EuclidCoreIOTools.getArrayString(separator, array)); + assertEquals(expected, EuclidCoreIOTools.getArrayString(null, null, separator, array)); + assertEquals(expected, EuclidCoreIOTools.getArrayString(separator, array, Object::toString)); + assertEquals(expected, EuclidCoreIOTools.getArrayString(null, null, separator, array, Object::toString)); + + String prefix = "!prefix!"; + String suffix = "?suffix?"; + expected = "!prefix![blop=4, choux=-0.0727027174796302]-p-[blop=-4, choux=0.4845303753357886]-p-[blop=44, choux=0.2874116776971093]?suffix?"; + assertEquals(expected, EuclidCoreIOTools.getArrayString(prefix, suffix, separator, array)); + assertEquals(expected, EuclidCoreIOTools.getArrayString(prefix, suffix, separator, array, Object::toString)); + + expected = "blop=4-p-blop=-4-p-blop=44"; + assertEquals(expected, EuclidCoreIOTools.getArrayString(separator, array, e -> "blop=" + e.blop)); + assertEquals(expected, EuclidCoreIOTools.getArrayString(null, null, separator, array, e -> "blop=" + e.blop)); + } + + { // Testing with a null value + array[random.nextInt(array.length)] = null; + expected = "[blop=4, choux=-0.0727027174796302]-p-[blop=-4, choux=0.4845303753357886]-p-null"; + assertEquals(expected, EuclidCoreIOTools.getArrayString(separator, array)); + assertEquals(expected, EuclidCoreIOTools.getArrayString(null, null, separator, array)); + assertEquals(expected, EuclidCoreIOTools.getArrayString(separator, array, Object::toString)); + assertEquals(expected, EuclidCoreIOTools.getArrayString(null, null, separator, array, Object::toString)); + + String prefix = "!prefix!"; + String suffix = "?suffix?"; + expected = "!prefix![blop=4, choux=-0.0727027174796302]-p-[blop=-4, choux=0.4845303753357886]-p-null?suffix?"; + assertEquals(expected, EuclidCoreIOTools.getArrayString(prefix, suffix, separator, array)); + assertEquals(expected, EuclidCoreIOTools.getArrayString(prefix, suffix, separator, array, Object::toString)); + + expected = "blop=4-p-blop=-4-p-null"; + assertEquals(expected, EuclidCoreIOTools.getArrayString(separator, array, e -> "blop=" + e.blop)); + assertEquals(expected, EuclidCoreIOTools.getArrayString(null, null, separator, array, e -> "blop=" + e.blop)); + } } @Test @@ -443,23 +464,44 @@ public void testGetCollectionString() Random random = new Random(345345); String expected; String separator = "-p-"; - List array = Arrays.asList(nextTestObjectArray(random, 3)); - - expected = "[blop=4, choux=-0.0727027174796302]-p-[blop=-4, choux=0.4845303753357886]-p-[blop=44, choux=0.2874116776971093]"; - assertEquals(expected, EuclidCoreIOTools.getCollectionString(separator, array)); - assertEquals(expected, EuclidCoreIOTools.getCollectionString(null, null, separator, array)); - assertEquals(expected, EuclidCoreIOTools.getCollectionString(separator, array, Object::toString)); - assertEquals(expected, EuclidCoreIOTools.getCollectionString(null, null, separator, array, Object::toString)); - - String prefix = "!prefix!"; - String suffix = "?suffix?"; - expected = "!prefix![blop=4, choux=-0.0727027174796302]-p-[blop=-4, choux=0.4845303753357886]-p-[blop=44, choux=0.2874116776971093]?suffix?"; - assertEquals(expected, EuclidCoreIOTools.getCollectionString(prefix, suffix, separator, array)); - assertEquals(expected, EuclidCoreIOTools.getCollectionString(prefix, suffix, separator, array, Object::toString)); - - expected = "blop=4-p-blop=-4-p-blop=44"; - assertEquals(expected, EuclidCoreIOTools.getCollectionString(separator, array, e -> "blop=" + e.blop)); - assertEquals(expected, EuclidCoreIOTools.getCollectionString(null, null, separator, array, e -> "blop=" + e.blop)); + List list = Arrays.asList(nextTestObjectArray(random, 3)); + + { // Testing without any null value + expected = "[blop=4, choux=-0.0727027174796302]-p-[blop=-4, choux=0.4845303753357886]-p-[blop=44, choux=0.2874116776971093]"; + assertEquals(expected, EuclidCoreIOTools.getCollectionString(separator, list)); + assertEquals(expected, EuclidCoreIOTools.getCollectionString(null, null, separator, list)); + assertEquals(expected, EuclidCoreIOTools.getCollectionString(separator, list, Object::toString)); + assertEquals(expected, EuclidCoreIOTools.getCollectionString(null, null, separator, list, Object::toString)); + + String prefix = "!prefix!"; + String suffix = "?suffix?"; + expected = "!prefix![blop=4, choux=-0.0727027174796302]-p-[blop=-4, choux=0.4845303753357886]-p-[blop=44, choux=0.2874116776971093]?suffix?"; + assertEquals(expected, EuclidCoreIOTools.getCollectionString(prefix, suffix, separator, list)); + assertEquals(expected, EuclidCoreIOTools.getCollectionString(prefix, suffix, separator, list, Object::toString)); + + expected = "blop=4-p-blop=-4-p-blop=44"; + assertEquals(expected, EuclidCoreIOTools.getCollectionString(separator, list, e -> "blop=" + e.blop)); + assertEquals(expected, EuclidCoreIOTools.getCollectionString(null, null, separator, list, e -> "blop=" + e.blop)); + } + + { // Testing with a null value + list.set(random.nextInt(list.size()), null); + expected = "[blop=4, choux=-0.0727027174796302]-p-[blop=-4, choux=0.4845303753357886]-p-null"; + assertEquals(expected, EuclidCoreIOTools.getCollectionString(separator, list)); + assertEquals(expected, EuclidCoreIOTools.getCollectionString(null, null, separator, list)); + assertEquals(expected, EuclidCoreIOTools.getCollectionString(separator, list, Object::toString)); + assertEquals(expected, EuclidCoreIOTools.getCollectionString(null, null, separator, list, Object::toString)); + + String prefix = "!prefix!"; + String suffix = "?suffix?"; + expected = "!prefix![blop=4, choux=-0.0727027174796302]-p-[blop=-4, choux=0.4845303753357886]-p-null?suffix?"; + assertEquals(expected, EuclidCoreIOTools.getCollectionString(prefix, suffix, separator, list)); + assertEquals(expected, EuclidCoreIOTools.getCollectionString(prefix, suffix, separator, list, Object::toString)); + + expected = "blop=4-p-blop=-4-p-null"; + assertEquals(expected, EuclidCoreIOTools.getCollectionString(separator, list, e -> "blop=" + e.blop)); + assertEquals(expected, EuclidCoreIOTools.getCollectionString(null, null, separator, list, e -> "blop=" + e.blop)); + } } private static TestObject[] nextTestObjectArray(Random random, int length) From 14feac919f218c15d168fe58d99bac75d69d2b1c Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Mon, 14 Mar 2022 17:45:40 -0500 Subject: [PATCH 6/7] Fixed the documentation for AffineTransform, issue #44. --- src/main/java/us/ihmc/euclid/transform/AffineTransform.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/us/ihmc/euclid/transform/AffineTransform.java b/src/main/java/us/ihmc/euclid/transform/AffineTransform.java index 7db046762..459be81f9 100644 --- a/src/main/java/us/ihmc/euclid/transform/AffineTransform.java +++ b/src/main/java/us/ihmc/euclid/transform/AffineTransform.java @@ -164,7 +164,7 @@ public QuaternionReadOnly getRotationView() } /** - * Gets the read-only reference to the linear part of this transform. + * Gets the reference to the linear part of this transform. * * @return the linear part of this transform. */ @@ -175,7 +175,7 @@ public LinearTransform3D getLinearTransform() } /** - * Gets the read-only reference of the translation part of this affine transform. + * Gets the reference of the translation part of this affine transform. * * @return the translation part of this transform. */ From 1ac01361ed34ebc6b16a7c15ea0fbda5ba6bf782 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Mon, 14 Mar 2022 17:47:03 -0500 Subject: [PATCH 7/7] :bookmark: 0.17.2 --- build.gradle.kts | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle.kts b/build.gradle.kts index 203d64b5f..117a6c86e 100644 --- a/build.gradle.kts +++ b/build.gradle.kts @@ -5,7 +5,7 @@ plugins { ihmc { group = "us.ihmc" - version = "0.17.1" + version = "0.17.2" vcsUrl = "https://github.com/ihmcrobotics/euclid" openSource = true