public static Cluster getRandomCluster(Random random) { byte typeByte = (byte) RandomNumbers.nextInt(random, 0, Cluster.Type.values.length - 1); byte extrusionSideByte = (byte) RandomNumbers.nextInt(random, 0, Cluster.ExtrusionSide.values.length - 1); int numberOfRawPoints = RandomNumbers.nextInt(random, 1, 100); int numberOfNavigableExtrusions = RandomNumbers.nextInt(random, 1, 100); int numberOfNonNavigableExtrusions = RandomNumbers.nextInt(random, 1, 100); RigidBodyTransform transformToWorld = EuclidCoreRandomTools.nextRigidBodyTransform(random); List<Point3D> rawPointsInLocalExpected = new ArrayList<>(); List<Point2D> navigableExtrusionsInLocalExpected = new ArrayList<>(); List<Point2D> nonNavigableExtrusionsInLocalExpected = new ArrayList<>(); for (int i = 0; i < numberOfRawPoints; i++) rawPointsInLocalExpected.add(EuclidCoreRandomTools.nextPoint3D(random, 100.0)); for (int i = 0; i < numberOfNavigableExtrusions; i++) navigableExtrusionsInLocalExpected.add(EuclidCoreRandomTools.nextPoint2D(random, 100.0)); for (int i = 0; i < numberOfNonNavigableExtrusions; i++) nonNavigableExtrusionsInLocalExpected.add(EuclidCoreRandomTools.nextPoint2D(random, 100.0)); Cluster cluster = new Cluster(); cluster.setTransformToWorld(transformToWorld); cluster.setType(Cluster.Type.fromByte(typeByte)); cluster.setExtrusionSide(Cluster.ExtrusionSide.fromByte(extrusionSideByte)); for (int i = 0; i < numberOfRawPoints; i++) cluster.addRawPointInLocal(rawPointsInLocalExpected.get(i)); for (int i = 0; i < numberOfNavigableExtrusions; i++) cluster.addNavigableExtrusionInLocal(navigableExtrusionsInLocalExpected.get(i)); for (int i = 0; i < numberOfNonNavigableExtrusions; i++) cluster.addNonNavigableExtrusionInLocal(nonNavigableExtrusionsInLocalExpected.get(i)); return cluster; }
public static Cluster convertToCluster(VisibilityClusterMessage message) { Cluster cluster = new Cluster(); cluster.setType(Cluster.Type.fromByte(message.getType())); cluster.setExtrusionSide(Cluster.ExtrusionSide.fromByte(message.getExtrusionSide())); Pose3D poseInWorld = message.getPoseInWorld(); RigidBodyTransform transform = new RigidBodyTransform(); poseInWorld.get(transform); List<Point3D> rawPointsInLocal = message.getRawPointsInLocal(); List<Point3D> navigableExtrusionsInLocal = message.getNavigableExtrusionsInLocal(); List<Point3D> nonNavigableExtrusionsInLocal = message.getNonNavigableExtrusionsInLocal(); cluster.setTransformToWorld(transform); cluster.addRawPointsInLocal3D(rawPointsInLocal); for (int i = 0; i < navigableExtrusionsInLocal.size(); i++) cluster.addNavigableExtrusionInLocal(navigableExtrusionsInLocal.get(i)); for (int i = 0; i < nonNavigableExtrusionsInLocal.size(); i++) cluster.addNonNavigableExtrusionInLocal(nonNavigableExtrusionsInLocal.get(i)); return cluster; }
cluster.setExtrusionSide(ExtrusionSide.OUTSIDE); cluster.setTransformToWorld(transformFromHomeToWorld);
clusterToConvert.setTransformToWorld(transformToWorld); clusterToConvert.setType(Cluster.Type.fromByte(typeByte)); clusterToConvert.setExtrusionSide(Cluster.ExtrusionSide.fromByte(extrusionSideByte));
public static Cluster createHomeRegionCluster(PlanarRegion homeRegion, NavigableExtrusionDistanceCalculator calculator) { RigidBodyTransform transformToWorld = new RigidBodyTransform(); homeRegion.getTransformToWorld(transformToWorld); Cluster cluster = new Cluster(); cluster.setType(Type.POLYGON); cluster.setTransformToWorld(transformToWorld); cluster.addRawPointsInLocal2D(homeRegion.getConcaveHull()); cluster.setExtrusionSide(ExtrusionSide.INSIDE); double extrusionDistance = calculator.computeExtrusionDistance(homeRegion); ObstacleExtrusionDistanceCalculator nonNavigableCalculator = (p, h) -> extrusionDistance - NAV_TO_NON_NAV_DISTANCE; ObstacleExtrusionDistanceCalculator navigableCalculator = (p, h) -> extrusionDistance; boolean extrudeToTheLeft = cluster.getExtrusionSide() != ExtrusionSide.INSIDE; cluster.addNonNavigableExtrusionsInLocal(extrudePolygon(extrudeToTheLeft, cluster, nonNavigableCalculator)); cluster.addNavigableExtrusionsInLocal(extrudePolygon(extrudeToTheLeft, cluster, navigableCalculator)); cluster.updateBoundingBox(); return cluster; }