- * The test is performed by computing the distance between the point and the line, if that distance - * is below {@link EuclidGeometryTools#IS_POINT_ON_LINE_EPS} this method returns {@code true}. - *
- * - * @param lineSegmentStart the first endpoint of the line segment. Not modified. - * @param lineSegmentEnd the second endpoint of the line segment. Not modified. - * @return {@code true} if the query is considered to be lying on the line, {@code false} otherwise. - */ - public static boolean isPoint2DOnLineSegment2D(double pointX, double pointY, Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd) - { - return EuclidGeometryTools.distanceSquaredFromPoint2DToLineSegment2D(pointX, - pointY, - lineSegmentStart.getX(), - lineSegmentStart.getY(), - lineSegmentEnd.getX(), - lineSegmentEnd.getY()) < EuclidGeometryTools.IS_POINT_ON_LINE_EPS; - } - - /** - * Projects the provided {@code rotation} onto {@code axis} such that the original rotation can be - * decomposed into a rotation around {@code axis} and one around an orthogonal axis. - *- * rotation = orthogonalRotation * result - *
- * - * @param rotation is the original rotation to be projected onto {@code axis} - * @param axis is the desired rotation axis of the result. - * @param result will be modified to contain the component of {@code rotation} that is around - * {@code axis} - */ - public static void projectRotationOnAxis(QuaternionReadOnly rotation, Vector3DReadOnly axis, QuaternionBasics result) - { - double dotProduct = rotation.getX() * axis.getX() + rotation.getY() * axis.getY() + rotation.getZ() * axis.getZ(); - - double scale = dotProduct / axis.normSquared(); - double projectedX = scale * axis.getX(); - double projectedY = scale * axis.getY(); - double projectedZ = scale * axis.getZ(); - - result.set(projectedX, projectedY, projectedZ, rotation.getS()); - result.normalize(); - } - - public static boolean intersectionBetweenTwoLine2Ds(Point2DReadOnly firstPointOnLine1, - Point2DReadOnly secondPointOnLine1, - Point2DReadOnly firstPointOnLine2, - Point2DReadOnly secondPointOnLine2, - Point2DBasics intersectionToPack) - { - double pointOnLine1x = firstPointOnLine1.getX(); - double pointOnLine1y = firstPointOnLine1.getY(); - double lineDirection1x = secondPointOnLine1.getX() - firstPointOnLine1.getX(); - double lineDirection1y = secondPointOnLine1.getY() - firstPointOnLine1.getY(); - double pointOnLine2x = firstPointOnLine2.getX(); - double pointOnLine2y = firstPointOnLine2.getY(); - double lineDirection2x = secondPointOnLine2.getX() - firstPointOnLine2.getX(); - double lineDirection2y = secondPointOnLine2.getY() - firstPointOnLine2.getY(); - return EuclidGeometryTools.intersectionBetweenTwoLine2Ds(pointOnLine1x, - pointOnLine1y, - lineDirection1x, - lineDirection1y, - pointOnLine2x, - pointOnLine2y, - lineDirection2x, - lineDirection2y, - intersectionToPack); - } - - /** - * Computes the intersection between two infinitely long 2D lines each defined by a 2D point and a - * 2D direction and returns a percentage {@code alpha} along the first line such that the - * intersection coordinates can be computed as follows:- * Edge cases: - *
- * The result has the following properties: - *
- * The result has the following properties: - *
- * The result has the following properties: - *
- * The result has the following properties: - *
- * The result has the following properties: - *
- * The result has the following properties: - *
- * This method performs the following calculation: x = x - (x.n)n + (y.n)n, where: - *
- * Edge cases: - *
- * {@code matrix}ij ∈ [-1.0; 1.0]. - *
- *- * The approach used here generates a random 3D matrix with values in [-1.0, 1.0], and then performs A * AT which is guaranteed to result in a - * symmetric positive semi-definite matrix. We then add diagonal terms to make the matrix positive definite, and finally scale the matrix by a random double - * that upper bounds the absolute values of the positive definite matrix elements to 1.0. - *
- * - * @param random the random generator to use. - * @return the random positive definite matrix. - */ - public static Matrix3D nextPositiveDefiniteMatrix3D(Random random) - { - return nextPositiveDefiniteMatrix3D(random, 1.0); - } - - /** - * Generates a random positive definite matrix. - *- * {@code matrix}ij ∈ [-minMaxValue, minMaxValue] - *
- *- * The approach used here generates a random 3D matrix with values in [{@code -minMaxValue}, {@code minMaxValue}], and then performs A * AT, - * which is guaranteed to result in a symmetric positive semi-definite matrix. We then add diagonal terms to make the matrix positive definite, and finally - * scale the matrix by a random double that upper bounds the absolute values of the positive definite matrix elements to {@code minMaxValue}. - *
- * - * @param random the random generator to use. - * @param minMaxValue the maximum value for each element. - * @return the random positive definite matrix. - * @throws RuntimeException if {@code minMaxValue < 0}. - */ - public static Matrix3D nextPositiveDefiniteMatrix3D(Random random, double minMaxValue) - { - Matrix3D matrix3D = nextMatrix3D(random, minMaxValue); - matrix3D.multiplyTransposeOther(matrix3D); - - double diagonalDominanceScalar = Math.abs(minMaxValue); - matrix3D.addM00(diagonalDominanceScalar); - matrix3D.addM11(diagonalDominanceScalar); - matrix3D.addM22(diagonalDominanceScalar); - - double scalarToShrinkMatrixWithinBounds = nextDouble(random, 0.0, minMaxValue / matrix3D.maxAbsElement()); - matrix3D.scale(scalarToShrinkMatrixWithinBounds); - return matrix3D; - } - - /** - * Remove when this issue is fixed: - * https://github.com/ihmcrobotics/euclid/issues/57 - */ - private static final Field referenceFrameHasBeenRemoved; - static - { - try - { - referenceFrameHasBeenRemoved = ReferenceFrame.class.getDeclaredField("hasBeenRemoved"); - referenceFrameHasBeenRemoved.setAccessible(true); - } - catch (NoSuchFieldException e) - { - throw new RuntimeException(e); - } - } - - public static boolean hasBeenRemoved(ReferenceFrame referenceFrame) - { - try - { - return referenceFrameHasBeenRemoved.getBoolean(referenceFrame); - } - catch (IllegalAccessException e) - { - throw new RuntimeException(e); - } - } - - /** - * Remove when this issue is fixed: - * https://github.com/ihmcrobotics/euclid/issues/57 - */ - private static final Field referenceFrameName; - static - { - try - { - referenceFrameName = ReferenceFrame.class.getDeclaredField("frameName"); - referenceFrameName.setAccessible(true); - } - catch (NoSuchFieldException e) - { - throw new RuntimeException(e); - } - } - - public static String frameName(ReferenceFrame referenceFrame) - { - try - { - return referenceFrameName.get(referenceFrame).toString(); - } - catch (IllegalAccessException e) - { - throw new RuntimeException(e); - } - } - - public static FrameVector3DReadOnly newLinkedFrameVector3DReadOnly(ReferenceFrameHolder referenceFrameHolder, DMatrixRMaj source) - { - return newLinkedFrameVector3DReadOnly(referenceFrameHolder, 0, source); - } - - public static FrameVector3DReadOnly newLinkedFrameVector3DReadOnly(ReferenceFrameHolder referenceFrameHolder, int startIndex, DMatrixRMaj source) - { - int xIndex = startIndex; - int yIndex = startIndex + 1; - int zIndex = startIndex + 2; - return EuclidFrameFactories.newLinkedFrameVector3DReadOnly(referenceFrameHolder, - () -> source.get(xIndex, 0), - () -> source.get(yIndex, 0), - () -> source.get(zIndex, 0)); - } -} \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/EuclidGeometryMissingTools.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/EuclidGeometryMissingTools.java deleted file mode 100644 index b56e4c0a2..000000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/EuclidGeometryMissingTools.java +++ /dev/null @@ -1,118 +0,0 @@ -package us.ihmc.robotics; - -import us.ihmc.euclid.geometry.BoundingBox2D; -import us.ihmc.euclid.geometry.BoundingBox3D; -import us.ihmc.euclid.geometry.interfaces.BoundingBox2DReadOnly; -import us.ihmc.euclid.geometry.interfaces.BoundingBox3DReadOnly; -import us.ihmc.euclid.geometry.tools.EuclidGeometryTools; -import us.ihmc.euclid.referenceFrame.exceptions.ReferenceFrameMismatchException; -import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; - -public class EuclidGeometryMissingTools -{ - public static double getZOnPlane(Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal, double pointX, double pointY) - { - // The three components of the plane origin - double x0 = pointOnPlane.getX(); - double y0 = pointOnPlane.getY(); - double z0 = pointOnPlane.getZ(); - // The three components of the plane normal - double a = planeNormal.getX(); - double b = planeNormal.getY(); - double c = planeNormal.getZ(); - - // Given the plane equation: a*x + b*y + c*z + d = 0, with d = -(a*x0 + b*y0 + c*z0), we find z: - double z = a / c * (x0 - pointX) + b / c * (y0 - pointY) + z0; - return z; - } - - /** - * Returns the minimum XY distance between a 3D point and an infinitely long 3D line defined by two - * points. - *- * Edge cases: - *
- * WARNING: the 3D arguments are projected onto the XY-plane to perform the actual computation in - * 2D. - *
- * - * @param point the 3D point is projected onto the xy-plane. It's projection is used to - * compute the distance from the line. Not modified. - * @param firstPointOnLine the projection of this 3D onto the xy-plane refers to the first point on - * the 2D line. Not modified. - * @param secondPointOnLine the projection of this 3D onto the xy-plane refers to the second point - * one the 2D line. Not modified. - * @return the minimum distance between the 2D point and the 2D line. - * @throws ReferenceFrameMismatchException if the arguments are not expressed in the same reference - * frame. - */ - public static double distanceXYFromPoint3DToLine3D(FramePoint3DReadOnly point, FramePoint3DReadOnly firstPointOnLine, FramePoint3DReadOnly secondPointOnLine) - { - point.checkReferenceFrameMatch(firstPointOnLine); - point.checkReferenceFrameMatch(secondPointOnLine); - - double pointOnLineX = firstPointOnLine.getX(); - double pointOnLineY = firstPointOnLine.getY(); - double lineDirectionX = secondPointOnLine.getX() - firstPointOnLine.getX(); - double lineDirectionY = secondPointOnLine.getY() - firstPointOnLine.getY(); - return EuclidGeometryTools.distanceFromPoint2DToLine2D(point.getX(), point.getY(), pointOnLineX, pointOnLineY, lineDirectionX, lineDirectionY); - } - - /** - * Finds the intersection of two bounding boxes defined by a bounding box Allocates a new - * BoundingBox2D. TODO: Check, Unit test, move where BoundingBox union is - * - * @param a - * @param b - * @return the intersection bounding box, or null if no intersection - */ - public static BoundingBox2D computeIntersectionOfTwoBoundingBoxes(BoundingBox2DReadOnly a, BoundingBox2DReadOnly b) - { - double maxX = Math.min(a.getMaxX(), b.getMaxX()); - double maxY = Math.min(a.getMaxY(), b.getMaxY()); - double minX = Math.max(a.getMinX(), b.getMinX()); - double minY = Math.max(a.getMinY(), b.getMinY()); - - if ((maxX <= minX) || (maxY <= minY)) - return null; - - return new BoundingBox2D(minX, minY, maxX, maxY); - } - - /** - * Finds the intersection of two bounding boxes defined by a bounding box Allocates a new boundingBox3D. - * - * @param a - * @param b - * @return the intersection bounding box, or null if no intersection - */ - public static BoundingBox3D computeIntersectionOfTwoBoundingBoxes(BoundingBox3DReadOnly a, BoundingBox3DReadOnly b) - { - double maxX = Math.min(a.getMaxX(), b.getMaxX()); - double maxY = Math.min(a.getMaxY(), b.getMaxY()); - double maxZ = Math.min(a.getMaxZ(), b.getMaxZ()); - - double minX = Math.max(a.getMinX(), b.getMinX()); - double minY = Math.max(a.getMinY(), b.getMinY()); - double minZ = Math.max(a.getMinZ(), b.getMinZ()); - - if ((maxX <= minX) || (maxY <= minY) || (maxZ <= minZ)) - return null; - - return new BoundingBox3D(minX, minY, minZ, maxX, maxY, maxZ); - } - - public static double computeBoundingBoxVolume3D(BoundingBox3DReadOnly boundingBox) - { - return Math.abs(boundingBox.getMaxX() - boundingBox.getMinX()) - * Math.abs(boundingBox.getMaxY() - boundingBox.getMinY()) - * Math.abs(boundingBox.getMaxZ() - boundingBox.getMinZ()); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/EuclidGeometryPolygonMissingTools.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/EuclidGeometryPolygonMissingTools.java deleted file mode 100644 index 24908a4c0..000000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/EuclidGeometryPolygonMissingTools.java +++ /dev/null @@ -1,46 +0,0 @@ -package us.ihmc.robotics; - -import us.ihmc.euclid.geometry.tools.EuclidGeometryPolygonTools; -import us.ihmc.euclid.geometry.tools.EuclidGeometryTools; -import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly; - -import java.util.List; - -public class EuclidGeometryPolygonMissingTools -{ - public static boolean doLineSegment2DAndConvexPolygon2DIntersect(Point2DReadOnly lineSegmentStart, - Point2DReadOnly lineSegmentEnd, - List convexPolygon2D, - int numberOfVertices) - { - checkNumberOfVertices(convexPolygon2D, numberOfVertices); - - if (numberOfVertices == 0) - return false; - - if (numberOfVertices == 1) - return EuclidGeometryTools.isPoint2DOnLineSegment2D(convexPolygon2D.get(0), lineSegmentStart, lineSegmentEnd); - - if (numberOfVertices == 2) - return EuclidGeometryTools.doLineSegment2DsIntersect(convexPolygon2D.get(0), convexPolygon2D.get(1), lineSegmentStart, lineSegmentEnd); - - - for (int edgeIndex = 0; edgeIndex < numberOfVertices; edgeIndex++) - { - Point2DReadOnly edgeStart = convexPolygon2D.get(edgeIndex); - Point2DReadOnly edgeEnd = convexPolygon2D.get(EuclidGeometryPolygonTools.next(edgeIndex, numberOfVertices)); - - if (EuclidGeometryTools.doLineSegment2DsIntersect(edgeStart, edgeEnd, lineSegmentStart, lineSegmentEnd)) - return true; - - } - - return false; - } - - private static void checkNumberOfVertices(List convexPolygon2D, int numberOfVertices) - { - if (numberOfVertices < 0 || numberOfVertices > convexPolygon2D.size()) - throw new IllegalArgumentException("Illegal numberOfVertices: " + numberOfVertices + ", expected a value in ] 0, " + convexPolygon2D.size() + "]."); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/geometry/shapes/FramePlane3d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/geometry/shapes/FramePlane3d.java deleted file mode 100644 index 992976848..000000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/geometry/shapes/FramePlane3d.java +++ /dev/null @@ -1,313 +0,0 @@ -package us.ihmc.robotics.geometry.shapes; - -import us.ihmc.euclid.Axis3D; -import us.ihmc.euclid.geometry.Plane3D; -import us.ihmc.euclid.geometry.interfaces.Line3DReadOnly; -import us.ihmc.euclid.referenceFrame.FrameLine3D; -import us.ihmc.euclid.referenceFrame.FramePoint2D; -import us.ihmc.euclid.referenceFrame.FramePoint3D; -import us.ihmc.euclid.referenceFrame.FrameVector3D; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.ReferenceFrameHolder; -import us.ihmc.euclid.transform.RigidBodyTransform; -import us.ihmc.euclid.tuple3D.Point3D; -import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.euclid.tuple3D.interfaces.Point3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; - -public class FramePlane3d implements ReferenceFrameHolder -{ - private ReferenceFrame referenceFrame; - private final Plane3D plane3d; - - private final RigidBodyTransform temporaryTransformToDesiredFrame = new RigidBodyTransform(); - private final Vector3D temporaryVector = new Vector3D(); - private final Point3D temporaryPoint = new Point3D(); - - public FramePlane3d() - { - this(ReferenceFrame.getWorldFrame()); - } - - public FramePlane3d(ReferenceFrame referenceFrame, Plane3D plane3d) - { - this.referenceFrame = referenceFrame; - this.plane3d = plane3d; - } - - public FramePlane3d(ReferenceFrame referenceFrame) - { - this.referenceFrame = referenceFrame; - this.plane3d = new Plane3D(); - } - - public FramePlane3d(FramePlane3d framePlane3d) - { - this.referenceFrame = framePlane3d.referenceFrame; - this.plane3d = new Plane3D(framePlane3d.plane3d); - } - - public FramePlane3d(FrameVector3D normal, FramePoint3D point) - { - normal.checkReferenceFrameMatch(point); - this.referenceFrame = normal.getReferenceFrame(); - this.plane3d = new Plane3D(point, normal); - } - - public FramePlane3d(ReferenceFrame referenceFrame, Point3DReadOnly point, Vector3DReadOnly normal) - { - this.referenceFrame = referenceFrame; - this.plane3d = new Plane3D(point, normal); - } - - @Override - public ReferenceFrame getReferenceFrame() - { - return referenceFrame; - } - - public void getNormal(FrameVector3D normalToPack) - { - checkReferenceFrameMatch(normalToPack.getReferenceFrame()); - temporaryVector.set(this.plane3d.getNormal()); - normalToPack.set(temporaryVector); - } - - public FrameVector3D getNormalCopy() - { - FrameVector3D returnVector = new FrameVector3D(this.referenceFrame); - getNormal(returnVector); - return returnVector; - } - - public Vector3DBasics getNormal() - { - return plane3d.getNormal(); - } - - public void setNormal(double x, double y, double z) - { - plane3d.getNormal().set(x, y, z); - } - - public void setNormal(Vector3DReadOnly normal) - { - plane3d.getNormal().set(normal); - } - - public void getPoint(FramePoint3D pointToPack) - { - checkReferenceFrameMatch(pointToPack.getReferenceFrame()); - temporaryPoint.set(this.plane3d.getPoint()); - pointToPack.set(temporaryPoint); - } - - public FramePoint3D getPointCopy() - { - FramePoint3D pointToReturn = new FramePoint3D(this.getReferenceFrame()); - this.getPoint(pointToReturn); - return pointToReturn; - } - - public Point3DBasics getPoint() - { - return plane3d.getPoint(); - } - - public void setPoint(double x, double y, double z) - { - plane3d.getPoint().set(x, y, z); - } - - public void setPoint(Point3DReadOnly point) - { - plane3d.getPoint().set(point); - } - - public void setPoints(FramePoint3D pointA, FramePoint3D pointB, FramePoint3D pointC) - { - pointA.checkReferenceFrameMatch(referenceFrame); - pointB.checkReferenceFrameMatch(referenceFrame); - pointC.checkReferenceFrameMatch(referenceFrame); - - plane3d.set(pointA, pointB, pointC); - } - - public void changeFrame(ReferenceFrame desiredFrame) - { - if (desiredFrame != referenceFrame) - { - referenceFrame.getTransformToDesiredFrame(temporaryTransformToDesiredFrame, desiredFrame); - plane3d.applyTransform(temporaryTransformToDesiredFrame); - referenceFrame = desiredFrame; - } - - // otherwise: in the right frame already, so do nothing - } - - public boolean isOnOrAbove(FramePoint3D pointToTest) - { - checkReferenceFrameMatch(pointToTest); - - return plane3d.isOnOrAbove(pointToTest); - } - - public boolean isOnOrAbove(Point3DReadOnly pointToTest) - { - return plane3d.isOnOrAbove(pointToTest); - } - - public boolean isOnOrAbove(Point3DReadOnly pointToTest, double epsilon) - { - return plane3d.isOnOrAbove(pointToTest, epsilon); - } - - public boolean isOnOrBelow(FramePoint3D pointToTest) - { - checkReferenceFrameMatch(pointToTest); - - return plane3d.isOnOrBelow(pointToTest); - } - - /** - * Tests if the two planes are parallel by testing if their normals are collinear. - * The latter is done given a tolerance on the angle between the two normal axes in the range ]0; pi/2[. - * - *- * Edge cases: - *
- * Edge cases: - *
+ * In addition to representing a {@link Plane3DBasics}, a {@link ReferenceFrame} is associated to a + * {@link FramePlane3D}. This allows, for instance, to enforce, at runtime, that operations on planes + * occur in the same coordinate system. Also, via the method {@link #changeFrame(ReferenceFrame)}, + * one can easily calculates the value of a point in different reference frames. + *
+ *+ * Because a {@link FramePlane3D} extends {@link Plane3DBasics}, it is compatible with methods only + * requiring {@link Plane3DBasics}. However, these methods do NOT assert that the operation occur in + * the proper coordinate system. Use this feature carefully and always prefer using methods + * requiring {@link FramePlane3D}. + *
+ */ +public class FramePlane3D implements FramePlane3DBasics, Settable+ * Edge cases: + *
+ * Edge cases: + *
+ * Edge cases: + *
+ * Above is defined as the side of the plane toward which the normal is pointing. + *
+ * + * @param pointToTest the coordinates of the query. Not modified. + * @return {@code true} if the query is strictly on or above this plane, {@code false} otherwise. + */ + default boolean isOnOrAbove(FramePoint3DReadOnly pointToTest) + { + return isOnOrAbove(pointToTest, 0.0); + } + + /** + * Tests if the query point is located on or above this plane given the tolerance {@code epsilon}. + *+ * Above is defined as the side of the plane toward which the normal is pointing. + *
+ *+ *
+ * Below is defined as the side of the plane which the normal is pointing away from. + *
+ * + * @param pointToTest the coordinates of the query. Not modified. + * @return {@code true} if the query is strictly on or below this plane, {@code false} otherwise. + */ + default boolean isOnOrBelow(FramePoint3DReadOnly pointToTest) + { + return isOnOrBelow(pointToTest, 0.0); + } + + /** + * Tests if the query point is located on or below this plane given the tolerance {@code epsilon}. + *+ * Below is defined as the side of the plane which the normal is pointing away from. + *
+ *+ *
+ * Edge cases: + *
+ * This method will return {@code false} if the two planes are physically the same but either the + * point or vector of each plane is different. For instance, if {@code this.point == other.point} + * and {@code this.normal == - other.normal}, the two planes are physically the same but this method + * returns {@code false}. + *
+ */ + @Override + default boolean epsilonEquals(EuclidFrameGeometry geometry, double epsilon) + { + Plane3DReadOnly.super.epsilonEquals(geometry, epsilon); + if (geometry == this) + return true; + if (geometry == null) + return false; + if (!(geometry instanceof FramePlane3DReadOnly other)) + return false; + if (getReferenceFrame() != other.getReferenceFrame()) + return false; + return other.getNormal().epsilonEquals(getNormal(), epsilon) && other.getPoint().epsilonEquals(getPoint(), epsilon); + } + + /** {@inheritDoc} */ + @Override + default boolean equals(EuclidGeometry geometry) + { + if (geometry == this) + return true; + if (geometry == null) + return false; + if (!(geometry instanceof FramePlane3DReadOnly other)) + return false; + if (getReferenceFrame() != other.getReferenceFrame()) + return false; + return getPoint().equals(other.getPoint()) && getNormal().equals(other.getNormal()); + } + + /** {@inheritDoc} */ + @Override + default boolean geometricallyEquals(EuclidGeometry geometry, double epsilon) + { + if (geometry == this) + return true; + if (geometry == null) + return false; + if (!(geometry instanceof FramePlane3DReadOnly other)) + return false; + if (getReferenceFrame() != other.getReferenceFrame()) + return false; + return isCoincident(other, epsilon, epsilon); + } + +} diff --git a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameShapeTools.java b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameShapeTools.java index 27c60a2e6..eefdf0c27 100644 --- a/src/frame-shape/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameShapeTools.java +++ b/src/frame-shape/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameShapeTools.java @@ -1,16 +1,15 @@ package us.ihmc.euclid.referenceFrame.tools; import us.ihmc.euclid.geometry.interfaces.BoundingBox3DBasics; +import us.ihmc.euclid.geometry.interfaces.Line3DBasics; +import us.ihmc.euclid.geometry.interfaces.Plane3DReadOnly; +import us.ihmc.euclid.geometry.tools.EuclidGeometryTools; import us.ihmc.euclid.matrix.interfaces.RotationMatrixBasics; import us.ihmc.euclid.matrix.interfaces.RotationMatrixReadOnly; +import us.ihmc.euclid.referenceFrame.FrameVector3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FrameBox3DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FrameCapsule3DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FrameCylinder3DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FrameEllipsoid3DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FrameRamp3DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FrameSphere3DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FrameTorus3DReadOnly; +import us.ihmc.euclid.referenceFrame.exceptions.ReferenceFrameMismatchException; +import us.ihmc.euclid.referenceFrame.interfaces.*; import us.ihmc.euclid.referenceFrame.polytope.interfaces.FrameConvexPolytope3DReadOnly; import us.ihmc.euclid.shape.convexPolytope.interfaces.ConvexPolytope3DReadOnly; import us.ihmc.euclid.shape.convexPolytope.interfaces.Vertex3DReadOnly; @@ -47,6 +46,273 @@ private EuclidFrameShapeTools() // Suppresses default constructor, ensuring non-instantiability. } + /** + * Returns the minimum XY distance between a 3D point and an infinitely long 3D line defined by two + * points. + *+ * Edge cases: + *
+ * WARNING: the 3D arguments are projected onto the XY-plane to perform the actual computation in + * 2D. + *
+ * + * @param point the 3D point is projected onto the xy-plane. It's projection is used to + * compute the distance from the line. Not modified. + * @param firstPointOnLine the projection of this 3D onto the xy-plane refers to the first point on + * the 2D line. Not modified. + * @param secondPointOnLine the projection of this 3D onto the xy-plane refers to the second point + * one the 2D line. Not modified. + * @return the minimum distance between the 2D point and the 2D line. + * @throws ReferenceFrameMismatchException if the arguments are not expressed in the same reference + * frame. + */ + public static double distanceXYFromPoint3DToLine3D(FramePoint3DReadOnly point, FramePoint3DReadOnly firstPointOnLine, FramePoint3DReadOnly secondPointOnLine) + { + point.checkReferenceFrameMatch(firstPointOnLine); + point.checkReferenceFrameMatch(secondPointOnLine); + + double pointOnLineX = firstPointOnLine.getX(); + double pointOnLineY = firstPointOnLine.getY(); + double lineDirectionX = secondPointOnLine.getX() - firstPointOnLine.getX(); + double lineDirectionY = secondPointOnLine.getY() - firstPointOnLine.getY(); + return EuclidGeometryTools.distanceFromPoint2DToLine2D(point.getX(), point.getY(), pointOnLineX, pointOnLineY, lineDirectionX, lineDirectionY); + } + + /** + * Test if a given line segment intersects a given plane. + *+ * Edge cases: + *
+ * Edge cases: + *
+ * WARNING: This method generates garbage. + *
+ * + * @param firstPointOnPlane first point on the plane. Not modified. + * @param secondPointOnPlane second point on the plane. Not modified. + * @param thirdPointOnPlane third point on the plane. Not modified. + * @return the plane normal or {@code null} when the normal could not be determined. + * @throws ReferenceFrameMismatchException if the arguments are not expressed in the same reference + * frame. + */ + public static FrameVector3D getPlaneNormalGivenThreePoints(FramePoint3DReadOnly firstPointOnPlane, + FramePoint3DReadOnly secondPointOnPlane, + FramePoint3DReadOnly thirdPointOnPlane) + { + FrameVector3D normal = new FrameVector3D(); + boolean success = getPlaneNormalGivenThreePoints(firstPointOnPlane, secondPointOnPlane, thirdPointOnPlane, normal); + if (!success) + return null; + else + return normal; + } + + /** + * Computes the normal of a plane that is defined by three points. + *+ * Edge cases: + *
+ * Edge cases: + *
+ * WARNING: This method generates garbage. + *
+ * + * @param point the 3D point towards which the perpendicular vector should be + * pointing at. Not modified. + * @param firstPointOnLine a first point on the line. Not modified. + * @param secondPointOnLine a second point on the line. Not modified. + * @param orthogonalProjectionToPack a 3D point in which the projection of {@code point} onto the + * line is stored. Modified. Can be {@code null}. + * @return the vector perpendicular to the line and pointing to the {@code point}, or {@code null} + * when the method fails. + * @throws ReferenceFrameMismatchException if the arguments are not expressed in the same reference + * frame, except for {@code orthogonalProjectionToPack}. + */ + public static FrameVector3D getPerpendicularVectorFromLineToPoint(FramePoint3DReadOnly point, + FramePoint3DReadOnly firstPointOnLine, + FramePoint3DReadOnly secondPointOnLine, + FramePoint3DBasics orthogonalProjectionToPack) + { + FrameVector3D perpendicularVector = new FrameVector3D(); + + boolean success = getPerpendicularVectorFromLineToPoint(point, firstPointOnLine, secondPointOnLine, orthogonalProjectionToPack, perpendicularVector); + if (!success) + return null; + else + return perpendicularVector; + } + + /** + * Computes the perpendicular defined by an infinitely long 3D line (defined by two 3D points) and a + * 3D point. To do so, the orthogonal projection of the {@code point} on line is first computed. The + * perpendicular vector is computed as follows: + * {@code perpendicularVector = point - orthogonalProjection}, resulting in a vector going from the + * computed projection to the given {@code point} with a length equal to the distance between the + * point and the line. + *+ * Edge cases: + *
+ * Creates a new pose 2D reference frame as a child of the given {@param parentFrame} and initializes the transform to its parent frame. + *
+ *+ * This new reference frame is defined in the {@param parentFrame} and moves with it. + *
+ *+ * Its pose with respect to the parent frame can be modified at runtime by changing the transform in the methods outlined in this class, including setting + * the pose {@link #setPoseAndUpdate(FramePose2DReadOnly)}, setting the pose from a reference frame {@link #setPoseAndUpdate(Pose2DReferenceFrame)}, setting + * the translation {@link #setPositionAndUpdate(FramePoint2DReadOnly)}, or setting the orientation + * {@link #setOrientationAndUpdate(FrameOrientation2DReadOnly)}. + *
+ * + * @param frameName the name of the new frame. + * @param parentFrame the parent of the frame. It has to extend a {@link ReferenceFrame}. + */ + public Pose2DReferenceFrame(String frameName, ReferenceFrame parentFrame) + { + super(frameName, parentFrame); + + originPose = new FramePose2D(parentFrame); + } + + public Pose2DReferenceFrame(String frameName, FramePose2DReadOnly poseInParentFrame) + { + this(frameName, poseInParentFrame.getReferenceFrame()); + setPoseAndUpdate(poseInParentFrame); + } + + /** {@inheritDoc} */ + @Override + protected void updateTransformToParent(RigidBodyTransform transformToParent) + { + originPose.get(transformToParent); + } + + /** + * Sets the pose of this reference frame relative to its parent. The frame of the passed in tuple {@param pose} must be defined as + * {@link #getParent()}. If it is not, this method will throw a {@link ReferenceFrameMismatchException}. To avoid performing a frame check, please use + * {@link #setPoseAndUpdate(Pose2DReadOnly)}. + * + * @param pose pose of this frame relative to parent to set. + */ + public void setPoseAndUpdate(FramePose2DReadOnly pose) + { + pose.checkReferenceFrameMatch(getParent()); + setPoseAndUpdate((Pose2DReadOnly) pose); + } + + /** + * Sets the position of this reference frame relative to its parent. The frame of the passed in tuple {@param position} must be defined + * as {@link #getParent()}. If it is not, this method will throw a {@link ReferenceFrameMismatchException}. To avoid performing a frame check, please use + * {@link #setPositionAndUpdate(Point2DReadOnly)}. + * + * @param position position of this frame relative to parent to set. + */ + public void setPositionAndUpdate(FramePoint2DReadOnly position) + { + position.checkReferenceFrameMatch(getParent()); + setPositionAndUpdate((Point2DReadOnly) position); + } + + /** + * Sets the orientation of this reference frame relative to its parent. The frame of the passed in tuple {@param orientation} must be defined + * as {@link #getParent()}. If it is not, this method will throw a {@link ReferenceFrameMismatchException}. To avoid performing a frame check, please use + * {@link #setOrientationAndUpdate(Orientation2DReadOnly)}. + * + * @param orientation orientation of this frame relative to parent to set. + */ + public void setOrientationAndUpdate(FrameOrientation2DReadOnly orientation) + { + orientation.checkReferenceFrameMatch(getParent()); + setOrientationAndUpdate((Orientation2DReadOnly) orientation); + } + + /** + * Sets the pose of this reference frame relative to its parent. The frame of the passed in tuples {@param position} and {@param orientation} must be defined + * as {@link #getParent()}. If it is not, this method will throw a {@link ReferenceFrameMismatchException}. To avoid performing a frame check, please use + * {@link #setPoseAndUpdate(Pose2DReadOnly)} or {@link #setPoseAndUpdate(Point2DReadOnly, Orientation2DReadOnly)}. + * + * @param position position of this frame relative to parent to set. + * @param orientation orientation of this frame relative to parent to set. + */ + public void setPoseAndUpdate(FramePoint2DReadOnly position, FrameOrientation2DReadOnly orientation) + { + position.checkReferenceFrameMatch(getParent()); + orientation.checkReferenceFrameMatch(getParent()); + setPoseAndUpdate((Point2DReadOnly) position, (Orientation2DReadOnly) orientation); + } + + /** + * Sets the pose of this reference frame relative to its parent. The frame of the passed in {@param pose} must be defined as + * {@link #getParent()}. This method does not perform a frame check. For frame safe operations, see {@link #setPoseAndUpdate(FramePose2DReadOnly)}. + * + * @param pose pose of this frame relative to parent to set. + */ + public void setPoseAndUpdate(Pose2DReadOnly pose) + { + setPoseAndUpdate(pose.getPosition(), pose.getOrientation()); + } + + /** + * Sets the pose of this reference frame relative to its parent. The frame of the passed in {@param position} and {@param orientation} must be defined as + * {@link #getParent()}. This method does not perform a frame check. For frame safe operations, see + * {@link #setPoseAndUpdate(FramePoint2DReadOnly, FrameOrientation2DReadOnly)}. + * + * @param position position of this frame relative to parent to set. + * @param orientation orientation of this frame relative to parent to set. + */ + public void setPoseAndUpdate(Point2DReadOnly position, Orientation2DReadOnly orientation) + { + setPoseAndUpdate(position, orientation.getYaw()); + } + + /** + * Sets the pose of this reference frame relative to its parent. The frame of the passed in {@param pose} must be defined as + * {@link #getParent()}. This method does not perform a frame check. For frame safe operations, see + * {@link #setPoseAndUpdate(FramePoint2DReadOnly, FrameOrientation2DReadOnly)}. + * + * @param position position of this frame relative to parent to set. + * @param orientation orientation of this frame relative to parent to set. + */ + public void setPoseAndUpdate(Point2DReadOnly position, double orientation) + { + originPose.getPosition().set(position); + originPose.getOrientation().setYaw(orientation); + this.update(); + } + + /** + * Sets the position of this reference frame relative to its parent. This does not perform a frame check. For frame safe operations, please use + * {@link #setPositionAndUpdate(FramePoint2DReadOnly)}. + * + * @param position position of this frame relative to parent to set. + */ + public void setPositionAndUpdate(Point2DReadOnly position) + { + originPose.getPosition().set(position); + this.update(); + } + + /** + * Sets the orientation of this reference frame relative to its parent. This does not perform a frame check. For frame safe operations, please use + * {@link #setOrientationAndUpdate(FrameOrientation2DReadOnly)}. + * + * @param orientation orientation of this frame relative to parent to set. + */ + public void setOrientationAndUpdate(Orientation2DReadOnly orientation) + { + originPose.getOrientation().set(orientation); + this.update(); + } + + /** + * {@inheritDoc} + */ + @Override + public String toString() + { + return super.toString() + ", originPose = " + originPose; + } +} diff --git a/src/frame/java/us/ihmc/euclid/referenceFrame/PoseReferenceFrame.java b/src/frame/java/us/ihmc/euclid/referenceFrame/PoseReferenceFrame.java new file mode 100644 index 000000000..4019432b3 --- /dev/null +++ b/src/frame/java/us/ihmc/euclid/referenceFrame/PoseReferenceFrame.java @@ -0,0 +1,335 @@ +package us.ihmc.euclid.referenceFrame; + +import us.ihmc.euclid.geometry.interfaces.Pose3DReadOnly; +import us.ihmc.euclid.orientation.interfaces.Orientation3DReadOnly; +import us.ihmc.euclid.referenceFrame.exceptions.ReferenceFrameMismatchException; +import us.ihmc.euclid.referenceFrame.interfaces.FrameOrientation3DReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; +import us.ihmc.euclid.transform.RigidBodyTransform; +import us.ihmc.euclid.transform.interfaces.RigidBodyTransformReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; +import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; + +/** + * This defines a mutable {@link ReferenceFrame}, where the pose of this frame relative to the parent {@link ReferenceFrame}, which is accessible as + * {@link #getParent()} can be updated via a variety of methods. + */ +public class PoseReferenceFrame extends ReferenceFrame +{ + /** + * The pose of this frame relative to its parent. + */ + private final FramePose3D originPose; + + /** + *+ * Creates a new pose reference frame as a child of the given {@param parentFrame} and initializes the transform to its parent frame. + *
+ *+ * This new reference frame is defined in the {@param parentFrame} and moves with it. + *
+ *+ * Its pose with respect to the parent frame can be modified at runtime by changing the transform in the methods outlined in this class, including setting + * the pose {@link #setPoseAndUpdate(FramePose3DReadOnly)}, setting the pose from a reference frame {@link #setPoseAndUpdate(PoseReferenceFrame)}, setting + * the translation {@link #setPositionAndUpdate(FramePoint3DReadOnly)}, setting the orientation {@link #setOrientationAndUpdate(FrameOrientation3DReadOnly)}, + * or setting the transform {@link #setTransformAndUpdate(RigidBodyTransformReadOnly)} + *
+ * + * @param frameName the name of the new frame. + * @param parentFrame the parent of the frame. It has to extend a {@link ReferenceFrame}. + */ + public PoseReferenceFrame(String frameName, ReferenceFrame parentFrame) + { + super(frameName, parentFrame, parentFrame.isAStationaryFrame(), false); + + originPose = new FramePose3D(parentFrame); + } + + /** + *+ * Creates a new pose reference frame with a defined pose relative to the parent frame, both of which are defined by {@param poseInParentFrame}. + *
+ *+ * Its pose with respect to the parent frame can be modified at runtime by changing the transform in the methods outlined in this class, including setting + * the pose {@link #setPoseAndUpdate(FramePose3DReadOnly)}, setting the pose from a reference frame {@link #setPoseAndUpdate(PoseReferenceFrame)}, setting + * the translation {@link #setPositionAndUpdate(FramePoint3DReadOnly)}, setting the orientation {@link #setOrientationAndUpdate(FrameOrientation3DReadOnly)}, + * or setting the transform {@link #setTransformAndUpdate(RigidBodyTransformReadOnly)} + *
+ * + * @param frameName the name of the new frame. + * @param poseInParentFrame pose of the frame, with the parent frame provided by {@param poseInParentFrame.getParent()}. + */ + public PoseReferenceFrame(String frameName, FramePose3DReadOnly poseInParentFrame) + { + this(frameName, poseInParentFrame.getReferenceFrame()); + setPoseAndUpdate(poseInParentFrame); + } + + /** + *+ * Creates a new pose reference frame as a child of the given {@param parentFrame} and initializes the transform to its parent frame. + *
+ *+ * This new reference frame is defined with a transform relative to the parent {@param parentFrame} as transform {@param transformToParent}. + *
+ *+ * Its pose with respect to the parent frame can be modified at runtime by changing the transform in the methods outlined in this class, including setting + * the pose {@link #setPoseAndUpdate(FramePose3DReadOnly)}, setting the pose from a reference frame {@link #setPoseAndUpdate(PoseReferenceFrame)}, setting + * the translation {@link #setPositionAndUpdate(FramePoint3DReadOnly)}, setting the orientation {@link #setOrientationAndUpdate(FrameOrientation3DReadOnly)}, + * or setting the transform {@link #setTransformAndUpdate(RigidBodyTransformReadOnly)} + *
+ * + * @param frameName the name of the new frame. + * @param parentFrame the parent of the frame. It has to extend a {@link ReferenceFrame}. + */ + public PoseReferenceFrame(String frameName, ReferenceFrame parentFrame, RigidBodyTransformReadOnly transformToParent) + { + this(frameName, parentFrame); + setTransformAndUpdate(transformToParent); + } + + /** + * Sets the pose of this reference frame relative to its parent to match that of {@param poseReferenceFrame}. The parent frame of the passed in frame + * {@param poseReferenceFrame} must match this frame's. If it does not, this method will throw a {@link ReferenceFrameMismatchException}. + * + * @param poseReferenceFrame frame containing the pose to copy. + */ + public void setPoseAndUpdate(PoseReferenceFrame poseReferenceFrame) + { + getParent().checkReferenceFrameMatch(poseReferenceFrame.getParent()); + setPoseAndUpdate(poseReferenceFrame.originPose); + } + + /** + * Sets the pose of this reference frame relative to its parent. The frame of the passed in tuple {@param pose} must be defined as + * {@link #getParent()}. If it is not, this method will throw a {@link ReferenceFrameMismatchException}. To avoid performing a frame check, please use + * {@link #setPoseAndUpdate(Pose3DReadOnly)} + * + * @param pose pose of this frame relative to parent to set. + */ + public void setPoseAndUpdate(FramePose3DReadOnly pose) + { + getParent().checkReferenceFrameMatch(pose); + setPoseAndUpdate((Pose3DReadOnly) pose); + } + + /** + * Sets the pose of this reference frame relative to its parent. The frame of the passed in tuples {@param position} and {@param orientation} must be defined + * as {@link #getParent()}. If it is not, this method will throw a {@link ReferenceFrameMismatchException}. To avoid performing a frame check, please use + * {@link #setPoseAndUpdate(Pose3DReadOnly)} or {@link #setPoseAndUpdate(Point3DReadOnly, Orientation3DReadOnly)}. + * + * @param position position of this frame relative to parent to set. + * @param orientation orientation of this frame relative to parent to set. + */ + public void setPoseAndUpdate(FramePoint3DReadOnly position, FrameOrientation3DReadOnly orientation) + { + position.checkReferenceFrameMatch(getParent()); + orientation.checkReferenceFrameMatch(getParent()); + setPoseAndUpdate((Point3DReadOnly) position, (Orientation3DReadOnly) orientation); + } + + /** + * Sets the position of this reference frame relative to its parent. The frame of the passed in tuple {@param position} must be defined + * as {@link #getParent()}. If it is not, this method will throw a {@link ReferenceFrameMismatchException}. To avoid performing a frame check, please use + * {@link #setPositionAndUpdate(Point3DReadOnly)}. + * + * @param position position of this frame relative to parent to set. + */ + public void setPositionAndUpdate(FramePoint3DReadOnly position) + { + position.checkReferenceFrameMatch(getParent()); + setPositionAndUpdate((Point3DReadOnly) position); + } + + /** + * Sets the orientation of this reference frame relative to its parent. The frame of the passed in tuple {@param orientation} must be defined + * as {@link #getParent()}. If it is not, this method will throw a {@link ReferenceFrameMismatchException}. To avoid performing a frame check, please use + * {@link #setOrientationAndUpdate(Orientation3DReadOnly)}. + * + * @param orientation orientation of this frame relative to parent to set. + */ + public void setOrientationAndUpdate(FrameOrientation3DReadOnly orientation) + { + orientation.checkReferenceFrameMatch(getParent()); + setOrientationAndUpdate((Orientation3DReadOnly) orientation); + } + + /** + * Sets the pose of this reference frame relative to its parent. The frame of the passed in {@param pose} must be defined as + * {@link #getParent()}. This method does not perform a frame check. For frame safe operations, see {@link #setPoseAndUpdate(FramePose3DReadOnly)}. + * + * @param pose pose of this frame relative to parent to set. + */ + public void setPoseAndUpdate(Pose3DReadOnly pose) + { + setPoseAndUpdate(pose.getPosition(), pose.getOrientation()); + } + + /** + * Sets the pose of this reference frame relative to its parent. The frame of the passed in {@param position} and {@param orientation} must be defined as + * {@link #getParent()}. This method does not perform a frame check. For frame safe operations, see + * {@link #setPoseAndUpdate(FramePoint3DReadOnly, FrameOrientation3DReadOnly)}. + * + * @param position position of this frame relative to parent to set. + * @param orientation orientation of this frame relative to parent to set. + */ + public void setPoseAndUpdate(Point3DReadOnly position, Orientation3DReadOnly orientation) + { + originPose.set(position, orientation); + this.update(); + } + + /** + * Sets the transform of this reference frame relative to its parent to be {@param transform}. + * + * @param transformToParent transform relative to parent to set. + */ + public void setTransformAndUpdate(RigidBodyTransformReadOnly transformToParent) + { + originPose.set(transformToParent); + this.update(); + } + + /** + * Sets the position of this reference frame relative to its parent. This does not perform a frame check. For frame safe operations, please use + * {@link #setPositionAndUpdate(FramePoint3DReadOnly)}. + * + * @param position position of this frame relative to parent to set. + */ + public void setPositionAndUpdate(Point3DReadOnly position) + { + setPositionAndUpdate(position.getX(), position.getY(), position.getZ()); + } + + /** + * Sets the orientation of this reference frame relative to its parent. This does not perform a frame check. For frame safe operations, please use + * {@link #setOrientationAndUpdate(FrameOrientation3DReadOnly)}. + * + * @param orientation orientation of this frame relative to parent to set. + */ + public void setOrientationAndUpdate(Orientation3DReadOnly orientation) + { + originPose.getOrientation().set(orientation); + this.update(); + } + + /** + * Sets the position of this reference frame relative to its parent to the corresponding x, y, and z locations. + * + * @param x x position of this frame relative to parent to set. + * @param y y position of this frame relative to parent to set. + * @param z z position of this frame relative to parent to set. + */ + public void setPositionAndUpdate(double x, double y, double z) + { + originPose.getPosition().set(x, y, z); + this.update(); + } + + /** + * Sets the quaternion coordinations of this reference frame relative to its parent to the corresponding qx, qy, qz, and qs locations. + * + * @param qx quaternion value qx of this frame relative to parent to set. + * @param qy quaternion value qy of this frame relative to parent to set. + * @param qz quaternion value qz of this frame relative to parent to set. + * @param qs quaternion value qs of this frame relative to parent to set. + */ + public void setOrientationAndUpdate(double qx, double qy, double qz, double qs) + { + originPose.getOrientation().set(qx, qy, qz, qs); + update(); + } + + /** + * Prepends a translation to this reference frame, relative to its parent, of a magnitude of x, y, and z. + * + * @param x x translation to prepend relative to the parent. + * @param y y translation to prepend relative to the parent. + * @param z z translation to prepend relative to the parent. + */ + public void prependTranslationAndUpdate(double x, double y, double z) + { + originPose.prependTranslation(x, y, z); + this.update(); + } + + /** + * Returns the x translation of this frame relative to the parent frame. + * + * @return x translation + */ + public double getTranslationX() + { + return getPose().getX(); + } + + /** + * Returns the y translation of this frame relative to the parent frame. + * + * @return y translation + */ + public double getTranslationY() + { + return getPose().getY(); + } + + /** + * Returns the z translation of this frame relative to the parent frame. + * + * @return z translation + */ + public double getTranslationZ() + { + return getPose().getZ(); + } + + /** + * Returns the pose of this frame relative to the parent frame. + * + * @return pose of this frame relative to the parent. + */ + public Pose3DReadOnly getPose() + { + return originPose; + } + + /** + * Returns the translation of this frame relative to the parent frame. + * + * @return translation of this frame relative to the parent. + */ + public Point3DReadOnly getTranslation() + { + return getPose().getPosition(); + } + + /** + * Returns the orientation of this frame relative to the parent frame. + * + * @return orientation of this frame relative to the parent. + */ + public QuaternionReadOnly getOrientation() + { + return getPose().getOrientation(); + } + + /** + * {@inheritDoc} + */ + @Override + protected void updateTransformToParent(RigidBodyTransform transformToParentToPack) + { + originPose.checkReferenceFrameMatch(getParent()); + transformToParentToPack.set(originPose); + } + + /** + * {@inheritDoc} + */ + @Override + public String toString() + { + return super.toString() + ", originPose = " + originPose; + } +} diff --git a/src/frame/java/us/ihmc/euclid/referenceFrame/TranslationReferenceFrame.java b/src/frame/java/us/ihmc/euclid/referenceFrame/TranslationReferenceFrame.java new file mode 100644 index 000000000..9b4051cf7 --- /dev/null +++ b/src/frame/java/us/ihmc/euclid/referenceFrame/TranslationReferenceFrame.java @@ -0,0 +1,98 @@ +package us.ihmc.euclid.referenceFrame; + +import us.ihmc.euclid.referenceFrame.exceptions.ReferenceFrameMismatchException; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; +import us.ihmc.euclid.transform.RigidBodyTransform; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; + +/** + * This defines a mutable {@link ReferenceFrame}, where the translation to the parent {@link ReferenceFrame}, which is accessible as {@link #getParent()} can be + * updated via {@link #setTranslationAndUpdate(Tuple3DReadOnly)} or {@link #setTranslationAndUpdate(FrameTuple3DReadOnly)}. + */ +public class TranslationReferenceFrame extends ReferenceFrame +{ + /** + * The translation of this frame relative to its parent. + */ + public final FrameVector3D translationVector; + + /** + *+ * Creates a new translation reference frame as a child of the given {@param parentFrame} and initializes the transform to its parent frame. + *
+ *+ * This new reference frame is defined in the {@param parentFrame} and moves with it. + *
+ *+ * Its pose with respect to the parent frame can be modified at runtime by changing the transform in the methods + * {@link #setTranslationAndUpdate(FrameTuple3DReadOnly)} and {@link #setTranslationAndUpdate(Tuple3DReadOnly)}. + *
+ * + * @param frameName the name of the new frame. + * @param parentFrame the parent of the frame. It has to extend a {@link ReferenceFrame}. + */ + public TranslationReferenceFrame(String frameName, ReferenceFrame parentFrame) + { + super(frameName, parentFrame, false, parentFrame.isZupFrame()); + + translationVector = new FrameVector3D(parentFrame); + } + + /** + *+ * Creates a new translation reference frame as a child of the given {@param parentFrame} and initializes the transform to its parent frame. + *
+ *+ * This new reference frame is defined with a constant translation of {@param translationToParent} relative to {@param parentFrame} and moves with it. + *
+ *+ * Its pose with respect to the parent frame can be modified at runtime by changing the transform in the methods + * {@link #setTranslationAndUpdate(FrameTuple3DReadOnly)} and {@link #setTranslationAndUpdate(Tuple3DReadOnly)}. + *
+ * + * @param frameName the name of the new frame. + * @param parentFrame the parent of the frame. It has to extend a {@link ReferenceFrame}. + * @param translationToParent the initial transform to the parent frame. + */ + public TranslationReferenceFrame(String frameName, ReferenceFrame parentFrame, FrameVector3DReadOnly translationToParent) + { + this(frameName, parentFrame); + setTranslationAndUpdate(translationToParent); + } + + /** + * {@inheritDoc} + **/ + @Override + protected void updateTransformToParent(RigidBodyTransform transformToParentToPack) + { + transformToParentToPack.setIdentity(); + transformToParentToPack.getTranslation().set(translationVector); + } + + /** + * Sets the translation of this reference frame relative to its parent. The frame of the passed in tuple {@param translation} must be defined as + * {@link #getParent()}. If it is not, this method will throw a {@link ReferenceFrameMismatchException}. To avoid performing a frame check, please use + * {@link #setTranslationAndUpdate(Tuple3DReadOnly)} + * + * @param translation translate relative to parent to set. + */ + public void setTranslationAndUpdate(FrameTuple3DReadOnly translation) + { + translationVector.checkReferenceFrameMatch(translation); + setTranslationAndUpdate((Tuple3DReadOnly) translation); + } + + /** + * Sets the translation of this reference frame relative to its parent. The frame of the passed in tuple {@param translation} is assumed to be the same as + * {@link #getParent()}. This method does not perform a frame check. For frame safe operations, see {@link #setTranslationAndUpdate(FrameTuple3DReadOnly)}. + * + * @param translation translate relative to parent to set. + */ + public void setTranslationAndUpdate(Tuple3DReadOnly translation) + { + translationVector.set(translation); + this.update(); + } +} diff --git a/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameFactories.java b/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameFactories.java index 211fce99c..11474ede5 100644 --- a/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameFactories.java +++ b/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameFactories.java @@ -88,12 +88,7 @@ import us.ihmc.euclid.tuple3D.Point3D; import us.ihmc.euclid.tuple3D.UnitVector3D; import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.euclid.tuple3D.interfaces.Point3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.UnitVector3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.UnitVector3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.*; import us.ihmc.euclid.tuple4D.Quaternion; import us.ihmc.euclid.tuple4D.interfaces.QuaternionBasics; import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; diff --git a/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameTools.java b/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameTools.java index 662f0d9ae..8ec308352 100644 --- a/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameTools.java +++ b/src/frame/java/us/ihmc/euclid/referenceFrame/tools/EuclidFrameTools.java @@ -6,36 +6,27 @@ import us.ihmc.euclid.Axis3D; import us.ihmc.euclid.Location; import us.ihmc.euclid.axisAngle.AxisAngle; +import us.ihmc.euclid.geometry.BoundingBox2D; +import us.ihmc.euclid.geometry.BoundingBox3D; import us.ihmc.euclid.geometry.exceptions.BoundingBoxException; +import us.ihmc.euclid.geometry.interfaces.BoundingBox2DReadOnly; +import us.ihmc.euclid.geometry.interfaces.BoundingBox3DReadOnly; +import us.ihmc.euclid.geometry.interfaces.ConvexPolygon2DReadOnly; import us.ihmc.euclid.geometry.tools.EuclidGeometryTools; +import us.ihmc.euclid.matrix.interfaces.CommonMatrix3DBasics; import us.ihmc.euclid.orientation.interfaces.Orientation3DBasics; -import us.ihmc.euclid.referenceFrame.FramePoint2D; -import us.ihmc.euclid.referenceFrame.FramePoint3D; -import us.ihmc.euclid.referenceFrame.FrameVector2D; -import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.*; import us.ihmc.euclid.referenceFrame.exceptions.ReferenceFrameMismatchException; -import us.ihmc.euclid.referenceFrame.interfaces.EuclidFrameGeometry; -import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameOrientation3DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FixedFramePoint2DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FixedFramePoint3DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameVector2DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameVector3DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FrameOrientation3DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FrameOrientation3DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FrameVector2DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FrameVector2DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.*; +import us.ihmc.euclid.tools.EuclidCoreTools; import us.ihmc.euclid.tuple2D.Point2D; +import us.ihmc.euclid.tuple2D.interfaces.Point2DBasics; +import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly; +import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; import us.ihmc.euclid.tuple3D.Point3D; import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.euclid.tuple3D.interfaces.Point3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.*; +import us.ihmc.euclid.tuple4D.interfaces.Vector4DReadOnly; /** * Extension of the tools provided in {@link EuclidGeometryTools} for frame geometries. @@ -7903,4 +7894,470 @@ public static boolean geometricallyEquals(EuclidFrameGeometry a, EuclidFrameGeom { return (a == b) || (a != null && a.geometricallyEquals(b, epsilon)); } + + /** + * Calculates the 3D part of the given {@code input} that is parallel to the given + * {@code normalAxis} and stores the result in {@code inputNormalPartToPack}. + *+ * The result has the following properties: + *
+ * The result has the following properties: + *
+ * The result has the following properties: + *
+ * The result has the following properties: + *
+ * Edge cases: + *
+ * Edge cases: + *
+ * Edge cases: + *
+ * Edge cases: + *
+ * Edge cases: + *
+ * Edge cases: + *
+ * WARNING: generates garbage + *
+ * Allocates a new BoundingBox2D. TODO: Check, Unit test, move where BoundingBox union is + * + * @param a first bounding box to check + * @param b second bounding box to check + * @return the intersection bounding box, or null if no intersection + */ + public static FrameBoundingBox2D computeIntersectionOfTwoBoundingBoxes(FrameBoundingBox2DReadOnly a, FrameBoundingBox2DReadOnly b) + { + a.checkReferenceFrameMatch(b); + BoundingBox2D unframedBox = EuclidGeometryTools.computeIntersectionOfTwoBoundingBoxes(a, b); + if (unframedBox == null) + return null; + return new FrameBoundingBox2D(a.getReferenceFrame(), unframedBox); + } + + /** + * Finds the intersection of two bounding boxes defined by a bounding box. + *
+ * WARNING: generates garbage + *
+ * Allocates a new BoundingBox2D. TODO: Check, Unit test, move where BoundingBox union is + * + * @param a first bounding box to check + * @param b second bounding box to check + * @return the intersection bounding box, or null if no intersection + */ + public static FrameBoundingBox3D computeIntersectionOfTwoBoundingBoxes(FrameBoundingBox3DReadOnly a, FrameBoundingBox3DReadOnly b) + { + a.checkReferenceFrameMatch(b); + BoundingBox3D unframedBox = EuclidGeometryTools.computeIntersectionOfTwoBoundingBoxes(a, b); + if (unframedBox == null) + return null; + return new FrameBoundingBox3D(a.getReferenceFrame(), unframedBox); + } + + /** + * Determines if the polygonToTest is fully inside the convex polygon by some epsilon distance. + * + * @return {@code true} if {@code polygonToTest} is fully contained inside {@code polygon}. {@code false} otherwise. + */ + public static boolean isPolygonInside(FrameConvexPolygon2DReadOnly polygonToTest, double epsilon, FrameConvexPolygon2DReadOnly polygon) + { + polygonToTest.checkReferenceFrameMatch(polygon); + return EuclidGeometryTools.isPolygonInside(polygonToTest, epsilon, polygon); + } + + /** + * Determines if the polygonToTest is fully inside the convex polygon by some epsilon distance. + * + * @return {@code true} if {@code polygonToTest} is fully contained inside {@code polygon}. {@code false} otherwise. + */ + public static boolean isPolygonInside(FrameConvexPolygon2DReadOnly polygonToTest, FrameConvexPolygon2DReadOnly polygon) + { + polygonToTest.checkReferenceFrameMatch(polygon); + return EuclidGeometryTools.isPolygonInside(polygonToTest, polygon); + } + + /** + * This method implements the same operation as + * {@link EuclidGeometryTools#orientation3DFromZUpToVector3D(Vector3DReadOnly, Orientation3DBasics)} + * except that it does not rely on {@code Math#acos(double)} making it faster. + * + * @param vector the vector that is rotated with respect to {@code zUp}. Not modified. + * @param rotationToPack the minimum rotation from {@code zUp} to the given {@code vector}. + * Modified. + * @see EuclidGeometryTools#orientation3DFromZUpToVector3D(Vector3DReadOnly, Orientation3DBasics) + */ + public static void rotationMatrix3DFromZUpToVector3D(FrameVector3DReadOnly vector, FixedFrameCommonMatrix3DBasics rotationToPack) + { + vector.checkReferenceFrameMatch(rotationToPack); + EuclidGeometryTools.rotationMatrix3DFromZUpToVector3D(vector, rotationToPack); + } + + /** + * Compute Intersection-over-Union (IoU) of two 3D bounding boxes. This is the percentage of volume of the two bounding boxes that are intersecting. + *
+ * WARNING: generates garbage. + */ + public static double computeIntersectionOverUnionOfTwoBoundingBoxes(FrameBoundingBox3DReadOnly a, FrameBoundingBox3DReadOnly b) + { + a.checkReferenceFrameMatch(b); + return EuclidGeometryTools.computeIntersectionOverUnionOfTwoBoundingBoxes(a, b); + } + + /** + * Compute Intersection-over-Union (IoU) of two 3D bounding boxes. This is the percentage of volume of the two bounding boxes that are intersecting. + *
+ * WARNING: generates garbage. + */ + public static double computeIntersectionOverSmallerOfTwoBoundingBoxes(FrameBoundingBox3DReadOnly a, FrameBoundingBox3DReadOnly b) + { + a.checkReferenceFrameMatch(b); + return EuclidGeometryTools.computeIntersectionOverSmallerOfTwoBoundingBoxes(a, b); + } + + /** + * Finds the projection of a 3D point onto a 3D plane given in general form. + * Uses: projectedPoint = point - (normal.dot(point) + planeScalar) * (normal) + *
+ * WARNING: generates garbage. + * + * @param plane Coefficients of the general form of plane equation (ax + by + cz + d = 0) as Vector4D + * @param point Point to be projected onto the plane as Point3D + * @return Projected point onto the plane as Point3D + */ + public static FramePoint3D projectPointOntoPlane(FrameVector4DReadOnly plane, FramePoint3DReadOnly point) + { + plane.checkReferenceFrameMatch(point); + return new FramePoint3D(plane.getReferenceFrame(), EuclidGeometryTools.projectPointOntoPlane(plane, point)); + } } diff --git a/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryPolygonTools.java b/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryPolygonTools.java index c1336f74a..3f36f81c9 100644 --- a/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryPolygonTools.java +++ b/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryPolygonTools.java @@ -2870,6 +2870,7 @@ else if (candidateCoord1 == bestCoord1 && bound2.isFirstBetter(candidateCoord2, * @return the wrapped index. * @deprecated Use {@link EuclidCoreTools#wrap(int,int)} instead */ + @Deprecated public static int wrap(int index, int listSize) { return EuclidCoreTools.wrap(index, listSize); @@ -2891,6 +2892,7 @@ public static int wrap(int index, int listSize) * @return the wrapped incremented index. * @deprecated Use {@link EuclidCoreTools#next(int,int)} instead */ + @Deprecated public static int next(int index, int listSize) { return EuclidCoreTools.next(index, listSize); @@ -2912,6 +2914,7 @@ public static int next(int index, int listSize) * @return the wrapped decremented index. * @deprecated Use {@link EuclidCoreTools#previous(int,int)} instead */ + @Deprecated public static int previous(int index, int listSize) { return EuclidCoreTools.previous(index, listSize); diff --git a/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryTools.java b/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryTools.java index 8953a1516..41c7c7b8a 100644 --- a/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryTools.java +++ b/src/geometry/java/us/ihmc/euclid/geometry/tools/EuclidGeometryTools.java @@ -1,34 +1,35 @@ package us.ihmc.euclid.geometry.tools; -import static us.ihmc.euclid.tools.EuclidCoreTools.normSquared; - -import java.util.ArrayList; -import java.util.Collection; -import java.util.List; - import us.ihmc.euclid.Axis2D; import us.ihmc.euclid.Axis3D; import us.ihmc.euclid.Location; import us.ihmc.euclid.axisAngle.AxisAngle; +import us.ihmc.euclid.geometry.BoundingBox2D; +import us.ihmc.euclid.geometry.BoundingBox3D; import us.ihmc.euclid.geometry.exceptions.BoundingBoxException; +import us.ihmc.euclid.geometry.interfaces.BoundingBox2DReadOnly; +import us.ihmc.euclid.geometry.interfaces.BoundingBox3DReadOnly; +import us.ihmc.euclid.geometry.interfaces.ConvexPolygon2DReadOnly; +import us.ihmc.euclid.matrix.RotationMatrix; +import us.ihmc.euclid.matrix.interfaces.CommonMatrix3DBasics; import us.ihmc.euclid.orientation.interfaces.Orientation3DBasics; import us.ihmc.euclid.orientation.interfaces.Orientation3DReadOnly; import us.ihmc.euclid.tools.EuclidCoreTools; import us.ihmc.euclid.tools.TupleTools; import us.ihmc.euclid.tuple2D.Point2D; import us.ihmc.euclid.tuple2D.Vector2D; -import us.ihmc.euclid.tuple2D.interfaces.Point2DBasics; -import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly; -import us.ihmc.euclid.tuple2D.interfaces.UnitVector2DReadOnly; -import us.ihmc.euclid.tuple2D.interfaces.Vector2DBasics; -import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; +import us.ihmc.euclid.tuple2D.interfaces.*; import us.ihmc.euclid.tuple3D.Point3D; +import us.ihmc.euclid.tuple3D.UnitVector3D; import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.euclid.tuple3D.interfaces.Point3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.UnitVector3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.*; +import us.ihmc.euclid.tuple4D.interfaces.Vector4DReadOnly; + +import java.util.ArrayList; +import java.util.Collection; +import java.util.List; + +import static us.ihmc.euclid.tools.EuclidCoreTools.normSquared; /** * This class provides a large variety of basics geometry operations. @@ -37,15 +38,25 @@ */ public class EuclidGeometryTools { - /** Tolerance used to identify edge cases. */ + /** + * Tolerance used to identify edge cases. + */ public static final double ONE_MILLIONTH = 1.0e-6; - /** Tolerance used to identify edge cases. */ + /** + * Tolerance used to identify edge cases. + */ public static final double ONE_TEN_MILLIONTH = 1.0e-7; - /** Tolerance used to identify edge cases. */ + /** + * Tolerance used to identify edge cases. + */ public static final double ONE_TRILLIONTH = 1.0e-12; - /** Tolerance used to identify edge cases. */ + /** + * Tolerance used to identify edge cases. + */ public static final double IS_POINT_ON_LINE_EPS = 1.0e-8; - /** Constant used to save some computation. */ + /** + * Constant used to save some computation. + */ public static final double HALF_PI = 0.5 * Math.PI; private EuclidGeometryTools() @@ -1589,7 +1600,7 @@ public static double triangleArea(Point3DReadOnly a, Point3DReadOnly b, Point3DR * s = 0.5(a + b + c) * A = √{s(s - a)(s - b)(s - c)} * - * + *
* No specific ordering of the arguments is required. *
* Note that the method {@link #triangleArea(Point2DReadOnly, Point2DReadOnly, Point2DReadOnly)} is @@ -1614,7 +1625,7 @@ public static double triangleAreaHeron1(double lengthA, double lengthB, double l *
* A = 0.25 * √{4(a2b2 + a2c2 + b2c2) - (a2 + b2 + c2)2}
*
- *
+ * * No specific ordering of the arguments is required. *
* Note that the method {@link #triangleArea(Point2DReadOnly, Point2DReadOnly, Point2DReadOnly)} is @@ -2613,7 +2624,6 @@ public static double signedDistanceFromPoint3DToPlane3D(double pointX, double planeNormalY = planeFirstTangentZ * planeSecondTangentX - planeFirstTangentX * planeSecondTangentZ; double planeNormalZ = planeFirstTangentX * planeSecondTangentY - planeFirstTangentY * planeSecondTangentX; return signedDistanceFromPoint3DToPlane3D(pointX, pointY, pointZ, pointOnPlaneX, pointOnPlaneY, pointOnPlaneZ, planeNormalX, planeNormalY, planeNormalZ); - } /** @@ -3261,10 +3271,10 @@ public static double dotProduct(Point3DReadOnly start1, Point3DReadOnly end1, Po * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line and the bounding box. It is either equal to - * 0 or 2. + * 0 or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ public static int intersectionBetweenLine2DAndBoundingBox2D(Point2DReadOnly boundingBoxMin, Point2DReadOnly boundingBoxMax, @@ -3315,10 +3325,10 @@ public static int intersectionBetweenLine2DAndBoundingBox2D(Point2DReadOnly boun * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line and the bounding box. It is either equal to - * 0, 1, or 2. + * 0, 1, or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ public static int intersectionBetweenLine2DAndBoundingBox2D(Point2DReadOnly boundingBoxMin, Point2DReadOnly boundingBoxMax, @@ -3390,10 +3400,10 @@ public static int intersectionBetweenLine2DAndBoundingBox2D(Point2DReadOnly boun * @param secondIntersectionToPack the coordinate of the second intersection. Can be * {@code null}. Modified. * @return the number of intersections between the line/line-segment/ray and the bounding box. It is - * either equal to 0, 1, or 2. + * either equal to 0, 1, or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ private static int intersectionBetweenLine2DAndBoundingBox2DImpl(Point2DReadOnly boundingBoxMin, Point2DReadOnly boundingBoxMax, @@ -3573,7 +3583,7 @@ private static int intersectionBetweenLine2DAndBoundingBox2DImpl(Point2DReadOnly // if we get here we have a line-segment or a ray colinear with one of the box surfaces and we need to check for edge cases if (canIntersectionOccurAfterEnd) {// we have a ray and need to test for the edge-case that can only occur with ray at this point - // check if ray origin lies on bounding box surface and ray or line are colinear with that bounding box surface + // check if ray origin lies on bounding box surface and ray or line are colinear with that bounding box surface boolean isRayOriginOnBox = false; if (isColinearX) @@ -3613,7 +3623,7 @@ else if (isColinearY) } else {// we have a line-segment and need to test for the edge-case that can only occur with a line-segment at this point - // check if start- and/or end- point lies on bounding box surface and the line-segment is colinear with that bounding box surface + // check if start- and/or end- point lies on bounding box surface and the line-segment is colinear with that bounding box surface boolean isLineSegmentStartOnBox = false; boolean isLineSegmentEndOnBox = false; double deltaXEndmin = boundingBoxMin.getX() - endX; @@ -3632,7 +3642,6 @@ else if (isColinearY) { // line-segment end point also lies on x surface of box isLineSegmentEndOnBox = true; } - } else if (isColinearY) {// check if start point of ray lies on bounding box surface @@ -3644,7 +3653,6 @@ else if (isColinearY) if (deltaXEndmin <= 0.0 && deltaXEndmax >= 0.0) { // line-segment end point also lies on x surface of box isLineSegmentEndOnBox = true; - } } @@ -3834,7 +3842,7 @@ private static boolean intersectionBetweenTwoLine2DsImpl(double start1x, if (Math.abs(determinant) < epsilon) { // The lines are parallel - // Check if they are collinear + // Check if they are collinear double dx = start2x - start1x; double dy = start2y - start1y; double cross = dx * direction1y - dy * direction1x; @@ -4064,7 +4072,7 @@ public static boolean intersectionBetweenLine2DAndLineSegment2D(Point2DReadOnly * intersects with the bounding box are returned as intersection points. * *
- * + * * @param boundingBoxMin the minimum coordinate of the bounding box. Not modified. * @param boundingBoxMax the maximum coordinate of the bounding box. Not modified. * @param firstPointOnLine a first point located on the infinitely long line. Not modified. @@ -4074,10 +4082,10 @@ public static boolean intersectionBetweenLine2DAndLineSegment2D(Point2DReadOnly * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line and the bounding box. It is either equal to - * 0, 1, or 2. + * 0, 1, or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ public static int intersectionBetweenLine3DAndBoundingBox3D(Point3DReadOnly boundingBoxMin, Point3DReadOnly boundingBoxMax, @@ -4120,7 +4128,7 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(Point3DReadOnly boun * intersects with the bounding box are returned as intersection points. * * - * + * * @param boundingBoxMin the minimum coordinate of the bounding box. Not modified. * @param boundingBoxMax the maximum coordinate of the bounding box. Not modified. * @param pointOnLine a point located on the infinitely long line. Not modified. @@ -4130,10 +4138,10 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(Point3DReadOnly boun * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line and the bounding box. It is either equal to - * 0, 1, or 2. + * 0, 1, or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ public static int intersectionBetweenLine3DAndBoundingBox3D(Point3DReadOnly boundingBoxMin, Point3DReadOnly boundingBoxMax, @@ -4174,7 +4182,7 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(Point3DReadOnly boun * intersects with the bounding box are returned as intersection points. * * - * + * * @param boundingBoxMinX the minimum x-coordinate of the bounding box. * @param boundingBoxMinY the minimum y-coordinate of the bounding box. * @param boundingBoxMinZ the minimum z-coordinate of the bounding box. @@ -4188,10 +4196,10 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(Point3DReadOnly boun * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line and the bounding box. It is either equal to - * 0, 1, or 2. + * 0, 1, or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ public static int intersectionBetweenLine3DAndBoundingBox3D(double boundingBoxMinX, double boundingBoxMinY, @@ -4248,7 +4256,7 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(double boundingBoxMi * intersects with the bounding box are returned as intersection points. * * - * + * * @param boundingBoxMinX the minimum x-coordinate of the bounding box. * @param boundingBoxMinY the minimum y-coordinate of the bounding box. * @param boundingBoxMinZ the minimum z-coordinate of the bounding box. @@ -4266,10 +4274,10 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(double boundingBoxMi * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line and the bounding box. It is either equal to - * 0, 1, or 2. + * 0, 1, or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ public static int intersectionBetweenLine3DAndBoundingBox3D(double boundingBoxMinX, double boundingBoxMinY, @@ -4338,7 +4346,7 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(double boundingBoxMi * intersection points. * * - * + * * @param boundingBoxMin the minimum coordinate of the bounding box. Not modified. * @param boundingBoxMax the maximum coordinate of the bounding box. Not modified. * @param startX the x-coordinate of a point located on the @@ -4362,10 +4370,10 @@ public static int intersectionBetweenLine3DAndBoundingBox3D(double boundingBoxMi * @param secondIntersectionToPack the coordinate of the second intersection. Can be * {@code null}. Modified. * @return the number of intersections between the line/line-segment/ray and the bounding box. It is - * either equal to 0, 1, or 2. + * either equal to 0, 1, or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ private static int intersectionBetweenLine3DAndBoundingBox3DImpl(Point3DReadOnly boundingBoxMin, Point3DReadOnly boundingBoxMax, @@ -4454,10 +4462,10 @@ private static int intersectionBetweenLine3DAndBoundingBox3DImpl(Point3DReadOnly * @param secondIntersectionToPack the coordinate of the second intersection. Can be * {@code null}. Modified. * @return the number of intersections between the line/line-segment/ray and the bounding box. It is - * either equal to 0, 1, or 2. + * either equal to 0, 1, or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ private static int intersectionBetweenLine3DAndBoundingBox3DImpl(double boundingBoxMinX, double boundingBoxMinY, @@ -4692,7 +4700,7 @@ private static int intersectionBetweenLine3DAndBoundingBox3DImpl(double bounding // if we get here we have a line-segment or a ray colinear with one of the box surfaces and we need to check for edge cases if (canIntersectionOccurAfterEnd) {// we have a ray and need to test for the edge-case that can only occur with ray at this point - // check if ray origin lies on bounding box surface and ray or line are colinear with that bounding box surface + // check if ray origin lies on bounding box surface and ray or line are colinear with that bounding box surface boolean isRayOriginOnBox = false; if (isColinearX) @@ -4742,7 +4750,7 @@ else if (isColinearZ) } else {// we have a line-segment and need to test for the edge-case that can only occur with a line-segment at this point - // check if start- and/or end- point lies on bounding box surface and the line-segment is colinear with that bounding box surface + // check if start- and/or end- point lies on bounding box surface and the line-segment is colinear with that bounding box surface boolean isLineSegmentStartOnBox = false; boolean isLineSegmentEndOnBox = false; double deltaXEndmin = boundingBoxMinX - endX; @@ -4763,7 +4771,6 @@ else if (isColinearZ) { // line-segment end point also lies on x surface of box isLineSegmentEndOnBox = true; } - } else if (isColinearY) {// check if start point of ray lies on bounding box surface @@ -4775,7 +4782,6 @@ else if (isColinearY) if (deltaXEndmin <= 0.0 && deltaXEndmax >= 0.0 && deltaZEndmin <= 0.0 && deltaZEndmax >= 0.0) { // line-segment end point also lies on x surface of box isLineSegmentEndOnBox = true; - } } else if (isColinearZ) @@ -4789,7 +4795,6 @@ else if (isColinearZ) { // line-segment end point also lies on x surface of box isLineSegmentEndOnBox = true; } - } if (isLineSegmentStartOnBox && isLineSegmentEndOnBox) { // start and end are on the box, we consider them intersections @@ -4841,7 +4846,6 @@ else if (isLineSegmentStartOnBox) } return numberOfIntersections; - } /** @@ -4880,9 +4884,9 @@ else if (isLineSegmentStartOnBox) * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line and the cylinder. It is either equal to 0, - * 1, or 2. + * 1, or 2. * @throws IllegalArgumentException if either {@code cylinderLength < 0} or - * {@code cylinderRadius < 0}. + * {@code cylinderRadius < 0}. */ public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, double cylinderRadius, @@ -4956,9 +4960,9 @@ public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line and the cylinder. It is either equal to 0, - * 1, or 2. + * 1, or 2. * @throws IllegalArgumentException if either {@code cylinderLength < 0} or - * {@code cylinderRadius < 0}. + * {@code cylinderRadius < 0}. */ public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, double cylinderRadius, @@ -5030,9 +5034,9 @@ public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line and the cylinder. It is either equal to 0, - * 1, or 2. + * 1, or 2. * @throws IllegalArgumentException if either {@code cylinderLength < 0} or - * {@code cylinderRadius < 0}. + * {@code cylinderRadius < 0}. */ public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, double cylinderRadius, @@ -5135,9 +5139,9 @@ public static int intersectionBetweenLine3DAndCylinder3D(double cylinderLength, * @param secondIntersectionToPack the coordinate of the second intersection. Can be * {@code null}. Modified. * @return the number of intersections between the line/line-segment/ray and the cylinder. It is - * either equal to 0, 1, or 2. + * either equal to 0, 1, or 2. * @throws IllegalArgumentException if either {@code cylinderLength < 0} or - * {@code cylinderRadius < 0}. + * {@code cylinderRadius < 0}. */ private static int intersectionBetweenLine3DAndCylinder3DImpl(double cylinderLength, double cylinderRadius, @@ -5244,8 +5248,8 @@ else if (dBottom < dIntersection1) // If dIntersection2 is not NaN, that means two intersections were found which is the max, so no need to check with the cylinder part. if (Double.isNaN(dIntersection2)) { // Compute possible intersections with the cylinder part - // Notation used: cylinder axis: pa + va * d; line equation: p + v * d - // Need to solve quadratic equation of the form A * d^2 + B * d + C = 0 + // Notation used: cylinder axis: pa + va * d; line equation: p + v * d + // Need to solve quadratic equation of the form A * d^2 + B * d + C = 0 // (v, va)*va double scaledAxisX1 = lineDirectionDotCylinderAxis * axisX; @@ -5430,9 +5434,9 @@ else if (dCylinder2 < dIntersection1) * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line/line-segment/ray and the ellipsoid. It is - * either equal to 0, 1, or 2. + * either equal to 0, 1, or 2. * @throws IllegalArgumentException if either {@code radiusX}, {@code radiusY}, or {@code radiusZ} - * is negative. + * is negative. */ public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, double radiusY, @@ -5495,9 +5499,9 @@ public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line/line-segment/ray and the ellipsoid. It is - * either equal to 0, 1, or 2. + * either equal to 0, 1, or 2. * @throws IllegalArgumentException if either {@code radiusX}, {@code radiusY}, or {@code radiusZ} - * is negative. + * is negative. */ public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, double radiusY, @@ -5556,9 +5560,9 @@ public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line/line-segment/ray and the ellipsoid. It is - * either equal to 0, 1, or 2. + * either equal to 0, 1, or 2. * @throws IllegalArgumentException if either {@code radiusX}, {@code radiusY}, or {@code radiusZ} - * is negative. + * is negative. */ public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, double radiusY, @@ -5642,9 +5646,9 @@ public static int intersectionBetweenLine3DAndEllipsoid3D(double radiusX, * @param secondIntersectionToPack the coordinate of the second intersection. Can be * {@code null}. Modified. * @return the number of intersections between the line/line-segment/ray and the ellipsoid. It is - * either equal to 0, 1, or 2. + * either equal to 0, 1, or 2. * @throws IllegalArgumentException if either {@code radiusX}, {@code radiusY}, or {@code radiusZ} - * is negative. + * is negative. */ private static int intersectionBetweenLine3DAndEllipsoid3DImpl(double radiusX, double radiusY, @@ -5785,7 +5789,7 @@ private static int intersectionBetweenLine3DAndEllipsoid3DImpl(double radiusX, * @param pointOnLine a point located on the line. Not modified. * @param lineDirection the direction of the line. Not modified. * @return the coordinates of the intersection, or {@code null} if the line is parallel to the - * plane. + * plane. */ public static Point3D intersectionBetweenLine3DAndPlane3D(Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal, @@ -5892,10 +5896,10 @@ public static boolean intersectionBetweenLine3DAndPlane3D(Point3DReadOnly pointO * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line segment and the bounding box. It is either - * equal to 0, 1, or 2. + * equal to 0, 1, or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ public static int intersectionBetweenLineSegment2DAndBoundingBox2D(Point2DReadOnly boundingBoxMin, Point2DReadOnly boundingBoxMax, @@ -5947,7 +5951,7 @@ public static int intersectionBetweenLineSegment2DAndBoundingBox2D(Point2DReadOn * first/last intersects with the bounding box boundary are returned as intersection points. * * - * + * * @param boundingBoxMin the minimum coordinate of the bounding box. Not modified. * @param boundingBoxMax the maximum coordinate of the bounding box. Not modified. * @param lineSegmentStart the first endpoint of the line segment. Not modified. @@ -5957,10 +5961,10 @@ public static int intersectionBetweenLineSegment2DAndBoundingBox2D(Point2DReadOn * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line segment and the bounding box. It is either - * equal to 0, 1, or 2. + * equal to 0, 1, or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ public static int intersectionBetweenLineSegment3DAndBoundingBox3D(Point3DReadOnly boundingBoxMin, Point3DReadOnly boundingBoxMax, @@ -6011,9 +6015,9 @@ public static int intersectionBetweenLineSegment3DAndBoundingBox3D(Point3DReadOn * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line segment and the cylinder. It is either equal - * to 0, 1, or 2. + * to 0, 1, or 2. * @throws IllegalArgumentException if either {@code cylinderLength < 0} or - * {@code cylinderRadius < 0}. + * {@code cylinderRadius < 0}. */ public static int intersectionBetweenLineSegment3DAndCylinder3D(double cylinderLength, double cylinderRadius, @@ -6085,9 +6089,9 @@ public static int intersectionBetweenLineSegment3DAndCylinder3D(double cylinderL * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line/line-segment/ray and the ellipsoid. It is - * either equal to 0, 1, or 2. + * either equal to 0, 1, or 2. * @throws IllegalArgumentException if either {@code radiusX}, {@code radiusY}, or {@code radiusZ} - * is negative. + * is negative. */ public static int intersectionBetweenLineSegment3DAndEllipsoid3D(double radiusX, double radiusY, @@ -6199,10 +6203,10 @@ public static Point3D intersectionBetweenLineSegment3DAndPlane3D(Point3DReadOnly * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the ray and the bounding box. It is either equal to - * 0, 1, or 2. + * 0, 1, or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ public static int intersectionBetweenRay2DAndBoundingBox2D(Point2DReadOnly boundingBoxMin, Point2DReadOnly boundingBoxMax, @@ -6273,7 +6277,6 @@ public static boolean intersectionBetweenRay2DAndLineSegment2D(double rayOriginX double end2x = lineSegmentEndX; double end2y = lineSegmentEndY; return intersectionBetweenTwoLine2DsImpl(start1x, start1y, false, end1x, end1y, true, start2x, start2y, false, end2x, end2y, false, intersectionToPack); - } /** @@ -6392,7 +6395,7 @@ public static boolean intersectionBetweenRay2DAndLineSegment2D(Point2DReadOnly r * intersection points. * * - * + * * @param boundingBoxMin the minimum coordinate of the bounding box. Not modified. * @param boundingBoxMax the maximum coordinate of the bounding box. Not modified. * @param rayOrigin the coordinate of the ray origin. Not modified. @@ -6402,10 +6405,10 @@ public static boolean intersectionBetweenRay2DAndLineSegment2D(Point2DReadOnly r * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the ray and the bounding box. It is either equal to - * 0, 1, or 2. + * 0, 1, or 2. * @throws BoundingBoxException if any of the minimum coordinates of the bounding box is strictly - * greater than the maximum coordinate of the bounding box on the same - * axis. + * greater than the maximum coordinate of the bounding box on the same + * axis. */ public static int intersectionBetweenRay3DAndBoundingBox3D(Point3DReadOnly boundingBoxMin, Point3DReadOnly boundingBoxMax, @@ -6457,10 +6460,10 @@ public static int intersectionBetweenRay3DAndBoundingBox3D(Point3DReadOnly bound * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line and the 3D box. It is either equal to 0, 1, - * or 2. If the ray origin is on the surface of the 3D box it is considered an intersection. + * or 2. If the ray origin is on the surface of the 3D box it is considered an intersection. * @throws IllegalArgumentException if {@code boxSize} contains values <= 0.0 * @see #intersectionBetweenRay3DAndBoundingBox3D(Point3DReadOnly, Point3DReadOnly, Point3DReadOnly, - * Vector3DReadOnly, Point3DBasics, Point3DBasics) + * Vector3DReadOnly, Point3DBasics, Point3DBasics) */ public static int intersectionBetweenRay3DAndBox3D(Point3DReadOnly boxPosition, Orientation3DReadOnly boxOrientation, @@ -6561,9 +6564,9 @@ public static int intersectionBetweenRay3DAndBox3D(Point3DReadOnly boxPosition, * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the ray and the bounding box. It is either equal to - * 0, 1, or 2. + * 0, 1, or 2. * @throws IllegalArgumentException if either {@code cylinderLength < 0} or - * {@code cylinderRadius < 0}. + * {@code cylinderRadius < 0}. */ public static int intersectionBetweenRay3DAndCylinder3D(double cylinderLength, double cylinderRadius, @@ -6635,9 +6638,9 @@ public static int intersectionBetweenRay3DAndCylinder3D(double cylinderLength, * @param secondIntersectionToPack the coordinate of the second intersection. Can be {@code null}. * Modified. * @return the number of intersections between the line/line-segment/ray and the ellipsoid. It is - * either equal to 0, 1, or 2. + * either equal to 0, 1, or 2. * @throws IllegalArgumentException if either {@code radiusX}, {@code radiusY}, or {@code radiusZ} - * is negative. + * is negative. */ public static int intersectionBetweenRay3DAndEllipsoid3D(double radiusX, double radiusY, @@ -7096,9 +7099,9 @@ public static boolean intersectionBetweenTwoPlane3Ds(Point3DReadOnly pointOnPlan intersectionDirectionToPack.scale(1.0 / EuclidCoreTools.squareRoot(det)); double normal3DotPoint1 = intersectionDirectionToPack.getX() * pointOnPlane1.getX() + intersectionDirectionToPack.getY() * pointOnPlane1.getY() - + intersectionDirectionToPack.getZ() * pointOnPlane1.getZ(); + + intersectionDirectionToPack.getZ() * pointOnPlane1.getZ(); double normal3DotPoint2 = intersectionDirectionToPack.getX() * pointOnPlane2.getX() + intersectionDirectionToPack.getY() * pointOnPlane2.getY() - + intersectionDirectionToPack.getZ() * pointOnPlane2.getZ(); + + intersectionDirectionToPack.getZ() * pointOnPlane2.getZ(); double d3 = 0.5 * (normal3DotPoint1 + normal3DotPoint2); pointOnIntersectionToPack.set(d1 * normal3Cross2X + d2 * normal1Cross3X + d3 * normal2Cross1X, @@ -7171,7 +7174,7 @@ public static boolean intersectionBetweenTwoPlane3Ds(Point3DReadOnly pointOnPlan * @param lengthSideB the length of the side B. * @param lengthSideC the length of the side C. * @return {@code true} if the lengths represents the three sides of a triangle, {@code false} - * otherwise. + * otherwise. * @throws IllegalArgumentException if any of the three lengths is negative. */ public static boolean isFormingTriangle(double lengthSideA, double lengthSideB, double lengthSideC) @@ -7203,8 +7206,8 @@ public static boolean isFormingTriangle(double lengthSideA, double lengthSideB, * @param rayDirectionX the x-component of the ray's direction. * @param rayDirectionY the y-component of the ray's direction. * @return {@link Location#AHEAD} if the query is located in front of the ray, - * {@link Location#BEHIND} if the query is behind the ray, and {@code null} if the query's - * projection onto the ray is exactly equal to the ray origin. + * {@link Location#BEHIND} if the query is behind the ray, and {@code null} if the query's + * projection onto the ray is exactly equal to the ray origin. */ public static Location whichPartOfRay2DIsPoint2DOn(double pointX, double pointY, @@ -7232,8 +7235,8 @@ else if (dotProduct < 0.0) * @param rayOrigin the ray's origin. Not modified. * @param rayDirection the ray's direction. Not modified. * @return {@link Location#AHEAD} if the query is located in front of the ray, - * {@link Location#BEHIND} if the query is behind the ray, and {@code null} if the query's - * projection onto the ray is exactly equal to the ray origin. + * {@link Location#BEHIND} if the query is behind the ray, and {@code null} if the query's + * projection onto the ray is exactly equal to the ray origin. */ public static Location whichPartOfRay2DIsPoint2DOn(Point2DReadOnly point, Point2DReadOnly rayOrigin, Vector2DReadOnly rayDirection) { @@ -7280,7 +7283,7 @@ public static boolean isPoint2DInFrontOfRay2D(Point2DReadOnly point, Point2DRead * @param b second vertex of the triangle. Not modified. * @param c third vertex of the triangle. Not modified. * @return {@code true} if the query is exactly inside the triangle. {@code false} if the query - * point is outside triangle or exactly on an edge of the triangle. + * point is outside triangle or exactly on an edge of the triangle. */ public static boolean isPoint2DInsideTriangleABC(Point2DReadOnly point, Point2DReadOnly a, Point2DReadOnly b, Point2DReadOnly c) { @@ -7388,7 +7391,7 @@ public static boolean isPoint2DOnLineSegment2D(Point2DReadOnly point, Point2DRea * @param firstPointOnLine a first point located on the line. Not modified. * @param secondPointOnLine a second point located on the line. Not modified. * @return {@link Location.LEFT}/{@link Location.RIGHT} if the point is on the left/right side of - * the line, or {@code null} if the point is exactly on the line. + * the line, or {@code null} if the point is exactly on the line. */ public static Location whichSideOfLine2DIsPoint2DOn(double pointX, double pointY, Point2DReadOnly firstPointOnLine, Point2DReadOnly secondPointOnLine) { @@ -7417,7 +7420,7 @@ public static Location whichSideOfLine2DIsPoint2DOn(double pointX, double pointY * @param pointOnLine a point positioned on the infinite line. Not modified. * @param lineDirection the direction of the infinite line. Not modified. * @return {@link Location.LEFT}/{@link Location.RIGHT} if the point is on the left/right side of - * the line, or {@code null} if the point is exactly on the line. + * the line, or {@code null} if the point is exactly on the line. */ public static Location whichSideOfLine2DIsPoint2DOn(double pointX, double pointY, Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection) { @@ -7446,7 +7449,7 @@ public static Location whichSideOfLine2DIsPoint2DOn(double pointX, double pointY * @param firstPointOnLine a first point located on the line. Not modified. * @param secondPointOnLine a second point located on the line. Not modified. * @return {@link Location.LEFT}/{@link Location.RIGHT} if the point is on the left/right side of - * the line, or {@code null} if the point is exactly on the line. + * the line, or {@code null} if the point is exactly on the line. */ public static Location whichSideOfLine2DIsPoint2DOn(Point2DReadOnly point, Point2DReadOnly firstPointOnLine, Point2DReadOnly secondPointOnLine) { @@ -7470,7 +7473,7 @@ public static Location whichSideOfLine2DIsPoint2DOn(Point2DReadOnly point, Point * @param pointOnLine a point positioned on the infinite line. Not modified. * @param lineDirection the direction of the infinite line. Not modified. * @return {@link Location.LEFT}/{@link Location.RIGHT} if the point is on the left/right side of - * the line, or {@code null} if the point is exactly on the line. + * the line, or {@code null} if the point is exactly on the line. */ public static Location whichSideOfLine2DIsPoint2DOn(Point2DReadOnly point, Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection) { @@ -7497,7 +7500,7 @@ public static Location whichSideOfLine2DIsPoint2DOn(Point2DReadOnly point, Point * @param lineDirectionX the x-component of the direction of the infinite line. * @param lineDirectionY the y-component of the direction of the infinite line. * @return {@link Location.LEFT}/{@link Location.RIGHT} if the point is on the left/right side of - * the line, or {@code null} if the point is exactly on the line. + * the line, or {@code null} if the point is exactly on the line. */ public static Location whichSideOfLine2DIsPoint2DOn(double pointX, double pointY, @@ -7532,7 +7535,7 @@ else if (crossProduct < 0.0) * @param firstPointOnLine a first point located on the line. Not modified. * @param secondPointOnLine a second point located on the line. Not modified. * @return {@code true} if the point is on the left side of the line, {@code false} if the point is - * on the right side or exactly on the line. + * on the right side or exactly on the line. */ public static boolean isPoint2DOnLeftSideOfLine2D(Point2DReadOnly point, Point2DReadOnly firstPointOnLine, Point2DReadOnly secondPointOnLine) { @@ -7554,7 +7557,7 @@ public static boolean isPoint2DOnLeftSideOfLine2D(Point2DReadOnly point, Point2D * @param firstPointOnLine a first point located on the line. Not modified. * @param secondPointOnLine a second point located on the line. Not modified. * @return {@code true} if the point is on the right side of the line, {@code false} if the point is - * on the left side or exactly on the line. + * on the left side or exactly on the line. */ public static boolean isPoint2DOnRightSideOfLine2D(Point2DReadOnly point, Point2DReadOnly firstPointOnLine, Point2DReadOnly secondPointOnLine) { @@ -7583,10 +7586,10 @@ public static boolean isPoint2DOnRightSideOfLine2D(Point2DReadOnly point, Point2 * @param testLeftSide the query of the side, when equal to {@code true} this will test for the * left side, {@code false} this will test for the right side. * @return {@code true} if the point is on the query side of the line, {@code false} if the point is - * on the opposite side or exactly on the line. + * on the opposite side or exactly on the line. * @deprecated Use - * {@link #whichSideOfLine2DIsPoint2DOn(double, double, double, double, double, double)} - * instead. + * {@link #whichSideOfLine2DIsPoint2DOn(double, double, double, double, double, double)} + * instead. */ @Deprecated public static boolean isPoint2DOnSideOfLine2D(double pointX, @@ -7622,10 +7625,10 @@ public static boolean isPoint2DOnSideOfLine2D(double pointX, * @param testLeftSide the query of the side, when equal to {@code true} this will test for the * left side, {@code false} this will test for the right side. * @return {@code true} if the point is on the query side of the line, {@code false} if the point is - * on the opposite side or exactly on the line. + * on the opposite side or exactly on the line. * @deprecated Use - * {@link #whichSideOfLine2DIsPoint2DOn(double, double, Point2DReadOnly, Point2DReadOnly)} - * instead. + * {@link #whichSideOfLine2DIsPoint2DOn(double, double, Point2DReadOnly, Point2DReadOnly)} + * instead. */ @Deprecated public static boolean isPoint2DOnSideOfLine2D(double pointX, @@ -7658,10 +7661,10 @@ public static boolean isPoint2DOnSideOfLine2D(double pointX, * @param testLeftSide the query of the side, when equal to {@code true} this will test for the * left side, {@code false} this will test for the right side. * @return {@code true} if the point is on the query side of the line, {@code false} if the point is - * on the opposite side or exactly on the line. + * on the opposite side or exactly on the line. * @deprecated Use - * {@link #whichSideOfLine2DIsPoint2DOn(double, double, Point2DReadOnly, Vector2DReadOnly)} - * instead. + * {@link #whichSideOfLine2DIsPoint2DOn(double, double, Point2DReadOnly, Vector2DReadOnly)} + * instead. */ @Deprecated public static boolean isPoint2DOnSideOfLine2D(double pointX, @@ -7694,10 +7697,10 @@ public static boolean isPoint2DOnSideOfLine2D(double pointX, * @param testLeftSide the query of the side, when equal to {@code true} this will test for the * left side, {@code false} this will test for the right side. * @return {@code true} if the point is on the query side of the line, {@code false} if the point is - * on the opposite side or exactly on the line. + * on the opposite side or exactly on the line. * @deprecated Use - * {@link #whichSideOfLine2DIsPoint2DOn(Point2DReadOnly, Point2DReadOnly, Point2DReadOnly)} - * instead. + * {@link #whichSideOfLine2DIsPoint2DOn(Point2DReadOnly, Point2DReadOnly, Point2DReadOnly)} + * instead. */ @Deprecated public static boolean isPoint2DOnSideOfLine2D(Point2DReadOnly point, @@ -7728,10 +7731,10 @@ public static boolean isPoint2DOnSideOfLine2D(Point2DReadOnly point, * @param testLeftSide the query of the side, when equal to {@code true} this will test for the * left side, {@code false} this will test for the right side. * @return {@code true} if the point is on the query side of the line, {@code false} if the point is - * on the opposite side or exactly on the line. + * on the opposite side or exactly on the line. * @deprecated Use - * {@link #whichSideOfLine2DIsPoint2DOn(Point2DReadOnly, Point2DReadOnly, Vector2DReadOnly)} - * instead. + * {@link #whichSideOfLine2DIsPoint2DOn(Point2DReadOnly, Point2DReadOnly, Vector2DReadOnly)} + * instead. */ @Deprecated public static boolean isPoint2DOnSideOfLine2D(Point2DReadOnly point, Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection, boolean testLeftSide) @@ -7763,7 +7766,7 @@ public static boolean isPoint2DOnSideOfLine2D(Point2DReadOnly point, Point2DRead * @param planeNormalY the y-component of the normal of the infinite plane. * @param planeNormalZ the z-component of the normal of the infinite plane. * @return {@link Location#ABOVE}/{@link Location#BELOW} if the point is above/below the plane, - * {@code null} if the point is exactly on the plane. + * {@code null} if the point is exactly on the plane. */ public static Location whichSideOfPlane3DIsPoint3DOn(double pointX, double pointY, @@ -7807,7 +7810,7 @@ else if (signedDistance < 0.0) * @param pointOnPlane the coordinates of a point positioned on the infinite plane. Not modified. * @param planeNormal the normal of the infinite plane. Not modified. * @return {@link Location#ABOVE}/{@link Location#BELOW} if the point is above/below the plane, - * {@code null} if the point is exactly on the plane. + * {@code null} if the point is exactly on the plane. */ public static Location whichSideOfPlane3DIsPoint3DOn(double pointX, double pointY, double pointZ, Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal) { @@ -7839,7 +7842,7 @@ public static Location whichSideOfPlane3DIsPoint3DOn(double pointX, double point * @param pointOnPlane the coordinates of a point positioned on the infinite plane. Not modified. * @param planeNormal the normal of the infinite plane. Not modified. * @return {@link Location#ABOVE}/{@link Location#BELOW} if the point is above/below the plane, - * {@code null} if the point is exactly on the plane. + * {@code null} if the point is exactly on the plane. */ public static Location whichSideOfPlane3DIsPoint3DOn(Point3DReadOnly point, Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal) { @@ -7872,9 +7875,9 @@ public static Location whichSideOfPlane3DIsPoint3DOn(Point3DReadOnly point, Poin * @param planeSecondTangentY the y-component of a second tangent of the infinite plane. * @param planeSecondTangentZ the z-component of a second tangent of the infinite plane. * @return {@link Location#ABOVE}/{@link Location#BELOW} if the point is above/below the plane, - * {@code null} if the point is exactly on the plane. + * {@code null} if the point is exactly on the plane. * @see #whichSideOfPlane3DIsPoint3DOn(double, double, double, double, double, double, double, - * double, double) + * double, double) */ public static Location whichSideOfPlane3DIsPoint3DOn(double pointX, double pointY, @@ -7916,9 +7919,9 @@ public static Location whichSideOfPlane3DIsPoint3DOn(double pointX, * @param planeFirstTangent a first tangent of the infinite plane. Not modified. * @param planeSecondTangent a second tangent of the infinite plane. Not modified. * @return {@link Location#ABOVE}/{@link Location#BELOW} if the point is above/below the plane, - * {@code null} if the point is exactly on the plane. + * {@code null} if the point is exactly on the plane. * @see #whichSideOfPlane3DIsPoint3DOn(double, double, double, double, double, double, double, - * double, double) + * double, double) */ public static Location whichSideOfPlane3DIsPoint3DOn(double pointX, double pointY, @@ -7960,9 +7963,9 @@ public static Location whichSideOfPlane3DIsPoint3DOn(double pointX, * @param planeFirstTangent a first tangent of the infinite plane. Not modified. * @param planeSecondTangent a second tangent of the infinite plane. Not modified. * @return {@link Location#ABOVE}/{@link Location#BELOW} if the point is above/below the plane, - * {@code null} if the point is exactly on the plane. + * {@code null} if the point is exactly on the plane. * @see #whichSideOfPlane3DIsPoint3DOn(double, double, double, double, double, double, double, - * double, double) + * double, double) */ public static Location whichSideOfPlane3DIsPoint3DOn(Point3DReadOnly point, Point3DReadOnly pointOnPlane, @@ -7997,10 +8000,10 @@ public static Location whichSideOfPlane3DIsPoint3DOn(Point3DReadOnly point, * @param testForAbove the query of the side, when equal to {@code true} this will test for the * above side, {@code false} this will test for the below side. * @return {@code true} if the point is on the query side of the plane, {@code false} if the point - * is on the opposite side or exactly on the plane. + * is on the opposite side or exactly on the plane. * @deprecated Use - * {@link #whichSideOfPlane3DIsPoint3DOn(double, double, double, double, double, double, double, double, double)} - * instead. + * {@link #whichSideOfPlane3DIsPoint3DOn(double, double, double, double, double, double, double, double, double)} + * instead. */ @Deprecated public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, @@ -8047,10 +8050,10 @@ public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, * @param testForAbove the query of the side, when equal to {@code true} this will test for the * above side, {@code false} this will test for the below side. * @return {@code true} if the point is on the query side of the plane, {@code false} if the point - * is on the opposite side or exactly on the plane. + * is on the opposite side or exactly on the plane. * @deprecated Use - * {@link #whichSideOfPlane3DIsPoint3DOn(double, double, double, Point3DReadOnly, Vector3DReadOnly, boolean)} - * instead. + * {@link #whichSideOfPlane3DIsPoint3DOn(double, double, double, Point3DReadOnly, Vector3DReadOnly, boolean)} + * instead. */ @Deprecated public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, @@ -8083,10 +8086,10 @@ public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, * @param testForAbove the query of the side, when equal to {@code true} this will test for the * above side, {@code false} this will test for the below side. * @return {@code true} if the point is on the query side of the plane, {@code false} if the point - * is on the opposite side or exactly on the plane. + * is on the opposite side or exactly on the plane. * @deprecated Use - * {@link #whichSideOfPlane3DIsPoint3DOn(Point3DReadOnly, Point3DReadOnly, Vector3DReadOnly)} - * instead. + * {@link #whichSideOfPlane3DIsPoint3DOn(Point3DReadOnly, Point3DReadOnly, Vector3DReadOnly)} + * instead. */ @Deprecated public static boolean isPoint3DAboveOrBelowPlane3D(Point3DReadOnly point, Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal, boolean testForAbove) @@ -8114,7 +8117,7 @@ public static boolean isPoint3DAboveOrBelowPlane3D(Point3DReadOnly point, Point3 * @param pointOnPlane the coordinates of a point positioned on the infinite plane. Not modified. * @param planeNormal the normal of the infinite plane. Not modified. * @return {@code true} if the point is strictly above the plane, {@code false} if the point is - * below or exactly on the plane. + * below or exactly on the plane. */ public static boolean isPoint3DAbovePlane3D(double pointX, double pointY, double pointZ, Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal) { @@ -8138,7 +8141,7 @@ public static boolean isPoint3DAbovePlane3D(double pointX, double pointY, double * @param pointOnPlane the coordinates of a point positioned on the infinite plane. Not modified. * @param planeNormal the normal of the infinite plane. Not modified. * @return {@code true} if the point is strictly above the plane, {@code false} if the point is - * below or exactly on the plane. + * below or exactly on the plane. */ public static boolean isPoint3DAbovePlane3D(Point3DReadOnly point, Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal) { @@ -8164,7 +8167,7 @@ public static boolean isPoint3DAbovePlane3D(Point3DReadOnly point, Point3DReadOn * @param pointOnPlane the coordinates of a point positioned on the infinite plane. Not modified. * @param planeNormal the normal of the infinite plane. Not modified. * @return {@code true} if the point is strictly below the plane, {@code false} if the point is - * above or exactly on the plane. + * above or exactly on the plane. */ public static boolean isPoint3DBelowPlane3D(double pointX, double pointY, double pointZ, Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal) { @@ -8188,7 +8191,7 @@ public static boolean isPoint3DBelowPlane3D(double pointX, double pointY, double * @param pointOnPlane the coordinates of a point positioned on the infinite plane. Not modified. * @param planeNormal the normal of the infinite plane. Not modified. * @return {@code true} if the point is strictly below the plane, {@code false} if the point is - * above or exactly on the plane. + * above or exactly on the plane. */ public static boolean isPoint3DBelowPlane3D(Point3DReadOnly point, Point3DReadOnly pointOnPlane, Vector3DReadOnly planeNormal) { @@ -8223,12 +8226,12 @@ public static boolean isPoint3DBelowPlane3D(Point3DReadOnly point, Point3DReadOn * @param testForAbove the query of the side, when equal to {@code true} this will test for * the above side, {@code false} this will test for the below side. * @return {@code true} if the point is on the query side of the plane, {@code false} if the point - * is on the opposite side or exactly on the plane. + * is on the opposite side or exactly on the plane. * @see #isPoint3DAboveOrBelowPlane3D(double, double, double, double, double, double, double, - * double, double, boolean) + * double, double, boolean) * @deprecated Use - * {@link #whichSideOfPlane3DIsPoint3DOn(double, double, double, double, double, double, double, double, double, double, double, double)} - * instead. + * {@link #whichSideOfPlane3DIsPoint3DOn(double, double, double, double, double, double, double, double, double, double, double, double)} + * instead. */ @Deprecated public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, @@ -8258,7 +8261,6 @@ public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, planeSecondTangentY, planeSecondTangentZ); return testForAbove ? side == Location.ABOVE : side == Location.BELOW; - } /** @@ -8284,12 +8286,12 @@ public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, * @param testForAbove the query of the side, when equal to {@code true} this will test for * the above side, {@code false} this will test for the below side. * @return {@code true} if the point is on the query side of the plane, {@code false} if the point - * is on the opposite side or exactly on the plane. + * is on the opposite side or exactly on the plane. * @see #isPoint3DAboveOrBelowPlane3D(double, double, double, double, double, double, double, - * double, double, boolean) + * double, double, boolean) * @deprecated Use - * {@link #whichSideOfPlane3DIsPoint3DOn(double, double, double, Point3DReadOnly, Vector3DReadOnly, Vector3DReadOnly)} - * instead. + * {@link #whichSideOfPlane3DIsPoint3DOn(double, double, double, Point3DReadOnly, Vector3DReadOnly, Vector3DReadOnly)} + * instead. */ @Deprecated public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, @@ -8302,7 +8304,6 @@ public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, { Location side = whichSideOfPlane3DIsPoint3DOn(pointX, pointY, pointZ, pointOnPlane, planeFirstTangent, planeSecondTangent); return testForAbove ? side == Location.ABOVE : side == Location.BELOW; - } /** @@ -8326,12 +8327,12 @@ public static boolean isPoint3DAboveOrBelowPlane3D(double pointX, * @param testForAbove the query of the side, when equal to {@code true} this will test for * the above side, {@code false} this will test for the below side. * @return {@code true} if the point is on the query side of the plane, {@code false} if the point - * is on the opposite side or exactly on the plane. + * is on the opposite side or exactly on the plane. * @see #isPoint3DAboveOrBelowPlane3D(double, double, double, double, double, double, double, - * double, double, boolean) + * double, double, boolean) * @deprecated Use - * {@link #whichSideOfPlane3DIsPoint3DOn(Point3DReadOnly, Point3DReadOnly, Vector3DReadOnly, Vector3DReadOnly)} - * instead. + * {@link #whichSideOfPlane3DIsPoint3DOn(Point3DReadOnly, Point3DReadOnly, Vector3DReadOnly, Vector3DReadOnly)} + * instead. */ @Deprecated public static boolean isPoint3DAboveOrBelowPlane3D(Point3DReadOnly point, @@ -8342,7 +8343,6 @@ public static boolean isPoint3DAboveOrBelowPlane3D(Point3DReadOnly point, { Location side = whichSideOfPlane3DIsPoint3DOn(point, pointOnPlane, planeFirstTangent, planeSecondTangent); return testForAbove ? side == Location.ABOVE : side == Location.BELOW; - } /** @@ -8366,9 +8366,9 @@ public static boolean isPoint3DAboveOrBelowPlane3D(Point3DReadOnly point, * @param planeFirstTangent a first tangent of the infinite plane. Not modified. * @param planeSecondTangent a second tangent of the infinite plane. Not modified. * @return {@code true} if the point is strictly above the plane, {@code false} if the point is - * below or exactly on the plane. + * below or exactly on the plane. * @see #isPoint3DAboveOrBelowPlane3D(double, double, double, double, double, double, double, - * double, double, boolean) + * double, double, boolean) */ public static boolean isPoint3DAbovePlane3D(double pointX, double pointY, @@ -8399,9 +8399,9 @@ public static boolean isPoint3DAbovePlane3D(double pointX, * @param planeFirstTangent a first tangent of the infinite plane. Not modified. * @param planeSecondTangent a second tangent of the infinite plane. Not modified. * @return {@code true} if the point is strictly above the plane, {@code false} if the point is - * below or exactly on the plane. + * below or exactly on the plane. * @see #isPoint3DAboveOrBelowPlane3D(double, double, double, double, double, double, double, - * double, double, boolean) + * double, double, boolean) */ public static boolean isPoint3DAbovePlane3D(Point3DReadOnly point, Point3DReadOnly pointOnPlane, @@ -8432,9 +8432,9 @@ public static boolean isPoint3DAbovePlane3D(Point3DReadOnly point, * @param planeFirstTangent a first tangent of the infinite plane. Not modified. * @param planeSecondTangent a second tangent of the infinite plane. Not modified. * @return {@code true} if the point is strictly below the plane, {@code false} if the point is - * above or exactly on the plane. + * above or exactly on the plane. * @see #isPoint3DAboveOrBelowPlane3D(double, double, double, double, double, double, double, - * double, double, boolean) + * double, double, boolean) */ public static boolean isPoint3DBelowPlane3D(double pointX, double pointY, @@ -8465,9 +8465,9 @@ public static boolean isPoint3DBelowPlane3D(double pointX, * @param planeFirstTangent a first tangent of the infinite plane. Not modified. * @param planeSecondTangent a second tangent of the infinite plane. Not modified. * @return {@code true} if the point is strictly below the plane, {@code false} if the point is - * above or exactly on the plane. + * above or exactly on the plane. * @see #isPoint3DAboveOrBelowPlane3D(double, double, double, double, double, double, double, - * double, double, boolean) + * double, double, boolean) */ public static boolean isPoint3DBelowPlane3D(Point3DReadOnly point, Point3DReadOnly pointOnPlane, @@ -9245,8 +9245,8 @@ public static boolean orthogonalProjectionOnPlane3D(double x, projectionToPack.set(x, y, z); projectionToPack.sub(pointOnPlane); - double signedDistance = projectionToPack.getX() * planeNormal.getX() + projectionToPack.getY() * planeNormal.getY() - + projectionToPack.getZ() * planeNormal.getZ(); + double signedDistance = + projectionToPack.getX() * planeNormal.getX() + projectionToPack.getY() * planeNormal.getY() + projectionToPack.getZ() * planeNormal.getZ(); signedDistance /= normalMagnitude * normalMagnitude; projectionToPack.set(x - signedDistance * planeNormal.getX(), y - signedDistance * planeNormal.getY(), z - signedDistance * planeNormal.getZ()); @@ -9276,7 +9276,7 @@ public static boolean orthogonalProjectionOnPlane3D(double x, * @param lineDirection2x x-component of the second line direction. * @param lineDirection2y y-component of the second line direction. * @return {@code alpha} the percentage along the first line of the intersection location. This - * method returns {@link Double#NaN} if the lines do not intersect. + * method returns {@link Double#NaN} if the lines do not intersect. */ public static double percentageOfIntersectionBetweenTwoLine2Ds(double pointOnLine1x, double pointOnLine1y, @@ -9301,7 +9301,7 @@ public static double percentageOfIntersectionBetweenTwoLine2Ds(double pointOnLin if (Math.abs(determinant) < ONE_TRILLIONTH) { // The lines are parallel - // Check if they are collinear + // Check if they are collinear double cross = dx * lineDirection1y - dy * lineDirection1x; if (Math.abs(cross) < ONE_TRILLIONTH) { @@ -9347,7 +9347,7 @@ public static double percentageOfIntersectionBetweenTwoLine2Ds(double pointOnLin * @param pointOnLine2 a point located on the second line. Not modified. * @param lineDirection2 the second line direction. Not modified. * @return {@code alpha} the percentage along the first line of the intersection location. This - * method returns {@link Double#NaN} if the lines do not intersect. + * method returns {@link Double#NaN} if the lines do not intersect. */ public static double percentageOfIntersectionBetweenTwoLine2Ds(Point2DReadOnly pointOnLine1, Vector2DReadOnly lineDirection1, @@ -9387,7 +9387,7 @@ public static double percentageOfIntersectionBetweenTwoLine2Ds(Point2DReadOnly p * @param pointOnLine a point located on the line. Not modified. * @param lineDirection the line direction. Not modified. * @return {@code alpha} the percentage along the line segment of the intersection location. This - * method returns {@link Double#NaN} if the line segment and the line do not intersect. + * method returns {@link Double#NaN} if the line segment and the line do not intersect. */ public static double percentageOfIntersectionBetweenLineSegment2DAndLine2D(Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd, @@ -9437,7 +9437,7 @@ else if (alpha > 1.0) * @param pointOnLine a point located on the line. Not modified. * @param lineDirection the direction of the line. Not modified. * @return the computed percentage along the line representing where the point projection is - * located. + * located. */ public static double percentageAlongLine2D(Point2DReadOnly point, Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection) { @@ -9464,7 +9464,7 @@ public static double percentageAlongLine2D(Point2DReadOnly point, Point2DReadOnl * @param pointOnLine a point located on the line. Not modified. * @param lineDirection the direction of the line. Not modified. * @return the computed percentage along the line representing where the point projection is - * located. + * located. */ public static double percentageAlongLine2D(double pointX, double pointY, Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection) { @@ -9493,7 +9493,7 @@ public static double percentageAlongLine2D(double pointX, double pointY, Point2D * @param lineDirectionX x-component of the direction of the line. * @param lineDirectionY y-component of the direction of the line. * @return the computed percentage along the line representing where the point projection is - * located. + * located. */ public static double percentageAlongLine2D(double pointX, double pointY, @@ -9543,7 +9543,7 @@ public static double percentageAlongLine2D(double pointX, * @param lineSegmentEndX the x-coordinate of the line segment second endpoint. * @param lineSegmentEndY the y-coordinate of the line segment second endpoint. * @return the computed percentage along the line segment representing where the point projection is - * located. + * located. */ public static double percentageAlongLineSegment2D(double pointX, double pointY, @@ -9593,7 +9593,7 @@ public static double percentageAlongLineSegment2D(double pointX, * @param lineSegmentStart the line segment first endpoint. Not modified. * @param lineSegmentEnd the line segment second endpoint. Not modified. * @return the computed percentage along the line segment representing where the point projection is - * located. + * located. */ public static double percentageAlongLineSegment2D(double pointX, double pointY, Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd) { @@ -9625,7 +9625,7 @@ public static double percentageAlongLineSegment2D(double pointX, double pointY, * @param lineSegmentStart the line segment first endpoint. Not modified. * @param lineSegmentEnd the line segment second endpoint. Not modified. * @return the computed percentage along the line segment representing where the point projection is - * located. + * located. */ public static double percentageAlongLineSegment2D(Point2DReadOnly point, Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd) { @@ -9651,7 +9651,7 @@ public static double percentageAlongLineSegment2D(Point2DReadOnly point, Point2D * @param pointOnLine a point located on the line. Not modified. * @param lineDirection the direction of the line. Not modified. * @return the computed percentage along the line representing where the point projection is - * located. + * located. */ public static double percentageAlongLine3D(Point3DReadOnly point, Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection) { @@ -9687,7 +9687,7 @@ public static double percentageAlongLine3D(Point3DReadOnly point, Point3DReadOnl * @param pointOnLine a point located on the line. Not modified. * @param lineDirection the direction of the line. Not modified. * @return the computed percentage along the line representing where the point projection is - * located. + * located. */ public static double percentageAlongLine3D(double pointX, double pointY, double pointZ, Point3DReadOnly pointOnLine, Vector3DReadOnly lineDirection) { @@ -9727,7 +9727,7 @@ public static double percentageAlongLine3D(double pointX, double pointY, double * @param lineDirectionY y-component of the direction of the line. * @param lineDirectionZ z-component of the direction of the line. * @return the computed percentage along the line representing where the point projection is - * located. + * located. */ public static double percentageAlongLine3D(double pointX, double pointY, @@ -9784,7 +9784,7 @@ public static double percentageAlongLine3D(double pointX, * @param lineSegmentEndY the y-coordinate of the line segment second endpoint. * @param lineSegmentEndZ the z-coordinate of the line segment second endpoint. * @return the computed percentage along the line segment representing where the point projection is - * located. + * located. */ public static double percentageAlongLineSegment3D(double pointX, double pointY, @@ -9840,7 +9840,7 @@ public static double percentageAlongLineSegment3D(double pointX, * @param lineSegmentStart the line segment first endpoint. Not modified. * @param lineSegmentEnd the line segment second endpoint. Not modified. * @return the computed percentage along the line segment representing where the point projection is - * located. + * located. */ public static double percentageAlongLineSegment3D(double pointX, double pointY, @@ -9884,7 +9884,7 @@ public static double percentageAlongLineSegment3D(double pointX, * @param lineSegmentStart the line segment first endpoint. Not modified. * @param lineSegmentEnd the line segment second endpoint. Not modified. * @return the computed percentage along the line segment representing where the point projection is - * located. + * located. */ public static double percentageAlongLineSegment3D(Point3DReadOnly point, Point3DReadOnly lineSegmentStart, Point3DReadOnly lineSegmentEnd) { @@ -10090,7 +10090,7 @@ public static void perpendicularVector2D(Vector2DReadOnly vector, Vector2DBasics * @param orthogonalProjectionToPack a 3D point in which the projection of {@code point} onto the * line is stored. Modified. Can be {@code null}. * @return the vector perpendicular to the line and pointing to the {@code point}, or {@code null} - * when the method fails. + * when the method fails. */ public static Vector3D perpendicularVector3DFromLine3DToPoint3D(Point3DReadOnly point, Point3DReadOnly firstPointOnLine, @@ -10188,7 +10188,7 @@ public static boolean perpendicularVector3DFromLine3DToPoint3D(Point3DReadOnly p * @param cathetusA the length of the cathetus A. * @return the length of the cathetus B. * @throws IllegalArgumentException if the length of the cathetus A is negative or greater than the - * hypotenuse C. + * hypotenuse C. */ public static double pythagorasGetCathetus(double hypotenuseC, double cathetusA) { @@ -10262,7 +10262,7 @@ public static double radiusOfArc(double chordLength, double chordAngle) * @param firstPointOnLine a first point located on the line. Not modified. * @param secondPointOnLine a second point located on the line. Not modified. * @return the minimum distance between the 2D point and the 2D line. The distance is negative if - * the query is located on the right side of the line. + * the query is located on the right side of the line. */ public static double signedDistanceFromPoint2DToLine2D(double pointX, double pointY, Point2DReadOnly firstPointOnLine, Point2DReadOnly secondPointOnLine) { @@ -10292,7 +10292,7 @@ public static double signedDistanceFromPoint2DToLine2D(double pointX, double poi * @param pointOnLine a point located on the line. Not modified. * @param lineDirection the direction of the line. Not modified. * @return the minimum distance between the 2D point and the 2D line. The distance is negative if - * the query is located on the right side of the line. + * the query is located on the right side of the line. */ public static double signedDistanceFromPoint2DToLine2D(double pointX, double pointY, Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection) { @@ -10323,7 +10323,7 @@ public static double signedDistanceFromPoint2DToLine2D(double pointX, double poi * @param firstPointOnLine a first point located on the line. Not modified. * @param secondPointOnLine a second point located on the line. Not modified. * @return the minimum distance between the 2D point and the 2D line. The distance is negative if - * the query is located on the right side of the line. + * the query is located on the right side of the line. */ public static double signedDistanceFromPoint2DToLine2D(Point2DReadOnly point, Point2DReadOnly firstPointOnLine, Point2DReadOnly secondPointOnLine) { @@ -10348,7 +10348,7 @@ public static double signedDistanceFromPoint2DToLine2D(Point2DReadOnly point, Po * @param pointOnLine a point located on the line. Not modified. * @param lineDirection the direction of the line. Not modified. * @return the minimum distance between the 2D point and the 2D line. The distance is negative if - * the query is located on the right side of the line. + * the query is located on the right side of the line. */ public static double signedDistanceFromPoint2DToLine2D(Point2DReadOnly point, Point2DReadOnly pointOnLine, Vector2DReadOnly lineDirection) { @@ -10376,7 +10376,7 @@ public static double signedDistanceFromPoint2DToLine2D(Point2DReadOnly point, Po * @param lineDirectionX x-component of the line direction. * @param lineDirectionY y-component of the line direction. * @return the minimum distance between the 2D point and the 2D line. The distance is negative if - * the query is located on the right side of the line. + * the query is located on the right side of the line. */ public static double signedDistanceFromPoint2DToLine2D(double pointX, double pointY, @@ -10631,8 +10631,8 @@ public static double triangleCircumradius(double lengthA, double lengthB, double public static double triangleIsoscelesHeight(double legLength, double baseLength) { if (legLength < 0.5 * baseLength) - throw new IllegalArgumentException("Malformed isosceles triangle, expected legLength > baseLength/2, was legLength: " + legLength + ", baseLength/2: " - + 0.5 * baseLength); + throw new IllegalArgumentException( + "Malformed isosceles triangle, expected legLength > baseLength/2, was legLength: " + legLength + ", baseLength/2: " + 0.5 * baseLength); if (baseLength < 0.0) throw new IllegalArgumentException("The base cannot have a negative length, baseLength = " + baseLength); @@ -10650,14 +10650,15 @@ public static double triangleIsoscelesHeight(double legLength, double baseLength * @param lengthOppositeSideC the length of the side C. * @return the value in radians of the unknown angle. * @throws IllegalArgumentException if the lengths do not describe a triangle, see - * {@link #isFormingTriangle(double, double, double)}. + * {@link #isFormingTriangle(double, double, double)}. */ public static double unknownTriangleAngleByLawOfCosine(double lengthNeighbourSideA, double lengthNeighbourSideB, double lengthOppositeSideC) { if (!isFormingTriangle(lengthNeighbourSideA, lengthNeighbourSideB, lengthOppositeSideC)) { - throw new IllegalArgumentException("Unable to build a Triangle of the given triangle sides a: " + lengthNeighbourSideA + " b: " + lengthNeighbourSideB - + " c: " + lengthOppositeSideC); + throw new IllegalArgumentException( + "Unable to build a Triangle of the given triangle sides a: " + lengthNeighbourSideA + " b: " + lengthNeighbourSideB + " c: " + + lengthOppositeSideC); } double numerator = lengthNeighbourSideA * lengthNeighbourSideA + lengthNeighbourSideB * lengthNeighbourSideB - lengthOppositeSideC * lengthOppositeSideC; @@ -10681,7 +10682,7 @@ public static double unknownTriangleAngleByLawOfCosine(double lengthNeighbourSid * @param angleBetweenAAndB the angle between the sides A and B. * @return the value of the unknown side length. * @throws IllegalArgumentException if {@code lengthSideA} and/or {@code lengthSideB} are negative, - * if {@code angleBetweenAAndB} is greater than pi. + * if {@code angleBetweenAAndB} is greater than pi. */ public static double unknownTriangleSideLengthByLawOfCosine(double lengthSideA, double lengthSideB, double angleBetweenAAndB) { @@ -10692,7 +10693,1006 @@ public static double unknownTriangleSideLengthByLawOfCosine(double lengthSideA, if (Math.abs(angleBetweenAAndB) > Math.PI) throw new IllegalArgumentException("angleBetweenAAndB " + angleBetweenAAndB + " does not define a triangle."); - return EuclidCoreTools.squareRoot(lengthSideA * lengthSideA + lengthSideB * lengthSideB - - 2.0 * lengthSideA * lengthSideB * EuclidCoreTools.cos(angleBetweenAAndB)); + return EuclidCoreTools.squareRoot( + lengthSideA * lengthSideA + lengthSideB * lengthSideB - 2.0 * lengthSideA * lengthSideB * EuclidCoreTools.cos(angleBetweenAAndB)); + } + + /** + * Tests if the point 2D is located on the infinitely long line 2D. + *+ * The test is performed by computing the distance between the point and the line, if that distance + * is below {@link EuclidGeometryTools#IS_POINT_ON_LINE_EPS} this method returns {@code true}. + *
+ * + * @param lineSegmentStart the first endpoint of the line segment. Not modified. + * @param lineSegmentEnd the second endpoint of the line segment. Not modified. + * @return {@code true} if the query is considered to be lying on the line, {@code false} otherwise. + */ + public static boolean isPoint2DOnLineSegment2D(double pointX, double pointY, Point2DReadOnly lineSegmentStart, Point2DReadOnly lineSegmentEnd) + { + return distanceSquaredFromPoint2DToLineSegment2D(pointX, + pointY, + lineSegmentStart.getX(), + lineSegmentStart.getY(), + lineSegmentEnd.getX(), + lineSegmentEnd.getY()) < IS_POINT_ON_LINE_EPS; + } + + /** + * This method implements the same operation as + * {@link EuclidGeometryTools#orientation3DFromFirstToSecondVector3D(double, double, double, double, double, double, Orientation3DBasics)} + * except that it does not rely on {@code Math#acos(double)} making it faster. + * + * @param firstVectorX x-component of the first vector. + * @param firstVectorY y-component of the first vector. + * @param firstVectorZ z-component of the first vector. + * @param secondVectorX x-component of the second vector that is rotated with respect to the first + * vector. + * @param secondVectorY y-component of the second vector that is rotated with respect to the first + * vector. + * @param secondVectorZ z-component of the second vector that is rotated with respect to the first + * vector. + * @param rotationToPack the minimum rotation from {@code firstVector} to the {@code secondVector}. + * Modified. + * @see EuclidGeometryTools#orientation3DFromFirstToSecondVector3D(double, double, double, double, + * double, double, Orientation3DBasics) + */ + public static void rotationMatrix3DFromFirstToSecondVector3D(double firstVectorX, + double firstVectorY, + double firstVectorZ, + double secondVectorX, + double secondVectorY, + double secondVectorZ, + CommonMatrix3DBasics rotationToPack) + { + double firstVectorLengthInv = 1.0 / Math.sqrt(normSquared(firstVectorX, firstVectorY, firstVectorZ)); + double secondVectorLengthInv = 1.0 / Math.sqrt(normSquared(secondVectorX, secondVectorY, secondVectorZ)); + firstVectorX *= firstVectorLengthInv; + firstVectorY *= firstVectorLengthInv; + firstVectorZ *= firstVectorLengthInv; + secondVectorX *= secondVectorLengthInv; + secondVectorY *= secondVectorLengthInv; + secondVectorZ *= secondVectorLengthInv; + + double rotationAxisX = firstVectorY * secondVectorZ - firstVectorZ * secondVectorY; + double rotationAxisY = firstVectorZ * secondVectorX - firstVectorX * secondVectorZ; + double rotationAxisZ = firstVectorX * secondVectorY - firstVectorY * secondVectorX; + double sinAngle = Math.sqrt(normSquared(rotationAxisX, rotationAxisY, rotationAxisZ)); + + boolean normalsAreParallel = sinAngle < ONE_TEN_MILLIONTH; + + if (normalsAreParallel) + { + rotationToPack.setIdentity(); + return; + } + + rotationAxisX /= sinAngle; + rotationAxisY /= sinAngle; + rotationAxisZ /= sinAngle; + + double cosAngle = firstVectorX * secondVectorX + firstVectorY * secondVectorY + firstVectorZ * secondVectorZ; + + if (cosAngle > 1.0) + cosAngle = 1.0; + else if (cosAngle < -1.0) + cosAngle = -1.0; + + double t = 1.0 - cosAngle; + + double xz = rotationAxisX * rotationAxisZ; + double xy = rotationAxisX * rotationAxisY; + double yz = rotationAxisY * rotationAxisZ; + + double m00 = t * rotationAxisX * rotationAxisX + cosAngle; + double m01 = t * xy - sinAngle * rotationAxisZ; + double m02 = t * xz + sinAngle * rotationAxisY; + double m10 = t * xy + sinAngle * rotationAxisZ; + double m11 = t * rotationAxisY * rotationAxisY + cosAngle; + double m12 = t * yz - sinAngle * rotationAxisX; + double m20 = t * xz - sinAngle * rotationAxisY; + double m21 = t * yz + sinAngle * rotationAxisX; + double m22 = t * rotationAxisZ * rotationAxisZ + cosAngle; + + if (rotationToPack instanceof RotationMatrix) + ((RotationMatrix) rotationToPack).setUnsafe(m00, m01, m02, m10, m11, m12, m20, m21, m22); + else + rotationToPack.set(m00, m01, m02, m10, m11, m12, m20, m21, m22); + } + + /** + * This method implements the same operation as + * {@link EuclidGeometryTools#orientation3DFromFirstToSecondVector3D(Vector3DReadOnly, Vector3DReadOnly, Orientation3DBasics)} + * except that it does not rely on {@code Math#acos(double)} making it faster. + * + * @param firstVector the first vector. Not modified. + * @param secondVector the second vector that is rotated with respect to the first vector. Not + * modified. + * @param rotationToPack the minimum rotation from {@code firstVector} to the {@code secondVector}. + * Modified. + * @see EuclidGeometryTools#orientation3DFromFirstToSecondVector3D(Vector3DReadOnly, + * Vector3DReadOnly, Orientation3DBasics) + */ + public static void rotationMatrix3DFromFirstToSecondVector3D(Vector3DReadOnly firstVector, + Vector3DReadOnly secondVector, + CommonMatrix3DBasics rotationToPack) + { + rotationMatrix3DFromFirstToSecondVector3D(firstVector.getX(), + firstVector.getY(), + firstVector.getZ(), + secondVector.getX(), + secondVector.getY(), + secondVector.getZ(), + rotationToPack); + } + + /** + * This method implements the same operation as + * {@link EuclidGeometryTools#orientation3DFromZUpToVector3D(Vector3DReadOnly, Orientation3DBasics)} + * except that it does not rely on {@code Math#acos(double)} making it faster. + * + * @param vector the vector that is rotated with respect to {@code zUp}. Not modified. + * @param vector the vector that is rotated with respect to {@code zUp}. Not modified. + * @param rotationToPack the minimum rotation from {@code zUp} to the given {@code vector}. + * Modified. + * @see EuclidGeometryTools#orientation3DFromZUpToVector3D(Vector3DReadOnly, Orientation3DBasics) + */ + public static void rotationMatrix3DFromZUpToVector3D(Vector3DReadOnly vector, CommonMatrix3DBasics rotationToPack) + { + rotationMatrix3DFromFirstToSecondVector3D(Axis3D.Z, vector, rotationToPack); + } + + /** + * Returns the square of the minimum distance between a point and a given line segment, and packs the closest point on the line segment in + * {@code pointOnSegmentToPack} + *+ * Edge cases: + *
+ * Edge cases: + *
+ * Edge cases: + *
+ * Edge cases: + *
+ * Edge cases: + *
+ * Edge cases: + *
+ * Edge cases: + *
+ * Edge cases: + *
+ * Edge cases: + *
+ * Edge cases: + *
+ * Edge cases: + *
+ * WARNING: generates garbage + *
+ * Allocates a new BoundingBox2D. TODO: Check, Unit test, move where BoundingBox union is + * + * @param a first bounding box to check + * @param b second bounding box to check + * @return the intersection bounding box, or null if no intersection + */ + public static BoundingBox2D computeIntersectionOfTwoBoundingBoxes(BoundingBox2DReadOnly a, BoundingBox2DReadOnly b) + { + double maxX = Math.min(a.getMaxX(), b.getMaxX()); + double maxY = Math.min(a.getMaxY(), b.getMaxY()); + double minX = Math.max(a.getMinX(), b.getMinX()); + double minY = Math.max(a.getMinY(), b.getMinY()); + + if ((maxX <= minX) || (maxY <= minY)) + return null; + + return new BoundingBox2D(minX, minY, maxX, maxY); + } + + /** + * Finds the intersection of two bounding boxes defined by a bounding box. + *
+ * WARNING: generates garbage + *
+ * Allocates a new boundingBox3D. + * + * @param a first bounding box to check + * @param b second bounding box to check + * @return the intersection bounding box, or null if no intersection + */ + public static BoundingBox3D computeIntersectionOfTwoBoundingBoxes(BoundingBox3DReadOnly a, BoundingBox3DReadOnly b) + { + double maxX = Math.min(a.getMaxX(), b.getMaxX()); + double maxY = Math.min(a.getMaxY(), b.getMaxY()); + double maxZ = Math.min(a.getMaxZ(), b.getMaxZ()); + + double minX = Math.max(a.getMinX(), b.getMinX()); + double minY = Math.max(a.getMinY(), b.getMinY()); + double minZ = Math.max(a.getMinZ(), b.getMinZ()); + + if ((maxX <= minX) || (maxY <= minY) || (maxZ <= minZ)) + return null; + + return new BoundingBox3D(minX, minY, minZ, maxX, maxY, maxZ); + } + + /** + * Computes the volume of the bounding box. + * + * @param boundingBox bounding box to inspect + * @return total volume of the bounding box. + */ + public static double computeBoundingBoxVolume3D(BoundingBox3DReadOnly boundingBox) + { + return Math.abs(boundingBox.getMaxX() - boundingBox.getMinX()) * Math.abs(boundingBox.getMaxY() - boundingBox.getMinY()) * Math.abs( + boundingBox.getMaxZ() - boundingBox.getMinZ()); + } + + /** + * Inspects whether a 2d line segment ever intersects with a 2d convex polygon. + * + * @param lineSegmentStart starting point of the line segment. + * @param lineSegmentEnd ending point of the line segment. + * @param convexPolygonVertices list of points that compose the convex polygon. + * @param numberOfVertices number of vertices contained in the convex polygon. + * @return whether the line segment intersects with the polygon. + */ + public static boolean doLineSegment2DAndConvexPolygon2DIntersect(Point2DReadOnly lineSegmentStart, + Point2DReadOnly lineSegmentEnd, + List convexPolygonVertices, + int numberOfVertices) + { + checkNumberOfVertices(convexPolygonVertices, numberOfVertices); + + if (numberOfVertices == 0) + return false; + + if (numberOfVertices == 1) + return isPoint2DOnLineSegment2D(convexPolygonVertices.get(0), lineSegmentStart, lineSegmentEnd); + + if (numberOfVertices == 2) + return doLineSegment2DsIntersect(convexPolygonVertices.get(0), convexPolygonVertices.get(1), lineSegmentStart, lineSegmentEnd); + + for (int edgeIndex = 0; edgeIndex < numberOfVertices; edgeIndex++) + { + Point2DReadOnly edgeStart = convexPolygonVertices.get(edgeIndex); + Point2DReadOnly edgeEnd = convexPolygonVertices.get(EuclidGeometryPolygonTools.next(edgeIndex, numberOfVertices)); + + if (doLineSegment2DsIntersect(edgeStart, edgeEnd, lineSegmentStart, lineSegmentEnd)) + return true; + } + + return false; + } + + private static void checkNumberOfVertices(List convexPolygon2D, int numberOfVertices) + { + if (numberOfVertices < 0 || numberOfVertices > convexPolygon2D.size()) + throw new IllegalArgumentException("Illegal numberOfVertices: " + numberOfVertices + ", expected a value in ] 0, " + convexPolygon2D.size() + "]."); + } + + /** + * Compute Intersection-over-Union (IoU) of two 3D bounding boxes. This is the percentage of volume of the two bounding boxes that are intersecting. + *
+ * WARNING: generates garbage. + */ + public static double computeIntersectionOverUnionOfTwoBoundingBoxes(BoundingBox3DReadOnly a, BoundingBox3DReadOnly b) + { + BoundingBox3D intersection = computeIntersectionOfTwoBoundingBoxes(a, b); + + if (intersection == null) + return 0.0; + + double intersectionVolume = computeBoundingBoxVolume3D(intersection); + double volumeA = computeBoundingBoxVolume3D(a); + double volumeB = computeBoundingBoxVolume3D(b); + double unionVolume = volumeA + volumeB - intersectionVolume; + + return intersectionVolume / unionVolume; + } + + /** + * Compute Intersection-over-Union (IoU) of two 3D bounding boxes. This is the percentage of volume of the two bounding boxes that are intersecting. + *
+ * WARNING: generates garbage. + */ + public static double computeIntersectionOverSmallerOfTwoBoundingBoxes(BoundingBox3DReadOnly a, BoundingBox3DReadOnly b) + { + BoundingBox3D intersection = computeIntersectionOfTwoBoundingBoxes(a, b); + + if (intersection == null) + return 0.0; + + double intersectionVolume = computeBoundingBoxVolume3D(intersection); + double volumeA = computeBoundingBoxVolume3D(a); + double volumeB = computeBoundingBoxVolume3D(b); + double smallerVolume = Math.min(volumeA, volumeB); + + return intersectionVolume / smallerVolume; + } + + /** + * Determines if the polygonToTest is fully inside the convex polygon by some epsilon distance. + * + * @return {@code true} if {@code polygonToTest} is fully contained inside {@code polygon}. {@code false} otherwise. + */ + public static boolean isPolygonInside(ConvexPolygon2DReadOnly polygonToTest, double epsilon, ConvexPolygon2DReadOnly polygon) + { + for (int i = 0; i < polygonToTest.getNumberOfVertices(); i++) + { + if (!polygon.isPointInside(polygonToTest.getVertex(i), epsilon)) + return false; + } + + return true; + } + + /** + * Determines if the polygonToTest is inside the convex polygon. + * + * @return {@code true} if {@code polygonToTest} is fully contained inside {@code polygon}. {@code false} otherwise. + */ + public static boolean isPolygonInside(ConvexPolygon2DReadOnly polygonToTest, ConvexPolygon2DReadOnly polygon) + { + return isPolygonInside(polygonToTest, 0.0, polygon); + } + + /** + * Finds the projection of a 3D point onto a 3D plane given in general form. + * Uses: projectedPoint = point - (normal.dot(point) + planeScalar) * (normal) + *
+ * WARNING: generates garbage. + * + * @param plane Coefficients of the general form of plane equation (ax + by + cz + d = 0) as Vector4D + * @param point Point to be projected onto the plane as Point3D + * @return Projected point onto the plane as Point3D + */ + public static Point3D projectPointOntoPlane(Vector4DReadOnly plane, Point3DReadOnly point) + { + UnitVector3D planeNormal = new UnitVector3D(plane.getX(), plane.getY(), plane.getZ()); + + Vector3D scaledNormal = new Vector3D(planeNormal); + scaledNormal.scale(planeNormal.dot(point) + plane.getS()); + + Point3D projectedPoint = new Point3D(); + projectedPoint.sub(point, scaledNormal); + + return projectedPoint; } } diff --git a/src/main/java/us/ihmc/euclid/QuaternionCalculus.java b/src/main/java/us/ihmc/euclid/QuaternionCalculus.java new file mode 100644 index 000000000..31b6196c1 --- /dev/null +++ b/src/main/java/us/ihmc/euclid/QuaternionCalculus.java @@ -0,0 +1,500 @@ +package us.ihmc.euclid; + +import us.ihmc.euclid.axisAngle.AxisAngle; +import us.ihmc.euclid.rotationConversion.QuaternionConversion; +import us.ihmc.euclid.tools.EuclidCoreTools; +import us.ihmc.euclid.tools.QuaternionTools; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics; +import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; +import us.ihmc.euclid.tuple4D.Quaternion; +import us.ihmc.euclid.tuple4D.Vector4D; +import us.ihmc.euclid.tuple4D.interfaces.QuaternionBasics; +import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; +import us.ihmc.euclid.tuple4D.interfaces.Vector4DBasics; +import us.ihmc.euclid.tuple4D.interfaces.Vector4DReadOnly; + +/** + * This class contains a number of tools related to computing dynamics of quaternions. Each method is guaranteed to be free of allocations, which requires + * instantiation for some methods. + * + *
+ * Specifically, it provides tools for exponential + * ({@link #exp(Vector3DReadOnly, QuaternionBasics)}) and logarithmic maps of quaternions, {@link #log(QuaternionReadOnly, Vector3DBasics)}. + *
+ * + *+ * Importantly, it also provides the interfaces for the ability to perform spherical linear interpolation, which is smooth interpolation on the + * SO3 manifold. This is commonly referred to as the SLERP algorithm, and can be performed calling + * {@link #interpolate(double, QuaternionReadOnly, QuaternionReadOnly, QuaternionBasics)}. + * Additional tools are provided to perform SLERP interpolation guaranteeing interpolation along the minimal path, with + * {@link #interpolate(double, QuaternionReadOnly, QuaternionReadOnly, QuaternionBasics, boolean)}. + *
+ * + *+ * This class also provides tools for mapping from and to angular velocities and accelerations and quaternion rates and accelerations. These can be done in + * both + * the local frame defined by the orientation, or the parent frame. These can be accessed using + * {@link #computeQDotInParentFrame(QuaternionReadOnly, Vector3DReadOnly, Vector4DBasics)}, + * {@link #computeQDotInRotatedFrame(QuaternionReadOnly, Vector3DReadOnly, Vector4DBasics)}, + * and various other methods. + *
+ *+ * Lastly, this class provides tools for estimating quaternion rates and accelerations via finite differences, provided various quaternion samples. These are + * useful for testing quaternion changes from generating trajectories, but can be used for a variety of other things. These can be accessed by + * {@link #computeQDotByFiniteDifferenceCentral(QuaternionReadOnly, QuaternionReadOnly, double, Vector4DBasics)} and + * {@link #computeQDDotByFiniteDifferenceCentral(QuaternionReadOnly, QuaternionReadOnly, QuaternionReadOnly, double, Vector4DBasics)}. + *
+ */ +public class QuaternionCalculus +{ + private final AxisAngle axisAngleForLog = new AxisAngle(); + private final Quaternion tempQ1ForInterpolation = new Quaternion(); + private final AxisAngle axisAngleForPow = new AxisAngle(); + private final Quaternion qConj = new Quaternion(); + + private final Vector4D intermediateQDot = new Vector4D(); + private final Vector4D qDotConj = new Vector4D(); + private final Vector3D intermediateAngularAcceleration = new Vector3D(); + private final Vector3D intermediateAngularVelocity = new Vector3D(); + private final Vector4D intermediateQDDot = new Vector4D(); + private final Vector4D pureQuatForMultiply = new Vector4D(); + + public QuaternionCalculus() + { + } + + /** + * This computes the logarithm of the quaternion a stores it in {@code resultToPack}, which is a pure quaternion. + * This is the equation: + *resultToPack = log(q)
+ * where q is a {@link QuaternionReadOnly}. + * + * @param q is a unit quaternion and describes a orientation. Not modified. + * @param resultToPack is used to store the result and is a pure quaternion (w = 0.0). Modified. + */ + public void log(QuaternionReadOnly q, Vector4DBasics resultToPack) + { + axisAngleForLog.set(q); + resultToPack.set(axisAngleForLog.getX(), axisAngleForLog.getY(), axisAngleForLog.getZ(), 0.0); + resultToPack.scale(axisAngleForLog.getAngle()); + } + + /** + * This computes the logarithm of the quaternion a stores it in {@code resultToPack}, which is a vector. + * This is the equation: + *resultToPack = log(q)
+ * where q is a {@link QuaternionReadOnly}. + * + * @param q is a unit quaternion and describes a orientation. Not modified. + * @param resultToPack is used to store the result. Modified. + */ + public void log(QuaternionReadOnly q, Vector3DBasics resultToPack) + { + axisAngleForLog.set(q); + resultToPack.set(axisAngleForLog.getX(), axisAngleForLog.getY(), axisAngleForLog.getZ()); + resultToPack.scale(axisAngleForLog.getAngle()); + } + + /** + * Applies the exponential transformation to the rotation vector. This is the same as computing the equivalent quaternion from a rotation vector. + * + * @param rotationVector rotation of which to compute the exponential. Not modified. + * @param quaternionToPack is used to store the result. Modified. + */ + public static void exp(Vector3DReadOnly rotationVector, QuaternionBasics quaternionToPack) + { + QuaternionConversion.convertRotationVectorToQuaternion(rotationVector, quaternionToPack); + } + + /** + * Performs a spherical linear interpolation (SLERP) from a quaternion between the start {@code q0} and end {@code q1} for a given alpha = [0, 1] using + * SLERP. The results are stored in {@code qInterpolatedToPack}. This effectively applies a constant angular velocity to go from {@code q0} to {@code q1}. + *+ * At {@code alpha = 0}, the output matches {@code q0}. At {@code alpha = 1}, the output matches {@code q1}. {@code alpha} is not constrained to be between + * [0, 1] + *
+ * SLERP Descriptions. + * Computes: resultToPack = q0 (q0-1 q1)alpha. + * + * @param alpha fraction to interpolate between {@code q0} and {@code q1}. Should be between [0, 1]. + * @param q0 quaternion value at the beginning of the interpolation (when {@code alpha} = 0). Not Modified. + * @param q1 quaternion value at the end of the interpolation (when {@code alpha} = 1). Not Modified. + * @param qInterpolatedToPack interpolated quaternion to store the results. Modified. + */ + public void interpolate(double alpha, QuaternionReadOnly q0, QuaternionReadOnly q1, QuaternionBasics qInterpolatedToPack) + { + interpolate(alpha, q0, q1, qInterpolatedToPack, true); + } + + /** + * Performs a spherical linear interpolation (SLERP) from a quaternion between the start {@code q0} and end {@code q1} for a given alpha = [0, 1] using + * SLERP. The results are stored in {@code qInterpolatedToPack}. This effectively applies a constant angular velocity to go from {@code q0} to {@code q1}. + *+ * At {@code alpha = 0}, the output matches {@code q0}. At {@code alpha = 1}, the output matches {@code q1}. {@code alpha} is not constrained to be between + * [0, 1] + *
+ *+ * When {@code preventExtraSpin} is true, this check ensures that the interpolation goes the minimum distance along the shortest path. As it's a spherical + * interpolation, {@code q0} can rotate either direction to reach {@code q1}. When {@code preventExtraSpin} is true, the math checks to make that the + * rotation + * from {@code q0} to {@code q1} is less than {@link Math#PI}. If it is greater than this value, then it negates {@code q1} to reverse the directionality + * of the rotation, ensuring that the interpolation is along the shortest path. + *
+ * SLERP Descriptions. + * Computes: resultToPack = q0 (q0-1 q1)alpha. + * + * @param alpha fraction to interpolate between {@code q0} and {@code q1}. Should be between [0, 1]. + * @param q0 quaternion value at the beginning of the interpolation (when {@code alpha} = 0). Not Modified. + * @param q1 quaternion value at the end of the interpolation (when {@code alpha} = 1). Not Modified. + * @param qInterpolatedToPack interpolated quaternion to store the results. Modified. + */ + public void interpolate(double alpha, QuaternionReadOnly q0, QuaternionReadOnly q1, QuaternionBasics qInterpolatedToPack, boolean preventExtraSpin) + { + tempQ1ForInterpolation.set(q1); + + // Checks if the interpolation is taking the shortest path. + if (preventExtraSpin && q0.dot(tempQ1ForInterpolation) < 0.0) + { + tempQ1ForInterpolation.negate(); + } + + qInterpolatedToPack.difference(q0, tempQ1ForInterpolation); + pow(qInterpolatedToPack, alpha, qInterpolatedToPack); + qInterpolatedToPack.multiply(q0, qInterpolatedToPack); + } + + /** + * This computes: resultToPack = q^power. + * + * @param q is a unit quaternion and describes an orientation. Not modifeid. + * @param power exponential to apply to the quaternion. + * @param resultToPack is used to store the result. Modified. + */ + public void pow(QuaternionReadOnly q, double power, QuaternionBasics resultToPack) + { + axisAngleForPow.set(q); + axisAngleForPow.setAngle(power * axisAngleForPow.getAngle()); + resultToPack.set(axisAngleForPow); + } + + /** + * This computes the angular velocity expressed in the rotation frame of a body from the rotation of the body expressed as a quaternion and its quaternion + * rate. + * + *+ * The rotation from the parent frame, Fparent to the current frame Fchild is defined by a quaternion {@code q}. This quaternion has a + * corresponding velocity, which is defined by the rate of change of its quaternion coordinates, {@code qDot}. + *
+ *+ * This method computes the local 3-dimensional angular velocity, expressed in the child frame Fchild, and stores it in + * {@code angularVelocityToPack}. + *
+ * + * @param q quaternion defining the rotation to the local frame. Not modified. + * @param qDot quaternion rate of change of {@code q}. Not modified. + * @param angularVelocityToPack angular velocity in the local frame. Modified. + */ + public void computeAngularVelocityInRotatedFrame(QuaternionReadOnly q, Vector4DReadOnly qDot, Vector3DBasics angularVelocityToPack) + { + qConj.setAndConjugate(q); + multiply(qConj, qDot, angularVelocityToPack); + angularVelocityToPack.scale(2.0); + } + + /** + * This computes the angular velocity expressed in the parent frame of a body from the rotation of the body expressed as a quaternion and its quaternion + * rate. + * + *+ * The rotation from the parent frame, Fparent to the current frame Fchild is defined by a quaternion {@code q}. This quaternion has a + * corresponding velocity, which is defined by the rate of change of its quaternion coordinates, {@code qDot}. + *
+ *+ * This method computes the 3-dimensional angular velocity, expressed in the parent frame Fparent, and stores it in + * {@code angularVelocityToPack}. + *
+ * + * @param q quaternion defining the rotation to the local frame. Not modified. + * @param qDot quaternion rate of change of {@code q}. Not modified. + * @param angularVelocityToPack angular velocity in the parent frame. Modified. + */ + public void computeAngularVelocityInParentFrame(QuaternionReadOnly q, Vector4DReadOnly qDot, Vector3DBasics angularVelocityToPack) + { + qConj.setAndConjugate(q); + multiply(qDot, qConj, angularVelocityToPack); + angularVelocityToPack.scale(2.0); + } + + /** + * This computes the quaternion rate from the rotation of a body expressed as a quaternion and its angular velocity expressed in the frame of its parent. + * + *+ * The rotation from the parent frame, Fparent to the current frame Fchild is defined by a quaternion {@code q}. This quaternion has a + * corresponding velocity, which is defined by the rate of change of its quaternion coordinates, {@code qDot}. + *
+ *+ * This method computes the quaternion rate, and stores it in {@code qDotToPack} + *
+ * + * @param q quaternion defining the rotation to the local frame. Not Modified. + * @param angularVelocityInParent angular velocity in the parent frame. Not Modified. + * @param qDotToPack quaternion rate of change of {@code q} to compute. Modified. + */ + public static void computeQDotInParentFrame(QuaternionReadOnly q, Vector3DReadOnly angularVelocityInParent, Vector4DBasics qDotToPack) + { + multiply(angularVelocityInParent, q, qDotToPack); + qDotToPack.scale(0.5); + } + + /** + * This computes the quaternion rate from the rotation of a body expressed as a quaternion and its angular velocity expressed in the rotated frame. + * + *+ * The rotation from the parent frame, Fparent to the current frame Fchild is defined by a quaternion {@code q}. This quaternion has a + * corresponding velocity, which is defined by the rate of change of its quaternion coordinates, {@code qDot}. + *
+ *+ * This method computes the quaternion rate, and stores it in {@code qDotToPack} + *
+ * + * @param q quaternion defining the rotation to the local frame. Not Modified. + * @param angularVelocityInRotated angular velocity in the rotated frame. Not Modified. + * @param qDotToPack quaternion rate of change of {@code q} to compute. Modified. + */ + public static void computeQDotInRotatedFrame(QuaternionReadOnly q, Vector3DReadOnly angularVelocityInRotated, Vector4DBasics qDotToPack) + { + multiply(q, angularVelocityInRotated, qDotToPack); + qDotToPack.scale(0.5); + } + + /** + * This computes the quaternion acceleration from the rotation of a body expressed as a quaternion, its quaternion rate, and its angular acceleration + * expressed in the frame of its parent. + * + *+ * The rotation from the parent frame, Fparent to the current frame Fchild is defined by a quaternion {@code q}. This quaternion has a + * corresponding velocity, which is defined by the rate of change of its quaternion coordinates, {@code qDot}, and acceleratoin, which is defined by the + * acceleration of its quaternion coordinates, {@code qDDot}. + *
+ *+ * This method computes the quaternion acceleration, and stores it in {@code qDDotToPack} + *
+ * + * @param q quaternion defining the rotation to the local frame. Not Modified. + * @param qDot quaternion rate of change of {@code q}. Not modified. + * @param angularAccelerationInParent angular acceleration in the parent frame. Not Modified. + * @param qDDotToPack quaternion acceleration of {@code q} to compute. Not modified. + */ + public void computeQDDotInParentFrame(QuaternionReadOnly q, Vector4DReadOnly qDot, Vector3DReadOnly angularAccelerationInParent, Vector4DBasics qDDotToPack) + { + computeAngularVelocityInParentFrame(q, qDot, intermediateAngularVelocity); + computeQDDotInParentFrame(q, qDot, intermediateAngularVelocity, angularAccelerationInParent, qDDotToPack); + } + + /** + * This computes the quaternion acceleration from the rotation of a body expressed as a quaternion and its angular velocity and acceleration + * expressed in the frame of its parent. + * + *+ * The rotation from the parent frame, Fparent to the current frame Fchild is defined by a quaternion {@code q}. This quaternion has a + * corresponding velocity, which is defined by the rate of change of its quaternion coordinates, {@code qDot}, and acceleratoin, which is defined by the + * acceleration of its quaternion coordinates, {@code qDDot}. + *
+ *+ * This method computes the quaternion acceleration, and stores it in {@code qDDotToPack} + *
+ * + * @param q quaternion defining the rotation to the local frame. Not Modified. + * @param angularVelocityInParent angular velocity in the parent frame. Not modified. + * @param angularAccelerationInParent angular acceleration in the parent frame. Not Modified. + * @param qDDotToPack quaternion acceleration of {@code q} to compute. Not modified. + */ + public void computeQDDotInParentFrame(QuaternionReadOnly q, + Vector3DReadOnly angularVelocityInParent, + Vector3DReadOnly angularAccelerationInParent, + Vector4DBasics qDDotToPack) + { + computeQDotInParentFrame(q, angularVelocityInParent, intermediateQDot); + computeQDDotInParentFrame(q, intermediateQDot, angularVelocityInParent, angularAccelerationInParent, qDDotToPack); + } + + /** + * This computes the angular acceleration expressed in the parent frame of a body from the rotation of the body expressed as a quaternion and its quaternion + * rate and acceleration. + * + *+ * The rotation from the parent frame, Fparent to the current frame Fchild is defined by a quaternion {@code q}. This quaternion has a + * corresponding velocity, which is defined by the rate of change and acceleration of its quaternion coordinates, {@code qDot} and {@code qDDot}. + *
+ *+ * This method computes the 3-dimensional angular acceleration, expressed in the parent frame Fparent, and stores it in + * {@code angularAccelerationToPack}. + *
+ * + * @param q quaternion defining the rotation to the local frame. Not modified. + * @param qDDot quaternion acceleration of {@code q}. Not modified. + * @param angularVelocityInParent angular velocity in the parent frame. Not modified. + * @param angularAccelerationInParentToPack angular acceleration in the parent frame. Modified. + */ + public void computeAngularAccelerationInParentFrame(QuaternionReadOnly q, + Vector4DReadOnly qDDot, + Vector3DReadOnly angularVelocityInParent, + Vector3DBasics angularAccelerationInParentToPack) + { + computeQDotInParentFrame(q, angularVelocityInParent, intermediateQDot); + computeAngularAccelerationInRotatedFrame(q, intermediateQDot, qDDot, angularAccelerationInParentToPack); + } + + /** + * This computes the angular acceleration expressed in the child frame of a body from the rotation of the body expressed as a quaternion and its quaternion + * rate and acceleration. + * + *+ * The rotation from the parent frame, Fparent to the current frame Fchild is defined by a quaternion {@code q}. This quaternion has a + * corresponding velocity, which is defined by the rate of change and acceleration of its quaternion coordinates, {@code qDot} and {@code qDDot}. + *
+ *+ * This method computes the 3-dimensional angular acceleration, expressed in the parent frame Fparent, and stores it in + * {@code angularAccelerationToPack}. + *
+ * + * @param q quaternion defining the rotation to the local frame. Not modified. + * @param qDot quaternion rate of change of {@code q}. Not modified. + * @param qDDot quaternion acceleration of {@code q}. Not modified. + * @param angularAccelerationInRotatedToPack angular acceleration in the child frame. Modified. + */ + public void computeAngularAccelerationInRotatedFrame(QuaternionReadOnly q, + Vector4DReadOnly qDot, + Vector4DReadOnly qDDot, + Vector3DBasics angularAccelerationInRotatedToPack) + { + qConj.setAndConjugate(q); + qDotConj.set(-qDot.getX(), -qDot.getY(), -qDot.getZ(), qDot.getS()); + multiply(qDot, qDotConj, intermediateAngularAcceleration); + multiply(qDDot, qConj, angularAccelerationInRotatedToPack); + angularAccelerationInRotatedToPack.add(intermediateAngularAcceleration); + angularAccelerationInRotatedToPack.scale(2.0); + } + + /** + * This is used to estimate the rate of change of the quaternion coordinates of a time series of data. + * + *If the time series is represented by
+ *[qprev, qcurrent, qnext]
+ *where the data measurements are separated by a time discretisation of {@param dt}, this method is used to compute the quaternion rate of the + * quaternion at the current time. It uses the previous {@param qPrevious} and next {@param qNext} values. The results are stored in {@param qDotToPack}.
+ * + * @param qPrevious previous quaternion estimate. Not modified. + * @param qNext next quaternion estimate. Not modified. + * @param dt time between quaternion estimates + * @param qDotToPack estimated quaternion rate at the current value. Modified. + */ + public static void computeQDotByFiniteDifferenceCentral(QuaternionReadOnly qPrevious, QuaternionReadOnly qNext, double dt, Vector4DBasics qDotToPack) + { + qDotToPack.sub(qNext, qPrevious); + qDotToPack.scale(0.5 / dt); + } + + /** + * This is used to estimate the acceleration of the quaternion coordinates of a time series of data. + * + *If the time series is represented by
+ *[qprev, qcurrent, qnext]
+ *where the data measurements are separated by a time discretisation of {@param dt}, this method is used to compute the quaternion acceleration + * at the current time. It uses the previous {@param qPrevious}, current {@param q}, and next {@param qNext} values. The results are stored in + * {@param qDotToPack}.
+ * + * @param qPrevious previous quaternion estimate. Not modified. + * @param q next quaternion estimate. Not modified. + * @param qNext next quaternion estimate. Not modified. + * @param dt time between quaternion estimates. + * @param qDDotToPack estimated quaternion rate at the current value. Modified. + */ + public static void computeQDDotByFiniteDifferenceCentral(QuaternionReadOnly qPrevious, + QuaternionReadOnly q, + QuaternionReadOnly qNext, + double dt, + Vector4DBasics qDDotToPack) + { + qDDotToPack.sub(qNext, q); + qDDotToPack.sub(q); + qDDotToPack.add(qPrevious); + qDDotToPack.scale(1.0 / EuclidCoreTools.square(dt)); + } + + /** + * This is an internal method used to compute the quaternion acceleration from the quaternion value, its rate, and the angular velocity and acceleration of + * that frame relative to the parent expressed in the parent. + */ + void computeQDDotInParentFrame(QuaternionReadOnly q, + Vector4DReadOnly qDot, + Vector3DReadOnly angularVelocityInParent, + Vector3DReadOnly angularAccelerationInParent, + Vector4DBasics qDDotToPack) + { + multiply(angularAccelerationInParent, q, intermediateQDDot); + multiply(angularVelocityInParent, qDot, qDDotToPack); + qDDotToPack.add(intermediateQDDot); + qDDotToPack.scale(0.5); + } + + private static void multiply(QuaternionReadOnly q, Vector3DReadOnly v, Vector4DBasics resultToPack) + { + setAsPureQuaternion(v, resultToPack); + QuaternionTools.multiply(q, resultToPack, resultToPack); + } + + private static void multiply(Vector3DReadOnly v, Vector4DReadOnly q, Vector4DBasics resultToPack) + { + setAsPureQuaternion(v, resultToPack); + QuaternionTools.multiply(resultToPack, q, resultToPack); + } + + private static void multiply(Vector3DReadOnly v, QuaternionReadOnly q, Vector4DBasics resultToPack) + { + setAsPureQuaternion(v, resultToPack); + QuaternionTools.multiply(resultToPack, q, resultToPack); + } + + private void multiply(Vector4DReadOnly q1, Vector4DReadOnly q2, Vector3DBasics resultToPack) + { + QuaternionTools.multiply(q1, q2, pureQuatForMultiply); + setVectorFromPureQuaternion(pureQuatForMultiply, resultToPack); + } + + private void multiply(Vector4DReadOnly q1, QuaternionReadOnly q2, Vector3DBasics resultToPack) + { + QuaternionTools.multiply(q1, q2, pureQuatForMultiply); + setVectorFromPureQuaternion(pureQuatForMultiply, resultToPack); + } + + private void multiply(QuaternionReadOnly q1, Vector4DReadOnly q2, Vector3DBasics resultToPack) + { + QuaternionTools.multiply(q1, q2, pureQuatForMultiply); + setVectorFromPureQuaternion(pureQuatForMultiply, resultToPack); + } + + /** + * Provides a convenience function for setting a 4D pure quaternion vector from a 3D vector, where the 4D vector is assumed to represent a pure quaternion (w + * = 0.0). + * + * @param vector rotation data source. Not modified. + * @param pureQuaternionToSet results to pack. Modified. + */ + private static void setAsPureQuaternion(Vector3DReadOnly vector, Vector4DBasics pureQuaternionToSet) + { + pureQuaternionToSet.set(vector.getX(), vector.getY(), vector.getZ(), 0.0); + } + + /** + * Provides a convenience function for setting a 3D vector from a 4D vector, where the 4D vector is assumed to represent a pure quaternion (w = 0.0). + * + * @param pureQuaternion quaternion data source. Not modified. + * @param vectorToPack results to pack. Modified. + */ + private static void setVectorFromPureQuaternion(Vector4DReadOnly pureQuaternion, Vector3DBasics vectorToPack) + { + vectorToPack.set(pureQuaternion.getX(), pureQuaternion.getY(), pureQuaternion.getZ()); + } +} diff --git a/src/main/java/us/ihmc/euclid/tools/EuclidCoreTools.java b/src/main/java/us/ihmc/euclid/tools/EuclidCoreTools.java index 9850d624c..7b25e94ce 100644 --- a/src/main/java/us/ihmc/euclid/tools/EuclidCoreTools.java +++ b/src/main/java/us/ihmc/euclid/tools/EuclidCoreTools.java @@ -14,22 +14,9 @@ import us.ihmc.euclid.orientation.interfaces.Orientation3DReadOnly; import us.ihmc.euclid.transform.interfaces.RigidBodyTransformBasics; import us.ihmc.euclid.transform.interfaces.RigidBodyTransformReadOnly; -import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly; -import us.ihmc.euclid.tuple2D.interfaces.Tuple2DBasics; -import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; -import us.ihmc.euclid.tuple2D.interfaces.Vector2DBasics; -import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Tuple3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; -import us.ihmc.euclid.tuple4D.interfaces.QuaternionBasics; -import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; -import us.ihmc.euclid.tuple4D.interfaces.Tuple4DBasics; -import us.ihmc.euclid.tuple4D.interfaces.Tuple4DReadOnly; -import us.ihmc.euclid.tuple4D.interfaces.Vector4DBasics; -import us.ihmc.euclid.tuple4D.interfaces.Vector4DReadOnly; +import us.ihmc.euclid.tuple2D.interfaces.*; +import us.ihmc.euclid.tuple3D.interfaces.*; +import us.ihmc.euclid.tuple4D.interfaces.*; import us.ihmc.euclid.yawPitchRoll.interfaces.YawPitchRollBasics; import us.ihmc.euclid.yawPitchRoll.interfaces.YawPitchRollReadOnly; @@ -285,7 +272,9 @@ public String toString() } }; - /** Constant representing the zero-matrix 3D, i.e. the 9 elements are zero. */ + /** + * Constant representing the zero-matrix 3D, i.e. the 9 elements are zero. + */ public static final Matrix3DReadOnly zeroMatrix3D = new Matrix3DReadOnly() { @Override @@ -1592,7 +1581,7 @@ else if (currentOrientation instanceof AxisAngleReadOnly aaCurr) ** @param previousTransform the transform at the previous time step. Not modified. * * @param currentTransform the transform at the current time step. Not modified. @@ -1768,4 +1757,316 @@ private static UnsupportedOperationException newUnsupportedOrientationException( previousOrientation.getClass() .getSimpleName())); } + + /** + * Checks whether all the elements contained within this tuple are finite. + * + * @param tuple Tuple to inspect. + * @return {@code true} if all elements are finite, {@code false} if any is not. + */ + public static boolean isFinite(Tuple3DReadOnly tuple) + { + return Double.isFinite(tuple.getX()) && Double.isFinite(tuple.getY()) && Double.isFinite(tuple.getZ()); + } + + /** + * Checks whether two rigid body transforms are equal within some epislon. + * + * @param a first transform to check. + * @param b first transform to check. + * @param rotationEpsilon epsilon tolerance for the rotation between the two. + * @param translationEpsilon epsilon tolerance for the translation between the two. + * @return {@code true} if the two transforms are equal, {@code false} if not. + */ + public static boolean epsilonEquals(RigidBodyTransformReadOnly a, RigidBodyTransformReadOnly b, double rotationEpsilon, double translationEpsilon) + { + return a.getRotation().geometricallyEquals(b.getRotation(), rotationEpsilon) && a.getTranslation() + .geometricallyEquals(b.getTranslation(), translationEpsilon); + } + + /** + * Computes the angle in radians from the first 3D vector to the second 3D vector. The computed + * angle is in the range [0; pi]. + * + * @param firstVectorX x value of the first vector. + * @param firstVectorY y value of the first vector. + * @param firstVectorZ z value of the first vector. + * @param isFirstVectorUnitary whether the first vector is a unit vector. + * @param secondVectorX x value of the second vector. + * @param secondVectorY y value of the second vector. + * @param secondVectorZ z value of the second vector. + * @param isSecondVectorUnitary whether the second vector is a unit vector. + * @return the angle in radians from the first vector to the second vector. + */ + public static double angleFromFirstToSecondVector3D(double firstVectorX, + double firstVectorY, + double firstVectorZ, + boolean isFirstVectorUnitary, + double secondVectorX, + double secondVectorY, + double secondVectorZ, + boolean isSecondVectorUnitary) + { + double firstVectorLength = isFirstVectorUnitary ? 1.0 : norm(firstVectorX, firstVectorY, firstVectorZ); + double secondVectorLength = isSecondVectorUnitary ? 1.0 : norm(secondVectorX, secondVectorY, secondVectorZ); + + double dotProduct = firstVectorX * secondVectorX + firstVectorY * secondVectorY + firstVectorZ * secondVectorZ; + dotProduct /= firstVectorLength * secondVectorLength; + + if (dotProduct > 1.0) + dotProduct = 1.0; + else if (dotProduct < -1.0) + dotProduct = -1.0; + + return acos(dotProduct); + } + + /** + * Computes the angle in radians from the first 3D vector to the second 3D vector. The computed + * angle is in the range [0; pi]. + * + * @param firstVector the first vector. Not modified. + * @param secondVector the second vector. Not modified. + * @return the angle in radians from the first vector to the second vector. + */ + public static double angleFromFirstToSecondVector3D(Vector3DReadOnly firstVector, Vector3DReadOnly secondVector) + { + return angleFromFirstToSecondVector3D(firstVector.getX(), + firstVector.getY(), + firstVector.getZ(), + firstVector instanceof UnitVector3DReadOnly, + secondVector.getX(), + secondVector.getY(), + secondVector.getZ(), + secondVector instanceof UnitVector3DReadOnly); + } + + /** + * Calculate the angular velocity by differentiating orientation. + * + * @param qStart the initial orientation at time t. + * @param qEnd the final orientation at time t + duration. + * @param duration the time interval between the 2 orientations. + * @param angularVelocityToPack the angular velocity. + */ + public static void differentiateOrientation(QuaternionReadOnly qStart, QuaternionReadOnly qEnd, double duration, Vector3DBasics angularVelocityToPack) + { + double q1x = qStart.getX(); + double q1y = qStart.getY(); + double q1z = qStart.getZ(); + double q1s = qStart.getS(); + + double q2x = qEnd.getX(); + double q2y = qEnd.getY(); + double q2z = qEnd.getZ(); + double q2s = qEnd.getS(); + + double diffx = q1s * q2x - q1x * q2s - q1y * q2z + q1z * q2y; + double diffy = q1s * q2y + q1x * q2z - q1y * q2s - q1z * q2x; + double diffz = q1s * q2z - q1x * q2y + q1y * q2x - q1z * q2s; + double diffs = q1s * q2s + q1x * q2x + q1y * q2y + q1z * q2z; + + if (diffs < 0.0) + { + diffx = -diffx; + diffy = -diffy; + diffz = -diffz; + diffs = -diffs; + } + + double sinHalfTheta = norm(diffx, diffy, diffz); + + double angle; + if (epsilonEquals(1.0, diffs, 1.0e-12)) + angle = 2.0 * sinHalfTheta / diffs; + else + angle = 2.0 * atan2(sinHalfTheta, diffs); + angularVelocityToPack.set(diffx, diffy, diffz); + angularVelocityToPack.scale(angle / (sinHalfTheta * duration)); + } + + /** + * Sets the yaw pitch roll but the doubles are given in degrees. + */ + public static void setYawPitchRollDegrees(Orientation3DBasics orientation3DBasics, double yaw, double pitch, double roll) + { + orientation3DBasics.setYawPitchRoll(Math.toRadians(yaw), Math.toRadians(pitch), Math.toRadians(roll)); + } + + /** + * Projects the provided {@code rotation} onto {@code axis} such that the original rotation can be + * decomposed into a rotation around {@code axis} and one around an orthogonal axis. + *
+ * rotation = orthogonalRotation * result + *
+ * + * @param rotation is the original rotation to be projected onto {@code axis} + * @param axis is the desired rotation axis of the result. + * @param result will be modified to contain the component of {@code rotation} that is around + * {@code axis} + */ + public static void projectRotationOnAxis(QuaternionReadOnly rotation, Vector3DReadOnly axis, QuaternionBasics result) + { + double dotProduct = rotation.getX() * axis.getX() + rotation.getY() * axis.getY() + rotation.getZ() * axis.getZ(); + + double scale = dotProduct / axis.normSquared(); + double projectedX = scale * axis.getX(); + double projectedY = scale * axis.getY(); + double projectedZ = scale * axis.getZ(); + + result.set(projectedX, projectedY, projectedZ, rotation.getS()); + result.normalize(); + } + + /** + * Calculates the 3D part of the given {@code input} that is parallel to the given + * {@code normalAxis} and stores the result in {@code inputNormalPartToPack}. + *+ * The result has the following properties: + *
+ * The result has the following properties: + *
+ * This method performs the following calculation: x = x - (x.n)n + (y.n)n, where: + *