bool CanMoveStraight(List <Waypoint> allWaypoints, int start, int end, Vector3 position, Vector3 direction) { for (int i = start + 1; i < end; i++) { Edge e = allWaypoints[i].Edge; float ta; if (!(Math.LineLineIntersectionTA(e.PointA, Vector3.Normalize(e.PointB - e.PointA), position, direction, out ta) && ta >= -0.1f && ta < (e.PointB - e.PointA).Length() + 0.1f)) { return(false); } } return(true); }
public List <Waypoint> FindPath(Vector3 start, Vector3 end, Node startNode, Node endNode) { Node s = startNode ?? BoundingRegion.GetNodeAt(start); Node e = endNode ?? BoundingRegion.GetNodeAt(end); if (s == null || e == null || s == e) { return(null); } List <Node> nodePath = FindPath(s, e); if (nodePath == null) { return(null); } List <Waypoint> allWaypoints = new List <Waypoint>(); allWaypoints.Add(new Waypoint { InternalIndex = allWaypoints.Count, OptimalPoint = end, Node = e, Edge = null //nodePath[0].Edges[nodePath[1]] }); for (int i = 0; i < nodePath.Count - 1; i++) { Edge ed = nodePath[i].Edges[nodePath[i + 1]]; allWaypoints.Add(new Waypoint { InternalIndex = allWaypoints.Count, OptimalPoint = (ed.PointA + ed.PointB) / 2, Node = nodePath[i + 1], Edge = ed }); } allWaypoints.Add(new Waypoint { InternalIndex = allWaypoints.Count, OptimalPoint = start, Node = s, Edge = null //nodePath[nodePath.Count - 2].Edges[nodePath[nodePath.Count - 1]] }); bool[] disabled = new bool[allWaypoints.Count]; for (int i = 0; i < disabled.Length; i++) { disabled[i] = false; } // The first iteration removes unneccessary waypoints // Go through, waypoint by waypoint for (int left = 0; left < allWaypoints.Count - 2; left++) { //And see if we can remove any succeding waypoints for this one for (int right = left + 2; right < allWaypoints.Count; right++) { if (CanMoveStraight(allWaypoints, left, right, allWaypoints[left].OptimalPoint, Vector3.Normalize(allWaypoints[right].OptimalPoint - allWaypoints[left].OptimalPoint))) { disabled[right - 1] = true; } else { left = right - 2; break; } } } List <Waypoint> waypoints = new List <Waypoint>(); for (int i = 0; i < allWaypoints.Count; i++) { if (!disabled[i]) { waypoints.Add(allWaypoints[i]); } } //This tries to move the waypoints along the edge they occupy to optimize the path, //basically cutting corners //Problem is, it actually looks a bit better if the units walk in the middle of the path //instead of taking the absolutely best path, so it's turned off #if (false) for (int i = 1; i < waypoints.Count - 1; i++) { Edge ed = waypoints[i].Edge; //We start by finding the intersection point of the current optimal point float currentT = (waypoints[i].OptimalPoint - ed.PointA).Length(); //And of the best point (a line from the previous waypoint to the next one float bestT; if (Math.LineLineIntersectionTA(ed.PointA, Vector3.Normalize(ed.PointB - ed.PointA), waypoints[i - 1].OptimalPoint, Vector3.Normalize(waypoints[i + 1].OptimalPoint - waypoints[i - 1].OptimalPoint), out bestT)) { //From that we know which "corner" is the best place for the optimal point Vector3 WP; if (bestT < currentT) { WP = ed.PointA; } else if (bestT > currentT) { WP = ed.PointB; } else { continue; } //Then we just need to check if it's ok to place it there if (CanMoveStraight(allWaypoints, waypoints[i - 1].InternalIndex, waypoints[i].InternalIndex, waypoints[i - 1].OptimalPoint, Vector3.Normalize(WP - waypoints[i - 1].OptimalPoint)) && CanMoveStraight(allWaypoints, waypoints[i].InternalIndex, waypoints[i + 1].InternalIndex, WP, Vector3.Normalize(waypoints[i + 1].OptimalPoint - WP))) { waypoints[i].OptimalPoint = WP; } } else { /*if (CanMoveStraight(nodePath, waypointNodeIndexes[i - 2], waypointNodeIndexes[i - 1] - 1, * waypoints[i - 2].OptimalPoint, Vector3.Normalize(waypoints[i - 1].OptimalPoint - waypoints[i - 2].OptimalPoint))) * waypoints[i].OptimalPoint = waypoints[i - 1].OptimalPoint;*/ } } #endif List <Waypoint> revWP = new List <Waypoint>(); revWP.Add(waypoints.Last()); for (int i = waypoints.Count - 2; i >= 0; i--) { if (waypoints[i] != revWP.Last()) { revWP.Add(waypoints[i]); } } return(revWP); }