/// <summary>
/// Make the agent follows the path
/// </summary>
/// <param name="_speed">speed</param>
/// <returns></returns>
public IEnumerator FollowPath(float _speed)
{
isMoving = true;
TDS_CustomNavPath _pathToFollow = CurrentPath;
int _index = 1;
while (Vector3.Distance(transform.position, _pathToFollow.NavigationPoints.Last().Position) > .5f)
{
if (Vector3.Distance(transform.position, _pathToFollow.NavigationPoints[_index].Position) <= .5f)
{
_index = _index + 1;
}
transform.position = Vector3.MoveTowards(transform.position, _pathToFollow.NavigationPoints[_index].Position, Time.deltaTime * _speed);
yield return(new WaitForEndOfFrame());
}
yield return(new WaitForEndOfFrame());
pathState = CalculatingState.Waiting;
isMoving = false;
CurrentPath.ClearPath();
OnDestinationReached?.Invoke();
yield break;
}
/// <summary>
/// Calculate path from an origin to a destination
/// Set the path when it can be calculated
/// </summary>
/// <param name="_origin">The Origin of the path </param>
/// <param name="_destination">The Destination of the path</param>
/// <param name="_path">The path to set</param>
/// <returns>Return if the path can be calculated</returns>
public bool CalculatePath(Vector3 _origin, Vector3 _destination, TDS_CustomNavPath _path)
{
isCalculating = true;
// GET TRIANGLES
// Get the origin triangle and the destination triangle
TDS_Triangle _currentTriangle = GetTriangleContainingPosition(_origin);
TDS_Triangle _targetedTriangle = GetTriangleContainingPosition(_destination);
// CREATE POINTS
TDS_NavPoint _currentPoint = null;
//Create a point on the origin position
TDS_NavPoint _originPoint = new TDS_NavPoint(_origin);
//Get the linked triangles for the origin point
_originPoint.LinkedTriangles.Add(_currentTriangle);
//Create a point on the destination position
TDS_NavPoint _destinationPoint = new TDS_NavPoint(_destination);
//ARRAY
TDS_NavPoint[] _linkedPoints = null;
//LISTS AND DICO
List <TDS_NavPoint> _openList = new List <TDS_NavPoint>();
Dictionary <TDS_NavPoint, TDS_NavPoint> _cameFrom = new Dictionary <TDS_NavPoint, TDS_NavPoint>();
/* ASTAR: Algorithm*/
// Add the origin point to the open and close List
//Set its heuristic cost and its selection state
_openList.Add(_originPoint);
_originPoint.HeuristicCostFromStart = 0;
_originPoint.HasBeenSelected = true;
_cameFrom.Add(_originPoint, _originPoint);
while (_openList.Count > 0)
{
//Get the point with the best heuristic cost
_currentPoint = GetBestPoint(_openList);
//If this point is in the targeted triangle,
if (GetTrianglesFromPoint(_currentPoint).Contains(_targetedTriangle))
{
//add the destination point to the close list and set the previous point to the current point
_cameFrom.Add(_destinationPoint, _currentPoint);
//Build the path
_path.BuildPath(_cameFrom);
//Clear all points selection state
foreach (KeyValuePair <int, TDS_NavPoint> pair in navPoints)
{
pair.Value.HasBeenSelected = false;
}
return(true);
}
//Get all linked points from the current point
_linkedPoints = GetLinkedPoints(_currentPoint);
for (int i = 0; i < _linkedPoints.Length; i++)
{
TDS_NavPoint _linkedPoint = _linkedPoints[i];
TDS_NavPoint _parentPoint;
// If the linked points is not selected yet
if (!_linkedPoint.HasBeenSelected)
{
// Calculate the heuristic cost from start of the linked point
float _cost = _currentPoint.HeuristicCostFromStart + HeuristicCost(_currentPoint, _linkedPoint);
_linkedPoint.HeuristicCostFromStart = _cost;
if (!_openList.Contains(_linkedPoint) || _cost < _linkedPoint.HeuristicCostFromStart)
{
if (IsInLineOfSight(_cameFrom[_currentPoint], _linkedPoint))
{
_cost = HeuristicCost(_cameFrom[_currentPoint], _linkedPoint);
_parentPoint = _cameFrom[_currentPoint];
}
else
{
_parentPoint = _currentPoint;
}
// Set the heuristic cost from start for the linked point
_linkedPoint.HeuristicCostFromStart = _cost;
//Its heuristic cost is equal to its cost from start plus the heuristic cost between the point and the destination
_linkedPoint.HeuristicPriority = HeuristicCost(_linkedPoint, _destinationPoint) + _cost;
//Set the point selected and add it to the open and closed list
_linkedPoint.HasBeenSelected = true;
_openList.Add(_linkedPoint);
_cameFrom.Add(_linkedPoint, _parentPoint);
}
}
}
}
return(false);
}
