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)