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; }
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 Point3DReadOnly getNavigableExtrusionInWorld(int i) { return toWorld3D(getNavigableExtrusionInLocal(i)); }
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(); }
assertEquals(clusterExpected.getExtrusionSide(), clusterActual.getExtrusionSide()); assertEquals(clusterExpected.getType(), clusterActual.getType()); RigidBodyTransform transformExpected = clusterExpected.getTransformToWorld(); ReferenceFrame localFrame = new ReferenceFrame("localFrame", worldFrame) EuclidCoreTestTools.assertRigidBodyTransformGeometricallyEquals(transformExpected, clusterActual.getTransformToWorld(), epsilon); int numberOfRawPoints = clusterExpected.getRawPointsInLocal2D().size(); assertEquals(numberOfRawPoints, clusterActual.getNumberOfRawPoints()); assertEquals(numberOfRawPoints, clusterExpected.getNumberOfRawPoints()); 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()); .assertPoint3DGeometricallyEquals("Point " + i + " failed.", clusterExpected.getRawPointInLocal(i), clusterActual.getRawPointInLocal(i), epsilon); EuclidCoreTestTools .assertPoint3DGeometricallyEquals("Point " + i + " failed.", clusterExpected.getRawPointInWorld(i), clusterActual.getRawPointInWorld(i), epsilon); FramePoint3D point = new FramePoint3D(localFrame, clusterExpected.getRawPointInLocal(i)); point.changeFrame(worldFrame); EuclidCoreTestTools.assertPoint3DGeometricallyEquals("Point " + i + " failed.", point, clusterExpected.getRawPointInWorld(i), epsilon); EuclidCoreTestTools.assertPoint3DGeometricallyEquals("Point " + i + " failed.", point, clusterActual.getRawPointInWorld(i), epsilon);
Cluster clusterToConvert = new Cluster(); clusterToConvert.setTransformToWorld(transformToWorld); clusterToConvert.setType(Cluster.Type.fromByte(typeByte)); clusterToConvert.setExtrusionSide(Cluster.ExtrusionSide.fromByte(extrusionSideByte)); clusterToConvert.addRawPointInLocal(rawPointsInLocalExpected.get(i)); for (int i = 0; i < numberOfNavigableExtrusions; i++) clusterToConvert.addNavigableExtrusionInLocal(navigableExtrusionsInLocalExpected.get(i)); for (int i = 0; i < numberOfNonNavigableExtrusions; i++) clusterToConvert.addNonNavigableExtrusionInLocal(nonNavigableExtrusionsInLocalExpected.get(i)); EuclidCoreTestTools.assertRigidBodyTransformGeometricallyEquals(transformToWorld, convertedCluster.getTransformToWorld(), epsilon); EuclidCoreTestTools.assertRigidBodyTransformGeometricallyEquals(transformToWorld, clusterToConvert.getTransformToWorld(), epsilon); assertEquals(numberOfRawPoints, convertedCluster.getNumberOfRawPoints()); assertEquals(numberOfRawPoints, clusterToConvert.getNumberOfRawPoints()); assertEquals(numberOfNavigableExtrusions, convertedCluster.getNumberOfNavigableExtrusions()); assertEquals(numberOfNavigableExtrusions, clusterToConvert.getNumberOfNavigableExtrusions()); assertEquals(numberOfNonNavigableExtrusions, convertedCluster.getNumberOfNonNavigableExtrusions()); assertEquals(numberOfNonNavigableExtrusions, clusterToConvert.getNumberOfNonNavigableExtrusions()); EuclidCoreTestTools.assertPoint3DGeometricallyEquals(expectedPoint, clusterToConvert.getRawPointInLocal(i), epsilon); EuclidCoreTestTools.assertPoint3DGeometricallyEquals(expectedPoint, convertedCluster.getRawPointInLocal(i), epsilon); EuclidCoreTestTools.assertPoint3DGeometricallyEquals(expectedFramePoint, clusterToConvert.getRawPointInLocal(i), epsilon); EuclidCoreTestTools.assertPoint3DGeometricallyEquals(expectedFramePoint, convertedCluster.getRawPointInLocal(i), epsilon); transformToWorld.transform(expectedPoint); expectedFramePoint.changeFrame(worldFrame); EuclidCoreTestTools.assertPoint3DGeometricallyEquals(expectedPoint, clusterToConvert.getRawPointInWorld(i), epsilon); EuclidCoreTestTools.assertPoint3DGeometricallyEquals(expectedPoint, clusterToConvert.getRawPointInWorld(i), epsilon); EuclidCoreTestTools.assertPoint3DGeometricallyEquals(expectedFramePoint, clusterToConvert.getRawPointInWorld(i), epsilon);
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 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; }
Cluster cluster = new Cluster(); cluster.setExtrusionSide(ExtrusionSide.OUTSIDE); cluster.setTransformToWorld(transformFromHomeToWorld); cluster.setType(Type.MULTI_LINE); cluster.addRawPointsInLocal3D(filterVerticalPolygonForMultiLineExtrusion(rawPointsInLocal, POPPING_MULTILINE_POINTS_THRESHOLD)); cluster.setType(Type.POLYGON); cluster.addRawPointsInLocal3D(rawPointsInLocal);
Cluster cluster = new Cluster(); int nPacketsRead = 0; cluster.addRawPointsInWorld(pointsTemp); pointsTemp.clear(); cluster = new Cluster(); clusters.add(cluster); nPacketsRead++; cluster.setTransformToWorld(rigidBodyTransform); cluster.addRawPointsInWorld(pointsTemp); pointsTemp.clear(); ConvexPolygon2D convexPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(cluster1.getRawPointsInLocal2D())); PlanarRegion planarRegion = new PlanarRegion(cluster1.getTransformToWorld(), convexPolygon);
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; }
Collection<Connection> connectionsToPack) List<? extends Point2DReadOnly> navigableExtrusions = clusterToBuildMapOf.getNavigableExtrusionsInLocal(); isNavigable = allClusters.stream().noneMatch(cluster -> cluster.isInsideNonNavigableZone(query)); 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;
Vector2D prevEdge = new Vector2D(); List<? extends Point2DReadOnly> sources = sourceCluster.getNavigableExtrusionsInLocal(); List<? extends Point2DReadOnly> targets = targetCluster.getNavigableExtrusionsInLocal(); 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 void addNavigableExtrusionInLocal(Point3DReadOnly navigableExtrusionInLocal) { addNavigableExtrusionInLocal(new Point2D(navigableExtrusionInLocal)); }
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; }
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; }