public void setPositionChangeOnlyXY(FramePoint2DReadOnly position2d) { position2d.checkReferenceFrameMatch(footstepPose); setX(position2d.getX()); setY(position2d.getY()); }
/** * 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); }
/** * Returns {@code true} only if the point is inside the triangle defined by the vertices a, b, and * c. The triangle can be clockwise or counter-clockwise ordered. * * @param point the point to check if lying inside the triangle. Not modified. * @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 {@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. * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ public static boolean isPoint2DInsideTriangleABC(FramePoint2DReadOnly point, FramePoint2DReadOnly a, FramePoint2DReadOnly b, FramePoint2DReadOnly c) { point.checkReferenceFrameMatch(a); point.checkReferenceFrameMatch(b); point.checkReferenceFrameMatch(c); return EuclidGeometryTools.isPoint2DInsideTriangleABC(point, a, b, c); }
private static void assertEquals(FramePoint2DReadOnly pointA, CoPTrajectoryPoint pointB, double epsilon) { assertEquals(new FramePoint3D(pointA.getReferenceFrame(), pointA.getX(), pointA.getY(), 0.0), pointB, epsilon); }
/** * 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. */ default void setIncludingFrame(FramePoint2DReadOnly pointOnLine, Vector2DReadOnly lineDirection) { setIncludingFrame(pointOnLine.getReferenceFrame(), pointOnLine, lineDirection); }
public static void verifyPolygonContains(FrameConvexPolygon2D convexPolygon2d, FramePoint2D framePointToTest, double epsilon) { for (int i = 0; i < convexPolygon2d.getNumberOfVertices(); i++) { if (convexPolygon2d.getVertex(i).epsilonEquals(framePointToTest, epsilon)) return; } throw new RuntimeException("List doesn't contain " + framePointToTest); }
/** * Tests on a per-vertex basis if this supplier and {@code other} are equal. * * @param other the other supplier to compare against this. * @return {@code true} if the two suppliers are equal. */ default boolean equals(FrameVertex2DSupplier other) { if (other == null) return false; if (getNumberOfVertices() != other.getNumberOfVertices()) return false; for (int i = 0; i < getNumberOfVertices(); i++) { if (!getVertex(i).equals(other.getVertex(i))) return false; } return true; }
/** * Computes the dot product between two vectors each defined by two points: * <ul> * <li>{@code vector1 = end1 - start1} * <li>{@code vector2 = end2 - start2} * </ul> * * @param start1 the origin of the first vector. Not modified. * @param end1 the end of the first vector. Not modified. * @param start2 the origin of the second vector. Not modified. * @param end2 the end of the second vector. Not modified. * @return the value of the dot product of the two vectors. * @throws ReferenceFrameMismatchException if the read-only arguments are not all expressed in the * same reference frame. */ public static double dotProduct(FramePoint2DReadOnly start1, FramePoint2DReadOnly end1, FramePoint2DReadOnly start2, FramePoint2DReadOnly end2) { start1.checkReferenceFrameMatch(end1); start1.checkReferenceFrameMatch(start2); start1.checkReferenceFrameMatch(end2); return EuclidGeometryTools.dotProduct(start1, end1, start2, end2); }
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; }
/** * 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} and {@code secondPointOnLine} are not * expressed in the same reference frame. */ default void set(Point2DReadOnly firstPointOnLine, FramePoint2DReadOnly secondPointOnLine) { set(secondPointOnLine.getReferenceFrame(), firstPointOnLine, secondPointOnLine); }
/** * Tests on a per-vertex basis if this supplier and {@code other} are equal to an * {@code epsilon}. * * @param other the other supplier to compare against this. * @param epsilon the tolerance to use. * @return {@code true} if the two suppliers are equal. */ default boolean epsilonEquals(FrameVertex2DSupplier other, double epsilon) { if (getNumberOfVertices() != other.getNumberOfVertices()) return false; for (int i = 0; i < getNumberOfVertices(); i++) { if (!getVertex(i).epsilonEquals(other.getVertex(i), epsilon)) return false; } return true; }
/** * 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()); } }
/** * 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); }
/** * 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); }
/** * 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); }
FramePoint2DReadOnly convexPoint = yoFrameConvexPolygon2d.getVertex(i); polyPoint.checkReferenceFrameMatch(convexPoint); assertTrue("not equal expected: " + polyPoint + " actual: " + convexPoint, MathTools.epsilonEquals(polyPoint.getX(), convexPoint.getX(), 1e-7)); assertTrue("not equal expected: " + polyPoint + " actual: " + convexPoint, MathTools.epsilonEquals(polyPoint.getY(), convexPoint.getY(), 1e-7));
/** * 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. */ default void setIncludingFrame(FramePoint2DReadOnly firstPointOnLine, Point2DReadOnly secondPointOnLine) { setIncludingFrame(firstPointOnLine.getReferenceFrame(), firstPointOnLine, secondPointOnLine); }
/** * 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; }