@Override public int getMapId() { return homeRegion.getRegionId(); }
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 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 checkPlanarRegionListsAreEqual(PlanarRegionsList listA, PlanarRegionsList listB) { assertEquals("Planar region lists are different sizes.", listA.getNumberOfPlanarRegions(), listB.getNumberOfPlanarRegions()); for (int i = 0; i < listA.getNumberOfPlanarRegions(); i++) { PlanarRegion planarRegionA = listA.getPlanarRegion(i); PlanarRegion planarRegionB = null; for (int j = 0; j < listB.getNumberOfPlanarRegions(); j++) { if (planarRegionA.getRegionId() == listB.getPlanarRegion(j).getRegionId()) { planarRegionB = listB.getPlanarRegion(j); break; } } assertFalse("Unable to find equivalent planar region", planarRegionB == null); checkPlanarRegionsEqual(i, planarRegionA, planarRegionB); } }
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 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); }
public static PlanarRegionMessage convertToPlanarRegionMessage(PlanarRegion planarRegion) int regionId = planarRegion.getRegionId(); Point3f regionOrigin = new Point3f(); Vector3f regionNormal = new Vector3f();
truncatedConcaveHullVertices.toArray(concaveHullVertices); PlanarRegion truncatedRegion = new PlanarRegion(transformFromRegionToWorld, concaveHullVertices, truncatedConvexPolygons); truncatedRegion.setRegionId(planarRegionToTruncate.getRegionId()); if (filter == null || filter.isPlanarRegionRelevant(truncatedRegion)) return truncatedRegion;
originalRegion.getTransformToWorld(transform); PlanarRegion newRegion = new PlanarRegion(transform, polygon); newRegion.setRegionId(regions.getPlanarRegion(i).getRegionId()); visible.addPlanarRegion(newRegion);
planarRegionToProcess.getTransformToWorld(regionTransform); currentRegionPose.setPose(regionTransform); currentRegionId.set(planarRegionToProcess.getRegionId());
planarRegionToProcess.getTransformToWorld(regionTransform); currentRegionPose.set(regionTransform); currentRegionId.set(planarRegionToProcess.getRegionId());
combinedRegion.setRegionId(regionA.getRegionId()); ret.addPlanarRegion(combinedRegion); foundMatchingRegion = true; region.setRegionId(regionA.getRegionId()); ret.addPlanarRegion(region);
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); }