/// <summary>
/// Generate Triangles from the navigation Datas
/// Then link triangles when they share 2 vertices
/// </summary>
private void GenerateTriangles()
{
Triangles = new List <Triangle>();
int _i, _j = 0;
for (_i = 0; _i < NavigationDatas.Indices.Length; _i += 3)
{
Triangles.Add(new Triangle(_j, new int[3] {
NavigationDatas.Indices[_i], NavigationDatas.Indices[_i + 1], NavigationDatas.Indices[_i + 2]
}));
_j++;
}
for (_i = 0; _i < Triangles.Count; _i++)
{
for (_j = 0; _j < Triangles.Count; _j++)
{
if (_i == _j)
{
continue;
}
if (GeometryHelper2D.VerticesInCommon(Triangles[_i], Triangles[_j]) == 2)
{
Triangles[_i].LinkedTriangles.Add(_j);
}
}
}
}
/// <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 Vector2[] BuildPath(Dictionary <int, int> _pathToBuild, Triangle _firstTriangle, Triangle _lastTriangle, Vector2 _origin, Vector2 _destination, List <Triangle> _trianglesDatas)
{
if (_pathToBuild.Count <= 1)
{
List <Vector2> _pathPoints = new List <Vector2>();
_pathPoints.Add(_origin);
_pathPoints.Add(_destination);
return(_pathPoints.ToArray());
}
#region BuildingAbsolutePath
// Building absolute path -> Link all triangle's CenterPosition together
// Adding _origin and destination to the path
Triangle _currentTriangle = _trianglesDatas[_pathToBuild[_lastTriangle.Index]];
List <Triangle> _absoluteTrianglePath = new List <Triangle>();
while (_currentTriangle != _firstTriangle)
{
_absoluteTrianglePath.Add(_currentTriangle);
_currentTriangle = _trianglesDatas[_pathToBuild[_currentTriangle.Index]];
}
_absoluteTrianglePath.Add(_currentTriangle);
//Reverse the path to start at the origin
_absoluteTrianglePath.Reverse();
#endregion
//Create the simplifiedPath
List <Vector2> _simplifiedPath = new List <Vector2>()
{
_origin
};
if (_absoluteTrianglePath.Count <= 1)
{
_simplifiedPath.Add(_destination);
return(_simplifiedPath.ToArray());
}
//Simplify the path with Funnel Algorithm
//Create both portals vertices arrays
Vector2[] _leftVertices = new Vector2[_absoluteTrianglePath.Count - 1];
Vector2[] _rightVertices = new Vector2[_absoluteTrianglePath.Count - 1];
//Create the apex
Vector2 _apex = _origin;
//Initialize portal vertices
Vector2 _startLinePoint = Vector2.zero;
Vector2 _endLinePoint = Vector2.zero;
Vector2 _vertex1 = Vector2.zero;
Vector2 _vertex2 = Vector2.zero;
#region Initialise Portal Vertices
//Initialize portal vertices between each triangles
for (int i = 1; i < _absoluteTrianglePath.Count - 1; i++)
{
_currentTriangle = _absoluteTrianglePath[i];
_startLinePoint = NavMeshManager.Instance.GetCenterPosition(_currentTriangle);
_endLinePoint = NavMeshManager.Instance.GetCenterPosition(_absoluteTrianglePath[i + 1]);
for (int j = 0; j < _currentTriangle.Vertices.Length; j++)
{
int k = j + 1 >= _currentTriangle.Vertices.Length ? 0 : j + 1;
_vertex1 = NavMeshManager.Instance.NavigationDatas.Vertices[_currentTriangle.Vertices[j]];
_vertex2 = NavMeshManager.Instance.NavigationDatas.Vertices[_currentTriangle.Vertices[k]];
if (GeometryHelper2D.IsIntersecting(_startLinePoint, _endLinePoint, _vertex1, _vertex2))
{
//Debug.Log(_startLinePoint + "///" + _endLinePoint + " intersect with " + _vertex1 + "///" + _vertex2);
if (GeometryHelper2D.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;
_endLinePoint = NavMeshManager.Instance.GetCenterPosition(_absoluteTrianglePath[1]);
_currentTriangle = _absoluteTrianglePath[0];
for (int j = 0; j < _currentTriangle.Vertices.Length; j++)
{
int k = j + 1 >= _currentTriangle.Vertices.Length ? 0 : j + 1;
_vertex1 = NavMeshManager.Instance.NavigationDatas.Vertices[_currentTriangle.Vertices[j]];
_vertex2 = NavMeshManager.Instance.NavigationDatas.Vertices[_currentTriangle.Vertices[k]];
if (GeometryHelper2D.IsIntersecting(_startLinePoint, _endLinePoint, _vertex1, _vertex2))
{
if (GeometryHelper2D.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
Vector2 _currentLeftVertex;
Vector2 _nextLeftVertex;
Vector2 _currentRightVertex;
Vector2 _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 (GeometryHelper2D.AngleSign(_apex, _currentLeftVertex, _nextLeftVertex) >= 0)
{
//if next side cross the other side, place new apex
if (GeometryHelper2D.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 (GeometryHelper2D.AngleSign(_apex, _currentRightVertex, _nextRightVertex) <= 0)
{
//if next side cross the other side, place new apex
if (GeometryHelper2D.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);
if (_simplifiedPath.Contains(Vector2.zero))
{
_simplifiedPath.Remove(Vector2.zero);
}
//Set the simplifiedPath
return(_simplifiedPath.ToArray());
}
/// <summary>
/// Make the agent move along the path and avoid the obstacles
/// </summary>
protected override void MovableUpdate()
{
if (currentPath == null || currentPath.Length == 0)
{
StopAgent();
return;
}
if (isMoving && (currentIndex > 0))
{
Vector2 _previousPosition = currentPath[currentIndex - 1];
Vector2 _nextPosition = currentPath[currentIndex];
if (Vector2.Distance(transform.position, LastPosition) < collider.bounds.extents.x)
{
StopAgent();
}
else
{
if (Vector2.Distance(transform.position, _nextPosition) <= collider.bounds.extents.x)
{
//Increasing path index
currentIndex++;
_previousPosition = currentPath[currentIndex - 1];
_nextPosition = currentPath[currentIndex];
}
/* Get the predicted Velocity and the Predicted position*/
Vector2 _predictedPosition = (Vector2)transform.position + velocity.normalized;
RaycastHit2D _obstacle;
if (CastCollider(_predictedPosition, out _obstacle))
{
Vector2 _dir = (_obstacle.point - (Vector2)_obstacle.transform.position).normalized;
Avoid(_dir);
}
else
{
/*Get the transposed Position of the predicted position on the segment between the previous and the next point
* The agent has to get closer while it's to far away from the path
*/
Vector2 _normalPoint = GeometryHelper2D.GetNormalPoint(_predictedPosition, _previousPosition, _nextPosition);
/* Direction of the segment between the previous and the next position normalized in order to go further on the path
* Targeted position is the normal point + an offset defined by the direction of the segment to go a little further on the path
* If the target is out of the segment between the previous and the next position, the target position is the next position
*/
Vector2 _dir = (_nextPosition - _previousPosition).normalized;
Vector2 _targetPosition = _normalPoint + _dir;
if (!GeometryHelper2D.PointContainedInSegment(_previousPosition, _nextPosition, _targetPosition))
{
_targetPosition = _nextPosition;
}
/* Distance between the predicted position and the normal point on the segment
* If the distance is greater than the radius, it has to steer to get closer
*/
float _distance = Vector2.Distance(_predictedPosition, _normalPoint);
float _scalarProduct = Vector2.Dot(velocity, _dir);
if (_distance > collider.bounds.extents.x || _scalarProduct == -1 || velocity == Vector2.zero)
{
Seek(_targetPosition);
}
}
velocity.Normalize();
Move(velocity);
}
}
base.MovableUpdate();
}
