public static Set<Connection> createStaticVisibilityMap(Point3DReadOnly observer, int observerRegionId, List<Cluster> clusters, int clustersRegionId) { Set<Connection> connections = new HashSet<>(); List<Point2DReadOnly> listOfTargetPoints = new ArrayList<>(); Point2D observer2D = new Point2D(observer); // Add all navigable points (including dynamic objects) to a list for (Cluster cluster : clusters) { if (cluster.isInsideNonNavigableZone(observer2D)) return Collections.emptySet(); for (Point2DReadOnly point : cluster.getNavigableExtrusionsInLocal()) { listOfTargetPoints.add(point); } } for (int j = 0; j < listOfTargetPoints.size(); j++) { Point2DReadOnly target = listOfTargetPoints.get(j); if (observer.distanceXYSquared(target) > MAGIC_NUMBER) { boolean targetIsVisible = isPointVisibleForStaticMaps(clusters, observer2D, target); if (targetIsVisible) { connections.add(new Connection(observer, observerRegionId, new Point3D(target), clustersRegionId)); } } } return connections; }
Collection<Connection> connectionsToPack) List<? extends Point2DReadOnly> navigableExtrusions = clusterToBuildMapOf.getNavigableExtrusionsInLocal();
Vector2D prevEdge = new Vector2D(); List<? extends Point2DReadOnly> sources = sourceCluster.getNavigableExtrusionsInLocal(); List<? extends Point2DReadOnly> targets = targetCluster.getNavigableExtrusionsInLocal();
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; }
public static NavigableRegion createNavigableRegion(PlanarRegion region, List<PlanarRegion> otherRegions, double orthogonalAngle, double clusterResolution, ObstacleRegionFilter obstacleRegionFilter, PlanarRegionFilter filter, NavigableExtrusionDistanceCalculator navigableCalculator, ObstacleExtrusionDistanceCalculator obstacleCalculator) { NavigableRegion navigableRegion = new NavigableRegion(region); PlanarRegion homeRegion = navigableRegion.getHomeRegion(); List<PlanarRegion> obstacleRegions = otherRegions.stream().filter(candidate -> obstacleRegionFilter.isRegionValidObstacle(candidate, homeRegion)) .collect(Collectors.toList()); obstacleRegions = PlanarRegionTools.filterRegionsByTruncatingVerticesBeneathHomeRegion(obstacleRegions, homeRegion, DEPTH_THRESHOLD_FOR_CONVEX_DECOMPOSITION, filter); navigableRegion.setHomeRegionCluster(ClusterTools.createHomeRegionCluster(homeRegion, navigableCalculator)); navigableRegion.addObstacleClusters(ClusterTools.createObstacleClusters(homeRegion, obstacleRegions, orthogonalAngle, obstacleCalculator)); for (Cluster cluster : navigableRegion.getAllClusters()) { PointCloudTools.doBrakeDownOn2DPoints(cluster.getNavigableExtrusionsInLocal(), clusterResolution); } Collection<Connection> connectionsForMap = VisibilityTools.createStaticVisibilityMap(navigableRegion.getAllClusters(), navigableRegion); if (ENABLE_GREEDY_FILTERS) { connectionsForMap = VisibilityTools.removeConnectionsFromExtrusionsOutsideRegions(connectionsForMap, homeRegion); connectionsForMap = VisibilityTools.removeConnectionsFromExtrusionsInsideNoGoZones(connectionsForMap, navigableRegion.getAllClusters()); } VisibilityMap visibilityMap = new VisibilityMap(); visibilityMap.setConnections(connectionsForMap); navigableRegion.setVisibilityMapInLocal(visibilityMap); return navigableRegion; }
assertEquals(numberOfNavigableExtrusions, clusterExpected.getNavigableExtrusionsInLocal().size()); assertEquals(numberOfNavigableExtrusions, clusterExpected.getNavigableExtrusionsInWorld().size()); assertEquals(numberOfNavigableExtrusions, clusterActual.getNavigableExtrusionsInLocal().size()); assertEquals(numberOfNavigableExtrusions, clusterActual.getNavigableExtrusionsInWorld().size());