/** * Compute the area of a triangle defined by its three vertices: a, b, and c. No specific ordering * of the vertices is required. * * @param a first vertex of the triangle. Not modified. * @param b second vertex of the triangle. Not modified. * @param c third vertex of the triangle. Not modified. * @return the are of the triangle. * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ public static double triangleArea(FramePoint2DReadOnly a, FramePoint2DReadOnly b, FramePoint2DReadOnly c) { a.checkReferenceFrameMatch(b); a.checkReferenceFrameMatch(c); return EuclidGeometryTools.triangleArea(a, b, c); }
/** * Determines if the query is exactly on or on the right side of the infinitely long line that goes * through the ray origin and which direction is perpendicular to the ray and directed towards the * left side. * * @param point the query. Not modified. * @param rayOrigin the ray's origin. Not modified. * @param rayDirection the ray's direction. Not modified. * @return {@code true} if the query is located in front of the ray. * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ public static boolean isPoint2DInFrontOfRay2D(FramePoint2DReadOnly point, FramePoint2DReadOnly rayOrigin, FrameVector2DReadOnly rayDirection) { point.checkReferenceFrameMatch(rayOrigin); point.checkReferenceFrameMatch(rayDirection); return EuclidGeometryTools.isPoint2DInFrontOfRay2D(point, rayOrigin, rayDirection); }
/** * Tests if the point 2D is located on the infinitely long line 2D. * <p> * 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}. * </p> * * @param point the coordinates of the query. Not modified. * @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. * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ public static boolean isPoint2DOnLineSegment2D(FramePoint2DReadOnly point, FramePoint2DReadOnly lineSegmentStart, FramePoint2DReadOnly lineSegmentEnd) { point.checkReferenceFrameMatch(lineSegmentStart); point.checkReferenceFrameMatch(lineSegmentEnd); return EuclidGeometryTools.isPoint2DOnLineSegment2D(point, lineSegmentStart, lineSegmentEnd); }
/** * Calculates and returns the distance between this frame point and {@code other}. * * @param other the other frame point used to measure the distance. Not modified. * @return the distance between the two frame points. * @throws ReferenceFrameMismatchException if {@code other} is not expressed in the same frame as * {@code this}. */ default double distance(FramePoint2DReadOnly other) { checkReferenceFrameMatch(other); return Point2DReadOnly.super.distance(other); }
/** * Compares {@code this} to {@code other} to determine if the two frame points are geometrically * similar, i.e. the distance between them is less than or equal to {@code epsilon}. * * @param other the frame point to compare to. Not modified. * @param epsilon the tolerance of the comparison. * @return {@code true} if the two frame points represent the same geometry, {@code false} * otherwise. * @throws ReferenceFrameMismatchException if {@code other} is not expressed in the same reference * frame as {@code this}. */ default boolean geometricallyEquals(FramePoint2DReadOnly other, double epsilon) { checkReferenceFrameMatch(other); return Point2DReadOnly.super.geometricallyEquals(other, epsilon); } }
/** * Calculates and returns the square of the distance between this frame point and {@code other}. * <p> * This method is usually preferred over {@link #distance(FramePoint2DReadOnly)} when calculation * speed matters and knowledge of the actual distance does not, i.e. when comparing distances * between several pairs of frame points. * </p> * * @param other the other frame point used to measure the square of the distance. Not modified. * @return the square of the distance between the two frame points. * @throws ReferenceFrameMismatchException if {@code other} is not expressed in the same frame as * {@code this}. */ default double distanceSquared(FramePoint2DReadOnly other) { checkReferenceFrameMatch(other); return Point2DReadOnly.super.distanceSquared(other); }
/** * Calculates and returns the distance between this frame point and {@code framePoint3DReadOnly} in * the XY-plane. * <p> * Effectively, this calculates the distance as follows:<br> * d<sub>xy</sub> = √((this.x - framePoint3DReadOnly.x)<sup>2</sup> + (this.y - * framePoint3DReadOnly.y)<sup>2</sup>) * </p> * * @param framePoint3DReadOnly the other frame point used to measure the distance. * @return the distance between the two frame points in the XY-plane. * @throws ReferenceFrameMismatchException if {@code framePoint3DReadOnly} is not expressed in the * same frame as {@code this}. */ default double distanceXY(FramePoint3DReadOnly framePoint3DReadOnly) { checkReferenceFrameMatch(framePoint3DReadOnly); return Point2DReadOnly.super.distanceXY(framePoint3DReadOnly); }
/** * Redefines this line with a new point, a new direction vector, and a new reference frame. * * @param pointOnLine new point on this line. Not modified. * @param lineDirection new direction of this line. Not modified. * @throws ReferenceFrameMismatchException if {@code pointOnLine} and {@code lineDirection} are not * expressed in the same reference frame */ default void setIncludingFrame(FramePoint2DReadOnly pointOnLine, FrameVector2DReadOnly lineDirection) { pointOnLine.checkReferenceFrameMatch(lineDirection); setIncludingFrame(pointOnLine.getReferenceFrame(), pointOnLine, lineDirection); }
/** * Redefines this line such that it goes through the two given points in the given reference frame. * * @param firstPointOnLine first point on this line. Not modified. * @param secondPointOnLine second point on this line. Not modified. * @throws ReferenceFrameMismatchException if {@code firstPointOnLine} and {@code secondPointOnLine} * are not expressed in the same reference frame */ default void setIncludingFrame(FramePoint2DReadOnly firstPointOnLine, FramePoint2DReadOnly secondPointOnLine) { firstPointOnLine.checkReferenceFrameMatch(secondPointOnLine); setIncludingFrame(firstPointOnLine.getReferenceFrame(), firstPointOnLine, secondPointOnLine); }
/** * Redefines this line segment with new endpoints. * * @param firstEndpoint new endpoint of this line segment. Not modified * @param secondEndpoint new second endpoint of this line segment. Not modified. * @throws ReferenceFrameMismatchException if {@code firstEndpoint} and {@code secondEndpoint} are * not expressed in the same reference frame. */ default void setIncludingFrame(FramePoint2DReadOnly firstEndpoint, FramePoint2DReadOnly secondEndpoint) { firstEndpoint.checkReferenceFrameMatch(secondEndpoint); setIncludingFrame(firstEndpoint.getReferenceFrame(), firstEndpoint, secondEndpoint); }
/** * Redefines this line with a new point and a new direction vector. * * @param pointOnLine new point on this line. Not modified. * @param lineDirection new direction of this line. Not modified. * @throws ReferenceFrameMismatchException if {@code this}, {@code pointOnLine}, * {@code lineDirection} are not expressed in the same reference frame. */ default void set(FramePoint2DReadOnly pointOnLine, FrameVector2DReadOnly lineDirection) { pointOnLine.checkReferenceFrameMatch(lineDirection); set(pointOnLine.getReferenceFrame(), pointOnLine, lineDirection); }
/** * Redefines this line such that it goes through the two given points. * * @param firstPointOnLine first point on this line. Not modified. * @param secondPointOnLine second point on this line. Not modified. * @throws ReferenceFrameMismatchException if {@code this}, {@code firstPointOnLine}, * {@code secondPointOnLine} are not expressed in the same reference frame. */ default void set(FramePoint2DReadOnly firstPointOnLine, FramePoint2DReadOnly secondPointOnLine) { firstPointOnLine.checkReferenceFrameMatch(secondPointOnLine); set(firstPointOnLine.getReferenceFrame(), firstPointOnLine, secondPointOnLine); }
/** * Redefines this line such that it goes through the two given points in the given reference frame. * <p> * After calling this method this line is in the XY-plane. * </p> * * @param firstPointOnLine first point on this line. Not modified. * @param secondPointOnLine second point on this line. Not modified. * @throws ReferenceFrameMismatchException if {@code firstPointOnLine} and {@code secondPointOnLine} * are not expressed in the same reference frame */ default void setIncludingFrame(FramePoint2DReadOnly firstPointOnLine, FramePoint2DReadOnly secondPointOnLine) { firstPointOnLine.checkReferenceFrameMatch(secondPointOnLine); setIncludingFrame(firstPointOnLine.getReferenceFrame(), firstPointOnLine, secondPointOnLine); }
/** * Redefines this line segment with a new first endpoint and a vector going from the first to the * second endpoint. * * @param firstEndpoint new first endpoint. Not modified. * @param fromFirstToSecondEndpoint vector going from the first to the second endpoint. Not * modified. * @throws ReferenceFrameMismatchException if {@code firstEndpoint} and * {@code fromFirstToSecondEndpoint} are not expressed in the same reference frame. */ default void setIncludingFrame(FramePoint2DReadOnly firstEndpoint, FrameVector2DReadOnly fromFirstToSecondEndpoint) { firstEndpoint.checkReferenceFrameMatch(fromFirstToSecondEndpoint); setIncludingFrame(firstEndpoint.getReferenceFrame(), firstEndpoint, fromFirstToSecondEndpoint); }
/** * Redefines this line with a new point, a new direction vector, and a new reference frame. * <p> * After calling this method this line is in the XY-plane. * </p> * * @param pointOnLine new point on this line. Not modified. * @param lineDirection new direction of this line. Not modified. * @throws ReferenceFrameMismatchException if {@code pointOnLine} and {@code lineDirection} are not * expressed in the same reference frame */ default void setIncludingFrame(FramePoint2DReadOnly pointOnLine, FrameVector2DReadOnly lineDirection) { pointOnLine.checkReferenceFrameMatch(lineDirection); setIncludingFrame(pointOnLine.getReferenceFrame(), pointOnLine, lineDirection); }
/** * Redefines this line with a new point and a new direction vector. * <p> * After calling this method this line is in the XY-plane. * </p> * * @param pointOnLine new point on this line. Not modified. * @param lineDirection new direction of this line. Not modified. * @throws ReferenceFrameMismatchException if {@code this}, {@code pointOnLine}, * {@code lineDirection} are not expressed in the same reference frame. */ default void set(FramePoint2DReadOnly pointOnLine, FrameVector2DReadOnly lineDirection) { pointOnLine.checkReferenceFrameMatch(lineDirection); set(pointOnLine.getReferenceFrame(), pointOnLine, lineDirection); }
/** * Redefines this line such that it goes through the two given points. * <p> * After calling this method this line is in the XY-plane. * </p> * * @param firstPointOnLine first point on this line. Not modified. * @param secondPointOnLine second point on this line. Not modified. * @throws ReferenceFrameMismatchException if {@code this}, {@code firstPointOnLine}, * {@code secondPointOnLine} are not expressed in the same reference frame. */ default void set(FramePoint2DReadOnly firstPointOnLine, FramePoint2DReadOnly secondPointOnLine) { firstPointOnLine.checkReferenceFrameMatch(secondPointOnLine); set(firstPointOnLine.getReferenceFrame(), firstPointOnLine, secondPointOnLine); }
public void setXYFromPosition2dAndUpdate(FramePoint2DReadOnly position2d) { position2d.checkReferenceFrameMatch(getParent()); originPose.setPosition(position2d); this.update(); }
public void setPositionChangeOnlyXY(FramePoint2DReadOnly position2d) { position2d.checkReferenceFrameMatch(footstepPose); setX(position2d.getX()); setY(position2d.getY()); }
/** * yawAboutPoint * * @param pointToYawAbout FramePoint2d * @param yaw double * @return CartesianPositionFootstep */ public static void yawAboutPoint(FramePoint2DReadOnly pointToTransform, FramePoint2DReadOnly pointToYawAbout, double yaw, FramePoint2D resultToPack) { pointToTransform.checkReferenceFrameMatch(pointToYawAbout); double tempX = pointToTransform.getX() - pointToYawAbout.getX(); double tempY = pointToTransform.getY() - pointToYawAbout.getY(); double cosAngle = Math.cos(yaw); double sinAngle = Math.sin(yaw); double x = cosAngle * tempX + -sinAngle * tempY; tempY = sinAngle * tempX + cosAngle * tempY; tempX = x; resultToPack.setIncludingFrame(pointToYawAbout); resultToPack.add(tempX, tempY); }