public byte toByte() { return (byte) ordinal(); }
public byte toByte() { return (byte) ordinal(); }
public Point3DReadOnly getNavigableExtrusionInWorld(int i) { return toWorld3D(getNavigableExtrusionInLocal(i)); }
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 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 Point3DReadOnly[][] getNavigableExtrusions() { Point3DReadOnly[][] allNavigableExtrusions = new Point3D[navigableRegions.size()][]; for (int i = 0; i < navigableRegions.size(); i++) { NavigableRegion localPlanner = navigableRegions.get(i); Point3DReadOnly[] navigableExtrusions = new Point3D[localPlanner.getAllClusters().size()]; for (Cluster cluster : localPlanner.getAllClusters()) { for (int j = 0; j < cluster.getNumberOfNavigableExtrusions(); j++) { navigableExtrusions[j] = cluster.getNavigableExtrusionInWorld(j); } } allNavigableExtrusions[i] = navigableExtrusions; } return allNavigableExtrusions; }
public Point3DReadOnly getNonNavigableExtrusionInWorld(int i) { return toWorld3D(getNonNavigableExtrusionInLocal(i)); }
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 void addNavigableExtrusionInLocal(Point3DReadOnly navigableExtrusionInLocal) { addNavigableExtrusionInLocal(new Point2D(navigableExtrusionInLocal)); }
public void addRawPointInWorld(Point3DReadOnly pointInWorld) { rawPointsLocal.add(toLocal3D(pointInWorld)); }
public Point3DReadOnly getRawPointInWorld(int i) { return toWorld3D(rawPointsLocal.get(i)); }
public void addRawPointsInLocal2D(Point2DReadOnly[] pointsInLocal) { addRawPointsInLocal2D(Arrays.asList(pointsInLocal)); }
public static List<Connection> getValidConnections(Collection<Connection> connections, Cluster cluster) { List<Connection> filteredConnections = new ArrayList<>(); for (Connection connection : connections) { Point2D source = connection.getSourcePoint2D(); Point2D target = connection.getTargetPoint2D(); if (!cluster.isInsideNonNavigableZone(source) && !cluster.isInsideNonNavigableZone(target)) filteredConnections.add(connection); } return filteredConnections; } }
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 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; } }
private Point2D toLocal2D(Point3DReadOnly pointInWorld) { return new Point2D(toLocal3D(pointInWorld)); } }
private Point3D toWorld3D(Point2DReadOnly pointInLocal) { return toWorld3D(new Point3D(pointInLocal)); }
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; }