public boolean epsilonEquals(PlanarRegion other, double epsilon) { if (!fromLocalToWorldTransform.epsilonEquals(other.fromLocalToWorldTransform, epsilon)) return false; // Not necessary, but just in case if (!fromWorldToLocalTransform.epsilonEquals(other.fromWorldToLocalTransform, epsilon)) return false; if (getNumberOfConvexPolygons() != other.getNumberOfConvexPolygons()) return false; for (int i = 0; i < getNumberOfConvexPolygons(); i++) { if (!convexPolygons.get(i).epsilonEquals(other.convexPolygons.get(i), epsilon)) return false; } return true; }
public boolean epsilonEquals(PlanarRegion other, double epsilon) { if (!fromLocalToWorldTransform.epsilonEquals(other.fromLocalToWorldTransform, epsilon)) return false; // Not necessary, but just in case if (!fromWorldToLocalTransform.epsilonEquals(other.fromWorldToLocalTransform, epsilon)) return false; if (getNumberOfConvexPolygons() != other.getNumberOfConvexPolygons()) return false; for (int i = 0; i < getNumberOfConvexPolygons(); i++) { if (!convexPolygons.get(i).epsilonEquals(other.convexPolygons.get(i), epsilon)) return false; } return true; }
/** * Returns the last convex polygon representing a portion of this region and removes it from this * planar region. Special case: returns null when this region is empty. The polygon is expressed * in the region local coordinates. */ public ConvexPolygon2D pollLastConvexPolygon() { if (isEmpty()) return null; else return pollConvexPolygon(getNumberOfConvexPolygons() - 1); }
/** * Returns the last convex polygon representing a portion of this region. * Special case: returns null when this region is empty. * The polygon is expressed in the region local coordinates. */ public ConvexPolygon2d getLastConvexPolygon() { if (isEmpty()) return null; else return getConvexPolygon(getNumberOfConvexPolygons() - 1); }
/** * Returns the last convex polygon representing a portion of this region and removes it from this planar region. * Special case: returns null when this region is empty. * The polygon is expressed in the region local coordinates. */ public ConvexPolygon2d pollLastConvexPolygon() { if (isEmpty()) return null; else return pollConvexPolygon(getNumberOfConvexPolygons() - 1); }
public static double computePlanarRegionArea(PlanarRegion planarRegion) { double area = 0.0; for (int i = 0; i < planarRegion.getNumberOfConvexPolygons(); i++) { area += planarRegion.getConvexPolygon(i).getArea(); } return area; }
/** * Returns the last convex polygon representing a portion of this region. Special case: returns * null when this region is empty. The polygon is expressed in the region local coordinates. */ public ConvexPolygon2D getLastConvexPolygon() { if (isEmpty()) return null; else return getConvexPolygon(getNumberOfConvexPolygons() - 1); }
/** * 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); } } }
/** * 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); } } }
/** * 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); Point2DBasics[] intersectionPoints = polygonToCheck.intersectionWith(projectedLineSegment); if ((intersectionPoints != null) && (intersectionPoints.length > 0) && (intersectionPoints[0] != null)) return true; } // Did not find any intersection return false; }
/** * @return a full depth copy of this region. The copy can be entirely modified without interfering with this region. */ public PlanarRegion copy() { RigidBodyTransform transformToWorldCopy = new RigidBodyTransform(fromLocalToWorldTransform); List<Point2d[]> concaveHullsCopy = new ArrayList<>(); for (int hullIndex = 0; hullIndex < concaveHullsVertices.size(); hullIndex++) { Point2d[] hullVertices = new Point2d[concaveHullsVertices.get(hullIndex).length]; for (int i = 0; i < hullVertices.length; i++) hullVertices[i] = new Point2d(concaveHullsVertices.get(hullIndex)[i]); concaveHullsCopy.add(hullVertices); } List<ConvexPolygon2d> convexPolygonsCopy = new ArrayList<>(); for (int i = 0; i < getNumberOfConvexPolygons(); i++) convexPolygonsCopy.add(new ConvexPolygon2d(convexPolygons.get(i))); PlanarRegion planarRegion = new PlanarRegion(transformToWorldCopy, concaveHullsCopy, convexPolygonsCopy); planarRegion.setRegionId(regionId); return planarRegion; }
public void set(PlanarRegion other) { fromLocalToWorldTransform.set(other.fromLocalToWorldTransform); fromWorldToLocalTransform.set(other.fromWorldToLocalTransform); convexPolygons.clear(); for (int i = 0; i < other.getNumberOfConvexPolygons(); i++) convexPolygons.add(new ConvexPolygon2D(other.convexPolygons.get(i))); updateBoundingBox(); convexHull.set(other.convexHull); }
public void set(PlanarRegion other) { fromLocalToWorldTransform.set(other.fromLocalToWorldTransform); fromWorldToLocalTransform.set(other.fromWorldToLocalTransform); convexPolygons.clear(); for (int i = 0; i < other.getNumberOfConvexPolygons(); i++) convexPolygons.add(new ConvexPolygon2d(other.convexPolygons.get(i))); updateBoundingBox(); convexHull.setAndUpdate(other.convexHull); }
/** * @return a full depth copy of this region. The copy can be entirely modified without * interfering with this region. */ public PlanarRegion copy() { RigidBodyTransform transformToWorldCopy = new RigidBodyTransform(fromLocalToWorldTransform); Point2D[] concaveHullCopy = new Point2D[concaveHullsVertices.length]; for (int i = 0; i < concaveHullsVertices.length; i++) concaveHullCopy[i] = new Point2D(concaveHullsVertices[i]); List<ConvexPolygon2D> convexPolygonsCopy = new ArrayList<>(); for (int i = 0; i < getNumberOfConvexPolygons(); i++) convexPolygonsCopy.add(new ConvexPolygon2D(convexPolygons.get(i))); PlanarRegion planarRegion = new PlanarRegion(transformToWorldCopy, concaveHullCopy, convexPolygonsCopy); planarRegion.setRegionId(regionId); return planarRegion; }
private static void writePlanarRegionVertices(Path folderPath, PlanarRegion region, int regionIndex) throws IOException { File regionFile = new File(folderPath.toFile(), "region" + region.getRegionId() + "_" + regionIndex); FileWriter fileWriter = new FileWriter(regionFile); for (Point2D vertex : region.getConcaveHull()) { fileWriter.write(vertex.getX() + ", " + vertex.getY() + "\n"); } for (int polygonIndex = 0; polygonIndex < region.getNumberOfConvexPolygons(); polygonIndex++) { ConvexPolygon2D convexPolygon = region.getConvexPolygon(polygonIndex); for (int vertexIndex = 0; vertexIndex < convexPolygon.getNumberOfVertices(); vertexIndex++) { Point2DReadOnly vertex = convexPolygon.getVertex(vertexIndex); fileWriter.write(vertex.getX() + ", " + vertex.getY() + "\n"); } } fileWriter.close(); }
private static void checkPlanarRegionsEqual(int regionId, PlanarRegion planarRegionA, PlanarRegion planarRegionB) { Point3D centerA = new Point3D(); Point3D centerB = new Point3D(); planarRegionA.getPointInRegion(centerA); planarRegionB.getPointInRegion(centerB); Vector3D normalA = new Vector3D(); Vector3D normalB = new Vector3D(); planarRegionA.getNormal(normalA); planarRegionB.getNormal(normalB); EuclidCoreTestTools.assertPoint3DGeometricallyEquals("Center of regions " + regionId + " are not equal.", centerA, centerB, epsilon); EuclidCoreTestTools.assertVector3DGeometricallyEquals("Normal of regions " + regionId + " are not equal.", normalA, normalB, epsilon); assertEquals("Number of convex polygons of " + regionId + " not equal. ", planarRegionA.getNumberOfConvexPolygons(), planarRegionB.getNumberOfConvexPolygons()); for (int i = 0; i < planarRegionA.getNumberOfConvexPolygons(); i++) { assertTrue("Convex polygon " + i + " of planar region " + regionId + " is not equal", planarRegionA.getConvexPolygon(i).epsilonEquals(planarRegionB.getConvexPolygon(i), epsilon)); } }
private void updateConvexHull() { convexHull.clear(); for (int i = 0; i < this.getNumberOfConvexPolygons(); i++) { ConvexPolygon2d convexPolygon = this.getConvexPolygon(i); for (int j = 0; j < convexPolygon.getNumberOfVertices(); j++) convexHull.addVertex(convexPolygon.getVertex(j)); } convexHull.update(); }
private void updateConvexHull() { convexHull.clear(); for (int i = 0; i < this.getNumberOfConvexPolygons(); i++) { ConvexPolygon2D convexPolygon = this.getConvexPolygon(i); for (int j = 0; j < convexPolygon.getNumberOfVertices(); j++) convexHull.addVertex(convexPolygon.getVertex(j)); } convexHull.update(); }
private void updateBoundingBox() { boundingBox3dInWorld.set(Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN); for (int i = 0; i < this.getNumberOfConvexPolygons(); i++) { ConvexPolygon2d convexPolygon = this.getConvexPolygon(i); for (int j = 0; j < convexPolygon.getNumberOfVertices(); j++) { Point2d vertex = convexPolygon.getVertex(j); tempPointForConvexPolygonProjection.set(vertex.x, vertex.y, 0.0); fromLocalToWorldTransform.transform(tempPointForConvexPolygonProjection); this.boundingBox3dInWorld.updateToIncludePoint(tempPointForConvexPolygonProjection); } } }
private void updateBoundingBox() { boundingBox3dInWorld.set(Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN); for (int i = 0; i < this.getNumberOfConvexPolygons(); i++) { ConvexPolygon2D convexPolygon = this.getConvexPolygon(i); for (int j = 0; j < convexPolygon.getNumberOfVertices(); j++) { Point2DReadOnly vertex = convexPolygon.getVertex(j); tempPointForConvexPolygonProjection.set(vertex.getX(), vertex.getY(), 0.0); fromLocalToWorldTransform.transform(tempPointForConvexPolygonProjection); this.boundingBox3dInWorld.updateToIncludePoint(tempPointForConvexPolygonProjection); } } Point3DReadOnly minPoint = boundingBox3dInWorld.getMinPoint(); Point3DReadOnly maxPoint = boundingBox3dInWorld.getMaxPoint(); this.boundingBox3dInWorld.setMin(minPoint.getX() - boundingBoxEpsilon, minPoint.getY() - boundingBoxEpsilon, minPoint.getZ() - boundingBoxEpsilon); this.boundingBox3dInWorld.setMax(maxPoint.getX() + boundingBoxEpsilon, maxPoint.getY() + boundingBoxEpsilon, maxPoint.getZ() + boundingBoxEpsilon); }