@Override public Point2d[] intersectionWith(ConvexPolygon2d convexPolygon) { return convexPolygon.intersectionWith(this); }
public static Point2d[] intersection(LineSegment2d lineSegment, ConvexPolygon2d convexPolygon) { Point2d[] intersectingPoints = convexPolygon.intersectionWith(lineSegment); if (intersectingPoints == null) return null; Point2d[] ret = new Point2d[intersectingPoints.length]; for (int i = 0; i < intersectingPoints.length; i++) { ret[i] = new Point2d(intersectingPoints[i]); } return ret; // throw new RuntimeException("Not yet implemented"); }
/** * Returns all of the intersections when the convexPolygon is projected vertically onto this PlanarRegion. * @param convexPolygonInWorld Polygon to project vertically. * @param intersectionsInPlaneFrameToPack ArrayList of ConvexPolygon2d to pack with the intersections. */ public void getPolygonIntersectionsWhenProjectedVertically(ConvexPolygon2d convexPolygonInWorld, ArrayList<ConvexPolygon2d> intersectionsInPlaneFrameToPack) { // Instead of projecting all the polygons of this region onto the world XY-plane, // the given convex polygon is projected along the z-world axis to be snapped onto plane. ConvexPolygon2d projectedPolygon = projectPolygonVerticallyToRegion(convexPolygonInWorld); // Now, just need to go through each polygon of this region and see there is at least one intersection for (int i = 0; i < getNumberOfConvexPolygons(); i++) { ConvexPolygon2d intersectingPolygon = convexPolygons.get(i).intersectionWith(projectedPolygon); if (intersectingPolygon != null) { intersectionsInPlaneFrameToPack.add(intersectingPolygon); } } }
/** * Returns all of the intersections when the convexPolygon is projected vertically onto this PlanarRegion. * @param convexPolygonInWorld Polygon to project vertically. * @param intersectionsInPlaneFrameToPack ArrayList of ConvexPolygon2d to pack with the intersections. */ public void getLineSegmentIntersectionsWhenProjectedVertically(LineSegment2d lineSegmentInWorld, ArrayList<Point2d[]> intersectionsInPlaneFrameToPack) { // Instead of projecting all the polygons of this region onto the world XY-plane, // the given lineSegment is projected along the z-world axis to be snapped onto plane. LineSegment2d projectedLineSegment = projectLineSegmentVerticallyToRegion(lineSegmentInWorld); // Now, just need to go through each polygon of this region and see there is at least one intersection for (int i = 0; i < getNumberOfConvexPolygons(); i++) { Point2d[] intersectionPoints = convexPolygons.get(i).intersectionWith(projectedLineSegment); if ((intersectionPoints != null) && (intersectionPoints.length > 0) && (intersectionPoints[0] != null)) { intersectionsInPlaneFrameToPack.add(intersectionPoints); } } }
@Override public FramePoint2d[] intersectionWith(FrameLineSegment2d lineSegment) { //TODO: Memory inefficient. Don't create new objects... checkReferenceFrameMatch(lineSegment); Point2d[] intersection = this.convexPolygon.intersectionWith(lineSegment.lineSegment); if (intersection == null) return null; FramePoint2d[] ret = new FramePoint2d[intersection.length]; for (int i = 0; i < intersection.length; i++) { ret[i] = new FramePoint2d(lineSegment.referenceFrame, intersection[i]); } return ret; }
/** * Check if the given lineSegment intersects this region projected onto the XY-plane. * @param lineSegmentInWorld * @return true if the lineSegment intersects this PlanarRegion. */ public boolean isLineSegmentIntersecting(LineSegment2d lineSegmentInWorld) { // Instead of projecting all the polygons of this region onto the world XY-plane, // the given lineSegment is projected along the z-world axis to be snapped onto plane. LineSegment2d projectedLineSegment = projectLineSegmentVerticallyToRegion(lineSegmentInWorld); // Now, just need to go through each polygon of this region and see there is at least one intersection for (int i = 0; i < getNumberOfConvexPolygons(); i++) { ConvexPolygon2d polygonToCheck = convexPolygons.get(i); Point2d[] intersectionPoints = polygonToCheck.intersectionWith(projectedLineSegment); if ((intersectionPoints != null) && (intersectionPoints.length > 0) && (intersectionPoints[0] != null)) return true; } // Did not find any intersection return false; }
/** * @deprecated Creates garbage. Use an instance of FrameConvexPolygonWithLineIntersector. */ @Override public FramePoint2d[] intersectionWith(FrameLine2d line) { checkReferenceFrameMatch(line); Point2d[] intersection = this.convexPolygon.intersectionWith(line.line); if (intersection == null) return null; FramePoint2d[] ret = new FramePoint2d[intersection.length]; for (int i = 0; i < intersection.length; i++) { ret[i] = new FramePoint2d(line.referenceFrame, intersection[i]); } return ret; }
@Override public FrameConvexPolygon2d intersectionWith(FrameConvexPolygon2d secondConvexPolygon) { checkReferenceFrameMatch(secondConvexPolygon); ConvexPolygon2d intersection = this.convexPolygon.intersectionWith(secondConvexPolygon.convexPolygon); if (intersection == null) return null; return new FrameConvexPolygon2d(secondConvexPolygon.getReferenceFrame(), intersection); }
public boolean intersectionWith(FrameConvexPolygon2d secondConvexPolygon, FrameConvexPolygon2d intersectionToPack) { checkReferenceFrameMatch(secondConvexPolygon); intersectionToPack.clear(secondConvexPolygon.getReferenceFrame()); boolean success = convexPolygon.intersectionWith(secondConvexPolygon.convexPolygon, intersectionToPack.convexPolygon); return success; }
/** * Check if the given polygon intersects this region projected onto the XY-plane. * @param convexPolygon2d * @return true if the polygon intersects this PlanarRegion. */ public boolean isPolygonIntersecting(ConvexPolygon2d convexPolygon2d) { BoundingBox2d polygonBoundingBox = convexPolygon2d.getBoundingBox(); if (!boundingBox3dInWorld.intersectsInXYPlane(polygonBoundingBox)) return false; // Instead of projecting all the polygons of this region onto the world XY-plane, // the given convex polygon is projected along the z-world axis to be snapped onto plane. ConvexPolygon2d projectedPolygon = projectPolygonVerticallyToRegion(convexPolygon2d); ConvexPolygon2d dummyPolygon = new ConvexPolygon2d(); // Now, just need to go through each polygon of this region and see there is at least one intersection for (int i = 0; i < getNumberOfConvexPolygons(); i++) { ConvexPolygon2d polygonToCheck = convexPolygons.get(i); boolean hasIntersection = polygonToCheck.intersectionWith(projectedPolygon, dummyPolygon); if (hasIntersection) return true; } // Did not find any intersection return false; }
private static boolean computeIntersectionOfPolygonsIfOnePolygonHasExactlyTwoVerticesAndTheOtherHasAtLeastTwoVertices( ConvexPolygon2d polygonWithExactlyTwoVertices, ConvexPolygon2d polygonWithAtLeastTwoVertices, ConvexPolygon2d intersectingPolygon) { LineSegment2d polygonWithTwoVerticesAsLineSegment = new LineSegment2d(polygonWithExactlyTwoVertices.getVertex(0), polygonWithExactlyTwoVertices.getVertex(1)); Point2d[] intersection = polygonWithAtLeastTwoVertices.intersectionWith(polygonWithTwoVerticesAsLineSegment); if (intersection == null) { intersectingPolygon.clearAndUpdate(); return false; } else { intersectingPolygon.setAndUpdate(intersection, intersection.length); return true; } }
public static int cutPolygonWithLine(Line2d cuttingLine, ConvexPolygon2d polygonToCut, RobotSide sideOfLineToCut) { Point2d[] intersectionPoints = polygonToCut.intersectionWith(cuttingLine); if (intersectionPoints == null || intersectionPoints.length == 1) { return -1; } else { int numberOfVerticesRemoved = 0; int index = 0; while (index < polygonToCut.getNumberOfVertices()) { Point2d vertex = polygonToCut.getVertexUnsafe(index); if (cuttingLine.isPointOnSideOfLine(vertex, sideOfLineToCut)) { polygonToCut.removeVertex(index); numberOfVerticesRemoved++; } else { index++; } } polygonToCut.addVertices(intersectionPoints, intersectionPoints.length); polygonToCut.update(); return numberOfVerticesRemoved; } }
/** * Returns a transform that will move the given polygon into a planar region. Problematic if the planar region consists of * multiple sub convex polygons. The polygon to wiggle must have the same transform to world as the planar region. * * @param polygonToWiggleInRegionFrame * @param regionToWiggleInto * @param wiggleParameters * @return */ public static RigidBodyTransform wigglePolygonIntoRegion(ConvexPolygon2d polygonToWiggleInRegionFrame, PlanarRegion regionToWiggleInto, WiggleParameters parameters) { // find the part of the region that has the biggest intersection with the polygon ConvexPolygon2d bestMatch = null; double overlap = 0.0; for (int i = 0; i < regionToWiggleInto.getNumberOfConvexPolygons(); i++) { ConvexPolygon2d intersection = new ConvexPolygon2d(); regionToWiggleInto.getConvexPolygon(i).intersectionWith(polygonToWiggleInRegionFrame, intersection); if (intersection.getArea() > overlap) { overlap = intersection.getArea(); bestMatch = regionToWiggleInto.getConvexPolygon(i); } } if (bestMatch == null) return null; return findWiggleTransform(polygonToWiggleInRegionFrame, bestMatch, parameters); }
ConvexPolygon2d intersectingPolygon = convexPolygons.get(i).intersectionWith(projectedPolygon);
footPolygonInRegion.setAndUpdate(footStepPolygon); footPolygonInRegion.applyTransformAndProjectToXYPlane(soleToRegion); ConvexPolygon2d foothold = regionToMoveTo.getConvexHull().intersectionWith(footPolygonInRegion); soleToRegion.invert(); foothold.applyTransformAndProjectToXYPlane(soleToRegion);