@Override public double computeExtrusionDistance(PlanarRegion navigableRegionToBeExtruded) { return getExtrusionDistanceIfNotTooHighToStep(); } };
/** * This calculator is used when extruding the projection of an obstacle onto a navigable region. * * @return the calculator use for obstacle extrusion. */ default ObstacleExtrusionDistanceCalculator getObstacleExtrusionDistanceCalculator() { return (pointToExtrude, obstacleHeight) -> { if(obstacleHeight < 0.0) { return 0.0; } else if(obstacleHeight < getTooHighToStepDistance()) { return getExtrusionDistanceIfNotTooHighToStep(); } else { return getExtrusionDistance(); } }; }
public static NavigableRegion createNavigableRegion(PlanarRegion region, List<PlanarRegion> otherRegions, VisibilityGraphsParameters parameters) { PlanarRegionFilter planarRegionFilter = parameters.getPlanarRegionFilter(); double orthogonalAngle = parameters.getRegionOrthogonalAngle(); double clusterResolution = parameters.getClusterResolution(); NavigableExtrusionDistanceCalculator navigableCalculator = parameters.getNavigableExtrusionDistanceCalculator(); ObstacleExtrusionDistanceCalculator obstacleCalculator = parameters.getObstacleExtrusionDistanceCalculator(); ObstacleRegionFilter obstacleRegionFilter = parameters.getObstacleRegionFilter(); return createNavigableRegion(region, otherRegions, orthogonalAngle, clusterResolution, obstacleRegionFilter, planarRegionFilter, navigableCalculator, obstacleCalculator); }
public void set(VisibilityGraphsParameters visibilityGraphsParameters) { setMaxInterRegionConnectionLength(visibilityGraphsParameters.getMaxInterRegionConnectionLength()); setNormalZThresholdForAccessibleRegions(visibilityGraphsParameters.getNormalZThresholdForAccessibleRegions()); setExtrusionDistance(visibilityGraphsParameters.getExtrusionDistance()); setExtrusionDistanceIfNotTooHighToStep(visibilityGraphsParameters.getExtrusionDistanceIfNotTooHighToStep()); setTooHighToStepDistance(visibilityGraphsParameters.getTooHighToStepDistance()); setClusterResolution(visibilityGraphsParameters.getClusterResolution()); setExplorationDistanceFromStartGoal(visibilityGraphsParameters.getExplorationDistanceFromStartGoal()); setPlanarRegionMinArea(visibilityGraphsParameters.getPlanarRegionMinArea()); setPlanarRegionMinSize(visibilityGraphsParameters.getPlanarRegionMinSize()); setRegionOrthogonalAngle(visibilityGraphsParameters.getRegionOrthogonalAngle()); setSearchHostRegionEpsilon(visibilityGraphsParameters.getSearchHostRegionEpsilon()); }
public static VisibilityMapMessage convertToVisibilityMapMessage(VisibilityMapHolder visibilityMapHolder) { return convertToVisibilityMapMessage(visibilityMapHolder.getMapId(), visibilityMapHolder.getVisibilityMapInWorld()); }
public static void assertVisibilityMapHoldersEqual(VisibilityMapHolder holderExpected, VisibilityMapHolder holderActual, double epsilon) { assertEquals(holderExpected.getMapId(), holderActual.getMapId()); assertVisibilityMapsEqual(holderExpected.getVisibilityMapInWorld(), holderActual.getVisibilityMapInWorld(), epsilon); assertVisibilityMapsEqual(holderExpected.getVisibilityMapInLocal(), holderActual.getVisibilityMapInLocal(), epsilon); for (Connection connection : holderExpected.getVisibilityMapInWorld().getConnections()) { assertEquals(holderExpected.getConnectionWeight(connection), holderActual.getConnectionWeight(connection), epsilon); } for (Connection connection : holderExpected.getVisibilityMapInLocal().getConnections()) { assertEquals(holderExpected.getConnectionWeight(connection), holderActual.getConnectionWeight(connection), epsilon); } }
public static void assertSingleSourceVisibilityMapHoldersEqual(VisibilityMapHolder holderExpected, VisibilityMapHolder holderActual, double epsilon) { assertEquals(holderExpected.getMapId(), holderActual.getMapId()); VisibilityGraphTestTools.assertVisibilityMapsEqual(holderExpected.getVisibilityMapInWorld(), holderActual.getVisibilityMapInWorld(), epsilon); for (Connection connection : holderExpected.getVisibilityMapInWorld().getConnections()) { assertEquals(holderExpected.getConnectionWeight(connection), holderActual.getConnectionWeight(connection), epsilon); } }
default InterRegionConnectionFilter getInterRegionConnectionFilter() { return new InterRegionConnectionFilter() { private final double maxLengthSquared = MathTools.square(getMaxInterRegionConnectionLength()); private final double maxDeltaHeight = getTooHighToStepDistance(); @Override public boolean isConnectionValid(ConnectionPoint3D source, ConnectionPoint3D target) { if (Math.abs(source.getZ() - target.getZ()) > maxDeltaHeight) return false; if (source.distanceSquared(target) > maxLengthSquared) return false; return true; } }; }
@Override public boolean isPlanarRegionRelevant(PlanarRegion region) { if (region.getConcaveHullSize() < getPlanarRegionMinSize()) return false; if (!Double.isFinite(getPlanarRegionMinArea()) || getPlanarRegionMinArea() <= 0.0) return true; return PlanarRegionTools.computePlanarRegionArea(region) >= getPlanarRegionMinArea(); } };
public static SimpleWeightedGraph<ConnectionPoint3D, DefaultWeightedEdge> createGlobalVisibilityGraph(Collection<VisibilityMapHolder> allVisibilityMapHolders) { SimpleWeightedGraph<ConnectionPoint3D, DefaultWeightedEdge> globalVisMap = new SimpleWeightedGraph<>(DefaultWeightedEdge.class); for (VisibilityMapHolder visibilityMapHolder : allVisibilityMapHolders) { VisibilityMap visibilityMap = visibilityMapHolder.getVisibilityMapInWorld(); for (Connection connection : visibilityMap) addConnectionToGraph(connection, visibilityMapHolder.getConnectionWeight(connection), globalVisMap); } return globalVisMap; }
@Override public boolean isPlanarRegionNavigable(PlanarRegion query, List<PlanarRegion> allOtherRegions) { return query.getNormal().getZ() >= getNormalZThresholdForAccessibleRegions(); } };
public static List<NavigableRegion> createNavigableRegions(List<PlanarRegion> allRegions, VisibilityGraphsParameters parameters) { if (allRegions.isEmpty()) return null; List<NavigableRegion> navigableRegions = new ArrayList<>(allRegions.size()); NavigableRegionFilter navigableRegionFilter = parameters.getNavigableRegionFilter(); for (int candidateIndex = 0; candidateIndex < allRegions.size(); candidateIndex++) { PlanarRegion candidate = allRegions.get(candidateIndex); List<PlanarRegion> otherRegions = new ArrayList<>(allRegions); Collections.swap(otherRegions, candidateIndex, otherRegions.size() - 1); otherRegions.remove(otherRegions.size() - 1); if (!navigableRegionFilter.isPlanarRegionNavigable(candidate, otherRegions)) continue; navigableRegions.add(createNavigableRegion(candidate, otherRegions, parameters)); } return navigableRegions; }
public void setPlanarRegions(List<PlanarRegion> regions) { if (regions != null) { regions = PlanarRegionTools.ensureClockwiseOrder(regions); regions = regions.stream().filter(parameters.getPlanarRegionFilter()::isPlanarRegionRelevant).collect(Collectors.toList()); } this.regions = regions; }
boolean isConnectionValid(ConnectionPoint3D source, ConnectionPoint3D target); }
public static NavigableRegion getRandomNavigableRegion(Random random) { int numberOfObstacleClusters = RandomNumbers.nextInt(random, 1, 50); PlanarRegion homeRegion = nextPlanarRegion(random); Cluster homeCluster = getRandomCluster(random); NavigableRegion navigableRegion = new NavigableRegion(homeRegion); navigableRegion.setHomeRegionCluster(homeCluster); for (int i = 0; i < numberOfObstacleClusters; i++) navigableRegion.addObstacleCluster(getRandomCluster(random)); navigableRegion.setVisibilityMapInLocal(getRandomSingleSourceVisibilityMap(random).getVisibilityMapInLocal()); return navigableRegion; }
public void set(VisibilityGraphsParameters parameters) { setMaxInterRegionConnectionLength(parameters.getMaxInterRegionConnectionLength()); setNormalZThresholdForAccessibleRegions(parameters.getNormalZThresholdForAccessibleRegions()); setRegionOrthogonalAngle(parameters.getRegionOrthogonalAngle()); setExtrusionDistance(parameters.getExtrusionDistance()); setExtrusionDistanceIfNotTooHighToStep(parameters.getExtrusionDistanceIfNotTooHighToStep()); setTooHighToStepDistance(parameters.getTooHighToStepDistance()); setClusterResolution(parameters.getClusterResolution()); setExplorationDistanceFromStartGoal(parameters.getExplorationDistanceFromStartGoal()); setPlanarRegionMinArea(parameters.getPlanarRegionMinArea()); setPlanarRegionMinSize(parameters.getPlanarRegionMinSize()); setSearchHostRegionEpsilon(parameters.getSearchHostRegionEpsilon()); }
private void packVisibilityGraphStatistics(VisibilityGraphStatistics statistics) { VisibilityMapHolder startMap = navigableRegionsManager.getStartMap(); VisibilityMapHolder goalMap = navigableRegionsManager.getGoalMap(); VisibilityMapHolder interRegionsMap = navigableRegionsManager.getInterRegionConnections(); List<NavigableRegion> navigableRegions = navigableRegionsManager.getNavigableRegions(); statistics.setStartVisibilityMapInWorld(startMap.getMapId(), startMap.getVisibilityMapInWorld()); statistics.setGoalVisibilityMapInWorld(goalMap.getMapId(), goalMap.getVisibilityMapInWorld()); statistics.setInterRegionsVisibilityMapInWorld(interRegionsMap.getMapId(), interRegionsMap.getVisibilityMapInWorld()); statistics.addNavigableRegions(navigableRegions); } }
public static void copyVisibilityGraphsParametersToPacket(VisibilityGraphsParametersPacket packet, VisibilityGraphsParameters parameters) { if (parameters == null) { return; } packet.setMaxInterRegionConnectionLength(parameters.getMaxInterRegionConnectionLength()); packet.setNormalZThresholdForAccessibleRegions(parameters.getNormalZThresholdForAccessibleRegions()); packet.setExtrusionDistance(parameters.getExtrusionDistance()); packet.setExtrusionDistanceIfNotTooHighToStep(parameters.getExtrusionDistanceIfNotTooHighToStep()); packet.setTooHighToStepDistance(parameters.getTooHighToStepDistance()); packet.setClusterResolution(parameters.getClusterResolution()); packet.setExplorationDistanceFromStartGoal(parameters.getExplorationDistanceFromStartGoal()); packet.setPlanarRegionMinArea(parameters.getPlanarRegionMinArea()); packet.setPlanarRegionMinSize(parameters.getPlanarRegionMinSize()); packet.setRegionOrthogonalAngle(parameters.getRegionOrthogonalAngle()); packet.setSearchHostRegionEpsilon(parameters.getSearchHostRegionEpsilon()); }
public static VisibilityGraphStatistics convertToVisibilityGraphStatistics(BodyPathPlanStatisticsMessage message) { VisibilityGraphStatistics statistics = new VisibilityGraphStatistics(); VisibilityMapHolder startMap = convertToSingleSourceVisibilityMap(message.getStartVisibilityMap()); VisibilityMapHolder goalMap = convertToSingleSourceVisibilityMap(message.getGoalVisibilityMap()); VisibilityMapHolder interRegionsMap = convertToInterRegionsVisibilityMap(message.getInterRegionsMap()); statistics.setStartVisibilityMapInWorld(startMap.getMapId(), startMap.getVisibilityMapInWorld()); statistics.setGoalVisibilityMapInWorld(goalMap.getMapId(), goalMap.getVisibilityMapInWorld()); statistics.setInterRegionsVisibilityMapInWorld(interRegionsMap.getMapId(), interRegionsMap.getVisibilityMapInWorld()); List<NavigableRegionMessage> navigableRegions = message.getNavigableRegions(); for (int i = 0; i < navigableRegions.size(); i++) statistics.addNavigableRegion(convertToNavigableRegion(navigableRegions.get(i))); return statistics; } }
private static void checkVisibilityGraphsParameters(VisibilityGraphsParameters parameters, VisibilityGraphsParametersPacket packet) { assertEquals(parameters.getMaxInterRegionConnectionLength(), packet.getMaxInterRegionConnectionLength(), epsilon); assertEquals(parameters.getNormalZThresholdForAccessibleRegions(), packet.getNormalZThresholdForAccessibleRegions(), epsilon); assertEquals(parameters.getExtrusionDistance(), packet.getExtrusionDistance(), epsilon); assertEquals(parameters.getExtrusionDistanceIfNotTooHighToStep(), packet.getExtrusionDistanceIfNotTooHighToStep(), epsilon); assertEquals(parameters.getTooHighToStepDistance(), packet.getTooHighToStepDistance(), epsilon); assertEquals(parameters.getClusterResolution(), packet.getClusterResolution(), epsilon); assertEquals(parameters.getExplorationDistanceFromStartGoal(), packet.getExplorationDistanceFromStartGoal(), epsilon); assertEquals(parameters.getPlanarRegionMinArea(), packet.getPlanarRegionMinArea(), epsilon); assertEquals(parameters.getPlanarRegionMinSize(), packet.getPlanarRegionMinSize()); assertEquals(parameters.getRegionOrthogonalAngle(), packet.getRegionOrthogonalAngle(), epsilon); assertEquals(parameters.getSearchHostRegionEpsilon(), packet.getSearchHostRegionEpsilon(), epsilon); } }