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); }