public List<Point3DReadOnly> calculateBodyPathWithOcclusions(Point3DReadOnly start, Point3DReadOnly goal)
{
List<Point3DReadOnly> path = calculateBodyPath(start, goal);
if (path == null)
{
if (!OcclusionTools.isTheGoalIntersectingAnyObstacles(navigableRegions.get(0), start, goal))
{
if(debug)
{
PrintTools.info("StraightLine available");
}
path = new ArrayList<>();
path.add(start);
path.add(goal);
return path;
}
NavigableRegion regionContainingPoint = PlanarRegionTools.getNavigableRegionContainingThisPoint(start, navigableRegions);
List<Cluster> intersectingClusters = OcclusionTools.getListOfIntersectingObstacles(regionContainingPoint.getObstacleClusters(), start, goal);
Cluster closestCluster = ClusterTools.getTheClosestCluster(start, intersectingClusters);
Point3D closestExtrusion = ClusterTools.getTheClosestVisibleExtrusionPoint(1.0, start, goal, closestCluster.getNavigableExtrusionsInWorld(),
regionContainingPoint.getHomeRegion());
path = calculateBodyPath(start, closestExtrusion);
path.add(goal);
return path;
}
else
{
return path;
}
}