// 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,
MobilityData mobility,
float radius)
{
float distance = (destination - start).magnitude;
Vector3 waypoint = start;
float time = 0f;
while (distance > COMPLETION_DIST)
{
Vector3 previous = waypoint;
waypoint = TakeStep(data, waypoint, destination, mobility, radius);
if (waypoint == NO_POSITION)
{
return(FOREVER);
}
time += STEP_SIZE / mobility.GetUnitSpeedMultiplier(data._map, waypoint, radius, (waypoint - previous).normalized);
distance = (destination - waypoint).magnitude;
}
time += distance / mobility.GetUnitSpeedMultiplier(data._map, waypoint, radius, (destination - waypoint).normalized);
return(time);
}
public Pathfinder(MovementComponent unit, PathfinderData data)
{
_unit = unit;
Data = data;
_path = new List <PathNode>();
FinalCompletionDist =
2f * Constants.MAP_SCALE + unit.Data.MinTurnRadius;
_nextUpdateTime = 0f;
}
private void UpdateWaypointAngle()
{
if (_path.Count >= 2)
{
Vector3 nodePos = PathfinderData.Position(_path[_path.Count - 1]);
Vector3 nextPos = PathfinderData.Position(_path[_path.Count - 2]);
WaypointAngleChange = Math.Abs(Vector3.Angle(nodePos - _unit.transform.position, nextPos - nodePos));
}
else
{
WaypointAngleChange = 180;
}
}
public Pathfinder(MovementComponent unit, PathfinderData data)
{
_unit = unit;
Data = data;
_path = new List <PathNode>();
FinalCompletionDist =
2f * Constants.MAP_SCALE + unit.Data.MinTurnRadius;
_nextUpdateTime = 0f;
Thread calculationThread = new Thread(() => PathFinderRunner());
calculationThread.IsBackground = true;
calculationThread.Start();
}
// 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,
MobilityData mobility,
float radius)
{
Vector3 straight = (destination - start).normalized;
_s_straightStep = false;
// Fan out in a two-point horizontal pattern to find a way forward
for (float ang1 = 0f; ang1 <= MAX_ANGLE; ang1 += ANGLE_SEARCH_INC)
{
for (int direction = -1; direction <= 1; direction += 2)
{
Vector3 direction1 = ang1 > 0f ? Quaternion.AngleAxis(ang1 * direction, Vector3.up) * straight : straight;
Vector3 midpoint = start + direction1 * STEP_SIZE;
float midspeed = mobility.GetUnitSpeedMultiplier(data._map, midpoint, radius, direction1);
if (midspeed > 0f)
{
for (float ang2 = 0f; ang2 <= ang1; ang2 += ANGLE_SEARCH_INC)
{
Vector3 direction2 = ang2 > 0f ? Quaternion.AngleAxis(ang2 * direction, Vector3.up) * straight : straight;
Vector3 endpoint = midpoint + straight * STEP_SIZE;
float endspeed = mobility.GetUnitSpeedMultiplier(data._map, endpoint, radius, direction2);
if (endspeed > 0f)
{
_s_straightStep = ang1 == 0f && ang2 == 0f;
return(_s_straightStep ? endpoint : midpoint);
}
}
}
}
}
// No step was found
return(NO_POSITION);
}
public Vector3 GetDestination()
{
return(_path.Count > 0 ? PathfinderData.Position(_path[0]) : NO_POSITION);
}
private void UpdateWaypoint()
{
PathNode targetNode = _path[_path.Count - 1];
float distance = Vector3.Distance(
_unit.transform.position, PathfinderData.Position(targetNode));
if (distance < (_path.Count > 1 ? COMPLETION_DIST : FinalCompletionDist))
{ // Unit arrived at the next path node
_path.RemoveAt(_path.Count - 1);
if (!HasDestination())
{ // Unit arrived at the destination
_waypoint = NO_POSITION;
return;
}
else
{
_previousNode = targetNode;
targetNode = _path[_path.Count - 1];
UpdateWaypointAngle();
}
}
Vector3 targetPosition = PathfinderData.Position(targetNode);
// If the unit is supposed to be moving along a road, this will push it more toward the center
// to avoid having it drive along next to the road.
if (targetNode.IsRoad && _previousNode != null && _previousNode.IsRoad)
{
targetPosition = GetRoadIntersection(
targetPosition,
PathfinderData.Position(_previousNode),
_unit.transform.position);
}
Vector3 newWaypoint = TakeStep(
Data,
_unit.transform.position,
targetPosition,
_unit.Mobility,
_unit.Data.Radius);
if (newWaypoint != NO_POSITION)
{
_waypoint = _s_straightStep ? targetPosition : newWaypoint;
}
else
{
// The unit has gotten stuck when following the previously computed path.
// Now recompute a new path to the destination using the global graph, this time using finite radius
float pathTime = Data.FindPath(
_path,
_unit.transform.position,
PathfinderData.Position(_path[0]),
_unit.Mobility,
_unit.Data.Radius,
Command);
//float pathTime = SetPath (path[0].position, command);
bool isTrapped = pathTime == FOREVER;
if (isTrapped)
{
Debug.Log("I am stuck!!!");
_waypoint = NO_POSITION;
}
else
{
// If this is an intermediate step of the path, then the pre-computed global graph might
// be broken and the corresponding arc should be recomputed to avoid having units cycle forever
if (_previousNode != null && _path.Count > 1)
{
Data.RemoveArc(_previousNode, targetNode);
Data.AddArc(_previousNode, targetNode);
}
}
}
}
