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