public bool TryFindPath(TerrainOverlayNetworkNode sourceNode, IntVector2 sourcePoint, TerrainOverlayNetworkNode destinationNode, IntVector2 destinationPoint, out MotionRoadmap result, IDebugCanvas debugCanvas = null)
{
if (debugCanvas != null)
{
debugCanvas.Transform = Matrix4x4.Identity;
}
if (sourceNode == destinationNode)
{
var roadmap = new MotionRoadmap();
if (sourcePoint == destinationPoint || sourceNode.LandPolyNode.SegmentInLandPolygonNonrecursive(sourcePoint, destinationPoint))
{
roadmap.Plan.Add(new MotionRoadmapWalkAction(sourceNode, sourcePoint, destinationPoint));
result = roadmap;
return(true);
}
var sourceVisibleWaypointLinks = sourceNode.CrossoverPointManager.FindVisibleWaypointLinks(sourcePoint, null, out var sourceVisibleWaypointLinksLength, out var sourceOptimalLinkToWaypoints);
var destinationVisibleWaypointLinks = sourceNode.CrossoverPointManager.FindVisibleWaypointLinks(destinationPoint, null, out var destinationVisibleWaypointLinksLength, out var destinationOptimalLinkToWaypoints);
var bestFirstWaypoint = -1;
var bestFirstWaypointCost = double.PositiveInfinity;
for (var i = 0; i < sourceVisibleWaypointLinksLength; i++)
{
var link = sourceVisibleWaypointLinks[i];
var firstWaypoint = link.PriorIndex;
var cost = link.TotalCost + destinationOptimalLinkToWaypoints[firstWaypoint].TotalCost;
if (cost < bestFirstWaypointCost)
{
bestFirstWaypoint = firstWaypoint;
bestFirstWaypointCost = cost;
}
}
roadmap.Plan.Add(new MotionRoadmapWalkAction(sourceNode, sourcePoint, sourceNode.CrossoverPointManager.Waypoints[bestFirstWaypoint]));
AddInterTerrainOverlayNetworkNodeWaypointToWaypointRoadmapActions(roadmap, sourceNode, bestFirstWaypoint, destinationOptimalLinkToWaypoints[bestFirstWaypoint].PriorIndex);
roadmap.Plan.Add(new MotionRoadmapWalkAction(sourceNode, sourceNode.CrossoverPointManager.Waypoints[destinationOptimalLinkToWaypoints[bestFirstWaypoint].PriorIndex], destinationPoint));
result = roadmap;
return(true);
}
const int SOURCE_POINT_CPI = -100;
const int DESTINATION_POINT_CPI = -200;
// todo: special-case if src is dst node
// Console.WriteLine("Src had " + sourceNode.CrossoverPointManager.CrossoverPoints.Count + " : " + string.Join(", ", sourceNode.CrossoverPointManager.CrossoverPoints));
var(_, _, _, sourceOptimalLinkToCrossovers) = sourceNode.CrossoverPointManager.FindOptimalLinksToCrossovers(sourcePoint);
var(_, _, _, destinationOptimalLinkToCrossovers) = destinationNode.CrossoverPointManager.FindOptimalLinksToCrossovers(destinationPoint);
var q = new PriorityQueue <ValueTuple <float, float, TerrainOverlayNetworkNode, int, TerrainOverlayNetworkNode, int, TerrainOverlayNetworkEdge> >((a, b) => a.Item1.CompareTo(b.Item1));
var priorityUpperBounds = new Dictionary <(TerrainOverlayNetworkNode, int), float>();
var predecessor = new Dictionary <(TerrainOverlayNetworkNode, int), (TerrainOverlayNetworkNode, int, TerrainOverlayNetworkEdge, float)>(); // visited
foreach (var kvp in sourceNode.OutboundEdgeGroups)
{
foreach (var g in kvp.Value)
{
foreach (var edge in g.Edges)
{
var cpiLink = sourceOptimalLinkToCrossovers[edge.SourceCrossoverIndex];
var worldCpiLinkCost = cpiLink.TotalCost * sourceNode.SectorNodeDescription.LocalToWorldScalingFactor;
priorityUpperBounds[(sourceNode, edge.SourceCrossoverIndex)] = worldCpiLinkCost;
public bool TryFindPath(double agentRadius, DoubleVector3 sourceWorld, DoubleVector3 destinationWorld, out MotionRoadmap roadmap, IDebugCanvas debugCanvas = null)
{
roadmap = null;
var terrainSnapshot = terrainService.CompileSnapshot();
var terrainOverlayNetwork = terrainSnapshot.OverlayNetworkManager.CompileTerrainOverlayNetwork(agentRadius);
if (!terrainOverlayNetwork.TryFindTerrainOverlayNode(sourceWorld.ToDotNetVector(), out var sourceNode))
{
return(false);
}
if (!terrainOverlayNetwork.TryFindTerrainOverlayNode(destinationWorld.ToDotNetVector(), out var destinationNode))
{
return(false);
}
var sourceLocal = Vector3.Transform(sourceWorld.ToDotNetVector(), sourceNode.SectorNodeDescription.WorldTransformInv);
var destinationLocal = Vector3.Transform(destinationWorld.ToDotNetVector(), destinationNode.SectorNodeDescription.WorldTransformInv);
return(TryFindPath(
sourceNode,
sourceLocal.ToOpenMobaVector().LossyToIntVector3().XY,
destinationNode,
destinationLocal.ToOpenMobaVector().LossyToIntVector3().XY,
out roadmap,
debugCanvas));
}
