/// <summary>
/// Root
/// </summary>
private SearchItem(int tri)
{
triangle = tri;
directDistToDest = 0;
previous = null;
distFromStart = 0;
nodeCount = 0;
}
public SearchItem(int triangle, int edge, float directDistToDest, ref Vector3 pointOfReference, SearchItem previous)
{
this.triangle = triangle;
this.edge = edge;
this.directDistToDest = directDistToDest;
this.previous = previous;
this.PointOfReference = pointOfReference;
distFromStart = previous.distFromStart + Vector3.Distance(previous.PointOfReference, pointOfReference);
nodeCount = previous.nodeCount + 1;
}
public NodeItem(Vector3 node, SearchItem item)
{
Node = node;
Item = item;
}
public void FindPath(Vector3 destination, SearchItem corridor, Path path)
{
var mesh = Tile.NavMesh;
path.Reset(corridor.NodeCount);
path.Add(destination); // the destination is the final vertex of the path
if (corridor.NodeCount < 3)
{
if (corridor.NodeCount == 2)
{
// Two neighboring nodes:
Vector3 newPoint;
Vector3 right, left;
// TODO: Project into 2d, so we can intersect two lines instead of a line and a plane
// greedily span a vertical plane over the line between start and destination
var start = corridor.Previous.PointOfReference;
//var plane = new Plane(start, destination, new Vector3(start.X, start.Y, start.Z + 10));
var startXY = corridor.Previous.PointOfReference.XY;
var endXY = destination.XY;
// and then intersect the plane with the edge that we want to traverse
mesh.GetInsideOrderedEdgePoints(corridor.Previous.Triangle, corridor.Edge, out right, out left);
var rightXY = right.XY;
var leftXY = left.XY;
//if (!Intersection.ClosestPointToPlane(right, left, plane, out newPoint))
//{
// this is geometrically impossible but might happen due to numerical inaccuracy
//Debugger.Break();
// newPoint = right;
//}
Vector2 newPointXY;
if (Intersection.IntersectionOfLineSegments2D(startXY, endXY, rightXY, leftXY, out newPointXY))
{
float newZ;
if (newPointXY.LerpZ(start, destination, out newZ))
{
newPoint = new Vector3(newPointXY, newZ);
}
else
{
// this is geometrically impossible but might happen due to numerical inaccuracy
Debugger.Break();
newPoint = right;
}
}
else
{
// this is geometrically impossible but might happen due to numerical inaccuracy
Debugger.Break();
newPoint = right;
}
path.Add(newPoint);
}
}
else
{
// More than two nodes:
//// if distance to new point is smaller than the threshold, replace, rather than add
//var distToLast = lastLeft
//if (distToLast < MinWaypointThreshold)
//{
//}
// steer backwards through the corridor
//Vector3 leftOrigin = destination, rightOrigin = destination;
var origin = destination;
Vector3 lastRight, lastLeft;
int lastLeftIdx = -1, lastRightIdx = -1;
var current = corridor;
mesh.GetInsideOrderedEdgePoints(current.Previous.Triangle, current.Edge, out lastRight, out lastLeft);
for (var i = corridor.NodeCount - 2; i > 0; i--)
{
current = current.Previous;
// get the two points that span the traversed edge
int leftIndex, rightIndex;
mesh.GetEdgePointIndices(current.Previous.Triangle, current.Edge, out rightIndex, out leftIndex);
var addedLeft = false;
if (leftIndex != lastLeftIdx)
{
// cast new ray and see if the last point is still inside the corridor
var left = mesh.Vertices[leftIndex];
var lastLeft2D = lastRight.XY;
var dir = left.XY - origin.XY;
var normal = dir.RightNormal();
var leftOrig2D = origin.XY;
var toLastLeft = lastLeft2D - leftOrig2D;
var dist = Vector2.Dot(toLastLeft, normal);
if (dist - MathUtil.Epsilonf > 0)
{
// right of the left ray -> put obstructing vertex into path
origin = lastLeft;
path.Add(lastLeft);
addedLeft = true;
}
lastLeft = left;
lastLeftIdx = leftIndex;
}
if (rightIndex != lastRightIdx)
{
var right = mesh.Vertices[rightIndex];
if (!addedLeft)
{
// cast new ray and see if the last point is still inside the corridor
var lastRight2D = lastLeft.XY;
var dir = right.XY - origin.XY;
var normal = dir.RightNormal();
var rightOrig2D = origin.XY;
var toLastRight = lastRight2D - rightOrig2D;
var dist = Vector2.Dot(toLastRight, normal);
if (dist + MathUtil.Epsilonf < 0)
{
// left of the right ray -> put obstructing vertex into path
origin = lastRight;
path.Add(lastRight);
}
}
lastRight = right;
lastRightIdx = rightIndex;
}
}
}
}
/// <summary>
/// Basic A* implementation that searches through the navmesh.
/// </summary>
public SearchItem FindCorridor(Vector3 start, Vector3 destination,
out System.Collections.Generic.HashSet <int> visited)
{
var mesh = NavMesh;
visited = null;
if (mesh == null)
{
return(SearchItem.Null);
}
var triStart = mesh.FindFirstTriangleUnderneath(start);
var triEnd = mesh.FindFirstTriangleUnderneath(destination);
if (triStart == -1 || triEnd == -1)
{
visited = null;
return(SearchItem.Null);
}
visited = new System.Collections.Generic.HashSet <int> {
triStart
};
// the start location is the first node
var current = new SearchItem(triStart, -1, Vector3.Distance(start, destination), ref start);
if (triStart == triEnd)
{
return(current);
}
// create fringe
var fringe = new IntervalHeap <SearchItem>(comparer)
{
current
};
// start search
var corridor = SearchItem.Null;
do
{
// next round
current = fringe.DeleteMin();
// get the vertices and neighbors of the current triangle
//var triangle = mesh.GetTriangle(current.Triangle);
var neighbors = mesh.GetNeighborsOf(current.Triangle);
//var poly = ((NavMesh)Mesh).Polygons[current.Triangle / 3];
// iterate over all neighbors
for (var edge = 0; edge < TerrainUtil.NeighborsPerTriangle; edge++)
{
var neighbor = neighbors[edge] * 3;
if (neighbor < 0 || visited.Contains(neighbor))
{
continue;
}
//var npoly = ((NavMesh)Mesh).Polygons[neighbor / 3];
//var ntri = Mesh.GetTriangle(neighbor);
visited.Add(neighbor);
// determine the edge that we want to traverse
Vector3 left, right;
mesh.GetInsideOrderedEdgePoints(current.Triangle, edge, out right, out left);
//switch (edge)
//{
// case TerrainUtil.ABEdgeIndex:
// left = triangle.Point1;
// right = triangle.Point2;
// break;
// case TerrainUtil.CAEdgeIndex:
// left = triangle.Point3;
// right = triangle.Point1;
// break;
// case TerrainUtil.BCEdgeIndex:
// left = triangle.Point2;
// right = triangle.Point3;
// break;
// default:
// throw new Exception("Impossible");
//}
var refPoint = (left + right) / 2.0f; // the middle of the edge to be traversed
var dist = Vector3.Distance(refPoint, destination);
var newItem = new SearchItem(neighbor, edge, dist, ref refPoint, current);
if (triEnd == neighbor)
{
// we are done
corridor = newItem;
goto Done;
}
fringe.Add(newItem);
}
} while (!fringe.IsEmpty);
Done:
return(corridor);
}
public void FindPathStringPull(Vector3 origin3d, Vector3 destination3d, SearchItem corridor, Path path)
{
var mesh = NavMesh;
var corridorStart = corridor;
var pathNodes = new List <NodeItem>(corridor.NodeCount);
pathNodes.Add(new NodeItem(destination3d, corridorStart));
if (corridor.Edge == -1) // origin and destination are within the same triangle
{
pathNodes.Add(new NodeItem(origin3d, corridorStart));
path.Reset(pathNodes.Count);
path.Add(pathNodes);
return;
}
Vector3 curTriApex3d, curTriLeft3d, curTriRight3d;
Vector3 prevTriApex3d, prevTriLeft3d, prevTriRight3d;
Vector2 curTriApex, destination, curTriLeft, curTriRight;
int curTriangle, curEdge;
bool apexInFunnelLeft;
bool apexInFunnelRight;
while (corridor.Previous.Previous != SearchItem.Null) // The next triangle contains the starting point
{
curTriangle = corridor.Previous.Triangle;
curEdge = corridor.Edge;
mesh.GetOutsideOrderedEdgePointsPlusApex(curTriangle, curEdge, out curTriRight3d, out curTriLeft3d,
out curTriApex3d);
destination = destination3d.XY;
curTriApex = curTriApex3d.XY;
curTriLeft = curTriLeft3d.XY;
curTriRight = curTriRight3d.XY;
var foundNewDestination = false;
while (!foundNewDestination) // Found new point for the path
{
if (corridor.Previous.Previous == SearchItem.Null)
{
break;
}
apexInFunnelLeft = GeometryHelpers.IsRightOfOrOn(curTriApex, destination, curTriLeft);
apexInFunnelRight = GeometryHelpers.IsLeftOfOrOn(curTriApex, destination, curTriRight);
if (apexInFunnelLeft && apexInFunnelRight)
{
// the apex of the next triangle in the corridor is within the funnel. Narrow the funnel.
var prevEdge = corridor.Previous.Edge;
var prevTriangle = corridor.Previous.Previous.Triangle;
mesh.GetOutsideOrderedEdgePointsPlusApex(prevTriangle, prevEdge, out prevTriRight3d,
out prevTriLeft3d, out prevTriApex3d);
var prevTriApex = prevTriApex3d.XY;
var prevTriLeft = prevTriLeft3d.XY;
var prevTriRight = prevTriRight3d.XY;
if (curTriLeft == prevTriLeft)
{
curTriRight3d = curTriApex3d;
curTriRight = curTriApex;
}
else if (curTriRight == prevTriRight)
{
curTriLeft3d = curTriApex3d;
curTriLeft = curTriApex;
}
else
{
throw new Exception("WTF! You screwed the pooch here pal, find the error.");
}
curTriApex3d = prevTriApex3d;
curTriApex = prevTriApex;
}
else if (!apexInFunnelLeft && apexInFunnelRight)
{
destination3d = curTriLeft3d;
pathNodes.Add(new NodeItem(destination3d, corridor.Previous));
//path.Add(destination3d);
foundNewDestination = true;
}
else if (apexInFunnelLeft)
{
destination3d = curTriRight3d;
pathNodes.Add(new NodeItem(destination3d, corridor.Previous));
//path.Add(destination3d);
foundNewDestination = true;
}
else
{
throw new Exception("This should never happen.");
}
corridor = corridor.Previous;
}
}
// The next triangle in the corridor contains the starting point
curTriangle = corridor.Previous.Triangle;
curEdge = corridor.Edge;
mesh.GetOutsideOrderedEdgePointsPlusApex(curTriangle, curEdge, out curTriRight3d, out curTriLeft3d,
out curTriApex3d);
destination = destination3d.XY;
curTriApex = origin3d.XY;
curTriLeft = curTriLeft3d.XY;
curTriRight = curTriRight3d.XY;
apexInFunnelLeft = GeometryHelpers.IsRightOfOrOn(curTriApex, destination, curTriLeft);
apexInFunnelRight = GeometryHelpers.IsLeftOfOrOn(curTriApex, destination, curTriRight);
if (apexInFunnelLeft && apexInFunnelRight)
{
// the starting point of the corridor is within the funnel. We're done.
pathNodes.Add(new NodeItem(origin3d, corridor.Previous));
//path.Add(origin3d);
}
else if (!apexInFunnelLeft && apexInFunnelRight)
{
// the starting point of the corridor is outside the funnel on the left
//add the left point and then the starting point
pathNodes.Add(new NodeItem(curTriLeft3d, corridor.Previous));
pathNodes.Add(new NodeItem(origin3d, corridor.Previous));
}
else if (apexInFunnelLeft)
{
// the starting point of the corridor is outside the funnel on the right
//add the right point and then the starting point
pathNodes.Add(new NodeItem(curTriRight3d, corridor.Previous));
pathNodes.Add(new NodeItem(origin3d, corridor.Previous));
}
else
{
throw new Exception("This should never happen.");
}
StringPull(pathNodes);
path.Reset(pathNodes.Count);
path.Add(pathNodes);
}
public NodeItem(Vector3 node, SearchItem item)
{
Node = node;
Item = item;
}
public SearchItem(int triangle, int edge, float directDistToDest, ref Vector3 pointOfReference, SearchItem previous)
{
this.triangle = triangle;
this.edge = edge;
this.directDistToDest = directDistToDest;
this.previous = previous;
this.PointOfReference = pointOfReference;
distFromStart = previous.distFromStart + Vector3.Distance(previous.PointOfReference, pointOfReference);
nodeCount = previous.nodeCount + 1;
}