/// <summary>
/// Method used to smooth a received path, using a string pulling technique
/// it returns a new path, where the path positions are selected in order to provide a smoother path
/// </summary>
/// <param name="data"></param>
/// <param name="globalPath"></param>
/// <returns></returns>
public static GlobalPath SmoothPath(KinematicData data, GlobalPath globalPath)
{
NavMeshEdge edge;
var lookAhead = 3;
Vector3 lookAheadTarget;
var smoothedPath = new GlobalPath
{
IsPartial = globalPath.IsPartial
};
//we will string pull from the begginning to the end
var endPosition = globalPath.PathPositions.Last();
var currentPosition = data.position;
for (int i = 0; i < globalPath.PathNodes.Count; i++)
{
edge = globalPath.PathNodes[i] as NavMeshEdge;
if (edge != null)
{
if ((i + lookAhead) < globalPath.PathNodes.Count)
{
lookAheadTarget = globalPath.PathNodes[i + lookAhead].LocalPosition;
}
else
{
lookAheadTarget = endPosition;
}
if (!lookAheadTarget.Equals(currentPosition))
{
var closestPointInEdge = MathHelper.ClosestPointInLineSegment2ToLineSegment1(currentPosition, lookAheadTarget, edge.PointOne, edge.PointTwo, lookAheadTarget);
smoothedPath.PathPositions.Add(closestPointInEdge);
currentPosition = closestPointInEdge;
}
}
}
smoothedPath.PathPositions.Add(endPosition);
return smoothedPath;
}
public override Path GetPath(GlobalPath path)
{
return path;
}
protected GlobalPath CalculateSolution(NodeRecord node, bool partial)
{
var path = new GlobalPath
{
IsPartial = partial
};
var currentNode = node;
path.PathPositions.Add(this.GoalPosition);
//I need to remove the first Node and the last Node because they correspond to the dummy first and last Polygons that were created by the initialization.
//And for instance I don't want to be forced to go to the center of the initial polygon before starting to move towards my destination.
//skip the last node, but only if the solution is not partial (if the solution is partial, the last node does not correspond to the dummy goal polygon)
if (!partial && currentNode.parent != null)
{
currentNode = currentNode.parent;
}
while (currentNode.parent != null)
{
path.PathNodes.Add(currentNode.node); //we need to reverse the list because this operator add elements to the end of the list
path.PathPositions.Add(currentNode.node.LocalPosition);
if (currentNode.parent.parent == null) break; //this skips the first node
currentNode = currentNode.parent;
}
path.PathNodes.Reverse();
path.PathPositions.Reverse();
return path;
}
public virtual bool Search(out GlobalPath solution, bool returnPartialSolution = false)
{
var startTime = Time.realtimeSinceStartup;
var processedNodes = 0;
int count;
while (processedNodes < this.NodesPerSearch)
{
count = this.Open.CountOpen();
if (count == 0)
{
solution = null;
this.InProgress = false;
this.CleanUp();
this.TotalProcessingTime += Time.realtimeSinceStartup - startTime;
return true;
}
if (count > this.MaxOpenNodes)
{
this.MaxOpenNodes = count;
}
var bestNode = this.Open.GetBestAndRemove();
//goal node found, return the shortest Path
if (bestNode.node == this.GoalNode)
{
solution = this.CalculateSolution(bestNode, false);
this.InProgress = false;
this.CleanUp();
this.TotalProcessingTime += Time.realtimeSinceStartup - startTime;
return true;
}
this.Closed.AddToClosed(bestNode);
processedNodes++;
this.TotalProcessedNodes++;
var outConnections = bestNode.node.OutEdgeCount;
for (int i = 0; i < outConnections; i++)
{
var childNode = GenerateChildNodeRecord(bestNode, bestNode.node.EdgeOut(i));
var closedSearch = this.Closed.SearchInClosed(childNode);
if (closedSearch != null) continue;
var openSearch = this.Open.SearchInOpen(childNode);
if (openSearch != null)
{
if (childNode.fValue <= openSearch.fValue)
{
this.Open.Replace(openSearch, childNode);
}
}
else
{
this.Open.AddToOpen(childNode);
}
}
}
this.TotalProcessingTime += Time.realtimeSinceStartup - startTime;
//this is very unlikely but it might happen that we process all nodes alowed in this cycle but there are no more nodes to process
if (this.Open.CountOpen() == 0)
{
solution = null;
this.InProgress = false;
this.CleanUp();
return true;
}
//if the caller wants create a partial Path to reach the current best node so far
if (returnPartialSolution)
{
var bestNodeSoFar = this.Open.PeekBest();
solution = this.CalculateSolution(bestNodeSoFar, true);
}
else
{
solution = null;
}
return false;
}
public override Path GetPath(GlobalPath path)
{
return path.LocalPaths[0];
}
public abstract Path GetPath(GlobalPath path);
public override bool Search(out GlobalPath solution, bool returnPartialSolution = false)
{
var startTime = Time.realtimeSinceStartup;
var processedNodes = 0;
int count;
while (processedNodes < this.NodesPerSearch)
{
count = this.Open.CountOpen();
if (count == 0)
{
solution = null;
this.InProgress = false;
this.CleanUp();
this.TotalProcessingTime += Time.realtimeSinceStartup - startTime;
return true;
}
if (count > this.MaxOpenNodes)
{
this.MaxOpenNodes = count;
}
var bestNode = this.NodeRecordArray.GetBestAndRemove();
//goal node found, return the shortest Path
if (bestNode.node == this.GoalNode)
{
solution = this.CalculateSolution(bestNode, false);
this.InProgress = false;
this.CleanUp();
this.TotalProcessingTime += Time.realtimeSinceStartup - startTime;
return true;
}
this.NodeRecordArray.AddToClosed(bestNode);
processedNodes++;
this.TotalProcessedNodes++;
//put your code here
//or if you would like, you can change just these lines of code this in the original A* Pathfinding Class,
//create a ProcessChildNode method in the base class with the code from the previous A* algorithm.
//if you do this, then you don't need to implement this search method method. Don't forget to override the ProcessChildMethod if you do this
var outConnections = bestNode.node.OutEdgeCount;
for (int i = 0; i < outConnections; i++)
{
this.ProcessChildNode(bestNode,bestNode.node.EdgeOut(i));
}
}
this.TotalProcessingTime += Time.realtimeSinceStartup - startTime;
//this is very unlikely but it might happen that we process all nodes alowed in this cycle but there are no more nodes to process
if (this.Open.CountOpen() == 0)
{
solution = null;
this.InProgress = false;
this.CleanUp();
return true;
}
//if the caller wants create a partial Path to reach the current best node so far
if (returnPartialSolution)
{
var bestNodeSoFar = this.Open.PeekBest();
solution = this.CalculateSolution(bestNodeSoFar, true);
}
else
{
solution = null;
}
return false;
}
