PlanarRegion planarRegion = new PlanarRegion(cluster1.getTransformToWorld(), convexPolygon);
public static VisibilityClusterMessage convertToVisibilityClusterMessage(Cluster cluster) { VisibilityClusterMessage message = new VisibilityClusterMessage(); if (cluster == null) return message; List<? extends Point3DReadOnly> rawPointsInLocal = cluster.getRawPointsInLocal3D(); List<Point2DReadOnly> navigableExtrusionsInLocal = cluster.getNavigableExtrusionsInLocal(); List<Point2DReadOnly> nonNavigableExtrusionsInLocal = cluster.getNonNavigableExtrusionsInLocal(); message.setExtrusionSide(cluster.getExtrusionSide().toByte()); message.setType(cluster.getType().toByte()); message.getPoseInWorld().set(cluster.getTransformToWorld()); for (int i = 0; i < rawPointsInLocal.size(); i++) message.getRawPointsInLocal().add().set(rawPointsInLocal.get(i)); for (int i = 0; i < navigableExtrusionsInLocal.size(); i++) message.getNavigableExtrusionsInLocal().add().set(navigableExtrusionsInLocal.get(i)); for (int i = 0; i < nonNavigableExtrusionsInLocal.size(); i++) message.getNonNavigableExtrusionsInLocal().add().set(nonNavigableExtrusionsInLocal.get(i)); return message; }
Cluster convertedCluster = VisibilityGraphMessagesConverter.convertToCluster(message); EuclidCoreTestTools.assertRigidBodyTransformGeometricallyEquals(transformToWorld, convertedCluster.getTransformToWorld(), epsilon); EuclidCoreTestTools.assertRigidBodyTransformGeometricallyEquals(transformToWorld, clusterToConvert.getTransformToWorld(), epsilon); assertEquals(numberOfRawPoints, convertedCluster.getNumberOfRawPoints()); assertEquals(numberOfRawPoints, clusterToConvert.getNumberOfRawPoints());
assertEquals(clusterExpected.getType(), clusterActual.getType()); RigidBodyTransform transformExpected = clusterExpected.getTransformToWorld(); ReferenceFrame localFrame = new ReferenceFrame("localFrame", worldFrame) EuclidCoreTestTools.assertRigidBodyTransformGeometricallyEquals(transformExpected, clusterActual.getTransformToWorld(), epsilon);