/// <summary>
/// Build a path using Astar resources
/// Get the last point and get all its parent to build the path
/// </summary>
/// <param name="_pathToBuild">Astar resources</param>
static void BuildPath(Dictionary <Triangle, Triangle> _pathToBuild, CustomNavPath _path, Vector3 _origin, Vector3 _destination)
{
if (_pathToBuild.Count == 1)
{
List <Vector3> _pathPoints = new List <Vector3>();
_pathPoints.Add(_origin);
_pathPoints.Add(_destination);
_path.SetPath(_pathPoints);
return;
}
#region BuildingAbsolutePath
// Building absolute path -> Link all triangle's CenterPosition together
// Adding _origin and destination to the path
Triangle _currentTriangle = _pathToBuild.Last().Key;
List <Triangle> _absoluteTrianglePath = new List <Triangle>();
while (_currentTriangle != _pathToBuild.First().Key)
{
_absoluteTrianglePath.Add(_currentTriangle);
_currentTriangle = _pathToBuild[_currentTriangle];
}
_absoluteTrianglePath.Add(_currentTriangle);
//Reverse the path to start at the origin
_absoluteTrianglePath.Reverse();
// _path.SetPath(_absoluteTrianglePath.Select(t => t.CenterPosition).ToList());
// return;
#endregion
//Create the simplifiedPath
List <Vector3> _simplifiedPath = new List <Vector3>()
{
_origin
};
//If there is only the origin and the destination, the path doesn't have to be simplified
if (_absoluteTrianglePath.Count <= 1)
{
_simplifiedPath.Add(_destination);
_path.SetPath(_simplifiedPath);
return;
}
//Simplify the path with Funnel Algorithm
//Create both portals vertices arrays
Vector3[] _leftVertices = new Vector3[_absoluteTrianglePath.Count - 1];
Vector3[] _rightVertices = new Vector3[_absoluteTrianglePath.Count - 1];
//Create the apex
Vector3 _apex = _origin;
//Initialize portal vertices
Vector3 _startLinePoint = Vector3.zero;
Vector3 _endLinePoint = Vector3.zero;
Vector3 _vertex1 = Vector3.zero;
Vector3 _vertex2 = Vector3.zero;
_currentTriangle = null;
#region Initialise Portal Vertices
//Initialize portal vertices between each triangles
for (int i = 1; i < _absoluteTrianglePath.Count - 1; i++)
{
_currentTriangle = _absoluteTrianglePath[i];
_startLinePoint = _currentTriangle.CenterPosition;
_endLinePoint = _absoluteTrianglePath[i + 1].CenterPosition;
for (int j = 0; j < _currentTriangle.Vertices.Length; j++)
{
int k = j + 1 >= _currentTriangle.Vertices.Length ? 0 : j + 1;
_vertex1 = _currentTriangle.Vertices[j].Position;
_vertex2 = _currentTriangle.Vertices[k].Position;;
if (GeometryHelper.IsIntersecting(_startLinePoint, _endLinePoint, _vertex1, _vertex2))
{
//Debug.Log(_startLinePoint + "///" + _endLinePoint + " intersect with " + _vertex1 + "///" + _vertex2);
if (GeometryHelper.AngleSign(_startLinePoint, _endLinePoint, _vertex1) > 0)
{
_leftVertices[i] = _vertex2;
_rightVertices[i] = _vertex1;
}
else
{
_leftVertices[i] = _vertex1;
_rightVertices[i] = _vertex2;
}
break;
}
}
}
//Initialize start portal vertices
_startLinePoint = _origin;
_startLinePoint.y = _absoluteTrianglePath[1].CenterPosition.y;
_endLinePoint = _absoluteTrianglePath[1].CenterPosition;
_currentTriangle = _absoluteTrianglePath[0];
for (int j = 0; j < _currentTriangle.Vertices.Length; j++)
{
int k = j + 1 >= _currentTriangle.Vertices.Length ? 0 : j + 1;
_vertex1 = _currentTriangle.Vertices[j].Position;
_vertex2 = _currentTriangle.Vertices[k].Position;;
if (GeometryHelper.IsIntersecting(_startLinePoint, _endLinePoint, _vertex1, _vertex2))
{
if (GeometryHelper.AngleSign(_startLinePoint, _endLinePoint, _vertex1) > 0)
{
_leftVertices[0] = _vertex2;
_rightVertices[0] = _vertex1;
}
else
{
_leftVertices[0] = _vertex1;
_rightVertices[0] = _vertex2;
}
break;
}
}
// Initialise end portal vertices -> Close the funnel
_leftVertices[_leftVertices.Length - 1] = _destination;
_rightVertices[_rightVertices.Length - 1] = _destination;
#endregion
//Step through the channel
Vector3 _currentLeftVertex;
Vector3 _nextLeftVertex;
Vector3 _currentRightVertex;
Vector3 _nextRightVertex;
//Set left and right indexes
int _leftIndex = 0;
int _rightIndex = 0;
for (int i = 1; i < _absoluteTrianglePath.Count - 1; i++)
{
_currentLeftVertex = _leftVertices[_leftIndex];
_nextLeftVertex = _leftVertices[i];
_currentRightVertex = _rightVertices[_rightIndex];
_nextRightVertex = _rightVertices[i];
//If the new left vertex is different process
if (_nextLeftVertex != _currentLeftVertex && i > _leftIndex)
{
//If the next point does not widden funnel, update
if (GeometryHelper.AngleSign(_apex, _currentLeftVertex, _nextLeftVertex) >= 0)
{
//if next side cross the other side, place new apex
if (GeometryHelper.AngleSign(_apex, _currentRightVertex, _nextLeftVertex) > 0)
{
// Set the new Apex
_apex = _currentRightVertex;
_simplifiedPath.Add(_apex);
//Set i to the apex index to be at the good index on the next loop
i = _rightIndex;
// Find new right vertex.
for (int j = _rightIndex; j < _rightVertices.Length; j++)
{
if (_rightVertices[j] != _apex)
{
_rightIndex = j;
break;
}
}
_leftIndex = i;
i--;
continue;
}
// else skip to the next vertex
else
{
_leftIndex = i;
}
}
//else skip
}
//else skip
// If the right vertex is different process
if (_nextRightVertex != _currentRightVertex && i > _rightIndex)
{
//If the next point does not widden funnel, update
if (GeometryHelper.AngleSign(_apex, _currentRightVertex, _nextRightVertex) <= 0)
{
//if next side cross the other side, place new apex
if (GeometryHelper.AngleSign(_apex, _currentLeftVertex, _nextRightVertex) < 0)
{
//Set the new Apex
_apex = _currentLeftVertex;
_simplifiedPath.Add(_apex);
//Set i to the apex index to be at the good index on the next loop
i = _leftIndex;
// Find next Left Index
for (int j = _leftIndex; j < _leftVertices.Length; j++)
{
if (_leftVertices[j] != _apex)
{
_leftIndex = j;
break;
}
}
_rightIndex = i;
i--;
continue;
}
//else skip to the next vertex
else
{
_rightIndex = i;
}
}
//else skip
}
//else skip
}
_simplifiedPath.Add(_destination);
//Set the simplifiedPath
_path.SetPath(_simplifiedPath);
}
