private bool HasLOSXY(NodeItem start, NodeItem dest)
{
// Are the points within the same triangle? Notice that triangles are guaranteed convex...
if (start.Item == dest.Item)
{
return(true);
}
var corridor = start.Item;
while (corridor.Previous != SearchItem.Null)
{
// We want to check that the line segment [start, dest] intersects the edge we crossed
var curTriangle = corridor.Previous.Triangle;
var edge = corridor.Edge;
Vector3 right3d, left3d;
NavMesh.GetOutsideOrderedEdgePoints(curTriangle, edge, out right3d, out left3d);
var right = right3d.XY;
var left = left3d.XY;
if (!GeometryHelpers.IsLeftOfOrOn(left, start.Node.XY, dest.Node.XY))
{
return(false);
}
if (!GeometryHelpers.IsRightOfOrOn(right, start.Node.XY, dest.Node.XY))
{
return(false);
}
corridor = corridor.Previous;
// Ok, the move into this triangle was legal, if PointB is in the tri, then we're done
if (corridor == dest.Item)
{
return(true);
}
}
// Default
return(false);
}
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);
}
