public boolean isPointInsidePolygon() { return polygon.isPointInside(this); }
/** * Given a 2D point expressed in the plane local frame, computes whether the point is in this region. * * @param xCoordinateInLocal x Coordinate of the 2D point in planar region local frame * @param yCoordinateInLocal y Coordinate of the 2D point in planar region local frame * @return true if the point is inside this region, false otherwise. */ public boolean isPointInside(double xCoordinateInLocal, double yCoordinateInLocal) { for (int i = 0; i < convexPolygons.size(); i++) { if (convexPolygons.get(i).isPointInside(xCoordinateInLocal, yCoordinateInLocal)) return true; } return false; }
/** * from http://softsurfer.com/Archive/algorithm_0111/algorithm_0111.htm#Pseudo-Code:%20Clip%20Segment-Polygon * Input: a 2D segment S from point P0 to point P1 * a 2D convex polygon W with n vertices V0,...,Vn-1,Vn=V0 */ public static boolean doesSegmentIntersectConvexPolygon2D(Point2d P0, Point2d P1, ConvexPolygon2d convexPolygon2d) { // if segment is a single point if (P0.equals(P1)) { return convexPolygon2d.isPointInside(P0); } // if either point is inside polygon if (convexPolygon2d.isPointInside(P0, .0001) || convexPolygon2d.isPointInside(P1, .0001)) return true; // if either point touches the polygon if (convexPolygon2d.pointIsOnPerimeter(P0) || convexPolygon2d.pointIsOnPerimeter(P1)) return true; return doesSegmentPassCompletelyThroughPolygon(P0, P1, convexPolygon2d); }
@Override public boolean isIncluded(Point3d inputPoint) { if (!initialized) return false; double x = inputPoint.getX() - position.getX(); double y = inputPoint.getY() - position.getY(); double cos = Math.cos(yaw); double sin = Math.sin(yaw); double xInFoot = x * cos + y * sin; double yInFoot = y * cos - x * sin; boolean isInside = footShapeWithBufferPolygon.isPointInside(xInFoot, yInFoot); return isInside; } }
for (Point2d point2d : pointsInFootstepFrame) if (maxSupportPolygon.isPointInside(point2d, distanceTolerance))