private static void writePlanarRegionsData(Path folderPath, PlanarRegionsList planarRegionData) throws IOException { File header = new File(folderPath.toFile(), "header.txt"); FileWriter fileWriter = new FileWriter(header); Map<Integer, MutableInt> regionIdToIndex = new HashMap<>(); for (PlanarRegion region : planarRegionData.getPlanarRegionsAsList()) { Point3D origin = new Point3D(); Vector3D normal = new Vector3D(); region.getPointInRegion(origin); region.getNormal(normal); int numberOfConvexPolygons = region.getNumberOfConvexPolygons(); int[] convexPolygonsSizes = new int[numberOfConvexPolygons]; for (int i = 0; i < numberOfConvexPolygons; i++) convexPolygonsSizes[i] = region.getConvexPolygon(i).getNumberOfVertices(); int regionId = region.getRegionId(); MutableInt regionIndex = regionIdToIndex.getOrDefault(regionId, new MutableInt(0)); regionIdToIndex.put(regionId, regionIndex); regionIndex.increment(); fileWriter.write("regionId: " + Integer.toString(regionId)); fileWriter.write(", index: " + Integer.toString(regionIndex.getValue().intValue())); fileWriter.write(", origin: " + origin.getX() + ", " + origin.getY() + ", " + origin.getZ()); fileWriter.write(", normal: " + normal.getX() + ", " + normal.getY() + ", " + normal.getZ()); fileWriter.write(", concave hull size: " + region.getConcaveHullSize()); fileWriter.write(", number of convex polygons: " + numberOfConvexPolygons + ", " + Arrays.toString(convexPolygonsSizes)); fileWriter.write("\n"); writePlanarRegionVertices(folderPath, region, regionIndex.intValue()); } fileWriter.close(); }
public static boolean isPointInLocalInsidePlanarRegion(PlanarRegion planarRegion, Point2DReadOnly pointInLocalToCheck, double epsilon) { ConvexPolygon2D convexHull = planarRegion.getConvexHull(); BoundingBox2D boundingBox = convexHull.getBoundingBox(); if (planarRegion.getConcaveHullSize() < convexHull.getNumberOfVertices()) throw new IllegalArgumentException("The concave hull of this polygon is not valid."); if (!boundingBox.isInsideEpsilon(pointInLocalToCheck, epsilon)) return false; if (!convexHull.isPointInside(pointInLocalToCheck, epsilon)) return false; if (MathTools.epsilonEquals(0.0, epsilon, 1.0e-10)) { return isPointInsidePolygon(planarRegion.getConcaveHull(), pointInLocalToCheck); } else { double[] epsilons = new double[planarRegion.getConcaveHullSize()]; Arrays.fill(epsilons, epsilon); List<Point2D> concaveHull = ClusterTools.extrudePolygon(true, Arrays.asList(planarRegion.getConcaveHull()), epsilons); return isPointInsidePolygon(concaveHull, pointInLocalToCheck); } }
@Override public boolean isPlanarRegionRelevant(PlanarRegion region) { if (region.getConcaveHullSize() < getPlanarRegionMinSize()) return false; if (!Double.isFinite(getPlanarRegionMinArea()) || getPlanarRegionMinArea() <= 0.0) return true; return PlanarRegionTools.computePlanarRegionArea(region) >= getPlanarRegionMinArea(); } };
planeNormalInRegionFrame.applyInverseTransform(transformFromRegionToWorld); Point2DReadOnly vertex2D = planarRegionToTruncate.getConcaveHullVertex(planarRegionToTruncate.getConcaveHullSize() - 1); Point3D vertex3D = new Point3D(vertex2D); double previousSignedDistance = signedDistanceFromPoint3DToPlane3D(vertex3D, pointOnPlaneInRegionFrame, planeNormalInRegionFrame); double epsilonDistance = 1.0e-10; for (int i = 0; i < planarRegionToTruncate.getConcaveHullSize(); i++)
public static void assertPlanarRegionsEqual(PlanarRegion expected, PlanarRegion actual, double epsilon) { RigidBodyTransform expectedTransform = new RigidBodyTransform(); RigidBodyTransform actualTransform = new RigidBodyTransform(); expected.getTransformToWorld(expectedTransform); actual.getTransformToWorld(actualTransform); EuclidCoreTestTools.assertRigidBodyTransformGeometricallyEquals(expectedTransform, actualTransform, epsilon); assertEquals(expected.getConcaveHullSize(), actual.getConcaveHullSize()); for (int i = 0; i < expected.getConcaveHullSize(); i++) { EuclidCoreTestTools.assertTuple2DEquals(expected.getConcaveHullVertex(i), actual.getConcaveHullVertex(i), epsilon); } assertEquals(expected.getNumberOfConvexPolygons(), actual.getNumberOfConvexPolygons()); for (int i = 0; i < expected.getNumberOfConvexPolygons(); i++) { ConvexPolygon2D expectedConvexPolygon = expected.getConvexPolygon(i); ConvexPolygon2D actualConvexPolygon = actual.getConvexPolygon(i); assertEquals(expectedConvexPolygon.getNumberOfVertices(), actualConvexPolygon.getNumberOfVertices()); assertTrue(expectedConvexPolygon.epsilonEquals(actualConvexPolygon, epsilon)); } } }