public bool FindShortestPath(NavMeshPoint startPosition, NavMeshPoint endPosition, List <NavMeshLine> pathLines = null)
{
if (!startPosition.Walkable(this) || !endPosition.Walkable(this))
{
return(false);
}
var startFace = startPosition.face;
var endFace = endPosition.face;
if (startFace != endFace && pathMatrix[area, startFace, endFace] == null)
{
return(false);
}
if (pathLines != null)
{
pathEdges.Clear();
while (startFace != endFace)
{
var pathEdge = pathMatrix[area, startFace, endFace];
pathEdges.Add(pathEdge);
startFace = pathEdge.face.id;
}
this.startPosition = startPosition;
this.endPosition = endPosition;
this.pathLines = pathLines;
FindPathCorners(0, pathEdges.Count);
this.pathLines = null;
}
return(true);
}
public NavMeshPoint FindTowardsPath(NavMeshPoint position, float rotation, float distance, bool slope, List <NavMeshLine> pathLines = null)
{
if (!position.Walkable(this))
{
return(position);
}
Vert pathVert = null;
Edge pathEdge = null;
var pathEdgeRatio = 0f;
var pathFace = faces[position.face];
NormalizeRotation(rotation);
startPosition = position;
endPosition = position;
endDistance = distance;
this.pathLines = pathLines;
while (endDistance > MaxZeroEpsilon)
{
var deltaX = sinRotation * endDistance;
var deltaZ = cosRotation * endDistance;
var endX = startPosition.x + deltaX;
var endZ = startPosition.z + deltaZ;
if (pathVert != null)
{
foreach (var edge in pathVert.edges)
{
if (edge.face.Walkable(this))
{
var next = edge.next;
var edgeRight = edge.Right(endX, endZ);
var nextRight = next.Right(endX, endZ);
if (edgeRight && nextRight)
{
pathFace = edge.face;
startPosition.face = pathFace.id;
pathVert = null;
break;
}
if (!edgeRight && !edge.Walkable(this))
{
if (Dot(next.vert.x - endX, next.vert.z - endZ, edge.deltaX, edge.deltaZ) > MaxZeroEpsilon)
{
pathEdge = edge;
pathEdgeRatio = 1;
}
}
else if (!nextRight && !next.Walkable(this))
{
if (Dot(endX - next.vert.x, endZ - next.vert.z, next.deltaX, next.deltaZ) > MaxZeroEpsilon)
{
pathEdge = next;
pathEdgeRatio = 0;
}
}
if (pathEdge != null)
{
pathFace = pathEdge.face;
startPosition.face = pathFace.id;
pathVert = null;
break;
}
}
}
if (pathVert != null)
{
if (slope && pathLines != null)
{
pathLines.Add(new NavMeshLine(endPosition, endDistance, 0, 0, 0));
}
break;
}
}
if (pathEdge != null)
{
if (!slope)
{
break;
}
var ratio = Projection(pathEdge.vert.x, pathEdge.vert.z, pathEdge.deltaX, pathEdge.deltaZ, endX, endZ);
if (ratio < MaxZeroEpsilon)
{
pathVert = pathEdge.vert;
ratio = pathEdgeRatio / (pathEdgeRatio - ratio);
}
else if (MinOneEpsilon < ratio)
{
pathVert = pathEdge.next.vert;
ratio = (1 - pathEdgeRatio) / (ratio - pathEdgeRatio);
}
else
{
MoveTowards(pathEdge.GetPoint(this, ratio), endDistance);
break;
}
MoveTowards(pathVert.GetPosition(this, pathFace.id), endDistance * ratio);
pathEdge = null;
continue;
}
if (pathFace.Contains(endX, endZ))
{
MoveTowards(new NavMeshPoint(this, pathFace.id, endX, pathFace.GetY(endX, endZ), endZ), endDistance);
break;
}
maxCrossEdge = null;
maxCrossEdgeRatio = 0;
maxCrossStartRatio = 0;
foreach (var edge in pathFace.edges)
{
FindMaxCross(edge, startPosition.x, startPosition.z, deltaX, deltaZ);
}
if (maxCrossEdge == null)
{
break;
}
if (maxCrossEdgeRatio < MaxZeroEpsilon)
{
pathVert = maxCrossEdge.vert;
MoveTowards(pathVert.GetPosition(this, pathFace.id), endDistance * maxCrossStartRatio);
}
else if (MinOneEpsilon < maxCrossEdgeRatio)
{
pathVert = maxCrossEdge.next.vert;
MoveTowards(pathVert.GetPosition(this, pathFace.id), endDistance * maxCrossStartRatio);
}
else
{
MoveTowards(maxCrossEdge.GetPoint(this, maxCrossEdgeRatio), endDistance * maxCrossStartRatio);
if (maxCrossEdge.Walkable(this))
{
pathFace = maxCrossEdge.pair.face;
startPosition.face = pathFace.id;
}
else
{
pathEdge = maxCrossEdge;
pathEdgeRatio = maxCrossEdgeRatio;
}
}
}
this.pathLines = null;
return(endPosition);
}
