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 createConvexDecompositionGraphics(PlanarRegionSegmentationRawData rawData, ConcaveHullFactoryResult concaveHullFactoryResult) { ConcaveHullCollection concaveHullCollection = concaveHullFactoryResult.getConcaveHullCollection(); double depthThreshold = polygonizerParameters.getDepthThreshold(); List<ConvexPolygon2D> convexPolygons = new ArrayList<>(); ConcaveHullDecomposition.recursiveApproximateDecomposition(concaveHullCollection, depthThreshold, convexPolygons); JavaFXMultiColorMeshBuilder meshBuilder = new JavaFXMultiColorMeshBuilder(new TextureColorAdaptivePalette(64)); int regionId = rawData.getRegionId(); RigidBodyTransform rigidBodyTransform = rawData.getTransformFromLocalToWorld(); 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 processOccupancyMapMessage(FootstepPlannerOccupancyMapMessage message) { palette.clearPalette(); JavaFXMultiColorMeshBuilder meshBuilder = new JavaFXMultiColorMeshBuilder(palette); Object<FootstepPlannerCellMessage> occupiedCells = message.getOccupiedCells(); for (int i = 0; i < occupiedCells.size(); i++) { FootstepPlannerCellMessage cell = occupiedCells.get(i); double x = cell.getXIndex() * FootstepNode.gridSizeXY; double y = cell.getYIndex() * FootstepNode.gridSizeXY; double z = getHeightAtPoint(x, y) + nodeOffsetZ; RigidBodyTransform transform = new RigidBodyTransform(); transform.setTranslation(x, y, z); if (cell.getNodeIsValid()) meshBuilder.addPolygon(transform, cellPolygon, validCellColor); else meshBuilder.addPolygon(transform, cellPolygon, rejectedCellColor); } footstepGraphToRender.set(new Pair<>(meshBuilder.generateMesh(), meshBuilder.generateMaterial())); }
private synchronized void processFootstepPath(FootstepPlan plan) { meshBuilder.clear(); SideDependentList<Color> colors = solutionWasReceived.get() ? solutionFootstepColors : intermediateFootstepColors; FramePose3D footPose = new FramePose3D(); RigidBodyTransform transformToWorld = new RigidBodyTransform(); ConvexPolygon2D foothold = new ConvexPolygon2D(); for (int i = 0; i < plan.getNumberOfSteps(); i++) { SimpleFootstep footstep = plan.getFootstep(i); Color regionColor = colors.get(footstep.getRobotSide()); footstep.getSoleFramePose(footPose); footPose.get(transformToWorld); transformToWorld.appendTranslation(0.0, 0.0, 0.01); if (footstep.hasFoothold()) footstep.getFoothold(foothold); else foothold.set(defaultFootPolygon); Point2D[] vertices = new Point2D[foothold.getNumberOfVertices()]; for (int j = 0; j < vertices.length; j++) { vertices[j] = new Point2D(foothold.getVertex(j)); } meshBuilder.addMultiLine(transformToWorld, vertices, 0.01, regionColor, true); meshBuilder.addPolygon(transformToWorld, foothold, regionColor); } meshReference.set(new Pair<>(meshBuilder.generateMesh(), meshBuilder.generateMaterial())); }
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); }
private void processFootMesh(FootstepNode node, FootstepNodeSnapData snapData, boolean valid) { meshBuilder.clear(); RigidBodyTransform planarTransformToWorld = new RigidBodyTransform(); FootstepNodeTools.getNodeTransform(node, planarTransformToWorld); RigidBodyTransform snappedTransformToWorld = new RigidBodyTransform(); ConvexPolygon2D foothold = snapData.getCroppedFoothold(); FootstepNodeTools.getSnappedNodeTransform(node, snapData.getSnapTransform(), snappedTransformToWorld); snappedTransformToWorld.appendTranslation(0.0, 0.0, 0.01); planarTransformToWorld.setTranslationZ(snappedTransformToWorld.getTranslationZ() + 0.1); Color regionColor = valid ? Color.GREEN : Color.RED; regionColor = Color.hsb(regionColor.getHue(), 0.9, 1.0); Point2D[] vertices = new Point2D[foothold.getNumberOfVertices()]; for (int j = 0; j < vertices.length; j++) { vertices[j] = new Point2D(foothold.getVertex(j)); } meshBuilder.addMultiLine(snappedTransformToWorld, vertices, 0.01, regionColor, true); meshBuilder.addPolygon(snappedTransformToWorld, foothold, regionColor); // TODO add mesh of planar footstep meshView.setOpacity(0.9); meshView.setMesh(meshBuilder.generateMesh()); meshView.setMaterial(meshBuilder.generateMaterial()); }