Merge branch 'release/0.17.2'

ihmcrobotics · Mar 14, 2022 · 889c78a · 889c78a
2 parents 30b795e + 1ac0136
commit 889c78a
Show file tree

Hide file tree

Showing 97 changed files with 4,829 additions and 1,439 deletions.
diff --git 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
 

diff --git 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/...me-shape/java/us/ihmc/euclid/referenceFrame/collision/EuclidFrameShapeCollisionTools.java b/...me-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/...ape/java/us/ihmc/euclid/referenceFrame/collision/epa/FrameExpandingPolytopeAlgorithm.java b/...ape/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/...ihmc/euclid/referenceFrame/collision/gjk/FrameGilbertJohnsonKeerthiCollisionDetector.java b/...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);