How to use

addPolygon

method

in

us.ihmc.javaFXToolkit.shapes.JavaFXMultiColorMeshBuilder

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());
}