ihmcrobotics/euclid

Vector math, geometry, reference frame, and shapes 2D & 3D

euclid's Issues

AffineTransform Documentation

AffineTransform getTranslation and getLinearTransform say they return a "read-only" yet they return modifiable objects.

Add EuclidGeometryTools.isPoint2DOnLine2D with an epsilon

All fixed-frame objects have a setMatchingFrame

Static tool method or otherwise to "sanitize"/correct slightly invalid orientations

Packing Quaternions directly with non-Euclid data, can result in NotARotationMatrix exceptions, as shown below. Currently there is a workaround to go through LinearTransform3D, that fixes the issue.

It'd be nice if there was just a method to sanitize these orientations, as LinearTransform3D must do somehow, with a static Tools method or something.

// Going through LinearTransform3D first prevents NotARotationMatrix exceptions.
LinearTransform3D linearTransform3D = new LinearTransform3D(EuclidGeometryTools.axisAngleFromZUpToVector3D(gpuPlanarRegion.getNormal()));
linearTransform3D.normalize();
FrameQuaternion orientation = new FrameQuaternion();
orientation.setIncludingFrame(cameraFrame, linearTransform3D.getAsQuaternion());
orientation.changeFrame(ReferenceFrame.getWorldFrame());

us.ihmc.euclid.exceptions.NotARotationMatrixException: The matrix is not a rotation matrix:
/ 0.305,  0.663,  0.683 \
|-0.220,  0.747, -0.627 |
\-0.927,  0.041,  0.374 /
	at us.ihmc.euclid.matrix.interfaces.Matrix3DReadOnly.checkIfRotationMatrix(Matrix3DReadOnly.java:393)
	at us.ihmc.euclid.matrix.interfaces.RotationMatrixBasics.set(RotationMatrixBasics.java:101)
	at us.ihmc.euclid.tools.RotationMatrixTools.multiplyImpl(RotationMatrixTools.java:511)
	at us.ihmc.euclid.tools.RotationMatrixTools.multiplyImpl(RotationMatrixTools.java:409)
	at us.ihmc.euclid.tools.RotationMatrixTools.multiply(RotationMatrixTools.java:308)
	at us.ihmc.euclid.matrix.interfaces.RotationMatrixBasics.append(RotationMatrixBasics.java:326)
	at us.ihmc.euclid.transform.interfaces.RigidBodyTransformBasics.multiply(RigidBodyTransformBasics.java:673)
	at us.ihmc.euclid.referenceFrame.ReferenceFrame.efficientComputeTransform(ReferenceFrame.java:924)
	at us.ihmc.euclid.referenceFrame.ReferenceFrame.getTransformToRoot(ReferenceFrame.java:862)
	at us.ihmc.euclid.referenceFrame.ReferenceFrame.transformFromThisToDesiredFrame(ReferenceFrame.java:811)
	at us.ihmc.euclid.referenceFrame.interfaces.FrameChangeable.changeFrame(FrameChangeable.java:36)
	at us.ihmc.avatar.gpuPlanarRegions.GPUPlanarRegionExtraction.lambda$computePlanarRegions$6(GPUPlanarRegionExtraction.java:459)

Misleading documentation for the bounding box to bounding intersection methods

Listing of tools classes

Put together a document listing all of the *Tools classes with a short summary for each one

ReferenceFrameTools.clearWorldFrameTree() should reset indices

To make it possible to reset the state of the controller to start, ReferenceFrameTools.clearWorldFrameTree() should set the framesAddedToTree variable in ReferenceFrame worldframe to 0.

Ray 2D & 3D are implemented

EuclidGeometryTools.percentageOfIntersectionBetweenTwoLine2Ds loses information about colinear edge case

For the colinear case, it would be nice if the return value could be identified as such.
Since a return of 0.0 is already a successful return value for non-parallel lines, 0.0 doesn't feel right as a return value.
I suggest Double.POSITIVE_INFINITY since the return value is Double.NaN for the parallel non-intersecting case.

What do you think?

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
   //      / lineDirection1x -lineDirection2x \   / alpha \   / pointOnLine2x - pointOnLine1x \
   //      |                                  | * |       | = |                               |
   //      \ lineDirection1y -lineDirection2y /   \ beta  /   \ pointOnLine2y - pointOnLine1y /
   // Here, only alpha or beta is needed.
   double determinant = -lineDirection1x * lineDirection2y + lineDirection1y * lineDirection2x;
   double dx = pointOnLine2x - pointOnLine1x;
   double dy = pointOnLine2y - pointOnLine1y;
   if (Math.abs(determinant) < EuclidGeometryTools.ONE_TRILLIONTH)
   { // The lines are parallel
      // Check if they are collinear
      double cross = dx * lineDirection1y - dy * lineDirection1x;
      if (Math.abs(cross) < EuclidGeometryTools.ONE_TRILLIONTH)
      {
         /*
          * The two lines are collinear. There's an infinite number of intersection. Let's just set the
          * result to pointOnLine1, i.e. alpha = 0.0.
          */
         return Double.POSITIVE_INFINITY;
      }
      else
      {
         return Double.NaN;
      }
   }
   else
   {
      double oneOverDeterminant = 1.0 / determinant;
      double AInverse00 = oneOverDeterminant * -lineDirection2y;
      double AInverse01 = oneOverDeterminant * lineDirection2x;
      double alpha = AInverse00 * dx + AInverse01 * dy;
      return alpha;
   }
}

Add EuclidGeometryTools.parameterAlongLine2D()

Add common `Matrix3D`s as constants in `EuclidCoreTools`

ReferenceFrame.efficientTransformToRoot is a little more robust to race conditions

Expected method Matrix3D.isZero(...)

FixedFramePoint3DBasics.setMatchingFrame(ReferenceFrame, double, double, double) would be a nice addition

How to get the pose-transform between poses

Hi, I'm looking into euclid and like it - it seems there are not that many robotics-centric spatial-math libs out there for java!

For my use-case, I often have to transform a point from one frame/pose to another frame/pose. For this, I'm used to functions like
poseA.between(poseB) giving me the transform between two poses and pose.inverse() giving me the inverse of the pose's transform - where pose would be in 2D or 3D. How would I do that in euclid?

For some context, I'm coming from the GTSAM way of pose calculus: https://github.com/devbharat/gtsam/blob/master/gtsam/geometry/Pose3.h

Thanks!

`EuclidFrameIOTools` does not reliably pass down format argument.

Example: EuclidFrameIOTools.getFrameTuple3DString(String, FrameTuple3DReadOnly) does not do anything with the given format string.

AxisAngle should implement GeometryObject

Write up summary of general conventions used in Euclid.

Calling FrameConvexPolygon2D.setIncludingFrame() doesn't set frame with empty vertex.

The following case current fails:

There are two random polygons, in random frames, polygonA and polygonB.

polygonA.clear();
polygonB.clear();
polygonB.setIncludingFrame(polygonA);
assert(polygonA.epsilonEquals(polygonB, 1e-5);

This fails because of a reference frame mismatch.

RigidBodyTransform set should accept RigidBodyTransformReadOnly

RigidBodyTranform:259 should be:

public void set(RigidBodyTransformReadOnly other)

Add intersection Line<->Ray, Ray<->Ray, Line segment<->Ray in EuclidGeometryTools

Add a more convenient option to create a reference frame

For when you need a lot of frames and you don't care too much what the name is and you just want to make sure each one is unique. I suggest just putting the calling classname and an index. I've been using a ThreadLocal MutableInt for this, but maybe a global AtomicInteger is better.

import org.apache.commons.lang3.mutable.MutableInt;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools;
import us.ihmc.euclid.transform.RigidBodyTransform;

public static ThreadLocal<MutableInt> INDEX = ThreadLocal.withInitial(MutableInt::new);
public static String getCallingClassName()
{
   StackTraceElement callerStackElement = new Throwable().getStackTrace()[2];
   return callerStackElement.getClassName();
}
public static ReferenceFrame constructFrameWithUnchangingTransformFromParent(ReferenceFrame parentFrame, RigidBodyTransform transformFromParent)
{
   return ReferenceFrameTools.constructFrameWithUnchangingTransformFromParent(getCallingClassName() + INDEX.get(),
                                                                              parentFrame,
                                                                              transformFromParent);
}
public static ReferenceFrame constructFrameWithUnchangingTransformToParent(ReferenceFrame parentFrame, RigidBodyTransform transformToParent)
{
   return ReferenceFrameTools.constructFrameWithUnchangingTransformToParent(getCallingClassName() + INDEX.get(),
                                                                            parentFrame,
                                                                            transformToParent);
}
public static ReferenceFrame constructFrameWithChangingTransformFromParent(ReferenceFrame parentFrame, RigidBodyTransform transformFromParent)
{
   return ReferenceFrameTools.constructFrameWithChangingTransformFromParent(getCallingClassName() + INDEX.get(),
                                                                            parentFrame,
                                                                            transformFromParent);
}
public static ReferenceFrame constructFrameWithChangingTransformToParent(ReferenceFrame parentFrame, RigidBodyTransform transformToParent)
{
   return ReferenceFrameTools.constructFrameWithChangingTransformToParent(getCallingClassName() + INDEX.get(),
                                                                          parentFrame,
                                                                          transformToParent);
}

EuclidFrameTestTools is updated with new frame geometries

Random Test values are not Random

Statically initialized seeds lead to same values in every sequence. E.g:
Quaternion Based TransformTest

Random random = new Random(45L);
QuaternionBasedTransform transform = EuclidCoreRandomTools.nextQuaternionBasedTransform(random);
will always result in:

x = -0.5599634331913013
y = 0.5960433401296669
z = 0.41656366724225924
s = 0.3970491169985922

Recommended solution is initializing with a source of random: System.nanoTime()

Missing method EuclidGeometryTools.distanceBetweenTwoLineSegment2Ds(...)

Method exists for 3D line segments, but not 2D. Similar to overall statement from Issue #18

Extend EuclidGeometryTestTools to include the new geometries

Implement margins for GJK to allow using the algorithm for small penetration

`Box3D` and `FrameBox3D` could use extra constructors with size argument defined as `Tuple3DReadOnly`

Move FrameLineSegment/2d to Euclid

The FrameLineSegment and FrameLineSegment2d classes in ihmc-robotics-toolkit needs to be moved to Euclid. They are math classes, and the dependency tree from SCS to YoVariables necessitates they exist lower down the chain.

tac

Expected setter of the kind `FramePose3D.setIncludingFrame(FrameShape3DPose)`

ReferenceFrame.getTransformToParent(RigidBodyTransform transformToPack) does not work correctly for frames with unchanging transforms

ReferenceFrame.getTransformToParent() returns a copy of transformToParent, while ReferenceFrame.getTransformToParent(RigidBodyTransform transformToPack) calls updateTransformToParent(transformToPack)

ReferenceFrames with unchanging transform to parent have an empty updateTransformToParent and set transformToParent in the constructor. Hence the result does not match ReferenceFrame.getTransformToParent()

ReferenceFrame supports streams.

Add methods for computing distance between a point and a bounding-box

Add EuclidGeometryTools.distanceSquaredFromPoint2DToLineSegment2D(Point2DReadOnly, ...)

Additional operations with `DenseMatrix64F` and tuples such add or sub

Add EuclidGeometryTools.rotate(Tuple2DBasics)

Create a ConvexPolygon2D from a ConvexPolygon2DReadOnly

I am pretty sure an exception is always thrown when trying to create a ConvexPolygon2D from a ConvexPolygon2DReadOnly that has not been updated under the hood. This is problematic since you can't update a ReadOnly one.

Exception in thread "GDXFootstepPlanGraphic-pool-28346-thread-5" us.ihmc.euclid.geometry.exceptions.OutdatedPolygonException: Call the update method before doing any other calculation!
	at us.ihmc.euclid.geometry.interfaces.ConvexPolygon2DReadOnly.checkIfUpToDate(ConvexPolygon2DReadOnly.java:57)
	at us.ihmc.euclid.geometry.interfaces.ConvexPolygon2DReadOnly.getVertex(ConvexPolygon2DReadOnly.java:168)
	at us.ihmc.euclid.geometry.interfaces.ConvexPolygon2DBasics.addVertices(ConvexPolygon2DBasics.java:183)
	at us.ihmc.euclid.geometry.interfaces.ConvexPolygon2DBasics.set(ConvexPolygon2DBasics.java:237)
	at us.ihmc.euclid.geometry.ConvexPolygon2D.<init>(ConvexPolygon2D.java:117)

ConvexPolygon2D updatedFoothold = new ConvexPolygon2D(minimalFootstep.getFoothold());
foothold.set(updatedFoothold);

EuclidFrameTestTools missing assertions for FramePose3D and FramePose2D

FrameConvexPolygon2D (and likely other classes) can't handle out of order vertices

The following test passes:

Point2D pointA = EuclidCoreRandomTools.nextPoint2D(random, 10.0);
Point2D pointB = EuclidCoreRandomTools.nextPoint2D(random, 10.0);

FrameConvexPolygon2D polygon = new FrameConvexPolygon2D();
polygon.addVertex(pointA);
polygon.addVertex(pointA);
polygon.addVertex(pointB);
polygon.addVertex(pointB);

polygon.update();

assertEquals(2, polygon.getNumberOfVertices());

while the following test fails:

Point2D pointA = EuclidCoreRandomTools.nextPoint2D(random, 10.0);
Point2D pointB = EuclidCoreRandomTools.nextPoint2D(random, 10.0);

FrameConvexPolygon2D polygon = new FrameConvexPolygon2D();
polygon.addVertex(pointA);
polygon.addVertex(pointB);
polygon.addVertex(pointA);
polygon.addVertex(pointB);

polygon.update();

assertEquals(2, polygon.getNumberOfVertices());

Something in the giftwrapping of the vertices is not functioning properly.

Extend EuclidGeometryRandomTools to include new geometries

Constructor of Pose3D with position and orientation should be more permissive on argument type

Should be Pose3D(Tuple3DReadOnly, Orientation3DReadOnly) instead of Pose3D(Point3DReadOnly, Orientation3DReadOnly)

A YawPitchRoll enum

Something like this which allows to make (UI stuff especially) abstract over yaw, pitch, and roll. These are often the gizmo handles or how you want to tell the chest to yaw, pitch, roll, etc..

public enum YawPitchRollAxis
{
   YAW(Axis3D.Z), PITCH(Axis3D.Y), ROLL(Axis3D.X);
   private final Axis3D axis3D;
   private final String lowerCasedName;
   private final String pascalCasedName;
   YawPitchRollAxis(Axis3D axis3D)
   {
      this.axis3D = axis3D;
      lowerCasedName = name().toLowerCase();
      pascalCasedName = StringUtils.capitalize(lowerCasedName);
   }
   public YawPitchRoll createYawPitchRoll(double angle)
   {
      YawPitchRoll yawPitchRoll = new YawPitchRoll();
      switch (this)
      {
         case YAW -> yawPitchRoll.setYaw(angle);
         case PITCH -> yawPitchRoll.setPitch(angle);
         case ROLL -> yawPitchRoll.setRoll(angle);
      }
      return yawPitchRoll;
   }
   private double getFromYawPitchRoll(YawPitchRollReadOnly yawPitchRollReadOnly)
   {
      return switch (this)
      {
         case YAW -> yawPitchRollReadOnly.getYaw();
         case PITCH -> yawPitchRollReadOnly.getPitch();
         case ROLL -> yawPitchRollReadOnly.getRoll();
      };
   }
   public Axis3D getAxis3D()
   {
      return axis3D;
   }
   public String getLowerCasedName()
   {
      return lowerCasedName;
   }
   public String getPascalCasedName()
   {
      return pascalCasedName;
   }
}

Move all FrameGeometryObjects (Ramp, Torus, etc.) from ihmc-robotics-toolkit into Euclid.

Not as critical as FrameLine and FramePose, but all FrameGeometryObjects should make their way into Euclid to solve the dependency tree issues.

`intersectionWith` methods for Ray3D on Shapes

There is very often int intersectionWith(Line3DReadOnly line, Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack), but when doing graphics picking, I have a ray and I don't want to collide with things behind the screen.

This just requests that wherever there's an intersectWith line, there's also one for a ray.

EuclidGeometryTools.areVector3DsParallel should probably do a greater or equal comparison instead of strictly greater.

Orientation3DBasics misses geometricallyEquals

There are a lot of methods that don't have their compatable 2D or 3D version.

For example, the method EuclidGeometryTools.closestPoint3DsBetweenTwoLineSegment3Ds exists, but not EuclidGeometryTools.closestPoint2DsBetweenTwoLineSegment2Ds , which I just pattern matched. Same thing with EuclidGeometryTools.distanceBetweenTwoLineSegment3Ds(). I'm doing them in the PlanarRegionTools for a lot of that stuff, but am not really testing it as thoroughly as I should.

RigidBodyTransform implements an interpolation method.

This should be straightforward as RotationMatrix already implement interpolate.

computeConvexPolyong2DArea spelled wrong

us.ihmc.euclid.geometry.tools.EuclidGeometryPolygonTools#computeConvexPolyong2DArea