// Build a path in an approximately straight line from start to destination by stringing together steps // This is NOT guaranteed to not get stuck in a local terrain feature // Returns the total normalized path time, or 'forever' if stuck public static float FindLocalPath( PathfinderData data, Vector3 start, Vector3 destination, MobilityType mobility, float radius) { float distance = (destination - start).magnitude; Vector3 waypoint = start; float time = 0f; while (distance > CompletionDist) { Vector3 previous = waypoint; waypoint = TakeStep(data, waypoint, destination, mobility, radius); if (waypoint == NoPosition) { return(Forever); } time += StepSize / mobility.GetUnitSpeed(data.terrain, data.map, waypoint, radius, (waypoint - previous).normalized); distance = (destination - waypoint).magnitude; } time += distance / mobility.GetUnitSpeed(data.terrain, data.map, waypoint, radius, (destination - waypoint).normalized); return(time); }
// Finds an intermediate step along the way from start to destination // Returns 'NoPosition' if stuck private static Vector3 TakeStep( PathfinderData data, Vector3 start, Vector3 destination, MobilityType mobility, float radius) { Vector3 straight = (destination - start).normalized; straightStep = false; // Fan out in a two-point horizontal pattern to find a way forward for (float ang1 = 0f; ang1 <= MaxAngle; ang1 += AngSearchInc) { for (int direction = -1; direction <= 1; direction += 2) { Vector3 direction1 = ang1 > 0f ? Quaternion.AngleAxis(ang1 * direction, Vector3.up) * straight : straight; Vector3 midpoint = start + direction1 * StepSize; float midspeed = mobility.GetUnitSpeed(data.terrain, data.map, midpoint, radius, direction1); if (midspeed > 0f) { for (float ang2 = 0f; ang2 <= ang1; ang2 += AngSearchInc) { Vector3 direction2 = ang2 > 0f ? Quaternion.AngleAxis(ang2 * direction, Vector3.up) * straight : straight; Vector3 endpoint = midpoint + straight * StepSize; float endspeed = mobility.GetUnitSpeed(data.terrain, data.map, endpoint, radius, direction2); if (endspeed > 0f) { straightStep = ang1 == 0f && ang2 == 0f; return(straightStep ? endpoint : midpoint); } } } } } // No step was found return(NoPosition); }