def force_waypathtraverse(owner, path):
"""Steering force for WAYPATHTRAVERSE behaviour.
Parameters
----------
owner: SimpleVehicle2d
The vehicle computing this force.
path: WaypointPath
Path to be followed by the owner
Notes
-----
This is the simple version; we merely head towards the next waypoint.
If there is only one waypoint left, we ARRIVE at it. Otherwise, we SEEK.
"""
# If no waypoint left, exit immediately
if path.newway is None:
return Point2d(0,0)
# If current destination is the last waypoint on this path, ARRIVE
# at that waypoint
if path.num_left() <=1:
return force_arrive(owner, path.newway)
# Otherwise, check if we've reached the next waypoint
# Note: No force is returned when we switch to the next waypoint
if (owner.pos - path.newway).sqnorm() <= WAYPOINT_TOLERANCE_SQ:
path.advance()
return Point2d(0,0)
# TODO: This is for testing only?
owner.waypoint = path.newway
return force_seek(owner, path.newway)
def force_waypathresume(owner, path, invk):
"""Steering force for WAYPATHRESUME behaviour.
Parameters
----------
owner: SimpleVehicle2d
The vehicle computing this force.
path: WaypointPath
Path to be followed by the owner
invk: positive float
Reciprocal of exponential decay constant. See Notes.
Notes
-----
If the vehicle is off course, this will give a balance between returning
directly to the current path edge and progressing to the next waypoint.
If the vehicle has already overshot the next waypoint, we head directly to
that waypoint, ignoring the path. Otherwise, follow an exponential decay
curve asymptotic to the path; although this curve doesn't actually pass
through the waypoint, it makes computations very quick, especially since
we store invk. Smaller values of invk imply a larger decay rate, and give
more immediate return to the path.
"""
# If no waypoint left, exit immediately
if path.newway is None:
return Point2d(0,0)
# This is the remaining direct distance to the next waypoint,
# using orthogonal projection operator.
rl = (path.newway - owner.pos)/path.edgevector
# If resume target is beyond the next waypoint, SEEK/ARRIVE to waypoint.
# Otherwise, SEEK (never ARRIVE) to the resume target
if invk >= rl: # Resume target is beyond the next waypoint
target = path.newway
# ARRIVE if this is the last waypoint; no further computation neeed
if path.num_left() <=1:
return force_arrive(owner, path.newway)
else: # Resume target is between last/next waypoints
target = path.newway + path.edgevector.scale(invk - rl)
# If we reach this part of the code, we must SEEK to either a target on
# the path or a waypoint that is not the last one in the path. So...
# Check if we're close enough to the next waypoint to switch.
# Note: No force is returned when we switch to the next waypoint
if (owner.pos - path.newway).sqnorm() <= WAYPOINT_TOLERANCE_SQ:
path.advance()
return Point2d(0,0)
else:
return force_seek(owner, target)
