/** * 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); }
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); }
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(); }
/** * 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); }
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 Node createConvexPolygonGraphics(PlanarRegion data) { List<ConvexPolygon2D> convexPolygons = new ArrayList<>(); for (int i = 0; i < data.getNumberOfConvexPolygons(); i++) convexPolygons.add(data.getConvexPolygon(i)); JavaFXMultiColorMeshBuilder meshBuilder = new JavaFXMultiColorMeshBuilder(new TextureColorAdaptivePalette(64)); int regionId = data.getRegionId(); RigidBodyTransform rigidBodyTransform = new RigidBodyTransform(); data.getTransformToWorld(rigidBodyTransform); Color regionColor = OcTreeMeshBuilder.getRegionColor(regionId); for (int i = 0; i < convexPolygons.size(); i++) { ConvexPolygon2D convexPolygon = convexPolygons.get(i); Color color = Color.hsb(regionColor.getHue(), 0.9, 0.5 + 0.5 * ((double) i / (double) convexPolygons.size())); meshBuilder.addPolygon(rigidBodyTransform, convexPolygon, color); } MeshView meshView = new MeshView(meshBuilder.generateMesh()); meshView.setMaterial(meshBuilder.generateMaterial()); 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); }
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); } } }
/** * Filter all the polygons and regions such that only polygon and regions with data remain. * Simplifies the algorithm for updating the YoVariables in {@link #processPlanarRegionsListQueue()}. * @param planarRegionsList the list of planar regions with non-empty regions and non-empty polygons. * @return */ private PlanarRegionsList filterEmptyRegionsAndEmptyPolygons(PlanarRegionsList planarRegionsList) { for (int regionIndex = planarRegionsList.getNumberOfPlanarRegions() - 1; regionIndex >= 0; regionIndex--) { PlanarRegion planarRegion = planarRegionsList.getPlanarRegion(regionIndex); for (int polygonIndex = planarRegion.getNumberOfConvexPolygons() - 1; polygonIndex >= 0; polygonIndex--) { if (planarRegion.getConvexPolygon(polygonIndex).isEmpty()) planarRegion.pollConvexPolygon(polygonIndex); } if (planarRegion.isEmpty()) planarRegionsList.pollPlanarRegion(regionIndex); } if (planarRegionsList.isEmpty()) return null; return planarRegionsList; }
/** * Filter all the polygons and regions such that only polygon and regions with data remain. * Simplifies the algorithm for updating the YoVariables in {@link #processPlanarRegionsListQueue()}. * @param planarRegionsList the list of planar regions with non-empty regions and non-empty polygons. * @return */ private PlanarRegionsList filterEmptyRegionsAndEmptyPolygons(PlanarRegionsList planarRegionsList) { for (int regionIndex = planarRegionsList.getNumberOfPlanarRegions() - 1; regionIndex >= 0; regionIndex--) { PlanarRegion planarRegion = planarRegionsList.getPlanarRegion(regionIndex); for (int polygonIndex = planarRegion.getNumberOfConvexPolygons() - 1; polygonIndex >= 0; polygonIndex--) { if (planarRegion.getConvexPolygon(polygonIndex).isEmpty()) planarRegion.pollConvexPolygon(polygonIndex); } if (planarRegion.isEmpty()) planarRegionsList.pollPlanarRegion(regionIndex); } if (planarRegionsList.isEmpty()) return null; return planarRegionsList; }
/** * Adds a PlanarRegion transforming from the current coordinate system. * Uses the given appearances in order, one for each Polygon in the PlanarRegion. Then loops on the appearances. * * @param planarRegion */ public static void addPlanarRegion(Graphics3DObject graphics3DObject, PlanarRegion planarRegion, AppearanceDefinition... appearances) { int numberOfConvexPolygons = planarRegion.getNumberOfConvexPolygons(); RigidBodyTransform transform = new RigidBodyTransform(); planarRegion.getTransformToWorld(transform); graphics3DObject.transform(transform); for (int i = 0; i < numberOfConvexPolygons; i++) { ConvexPolygon2D convexPolygon = planarRegion.getConvexPolygon(i); MeshDataHolder meshDataHolder = MeshDataGenerator.ExtrudedPolygon(convexPolygon, -0.0001); graphics3DObject.addInstruction(new Graphics3DAddMeshDataInstruction(meshDataHolder, appearances[i % appearances.length])); } transform.invert(); graphics3DObject.transform(transform); }
/** * Adds a PlanarRegion transforming from the current coordinate system. * Uses the given appearances in order, one for each Polygon in the PlanarRegion. Then loops on the appearances. * * @param planarRegion */ public void addPlanarRegion(PlanarRegion planarRegion, AppearanceDefinition... appearances) { int numberOfConvexPolygons = planarRegion.getNumberOfConvexPolygons(); RigidBodyTransform transform = new RigidBodyTransform(); planarRegion.getTransformToWorld(transform); transform(transform); for (int i = 0; i < numberOfConvexPolygons; i++) { ConvexPolygon2d convexPolygon = planarRegion.getConvexPolygon(i); MeshDataHolder meshDataHolder = MeshDataGenerator.ExtrudedPolygon(convexPolygon, 0.005); addInstruction(new Graphics3DAddMeshDataInstruction(meshDataHolder, appearances[i % appearances.length])); } transform.invert(); transform(transform); }
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)); } } }
/** * Will return the intersection point between a line and a single planar region. If the line does * not intersect the region this method will return null. */ public static Point3D intersectRegionWithLine(PlanarRegion region, Line3D projectionLineInWorld) { RigidBodyTransform regionToWorld = new RigidBodyTransform(); region.getTransformToWorld(regionToWorld); Vector3DReadOnly planeNormal = new Vector3D(0.0, 0.0, 1.0); Point3DReadOnly pointOnPlane = new Point3D(region.getConvexPolygon(0).getVertex(0)); Point3DBasics pointOnLineInLocal = new Point3D(projectionLineInWorld.getPoint()); Vector3DBasics directionOfLineInLocal = new Vector3D(projectionLineInWorld.getDirection()); pointOnLineInLocal.applyInverseTransform(regionToWorld); directionOfLineInLocal.applyInverseTransform(regionToWorld); Point3D intersectionWithPlaneInLocal = EuclidGeometryTools.intersectionBetweenLine3DAndPlane3D(pointOnPlane, planeNormal, pointOnLineInLocal, directionOfLineInLocal); if (intersectionWithPlaneInLocal == null) { return null; } if (region.isPointInside(intersectionWithPlaneInLocal.getX(), intersectionWithPlaneInLocal.getY())) { intersectionWithPlaneInLocal.applyTransform(regionToWorld); return intersectionWithPlaneInLocal; } return null; }
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 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); }
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); }