def __init__(self, params, scenario, vehicle):
self.params = params
self.scenario = scenario
self.vehicle = vehicle
self.pathLines = []
startPoint = self.scenario.startPoint
waypoints = self.scenario.wayPoints
self.avgSpeed = calcs.length(self.scenario.startVelocity)
for i in range(len(waypoints)):
distance = calcs.length(waypoints[i] - startPoint)
self.pathLines.append(
(distance / self.avgSpeed, startPoint, waypoints[i]))
startPoint = waypoints[i]
def _advanceGoal(self, position):
if self.scenario is None:
return
while len(self.scenario.wayPoints) > 0 and calcs.length(
self.scenario.wayPoints[0] -
position) <= COURSE_WAYPOINT_RADIUS:
self.scenario.wayPoints = self.scenario.wayPoints[1:]
def _detectSignificantPathChange(self, path):
"""
For debugging only. If path changes significantly, then the rate of simulation drops automatically.
"""
if self._path is not None and len(path.pathWaypoints) > 0:
(goal, radius) = self._currentPathWaypoint()
if goal is not None and calcs.length(
goal - path.pathWaypoints[0].position) < 1.0:
return False
return True
def _pathPointReached(self, position, goal, radius):
return goal is not None and calcs.length(position - goal) <= radius
def isPointLegal(self, point):
return calcs.length(point - self.scenario.startPoint
) * 1.2 > self.nfzGenerator._maxSize