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 createBorderTrianglesGraphics(PlanarRegionSegmentationRawData rawData, ConcaveHullFactoryResult concaveHullFactoryResult) { JavaFXMultiColorMeshBuilder meshBuilder = new JavaFXMultiColorMeshBuilder(new TextureColorAdaptivePalette(512)); Point3D planeOrigin = rawData.getOrigin(); Vector3D planeNormal = rawData.getNormal(); for (ConcaveHullVariables intermediateVariables : concaveHullFactoryResult.getIntermediateVariables()) { Set<QuadEdgeTriangle> borderTriangles = intermediateVariables.getBorderTriangles(); for (QuadEdgeTriangle borderTriangle : borderTriangles) { List<Point2D> triangleVerticesLocal = Arrays.stream(borderTriangle.getVertices()).map(v -> new Point2D(v.getX(), v.getY())) .collect(Collectors.toList()); List<Point3D> triangleVerticesWorld = PolygonizerTools.toPointsInWorld(triangleVerticesLocal, planeOrigin, planeNormal); double hue = 360.0 * random.nextDouble(); double saturation = 0.8 * random.nextDouble() + 0.1; double brightness = 0.9; meshBuilder.addPolyon(triangleVerticesWorld, Color.hsb(hue, saturation, brightness)); } } MeshView trianglesMeshView = new MeshView(meshBuilder.generateMesh()); trianglesMeshView.setMaterial(meshBuilder.generateMaterial()); return trianglesMeshView; }
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 Node createDelaunayTriangulationGraphics(PlanarRegionSegmentationRawData rawData, ConcaveHullFactoryResult concaveHullFactoryResult) { JavaFXMultiColorMeshBuilder meshBuilder = new JavaFXMultiColorMeshBuilder(new TextureColorAdaptivePalette(512)); Point3D planeOrigin = rawData.getOrigin(); Vector3D planeNormal = rawData.getNormal(); List<QuadEdgeTriangle> allTriangles = concaveHullFactoryResult.getAllTriangles(); for (QuadEdgeTriangle triangle : allTriangles) { List<Point2D> triangleVerticesLocal = Arrays.stream(triangle.getVertices()).map(v -> new Point2D(v.getX(), v.getY())).collect(Collectors.toList()); triangleVerticesLocal.forEach(vertex -> { vertex.setX(vertex.getX() * scaleX); vertex.setY(vertex.getY() * scaleY); }); List<Point3D> triangleVerticesWorld = PolygonizerTools.toPointsInWorld(triangleVerticesLocal, planeOrigin, planeNormal); double hue = 360.0 * random.nextDouble(); double saturation = 0.8 * random.nextDouble() + 0.1; double brightness = 0.9; meshBuilder.addPolyon(triangleVerticesWorld, Color.hsb(hue, saturation, brightness)); } MeshView trianglesMeshView = new MeshView(meshBuilder.generateMesh()); trianglesMeshView.setMaterial(meshBuilder.generateMaterial()); return trianglesMeshView; }
@Override public void run() { if (!enable.get()) return; LidarScanMessage message = newMessageToRender.getAndSet(null); if (message == null) return; Point3D32 scanPoint = new Point3D32(); meshBuilder.clear(); int numberOfScanPoints = message.getScan().size() / 3; for (int i = 0; i < numberOfScanPoints; i++) { double alpha = i / (double) numberOfScanPoints; Color color = Color.hsb(alpha * 240.0, 1.0, 1.0); MessageTools.unpackScanPoint(message, i, scanPoint); meshBuilder.addMesh(MeshDataGenerator.Tetrahedron(SCAN_POINT_SIZE), scanPoint, color); } MeshView scanMeshView = new MeshView(meshBuilder.generateMesh()); scanMeshView.setMaterial(meshBuilder.generateMaterial()); scanMeshToRender.set(scanMeshView); meshBuilder.clear(); }
private void computeScanMesh() { LidarScanMessage message = newMessageToRender.getAndSet(null); if (message == null) return; Point3D32 scanPoint = new Point3D32(); scanMeshBuilder.clear(); int numberOfScanPoints = message.getScan().size() / 3; for (int i = 0; i < numberOfScanPoints; i++) { double alpha = i / (double) numberOfScanPoints; Color color = Color.hsb(alpha * 240.0, 1.0, 1.0); MessageTools.unpackScanPoint(message, i, scanPoint); scanMeshBuilder.addMesh(MeshDataGenerator.Tetrahedron(SCAN_POINT_SIZE), scanPoint, color); } MeshView scanMeshView = new MeshView(scanMeshBuilder.generateMesh()); scanMeshView.setMaterial(scanMeshBuilder.generateMaterial()); scanMeshToRender.set(scanMeshView); scanMeshBuilder.clear(); }
private void computeScanMesh() { LidarScanMessage message = newMessageToRender.getAndSet(null); if (message == null) return; Point3f scanPoint = new Point3f(); scanMeshBuilder.clear(); for (int i = 0; i < message.getNumberOfScanPoints(); i++) { double alpha = i / (double) message.getNumberOfScanPoints(); Color color = Color.hsb(alpha * 240.0, 1.0, 1.0); message.getScanPoint(i, scanPoint); scanMeshBuilder.addMesh(MeshDataGenerator.Tetrahedron(SCAN_POINT_SIZE), scanPoint, color); } MeshView scanMeshView = new MeshView(scanMeshBuilder.generateMesh()); scanMeshView.setMaterial(scanMeshBuilder.generateMaterial()); scanMeshToRender.set(scanMeshView); scanMeshBuilder.clear(); }
public ArrowGraphic(double radius, double length, Color color) { TextureColorPalette1D colorPalette = new TextureColorPalette1D(); colorPalette.setHueBased(1.0, 1.0); JavaFXMultiColorMeshBuilder meshBuilder = new JavaFXMultiColorMeshBuilder(colorPalette); double coneHeight = 0.10 * length; double coneRadius = 1.5 * radius; meshBuilder.addCylinder(length, radius, new Point3D(), new AxisAngle(0.0, 1.0, 0.0, Math.PI / 2.0), color); meshBuilder.addCone(coneHeight, coneRadius, new Point3D(length, 0.0, 0.0), new AxisAngle(0.0, 1.0, 0.0, Math.PI / 2.0), color); this.arrow = new MeshView(meshBuilder.generateMesh()); arrow.setMaterial(meshBuilder.generateMaterial()); getChildren().add(arrow); }
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())); }
MeshView meshView = new MeshView(meshBuilder.generateMesh()); meshView.setMaterial(meshBuilder.generateMaterial()); view3dFactory.addNodeToView(meshView);
private UIOcTreeNodeMeshView createSubTreeMeshView(UIOcTreeNode subTreeRoot) { meshBuilder.clear(); Iterable<UIOcTreeNode> iterable = createLeafIterable(subTreeRoot, treeDepthForDisplay.getValue()); for (UIOcTreeNode node : iterable) { if (!node.isPartOfRegion() || !hidePlanarRegionNodes.getValue()) addNodeMesh(meshBuilder, displayType.getValue(), coloringType.getValue(), node); } OcTreeKey rootKey = subTreeRoot.getKeyCopy(); Mesh mesh = meshBuilder.generateMesh(); Material material = meshBuilder.generateMaterial(); UIOcTreeNodeMeshView meshView = new UIOcTreeNodeMeshView(rootKey, mesh, material); meshBuilder.clear(); return meshView; }
private Node createIntersectionsGraphics(PlanarRegionSegmentationRawData rawData) { JavaFXMultiColorMeshBuilder meshBuilder = new JavaFXMultiColorMeshBuilder(new TextureColorAdaptivePalette(32)); List<LineSegment2D> intersections2d = rawData.getIntersections(); Point3D planeOrigin = rawData.getOrigin(); Vector3D planeNormal = rawData.getNormal(); List<LineSegment3D> intersections = PolygonizerTools.toLineSegmentsInWorld(intersections2d, planeOrigin, planeNormal); for (LineSegment3D intersection : intersections) { meshBuilder.addLine(intersection.getFirstEndpoint(), intersection.getSecondEndpoint(), 0.0025, Color.RED); } MeshView meshView = new MeshView(meshBuilder.generateMesh()); meshView.setMaterial(meshBuilder.generateMaterial()); meshView.visibleProperty().bind(showIntersections); return meshView; }
@Override public void start(Stage primaryStage) throws Exception { View3DFactory view3dFactory = new View3DFactory(800, 600); view3dFactory.addCameraController(); view3dFactory.addWorldCoordinateSystem(0.3); MeshView meshView = new MeshView(); meshView.setMesh(colorMeshBuilder.generateMesh()); meshView.setMaterial(colorMeshBuilder.generateMaterial()); view3dFactory.addNodeToView(meshView); PhongMaterial material = new PhongMaterial(Color.DARKCYAN); // normalCylinder.setMaterial(material); // rootNode.getChildren().add(normalCylinder); material = new PhongMaterial(); material.setDiffuseColor(new Color(1.0, 1.0, 0.0, 0.0)); box.setMaterial(material); view3dFactory.addNodeToView(box); primaryStage.setScene(view3dFactory.getScene()); primaryStage.show(); }
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); }
@SuppressWarnings("unchecked") private static Node createPrimaryEdgesGraphics(QuadEdgeSubdivision quadEdgeSubdivision, PlanarRegionSegmentationRawData rawData) { List<QuadEdge> primaryEdges = (List<QuadEdge>) quadEdgeSubdivision.getPrimaryEdges(false); int regionId = rawData.getRegionId(); JavaFXMultiColorMeshBuilder meshBuilder = new JavaFXMultiColorMeshBuilder(new TextureColorAdaptivePalette(16)); Point3D planeOrigin = rawData.getOrigin(); Quaternion planeOrientation = rawData.getOrientation(); Color regionColor = OcTreeMeshBuilder.getRegionColor(regionId); for (QuadEdge edge : primaryEdges) { Point3D dest = PolygonizerTools.toPointInWorld(edge.dest().getX(), edge.dest().getY(), planeOrigin, planeOrientation); Point3D orig = PolygonizerTools.toPointInWorld(edge.orig().getX(), edge.orig().getY(), planeOrigin, planeOrientation); meshBuilder.addLine(dest, orig, 0.0015, regionColor); } MeshView meshView = new MeshView(meshBuilder.generateMesh()); meshView.setMaterial(meshBuilder.generateMaterial()); meshView.setMouseTransparent(true); return meshView; }
public JavaFXCoordinateSystem(double length) { TextureColorPalette1D colorPalette = new TextureColorPalette1D(); colorPalette.setHueBased(1.0, 1.0); JavaFXMultiColorMeshBuilder meshBuilder = new JavaFXMultiColorMeshBuilder(colorPalette); double radius = 0.02 * length; double coneHeight = 0.10 * length; double coneRadius = 0.05 * length; meshBuilder.addCylinder(length, radius, new Point3d(), new AxisAngle4d(0.0, 1.0, 0.0, Math.PI / 2.0), Color.RED); meshBuilder.addCone(coneHeight, coneRadius, new Point3d(length, 0.0, 0.0), new AxisAngle4d(0.0, 1.0, 0.0, Math.PI / 2.0), Color.RED); meshBuilder.addCylinder(length, radius, new Point3d(), new AxisAngle4d(1.0, 0.0, 0.0, -Math.PI / 2.0), Color.GREEN); meshBuilder.addCone(coneHeight, coneRadius, new Point3d(0.0, length, 0.0), new AxisAngle4d(1.0, 0.0, 0.0, -Math.PI / 2.0), Color.GREEN); meshBuilder.addCylinder(length, radius, new Point3d(), Color.BLUE); meshBuilder.addCone(coneHeight, coneRadius, new Point3d(0.0, 0.0, length), Color.BLUE); MeshView coordinateSystem = new MeshView(meshBuilder.generateMesh()); coordinateSystem.setMaterial(meshBuilder.generateMaterial()); getChildren().addAll(coordinateSystem); } }
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()); }
public JavaFXCoordinateSystem(double length) { TextureColorPalette1D colorPalette = new TextureColorPalette1D(); colorPalette.setHueBased(1.0, 1.0); JavaFXMultiColorMeshBuilder meshBuilder = new JavaFXMultiColorMeshBuilder(colorPalette); double radius = 0.02 * length; double coneHeight = 0.10 * length; double coneRadius = 0.05 * length; meshBuilder.addCylinder(length, radius, new Point3D(), new AxisAngle(0.0, 1.0, 0.0, Math.PI / 2.0), Color.RED); meshBuilder.addCone(coneHeight, coneRadius, new Point3D(length, 0.0, 0.0), new AxisAngle(0.0, 1.0, 0.0, Math.PI / 2.0), Color.RED); meshBuilder.addCylinder(length, radius, new Point3D(), new AxisAngle(1.0, 0.0, 0.0, -Math.PI / 2.0), Color.GREEN); meshBuilder.addCone(coneHeight, coneRadius, new Point3D(0.0, length, 0.0), new AxisAngle(1.0, 0.0, 0.0, -Math.PI / 2.0), Color.GREEN); meshBuilder.addCylinder(length, radius, new Point3D(), Color.BLUE); meshBuilder.addCone(coneHeight, coneRadius, new Point3D(0.0, 0.0, length), Color.BLUE); MeshView coordinateSystem = new MeshView(meshBuilder.generateMesh()); coordinateSystem.setMaterial(meshBuilder.generateMaterial()); getChildren().addAll(coordinateSystem); } }
@SuppressWarnings("unchecked") private static Node createEdgesGraphics(QuadEdgeSubdivision quadEdgeSubdivision, PlanarRegionSegmentationRawData rawData) { List<QuadEdge> edges = (List<QuadEdge>) quadEdgeSubdivision.getEdges(); int regionId = rawData.getRegionId(); JavaFXMultiColorMeshBuilder meshBuilder = new JavaFXMultiColorMeshBuilder(new TextureColorAdaptivePalette(16)); Point3D planeOrigin = rawData.getOrigin(); Quaternion planeOrientation = rawData.getOrientation(); Color regionColor = OcTreeMeshBuilder.getRegionColor(regionId); for (QuadEdge edge : edges) { Point3D dest = PolygonizerTools.toPointInWorld(edge.dest().getX(), edge.dest().getY(), planeOrigin, planeOrientation); Point3D orig = PolygonizerTools.toPointInWorld(edge.orig().getX(), edge.orig().getY(), planeOrigin, planeOrientation); meshBuilder.addLine(dest, orig, 0.0015, regionColor); } MeshView meshView = new MeshView(meshBuilder.generateMesh()); meshView.setMaterial(meshBuilder.generateMaterial()); meshView.setMouseTransparent(true); return meshView; }
private Node createBorderEdgesGraphics(PlanarRegionSegmentationRawData rawData, ConcaveHullFactoryResult concaveHullFactoryResult) { int regionId = rawData.getRegionId(); JavaFXMultiColorMeshBuilder meshBuilder = new JavaFXMultiColorMeshBuilder(new TextureColorAdaptivePalette(16)); Point3D planeOrigin = rawData.getOrigin(); Quaternion planeOrientation = rawData.getOrientation(); Color regionColor = OcTreeMeshBuilder.getRegionColor(regionId); for (ConcaveHullVariables intermediateVariables : concaveHullFactoryResult.getIntermediateVariables()) { Set<QuadEdge> borderEdges = intermediateVariables.getBorderEdges(); for (QuadEdge edge : borderEdges) { Point3D dest = PolygonizerTools.toPointInWorld(edge.dest().getX(), edge.dest().getY(), planeOrigin, planeOrientation); Point3D orig = PolygonizerTools.toPointInWorld(edge.orig().getX(), edge.orig().getY(), planeOrigin, planeOrientation); boolean isEdgeTooLong = dest.distance(orig) > parameters.getEdgeLengthThreshold(); Color lineColor = Color.hsb(regionColor.getHue(), regionColor.getSaturation(), isEdgeTooLong ? 0.25 : regionColor.getBrightness()); meshBuilder.addLine(dest, orig, 0.0015, lineColor); } } MeshView meshView = new MeshView(meshBuilder.generateMesh()); meshView.setMaterial(meshBuilder.generateMaterial()); meshView.setMouseTransparent(true); return meshView; }