public static List<PlanarRegion> filterPlanarRegionsByHullSize(int minNumberOfVertices, List<PlanarRegion> planarRegions) { if (minNumberOfVertices <= 0) return planarRegions; return planarRegions.stream().filter(region -> region.getConcaveHull().length >= minNumberOfVertices).collect(Collectors.toList()); }
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); } }
public static List<PlanarRegion> ensureClockwiseOrder(List<PlanarRegion> planarRegions) { List<PlanarRegion> copies = new ArrayList<>(planarRegions.size()); for (PlanarRegion planarRegion : planarRegions) { PlanarRegion copy = planarRegion.copy(); List<Point2DReadOnly> concaveHullVertices = Arrays.asList(copy.getConcaveHull()); ConcaveHullTools.ensureClockwiseOrdering(concaveHullVertices); copies.add(copy); } return copies; }
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(); }
public static boolean isPlanarRegionIntersectingWithCapsule(LineSegment3D capsuleSegmentInWorld, double capsuleRadius, PlanarRegion query) { RigidBodyTransform transformToWorld = new RigidBodyTransform(); query.getTransformToWorld(transformToWorld); Point3D firstEndPointInLocal = new Point3D(capsuleSegmentInWorld.getFirstEndpoint()); Point3D secondEndPointInLocal = new Point3D(capsuleSegmentInWorld.getSecondEndpoint()); firstEndPointInLocal.applyInverseTransform(transformToWorld); secondEndPointInLocal.applyInverseTransform(transformToWorld); Point2D[] concaveHull = query.getConcaveHull(); List<Point3D> convexPolygon3D = new ArrayList<>(); for (int i = 0; i < concaveHull.length; i++) convexPolygon3D.add(new Point3D(concaveHull[i])); double minDistanceToEdge = getDistanceFromLineSegment3DToConvexPolygon(firstEndPointInLocal, secondEndPointInLocal, convexPolygon3D); return minDistanceToEdge <= capsuleRadius; }
private Node createConcaveHullGraphics(PlanarRegion data) { int regionId = data.getRegionId(); JavaFXMultiColorMeshBuilder meshBuilder = new JavaFXMultiColorMeshBuilder(new TextureColorAdaptivePalette(16)); RigidBodyTransform transform = new RigidBodyTransform(); data.getTransformToWorld(transform); Point2D[] concaveHullVerticesLocal = data.getConcaveHull(); Color regionColor = OcTreeMeshBuilder.getRegionColor(regionId); List<Point3D> concaveHullVertices = Arrays.stream(concaveHullVerticesLocal).map(Point3D::new).map(p -> { transform.transform(p); return p; }).collect(Collectors.toList()); for (int vertexIndex = 0; vertexIndex < concaveHullVertices.size(); vertexIndex++) { Point3D vertex = concaveHullVertices.get(vertexIndex); Point3D nextVertex = ListWrappingIndexTools.getNext(vertexIndex, concaveHullVertices); Color lineColor = Color.hsb(regionColor.getHue(), regionColor.getSaturation(), regionColor.getBrightness()); meshBuilder.addLine(vertex, nextVertex, 0.0015, lineColor); } MeshView meshView = new MeshView(meshBuilder.generateMesh()); meshView.setMaterial(meshBuilder.generateMaterial()); meshView.setMouseTransparent(true); return meshView; }
private void buildMeshAndMaterial(PlanarRegionsList planarRegionsList) { if (VERBOSE) PrintTools.info(this, "Creating mesh and material for new planar regions."); RigidBodyTransform transformToWorld = new RigidBodyTransform(); List<MeshView> regionMeshViews = new ArrayList<>(); for (int regionIndex = 0; regionIndex < planarRegionsList.getNumberOfPlanarRegions(); regionIndex++) { JavaFXMeshBuilder meshBuilder = new JavaFXMeshBuilder(); PlanarRegion planarRegion = planarRegionsList.getPlanarRegion(regionIndex); int regionId = planarRegion.getRegionId(); planarRegion.getTransformToWorld(transformToWorld); meshBuilder.addMultiLine(transformToWorld, Arrays.asList(planarRegion.getConcaveHull()), VisualizationParameters.CONCAVEHULL_LINE_THICKNESS, true); for (int polygonIndex = 0; polygonIndex < planarRegion.getNumberOfConvexPolygons(); polygonIndex++) { ConvexPolygon2D convexPolygon2d = planarRegion.getConvexPolygon(polygonIndex); meshBuilder.addPolygon(transformToWorld, convexPolygon2d); } MeshView regionMeshView = new MeshView(meshBuilder.generateMesh()); regionMeshView.setMaterial(new PhongMaterial(getRegionColor(regionId))); regionMeshViews.add(regionMeshView); } graphicsToRender.set(regionMeshViews); }
@Test(timeout = 30000) @ContinuousIntegrationTest(estimatedDuration = 0.0) public void testTruncatePlanarRegionIfIntersectingWithPlane() throws Exception { Point3D groundOrigin = new Point3D(); Vector3D groundNormal = new Vector3D(0.0, 0.0, 1.0); Point3D squareOrigin = new Point3D(0.0, 0.0, -0.001); Vector3D squareNormal = new Vector3D(0.0, -1.0, 0.0); AxisAngle squareOrientation = EuclidGeometryTools.axisAngleFromZUpToVector3D(squareNormal); RigidBodyTransform squarePose = new RigidBodyTransform(squareOrientation, squareOrigin); double squareSide = 4.0; Point2D[] concaveHullVertices = {new Point2D(0.0, 0.0), new Point2D(0.0, squareSide), new Point2D(squareSide, squareSide), new Point2D(squareSide, 0.0)}; List<ConvexPolygon2D> convexPolygons = new ArrayList<>(); convexPolygons.add(new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(concaveHullVertices))); PlanarRegion verticalSquare = new PlanarRegion(squarePose, concaveHullVertices, convexPolygons); Point3D[] expectedVerticesInWorld = Arrays.stream(concaveHullVertices).map(p -> toWorld(p, squarePose)).toArray(Point3D[]::new); expectedVerticesInWorld[0].addZ(0.001); expectedVerticesInWorld[3].addZ(0.001); PlanarRegion truncatedSquare = PlanarRegionTools.truncatePlanarRegionIfIntersectingWithPlane(groundOrigin, groundNormal, verticalSquare, 0.05, null); RigidBodyTransform truncatedTransform = new RigidBodyTransform(); truncatedSquare.getTransformToWorld(truncatedTransform); EuclidCoreTestTools.assertRigidBodyTransformGeometricallyEquals(squarePose, truncatedTransform, EPSILON); Point3D[] actualVerticesInWorld = Arrays.stream(truncatedSquare.getConcaveHull()).map(p -> toWorld(p, squarePose)).toArray(Point3D[]::new); assertEquals(expectedVerticesInWorld.length, actualVerticesInWorld.length); for (int i = 0; i < expectedVerticesInWorld.length; i++) EuclidCoreTestTools.assertPoint3DGeometricallyEquals(expectedVerticesInWorld[i], actualVerticesInWorld[i], EPSILON); }
Point2d[] hullVertices = planarRegion.getConcaveHull(hullIndex); Point2f[] messageHullVertices = new Point2f[hullVertices.length];
/** * From the local coordinates of the {@code regionB}, this method computes and return the minimum * z-coordinate among the vertices of {@code regionA}'s concave hull. * * @param regionA the query. Not modified. * @param regionB the reference. Not modified. * @return the height of the lowest vertex of {@code regionA} above {@code regionB}. The returned * value is negative if the lowest vertex is below {@code regionB}. */ public static double computeMinHeightOfRegionAAboveRegionB(PlanarRegion regionA, PlanarRegion regionB) { RigidBodyTransform transformFromBToWorld = new RigidBodyTransform(); regionB.getTransformToWorld(transformFromBToWorld); RigidBodyTransform transformFromAToB = new RigidBodyTransform(); regionA.getTransformToWorld(transformFromAToB); transformFromAToB.preMultiplyInvertOther(transformFromBToWorld); double minZ = Double.POSITIVE_INFINITY; for (int i = 0; i < regionA.getConvexHull().getNumberOfVertices(); i++) { Point3D vertexA3D = new Point3D(regionA.getConcaveHull()[i]); transformFromAToB.transform(vertexA3D); minZ = Math.min(minZ, vertexA3D.getZ()); } return minZ; }
List<Point2DReadOnly> concaveHullVertices = new ArrayList<>(Arrays.asList(planarRegion.getConcaveHull())); double depthThreshold = 0.05; List<ConvexPolygon2D> convexPolygons = new ArrayList<>();
public static Cluster createHomeRegionCluster(PlanarRegion homeRegion, NavigableExtrusionDistanceCalculator calculator) { RigidBodyTransform transformToWorld = new RigidBodyTransform(); homeRegion.getTransformToWorld(transformToWorld); Cluster cluster = new Cluster(); cluster.setType(Type.POLYGON); cluster.setTransformToWorld(transformToWorld); cluster.addRawPointsInLocal2D(homeRegion.getConcaveHull()); cluster.setExtrusionSide(ExtrusionSide.INSIDE); double extrusionDistance = calculator.computeExtrusionDistance(homeRegion); ObstacleExtrusionDistanceCalculator nonNavigableCalculator = (p, h) -> extrusionDistance - NAV_TO_NON_NAV_DISTANCE; ObstacleExtrusionDistanceCalculator navigableCalculator = (p, h) -> extrusionDistance; boolean extrudeToTheLeft = cluster.getExtrusionSide() != ExtrusionSide.INSIDE; cluster.addNonNavigableExtrusionsInLocal(extrudePolygon(extrudeToTheLeft, cluster, nonNavigableCalculator)); cluster.addNavigableExtrusionsInLocal(extrudePolygon(extrudeToTheLeft, cluster, navigableCalculator)); cluster.updateBoundingBox(); return cluster; }
private Pair<Mesh, Material> generateMeshAndMaterial(PlanarRegionsListMessage newMessage) { meshBuilder.clear(); double lineWidth = 0.01; RigidBodyTransform transformToWorld = new RigidBodyTransform(); PlanarRegionsList planarRegionsList = PlanarRegionMessageConverter.convertToPlanarRegionsList(newMessage); for (int regionIndex = 0; regionIndex < planarRegionsList.getNumberOfPlanarRegions(); regionIndex++) { PlanarRegion planarRegion = planarRegionsList.getPlanarRegion(regionIndex); int regionId = planarRegion.getRegionId(); Color regionColor = getRegionColor(regionId); planarRegion.getTransformToWorld(transformToWorld); meshBuilder.addMultiLine(transformToWorld, planarRegion.getConcaveHull(), lineWidth, regionColor, true); for (int polygonIndex = 0; polygonIndex < planarRegion.getNumberOfConvexPolygons(); polygonIndex++) { ConvexPolygon2D convexPolygon2d = planarRegion.getConvexPolygon(polygonIndex); regionColor = Color.hsb(regionColor.getHue(), 0.9, 0.5 + 0.5 * ((double) polygonIndex / (double) planarRegion.getNumberOfConvexPolygons())); meshBuilder.addPolygon(transformToWorld, convexPolygon2d, regionColor); } } Material material = meshBuilder.generateMaterial(); Mesh mesh = meshBuilder.generateMesh(); return new Pair<>(mesh, material); }
private Pair<Mesh, Material> generateMeshAndMaterial(PlanarRegionsListMessage newMessage) { meshBuilder.clear(); double lineWidth = 0.01; RigidBodyTransform transformToWorld = new RigidBodyTransform(); PlanarRegionsList planarRegionsList = PlanarRegionMessageConverter.convertToPlanarRegionsList(newMessage); for (int regionIndex = 0; regionIndex < planarRegionsList.getNumberOfPlanarRegions(); regionIndex++) { PlanarRegion planarRegion = planarRegionsList.getPlanarRegion(regionIndex); int regionId = planarRegion.getRegionId(); Color regionColor = getRegionColor(regionId); planarRegion.getTransformToWorld(transformToWorld); meshBuilder.addMultiLine(transformToWorld, planarRegion.getConcaveHull(), lineWidth, regionColor, true); for (int polygonIndex = 0; polygonIndex < planarRegion.getNumberOfConvexPolygons(); polygonIndex++) { ConvexPolygon2D convexPolygon2d = planarRegion.getConvexPolygon(polygonIndex); regionColor = Color.hsb(regionColor.getHue(), 0.9, 0.5 + 0.5 * ((double) polygonIndex / (double) planarRegion.getNumberOfConvexPolygons())); meshBuilder.addPolygon(transformToWorld, convexPolygon2d, regionColor); } } Material material = meshBuilder.generateMaterial(); Mesh mesh = meshBuilder.generateMesh(); return new Pair<>(mesh, material); }