// 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);
}
public UnitData()
{
weaponData = new List <WeaponData>();
mobility = MobilityType.MobilityTypes[0];
radius = Mathf.Sqrt(length * width) / 2;
optimumTurnSpeed = Mathf.Sqrt(maxLateralAccel * minTurnRadius);
suspensionForward = suspension * radius / length;
suspensionSide = suspension * radius / width;
accelDampTime = 0.15f * movementSpeed / accelRate;
}
// 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);
}
private float TimeHeuristic(Vector3 pos1, Vector3 pos2, MobilityType mobility)
{
return(Vector3.Distance(pos1, pos2) * 3 / 4);
}
// Run the A* algorithm and put the result in path
// If no path was found, return 'forever' and put only the destination in path
// Returns the total path time
public float FindPath(
List <PathNode> path,
Vector3 start, Vector3 destination,
MobilityType mobility, float unitRadius,
MoveCommandType command)
{
path.Clear();
path.Add(new PathNode(destination, false));
PathNode cameFromDest = null;
float gScoreDest = Pathfinder.FindLocalPath(this, start, destination, mobility, unitRadius);
if (gScoreDest < Pathfinder.Forever)
{
if (command == MoveCommandType.Slow || command == MoveCommandType.Reverse)
{
return(gScoreDest);
}
}
// Initialize with all nodes accessible from the starting point
// (this can be optimized later by throwing out some from the start)
openSet.Clear();
foreach (PathNode neighbor in graph)
{
neighbor.isClosed = false;
neighbor.cameFrom = null;
neighbor.gScore = Pathfinder.Forever;
Vector3 neighborPos = Position(neighbor);
if ((start - neighborPos).magnitude < ArcMaxDist)
{
float gScoreNew = Pathfinder.FindLocalPath(this, start, neighborPos, mobility, unitRadius);
if (gScoreNew < Pathfinder.Forever)
{
neighbor.gScore = gScoreNew;
float fScoreNew = gScoreNew + TimeHeuristic(neighborPos, destination, mobility);
openSet.Enqueue(neighbor, fScoreNew);
}
}
}
while (openSet.Count > 0)
{
PathNode current = openSet.Dequeue();
current.isClosed = true;
if (gScoreDest < current.Priority)
{
break;
}
foreach (PathArc arc in current.arcs)
{
PathNode neighbor = arc.node1 == current ? arc.node2 : arc.node1;
if (neighbor.isClosed)
{
continue;
}
float arcTime = arc.time[mobility.Index];
if (arcTime >= Pathfinder.Forever)
{
continue;
}
float gScoreNew = current.gScore + arcTime;
if (gScoreNew >= neighbor.gScore)
{
continue;
}
float fScoreNew = gScoreNew + TimeHeuristic(Position(neighbor), destination, mobility);
if (!openSet.Contains(neighbor))
{
openSet.Enqueue(neighbor, fScoreNew);
}
else
{
openSet.UpdatePriority(neighbor, fScoreNew);
}
neighbor.gScore = gScoreNew;
neighbor.cameFrom = current;
}
float arcTimeDest = Pathfinder.Forever;
if (Vector3.Distance(Position(current), destination) < ArcMaxDist)
{
arcTimeDest = Pathfinder.FindLocalPath(this, Position(current), destination, mobility, unitRadius);
}
// Debug.Log(openSet.Count + " " + Position(current) + " " + current.isRoad + " " + Vector3.Distance(Position(current), destination) + " " + (current.gScore + arcTimeDest) + " " + gScoreDest);
if (arcTimeDest >= Pathfinder.Forever)
{
continue;
}
if (arcTimeDest < Pathfinder.Forever && command == MoveCommandType.Slow)
{
arcTimeDest = 0f;
}
float gScoreDestNew = current.gScore + arcTimeDest;
if (gScoreDestNew < gScoreDest)
{
gScoreDest = gScoreDestNew;
cameFromDest = current;
}
}
// Reconstruct best path
PathNode node = cameFromDest;
while (node != null)
{
path.Add(node);
node = node.cameFrom;
}
return(gScoreDest);
}
