private void OnDrawGizmos()
{
if (triangles.Count == 0)
{
return;
}
if (ShowLinks)
{
Gizmos.color = Color.green;
foreach (KeyValuePair <int, TDS_NavPoint> pair in navPoints)
{
int[] _linkedIndexes = pair.Value.GetAllNeighborsIndexes();
for (int i = 0; i < _linkedIndexes.Length; i++)
{
Gizmos.DrawLine(pair.Value.Position, navPoints[_linkedIndexes[i]].Position);
}
}
}
for (int i = 0; i < triangles.Count; i++)
{
TDS_Triangle _t = triangles[i];
for (int j = 0; j < 3; j++)
{
if (!navPoints.ContainsKey(_t.VerticesIndex[j]))
{
continue;
}
if (ShowVertices)
{
Gizmos.color = Color.red;
Gizmos.DrawSphere(navPoints[_t.VerticesIndex[j]].Position, .1f);
}
if (ShowSegments)
{
Gizmos.color = Color.blue;
if (j < 2)
{
Gizmos.DrawLine(navPoints[_t.VerticesIndex[j]].Position, navPoints[_t.VerticesIndex[j + 1]].Position);
}
else
{
Gizmos.DrawLine(navPoints[_t.VerticesIndex[j]].Position, navPoints[_t.VerticesIndex[0]].Position);
}
}
}
if (ShowMiddle)
{
Gizmos.color = Color.cyan;
for (int j = 0; j < _t.MiddleSegmentIndex.Count; j++)
{
if (navPoints.ContainsKey(_t.MiddleSegmentIndex[j]))
{
Gizmos.DrawSphere(navPoints[_t.MiddleSegmentIndex[j]].Position, .1f);
}
}
}
Gizmos.color = Color.green;
Gizmos.DrawSphere(_t.CenterPosition, .1f);
}
}
/// <summary>
/// Get all triangles' neighbors and add them to the list of Neighbors
/// </summary>
void LinkTriangles(TDS_Triangle _triangle)
{
//COMPARE CURRENT TRIANGLE WITH EVERY OTHER TRIANGLE
for (int i = 0; i < triangles.Count; i++)
{
// IF TRIANGLES ARE THE SAME, DON'T COMPARE THEM
if (triangles[i] != _triangle && !triangles[i].HasBeenLinked)
{
// GET THE VERTICES IN COMMON
int[] _verticesInCommon = GetVerticesInCommon(_triangle, triangles[i]);
// CHECK IF THERE IS THE RIGHT AMMOUNT OF VERTICES IN COMMON
if (_verticesInCommon.Length > 0)
{
_triangle.LinkedTriangles.Add(triangles[i]);
}
// IF THERE IS 2 TWO VERTICES IN COMMON, THE 2 TRIANGLES HAD 1 SIDE IN COMMON
if (_verticesInCommon.Length == 2)
{
Vector3 _pos = (navPoints[_verticesInCommon[0]].Position + navPoints[_verticesInCommon[1]].Position) / 2;
TDS_NavPoint _middlePoint = new TDS_NavPoint(_pos);
navPoints.Add(navPoints.Count, _middlePoint);
_triangle.MiddleSegmentIndex.Add(navPoints.Count - 1);
triangles[i].MiddleSegmentIndex.Add(navPoints.Count - 1);
}
}
}
LinkPoints(_triangle);
_triangle.HasBeenLinked = true;
}
public int[] GetAllNeighborsIndexes()
{
List <int> _indexes = new List <int>();
for (int i = 0; i < linkedTriangles.Count; i++)
{
TDS_Triangle _t = linkedTriangles[i];
_indexes.AddRange(_t.VerticesIndex);
_indexes.AddRange(_t.MiddleSegmentIndex);
}
return(_indexes.ToArray());
}
/// <summary>
/// CENTER IS EQUAL TO THE ARITHMETIQUE MEDIUM OF THE 3 POINTS
/// CREATE CENTER POINT OF A TRIANGLE AND ADD IT TO THE NAV POINTS DICO
/// </summary>
void GetCenterPosition(TDS_Triangle _triangle)
{
int[] _indexPositions = new int[3] {
_triangle.VerticesIndex[0], _triangle.VerticesIndex[1], _triangle.VerticesIndex[2]
};
TDS_NavPoint[] _points = new TDS_NavPoint[3] {
navPoints[_indexPositions[0]], navPoints[_indexPositions[1]], navPoints[_indexPositions[2]]
};
Vector3 _pos = (_points[0].Position + _points[1].Position + _points[2].Position) / 3;
_triangle.CenterPosition = _pos;
}
/// <summary>
/// For the triangle, link each point to the other points of the triangle
/// </summary>
/// <param name="_triangle">triangle that contains the points</param>
public void LinkPoints(TDS_Triangle _triangle)
{
List <int> _allIndexes = new List <int>();
_allIndexes.AddRange(_triangle.VerticesIndex);
_allIndexes.AddRange(_triangle.MiddleSegmentIndex);
for (int i = 0; i < _allIndexes.Count; i++)
{
TDS_NavPoint _point = navPoints[_allIndexes[i]];
_point.LinkedTriangles.Add(_triangle);
}
}
/// <summary>
/// Compare triangles
/// if the triangles have more than 1 vertices in common return true
/// </summary>
/// <param name="_triangle1">First triangle to compare</param>
/// <param name="_triangle2">Second triangle to compare</param>
/// <returns>If the triangles have more than 1 vertex.ices in common</returns>
int[] GetVerticesInCommon(TDS_Triangle _triangle1, TDS_Triangle _triangle2)
{
List <int> _verticesIndex = new List <int>();
for (int i = 0; i < _triangle1.VerticesIndex.Length; i++)
{
for (int j = 0; j < _triangle2.VerticesIndex.Length; j++)
{
if (_triangle1.VerticesIndex[i] == _triangle2.VerticesIndex[j])
{
_verticesIndex.Add(_triangle1.VerticesIndex[i]);
}
}
}
return(_verticesIndex.ToArray());
}
/// <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);
}
/// <summary>
/// Return if the position is inside of the triangle
/// </summary>
/// <param name="_position"></param>
/// <returns></returns>
bool IsInTriangle(Vector3 _position, TDS_Triangle _triangle)
{
Barycentric _barycentric = new Barycentric(navPoints[_triangle.VerticesIndex[0]].Position, navPoints[_triangle.VerticesIndex[1]].Position, navPoints[_triangle.VerticesIndex[2]].Position, _position);
return(_barycentric.IsInside);
}
/// <summary>
/// Get all nav mesh surfaces
/// Update all navmesh datas for each NavMeshSurfaces
/// Calculate triangulation
/// Add each triangle in the triangulation to a list of triangles
/// Link Triangles with its neighbors
/// </summary>
public void GetNavPointsFromNavSurfaces()
{
triangles.Clear();
navPoints.Clear();
List <NavMeshSurface> _surfaces = NavMeshSurface.activeSurfaces;
foreach (NavMeshSurface surface in _surfaces)
{
surface.BuildNavMesh();
}
NavMeshTriangulation _tr = NavMesh.CalculateTriangulation();
Vector3[] _vertices = _tr.vertices;
List <int> _modifiedIndices = _tr.indices.ToList();
//GET ALL NAV POINTS
int _previousIndex = 0;
for (int i = 0; i < _vertices.Length; i++)
{
////CREATE A POINT AT POSITION
Vector3 _pos = _vertices[i];
// CHECK IF NAV POINT ALREADY EXISTS
if (navPoints.Any(p => p.Value.Position == _pos))
{
////GET THE SAME POINT
KeyValuePair <int, TDS_NavPoint> _existingPoint = navPoints.Select(n => n).Where(p => p.Value.Position == _pos).First();
//// ORDER THE INDEX
for (int j = 0; j < _modifiedIndices.Count; j++)
{
// REPLACE INDEX
if (_modifiedIndices[j] == _previousIndex)
{
_modifiedIndices[j] = _existingPoint.Key;
}
// IF INDEX IS GREATER THAN THE REPLACED INDEX REMOVE ONE FROM THEM
if (_modifiedIndices[j] > _previousIndex)
{
_modifiedIndices[j]--;
}
}
}
// IF THE NAV POINT DOESN'T EXISTS ADD IT TO THE DICO
else
{
navPoints.Add(navPoints.Count, new TDS_NavPoint(_pos));
_previousIndex++;
}
}
//GET ALL TRIANGLES
for (int i = 0; i < _modifiedIndices.Count; i += 3)
{
int[] _pointsIndex = new int[3] {
_modifiedIndices[i], _modifiedIndices[i + 1], _modifiedIndices[i + 2]
};
TDS_Triangle _triangle = new TDS_Triangle(_pointsIndex);
triangles.Add(_triangle);
GetCenterPosition(_triangle);
}
for (int i = 0; i < triangles.Count; i++)
{
TDS_Triangle _t = triangles[i];
LinkTriangles(_t);
}
}
