if (!quickFeasibilityCheck(directionToCheck, prevEdge, nextEdge, clusterToBuildMapOf.getExtrusionSide())) continue; prevEdge.sub(target, ListWrappingIndexTools.getPrevious(targetIndex, navigableExtrusions)); nextEdge.sub(ListWrappingIndexTools.getNext(targetIndex, navigableExtrusions), target); if (!quickFeasibilityCheck(directionToCheck, prevEdge, nextEdge, clusterToBuildMapOf.getExtrusionSide())) continue;
if (!quickFeasibilityCheck(directionToCheck, prevEdge, nextEdge, sourceCluster.getExtrusionSide())) continue; prevEdge.sub(target, ListWrappingIndexTools.getPrevious(targetIndex, targets)); nextEdge.sub(ListWrappingIndexTools.getNext(targetIndex, targets), target); if (!quickFeasibilityCheck(directionToCheck, prevEdge, nextEdge, targetCluster.getExtrusionSide())) continue;
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 boolean isPointVisibleForStaticMaps(List<Cluster> clusters, Point2DReadOnly observer, Point2DReadOnly targetPoint) { for (Cluster cluster : clusters) { if (cluster.getExtrusionSide() == ExtrusionSide.OUTSIDE) { BoundingBox2D boundingBox = cluster.getNonNavigableExtrusionsBoundingBox(); // If both the target and observer are in the bounding box, we have to do the thorough check. if (!boundingBox.isInsideInclusive(observer) || !boundingBox.isInsideInclusive(targetPoint)) { if (!boundingBox.doesIntersectWithLineSegment2D(observer, targetPoint)) continue; } } if (!VisibilityTools.isPointVisible(observer, targetPoint, cluster.getNonNavigableExtrusionsInLocal())) { return false; } } return true; }
public static void extrudeObstacleCluster(Cluster cluster, ObstacleExtrusionDistanceCalculator calculator) { ObstacleExtrusionDistanceCalculator nonNavigableCalculator = (p, h) -> calculator.computeExtrusionDistance(p, h) - NAV_TO_NON_NAV_DISTANCE; ObstacleExtrusionDistanceCalculator navigableCalculator = calculator; int numberOfExtrusionsAtEndpoints = 5; switch (cluster.getType()) { case LINE: case MULTI_LINE: cluster.addNonNavigableExtrusionsInLocal(extrudeMultiLine(cluster, nonNavigableCalculator, numberOfExtrusionsAtEndpoints)); cluster.addNavigableExtrusionsInLocal(extrudeMultiLine(cluster, navigableCalculator, numberOfExtrusionsAtEndpoints)); break; case POLYGON: boolean extrudeToTheLeft = cluster.getExtrusionSide() != ExtrusionSide.INSIDE; try { cluster.addNonNavigableExtrusionsInLocal(extrudePolygon(extrudeToTheLeft, cluster, nonNavigableCalculator)); } catch(Exception e) { e.printStackTrace(); return; } cluster.addNavigableExtrusionsInLocal(extrudePolygon(extrudeToTheLeft, cluster, navigableCalculator)); break; default: throw new RuntimeException("Unhandled cluster type: " + cluster.getType()); } cluster.updateBoundingBox(); }
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; }
public static void assertClustersEqual(Cluster clusterExpected, Cluster clusterActual, double epsilon) assertEquals(clusterExpected.getExtrusionSide(), clusterActual.getExtrusionSide()); assertEquals(clusterExpected.getType(), clusterActual.getType());