public static List<Point2D> extrudeMultiLine(Cluster cluster, ObstacleExtrusionDistanceCalculator calculator, int numberOfExtrusionsAtEndpoints) { List<Point2DReadOnly> rawPoints = cluster.getRawPointsInLocal2D(); double[] extrusionDistances = cluster.getRawPointsInLocal3D().stream() .mapToDouble(rawPoint -> calculator.computeExtrusionDistance(new Point2D(rawPoint), rawPoint.getZ())).toArray(); return extrudeMultiLine(rawPoints, extrusionDistances, numberOfExtrusionsAtEndpoints); }
public static List<Point2D> extrudePolygon(boolean extrudeToTheLeft, Cluster cluster, ObstacleExtrusionDistanceCalculator calculator) { List<Point2DReadOnly> rawPoints = cluster.getRawPointsInLocal2D(); double[] extrusionDistances = cluster.getRawPointsInLocal3D().stream() .mapToDouble(rawPoint -> calculator.computeExtrusionDistance(new Point2D(rawPoint), rawPoint.getZ())).toArray(); return extrudePolygon(extrudeToTheLeft, rawPoints, extrusionDistances); }
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; }
assertEquals(numberOfRawPoints, clusterExpected.getRawPointsInLocal2D().size()); assertEquals(numberOfRawPoints, clusterExpected.getRawPointsInWorld().size()); assertEquals(numberOfRawPoints, clusterExpected.getRawPointsInLocal3D().size()); assertEquals(numberOfRawPoints, clusterActual.getRawPointsInLocal2D().size()); assertEquals(numberOfRawPoints, clusterActual.getRawPointsInWorld().size()); assertEquals(numberOfRawPoints, clusterActual.getRawPointsInLocal3D().size());