public static List<PlanarRegion> filterRegionsByTruncatingVerticesBeneathHomeRegion(List<PlanarRegion> regionsToCheck, PlanarRegion homeRegion, double depthThresholdForConvexDecomposition, PlanarRegionFilter filter) { List<PlanarRegion> filteredList = new ArrayList<>(); Point3D pointOnPlane = new Point3D(); Vector3D planeNormal = new Vector3D(); homeRegion.getPointInRegion(pointOnPlane); homeRegion.getNormal(planeNormal); for (PlanarRegion regionToCheck : regionsToCheck) { PlanarRegion truncatedPlanarRegion = truncatePlanarRegionIfIntersectingWithPlane(pointOnPlane, planeNormal, regionToCheck, depthThresholdForConvexDecomposition, filter); if (truncatedPlanarRegion != null) filteredList.add(truncatedPlanarRegion); } return filteredList; }
closestContainer.getHomeRegion().getPointInRegion(pointOnRegion); double minDistance = EuclidGeometryTools.distanceFromPoint3DToPlane3D(point, pointOnRegion, regionNormal); candidate.getHomeRegion().getPointInRegion(pointOnRegion); double distance = EuclidGeometryTools.distanceFromPoint3DToPlane3D(point, pointOnRegion, regionNormal); if (distance < minDistance)
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(); }
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)); } }
Vector3f regionNormal = new Vector3f(); planarRegion.getPointInRegion(regionOrigin); planarRegion.getNormal(regionNormal);
planarRegionToTruncate.getPointInRegion(pointOnRegion); planarRegionToTruncate.getNormal(regionNormal);
EuclidCoreTestTools.assertTuple3DEquals("Wrong region normal.", new Vector3D(0.0, 0.0, 1.0), actualNormal, 1.0e-10); Point3D actualOrigin = new Point3D(); planarRegion.getPointInRegion(actualOrigin); EuclidCoreTestTools.assertTuple3DEquals("Wrong region origin.", new Point3D(), actualOrigin, 1.0e-10); RigidBodyTransform actualTransform = new RigidBodyTransform();
regionTransform.transform(expectedOrigin); Point3D actualOrigin = new Point3D(); planarRegion.getPointInRegion(actualOrigin); EuclidCoreTestTools.assertTuple3DEquals("Wrong region origin.", expectedOrigin, actualOrigin, 1.0e-10); RigidBodyTransform actualTransform = new RigidBodyTransform();