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 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 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; }