/** * Gets the coordinates of two distinct points this line goes through. * * @param firstPointOnLineToPack the coordinates of a first point located on this line. Modified. * @param secondPointOnLineToPack the coordinates of a second point located on this line. * Modified. * @throws ReferenceFrameMismatchException if either of the given arguments is not expressed in * the same reference frame as this frame line. */ default void getTwoPointsOnLine(FixedFramePoint2DBasics firstPointOnLineToPack, FixedFramePoint2DBasics secondPointOnLineToPack) { checkReferenceFrameMatch(firstPointOnLineToPack); checkReferenceFrameMatch(secondPointOnLineToPack); Line2DReadOnly.super.getTwoPointsOnLine(firstPointOnLineToPack, secondPointOnLineToPack); }
/** * Tests on a per component basis, if this line 2D is exactly equal to {@code other}. * <p> * If the two lines have different frames, this method returns {@code false}. * </p> * * @param other the other line 2D to compare against this. Not modified. * @return {@code true} if the two lines are exactly equal component-wise and are expressed in * the same reference frame, {@code false} otherwise. */ default boolean equals(FrameLine2DReadOnly other) { if (other == null || getReferenceFrame() != other.getReferenceFrame()) return false; else return getPoint().equals(other.getPoint()) && getDirection().equals(other.getDirection()); } }
/** * Computes the coordinates of the possible intersection(s) between this line and the given * convex polygon 2D. * * @param convexPolygon the convex polygon this line may intersect. Not modified. * @return the intersections between between the line and the polygon or {@code null} if the * method failed or if there is no intersections. * @see #intersectionWith(ConvexPolygon2DReadOnly) */ default FramePoint2DBasics[] intersectionWith(FrameConvexPolygon2DReadOnly convexPolygon) { checkReferenceFrameMatch(convexPolygon); return intersectionWith((ConvexPolygon2DReadOnly) convexPolygon); }
/** * Modifies {@code perpendicularLineToPack} such that it is perpendicular to this line, with its * direction pointing to the left of this line, while going through the given point. * * @param point the point the line has to go through. Not modified. * @param perpendicularLineToPack the line perpendicular to {@code this} and going through * {@code point}. Modified. * @throws ReferenceFrameMismatchException if {@code this} and {@code point} are not expressed in * the same reference frame. */ default void perpendicularLineThroughPoint(FramePoint2DReadOnly point, FrameLine2DBasics perpendicularLineToPack) { checkReferenceFrameMatch(point); perpendicularLineToPack.setReferenceFrame(getReferenceFrame()); Line2DReadOnly.super.perpendicularLineThroughPoint(point, perpendicularLineToPack); }
/** * Sets this line to be the same as the given line. * * @param other the other line to copy. Not modified. */ default void setIncludingFrame(FrameLine2DReadOnly other) { setIncludingFrame(other.getReferenceFrame(), other); }
public static boolean isLineBetweenVertices(FrameLine2DReadOnly frameLine2d, FramePoint2DReadOnly leftVertex, FramePoint2DReadOnly rightVertex, boolean mustBeStrictlyBetween) { FramePoint2D lineStart = new FramePoint2D(frameLine2d.getPoint()); FrameVector2D lineDirection = new FrameVector2D(frameLine2d.getDirection()); double startToLeftVertexX = leftVertex.getX() - lineStart.getX(); double startToLeftVertexY = leftVertex.getY() - lineStart.getY(); double startToRightVertexX = rightVertex.getX() - lineStart.getX(); double startToRightVertexY = rightVertex.getY() - lineStart.getY(); // double crossProduct = vectorToEdge1X * vectorToEdge2Y - vectorToEdge1Y * vectorToEdge2X; double leftCrossProduct = lineDirection.getX() * startToLeftVertexY - lineDirection.getY() * startToLeftVertexX; double rightCrossProduct = lineDirection.getX() * startToRightVertexY - lineDirection.getY() * startToRightVertexX; if (mustBeStrictlyBetween) { if ((leftCrossProduct > 0.0) && (rightCrossProduct < 0.0)) return true; } else if ((leftCrossProduct >= 0.0) && (rightCrossProduct <= 0.0)) return true; return false; }
public static boolean doesLineIntersectPolygon(FrameConvexPolygon2DReadOnly polygon, FrameLine2DReadOnly line) { boolean onLeft = line.isPointOnLeftSideOfLine(polygon.getVertex(0)); for (int i = 0; i < polygon.getNumberOfVertices(); i++) { if (onLeft != line.isPointOnLeftSideOfLine(polygon.getVertex(i))) { return true; } } return false; }
if (cuttingLine.isPointOnSideOfLine(vertex, sideOfLineToCut == RobotSide.LEFT))
/** * Calculates and returns a line that is perpendicular to this line, with its direction pointing * to the left of this line, while going through the given point. * <p> * WARNING: This method generates garbage. * </p> * * @param point the point the line has to go through. Not modified. * @return the line perpendicular to {@code this} and going through {@code point}. * @throws ReferenceFrameMismatchException if {@code this} and {@code point} are not expressed in * the same reference frame. */ default FrameLine2DBasics perpendicularLineThroughPoint(FramePoint2DReadOnly point) { checkReferenceFrameMatch(point); return new FrameLine2D(getReferenceFrame(), Line2DReadOnly.super.perpendicularLineThroughPoint(point)); }
/** * Sets this line to be the same as the given line. * * @param referenceFrame the reference frame in which the given line is expressed. * @param other the line to copy. Not modified. * @throws ReferenceFrameMismatchException if {@code this} and {@code other} are not expressed in * the same reference frame. */ default void set(FrameLine2DReadOnly other) { set(other.getReferenceFrame(), other); }
/** * Gets the point and direction defining this line by storing their components in the given * arguments {@code pointToPack} and {@code directionToPack}. * * @param pointToPack point in which the coordinates of this line's point are stored. Modified. * @param directionToPack vector in which the components of this line's direction are stored. * Modified. * @throws ReferenceFrameMismatchException if either argument is not expressed in the same * reference frame as this frame line 2D. */ default void get(FixedFramePoint2DBasics pointToPack, FixedFrameVector2DBasics directionToPack) { checkReferenceFrameMatch(pointToPack); checkReferenceFrameMatch(directionToPack); Line2DReadOnly.super.get(pointToPack, directionToPack); }
/** * Computes the coordinates of the possible intersection(s) between this line and the given * convex polygon 2D. * * @param convexPolygon the convex polygon this line may intersect. Not modified. * @param firstIntersectionToPack point in which the coordinates of the first intersection. Can * be {@code null}. Modified. * @param secondIntersectionToPack point in which the coordinates of the second intersection. Can * be {@code null}. Modified. * @return the number of intersections between this line and the polygon. * @throws ReferenceFrameMismatchException if {@code this} and {@code other} are not expressed in * the same reference frame. * @see #intersectionWith(ConvexPolygon2DReadOnly, Point2DBasics, Point2DBasics) */ default int intersectionWith(FrameConvexPolygon2DReadOnly convexPolygon, Point2DBasics firstIntersectionToPack, FramePoint2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(convexPolygon); secondIntersectionToPack.setReferenceFrame(getReferenceFrame()); return Line2DReadOnly.super.intersectionWith(convexPolygon, firstIntersectionToPack, secondIntersectionToPack); }
/** * Tests on a per-component basis on the point and vector if this line is equal to {@code other} * with the tolerance {@code epsilon}. This method will return {@code false} if the two lines are * physically the same but either the point or vector of each line is different. For instance, if * {@code this.point == other.point} and {@code this.direction == - other.direction}, the two * lines are physically the same but this method returns {@code false}. * <p> * If the two lines have different frames, this method returns {@code false}. * </p> * * @param other the query. Not modified. * @param epsilon the tolerance to use. * @return {@code true} if the two lines are equal and are expressed in the same reference frame, * {@code false} otherwise. */ default boolean epsilonEquals(FrameLine2DReadOnly other, double epsilon) { if (getReferenceFrame() != other.getReferenceFrame()) return false; if (!getPoint().epsilonEquals(other.getPoint(), epsilon)) return false; if (!getDirection().epsilonEquals(other.getDirection(), epsilon)) return false; return true; }
/** * Sets this line to be the same as the given line. * <p> * After calling this method this line is in the XY-plane. * </p> * * @param frameLine2DReadOnly the line to copy. Not modified. */ default void setIncludingFrame(FrameLine2DReadOnly frameLine2DReadOnly) { setIncludingFrame(frameLine2DReadOnly.getReferenceFrame(), frameLine2DReadOnly); }
/** * Modifies {@code perpendicularLineToPack} such that it is perpendicular to this line, with its * direction pointing to the left of this line, while going through the given point. * * @param point the point the line has to go through. Not modified. * @param perpendicularLineToPack the line perpendicular to {@code this} and going through * {@code point}. Modified. * @throws ReferenceFrameMismatchException if {@code this}, {@code point}, and * {@code perpendicularLineToPack} are not expressed in the same reference frame. */ default void perpendicularLineThroughPoint(FramePoint2DReadOnly point, FixedFrameLine2DBasics perpendicularLineToPack) { checkReferenceFrameMatch(point); checkReferenceFrameMatch(perpendicularLineToPack); Line2DReadOnly.super.perpendicularLineThroughPoint(point, perpendicularLineToPack); }
/** * Computes the coordinates of the possible intersection(s) between this line and the given * convex polygon 2D. * * @param convexPolygon the convex polygon this line may intersect. Not modified. * @param firstIntersectionToPack point in which the coordinates of the first intersection. Can * be {@code null}. Modified. * @param secondIntersectionToPack point in which the coordinates of the second intersection. Can * be {@code null}. Modified. * @return the number of intersections between this line and the polygon. * @throws ReferenceFrameMismatchException if {@code this} and {@code convexPolygon} are not * expressed in the same reference frame. * @see #intersectionWith(ConvexPolygon2DReadOnly, Point2DBasics, Point2DBasics) */ default int intersectionWith(FrameConvexPolygon2DReadOnly convexPolygon, FramePoint2DBasics firstIntersectionToPack, FramePoint2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(convexPolygon); firstIntersectionToPack.setReferenceFrame(getReferenceFrame()); secondIntersectionToPack.setReferenceFrame(getReferenceFrame()); return Line2DReadOnly.super.intersectionWith(convexPolygon, firstIntersectionToPack, secondIntersectionToPack); }
/** * Sets this line to be the same as the given line. * <p> * After calling this method this line is in the XY-plane. * </p> * * @param referenceFrame the reference frame in which the given line is expressed. * @param frameLine2DReadOnly the line to copy. Not modified. * @throws ReferenceFrameMismatchException if {@code this} and {@code frameLine2DReadOnly} are not * expressed in the same reference frame. */ default void set(FrameLine2DReadOnly frameLine2DReadOnly) { set(frameLine2DReadOnly.getReferenceFrame(), frameLine2DReadOnly); }
/** * Computes the coordinates of the possible intersection(s) between this line and the given * convex polygon 2D. * * @param convexPolygon the convex polygon this line may intersect. Not modified. * @param firstIntersectionToPack point in which the coordinates of the first intersection. Can * be {@code null}. Modified. * @param secondIntersectionToPack point in which the coordinates of the second intersection. Can * be {@code null}. Modified. * @return the number of intersections between this line and the polygon. * @throws ReferenceFrameMismatchException if {@code this}, {@code convexPolygon}, * {@code firstIntersectionToPack}, and {@code secondIntersectionToPack} are not * expressed in the same reference frame. * @see #intersectionWith(ConvexPolygon2DReadOnly, Point2DBasics, Point2DBasics) */ default int intersectionWith(FrameConvexPolygon2DReadOnly convexPolygon, FixedFramePoint2DBasics firstIntersectionToPack, FixedFramePoint2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(convexPolygon); checkReferenceFrameMatch(firstIntersectionToPack); checkReferenceFrameMatch(secondIntersectionToPack); return Line2DReadOnly.super.intersectionWith(convexPolygon, firstIntersectionToPack, secondIntersectionToPack); }
/** * Computes the coordinates of the possible intersection(s) between this line and the given * convex polygon 2D. * * @param convexPolygon the convex polygon this line may intersect. Not modified. * @param firstIntersectionToPack point in which the coordinates of the first intersection. Can * be {@code null}. Modified. * @param secondIntersectionToPack point in which the coordinates of the second intersection. Can * be {@code null}. Modified. * @return the number of intersections between this line and the polygon. * @throws ReferenceFrameMismatchException if {@code this} and {@code convexPolygon} are not * expressed in the same reference frame. * @see #intersectionWith(ConvexPolygon2DReadOnly, Point2DBasics, Point2DBasics) */ default int intersectionWith(FrameConvexPolygon2DReadOnly convexPolygon, FramePoint2DBasics firstIntersectionToPack, Point2DBasics secondIntersectionToPack) { checkReferenceFrameMatch(convexPolygon); firstIntersectionToPack.setReferenceFrame(getReferenceFrame()); return Line2DReadOnly.super.intersectionWith(convexPolygon, firstIntersectionToPack, secondIntersectionToPack); }
/** * Gets the coordinates of two distinct points this line goes through. * * @param firstPointOnLineToPack the coordinates of a first point located on this line. Modified. * @param secondPointOnLineToPack the coordinates of a second point located on this line. * Modified. */ default void getTwoPointsOnLine(FramePoint2DBasics firstPointOnLineToPack, FramePoint2DBasics secondPointOnLineToPack) { firstPointOnLineToPack.setReferenceFrame(getReferenceFrame()); secondPointOnLineToPack.setReferenceFrame(getReferenceFrame()); Line2DReadOnly.super.getTwoPointsOnLine(firstPointOnLineToPack, secondPointOnLineToPack); }