public static List<Cluster> getListOfIntersectingObstacles(List<Cluster> obstacleClusters, Point3DReadOnly start, Point3DReadOnly goal) { List<Cluster> clustersTemp = new ArrayList<Cluster>(); for (Cluster cluster : obstacleClusters) { ArrayList<Point2D> list2D = new ArrayList<>(); for (Point3DReadOnly point3d : cluster.getNonNavigableExtrusionsInWorld()) { list2D.add(new Point2D(point3d)); } boolean visible = VisibilityTools.isPointVisible(new Point2D(start), new Point2D(goal), list2D); if (!visible) { clustersTemp.add(cluster); } } return clustersTemp; } }
public static Cluster getTheClosestCluster(Point3DReadOnly pointToSortFrom, List<Cluster> clusters) { double minDistance = Double.MAX_VALUE; Cluster closestCluster = null; for (Cluster cluster : clusters) { double distOfPoint = Double.MAX_VALUE; Point3DReadOnly closestPointInCluster = null; for (Point3DReadOnly point : cluster.getNonNavigableExtrusionsInWorld()) { double currentDistance = point.distanceSquared(pointToSortFrom); if (currentDistance < distOfPoint) { distOfPoint = currentDistance; closestPointInCluster = point; } } double currentDistance = closestPointInCluster.distanceSquared(pointToSortFrom); if (currentDistance < minDistance) { minDistance = currentDistance; closestCluster = cluster; } } return closestCluster; }
public static boolean isTheGoalIntersectingAnyObstacles(NavigableRegion region, Point3DReadOnly start, Point3DReadOnly goal) { for (Cluster cluster : region.getObstacleClusters()) { ArrayList<Point2D> list2D = new ArrayList<>(); for (Point3DReadOnly point3d : cluster.getNonNavigableExtrusionsInWorld()) { list2D.add(new Point2D(point3d)); } boolean visible = VisibilityTools.isPointVisible(new Point2D(start), new Point2D(goal), list2D); if (!visible) { return true; } } return false; }
.addMultiLine(cluster.getNavigableExtrusionsInWorld(), VisualizationParameters.NAVIGABLECLUSTER_LINE_THICKNESS, false); nonNavigableExtrusionsMeshBuilder .addMultiLine(cluster.getNonNavigableExtrusionsInWorld(), VisualizationParameters.NON_NAVIGABLECLUSTER_LINE_THICKNESS, false);
assertEquals(numberOfNonNavigableExtrusions, clusterExpected.getNonNavigableExtrusionsInWorld().size()); assertEquals(numberOfNonNavigableExtrusions, clusterExpected.getNonNavigableExtrusionsInLocal().size()); assertEquals(numberOfNonNavigableExtrusions, clusterActual.getNonNavigableExtrusionsInWorld().size()); assertEquals(numberOfNonNavigableExtrusions, clusterActual.getNonNavigableExtrusionsInLocal().size());