def _apply_robot_motion(self, steering_direction):
self.steering_force = steering_direction * self.robot.speed
self.robot.body.apply_force_at_world_point(self.steering_force * 10,
self.robot.body.position)
velocity_mag = la.norm(self.robot.body.velocity)
if velocity_mag > 0: #apply friction
friction = GRAVITY * self.robot.mass * self.robot.friction * -self.robot.body.velocity
self.robot.body.apply_force_at_world_point(
friction, self.robot.body.position)
self.robot.body.angular_velocity = self.robot.body.angular_velocity * .95
if self.robot.body.angular_velocity < .00001:
self.robot.body.angular_velocity = 0
if velocity_mag > 2.0: #update orientation
orientation_shift = angle(
self.robot.body.velocity) - self.robot.body.angle
orientation_shift = (orientation_shift + PI) % (2 * PI) - PI
if abs(orientation_shift) > .1:
self.robot.body.angle += .02 * np.sign(orientation_shift)
else:
self.robot.body.angle = angle(self.robot.body.velocity)
self.robot.previous_angle = self.robot.body.angle
elif velocity_mag < .05: #zero out velocity, may have no effect
self.robot.body.velocity = (0, 0)
def _reset_robot(self, center=False, collision_points=None):
previous_angle = vector(self.robot.body.angle)
previous_position = self.robot.body.position
if collision_points:
self.robot.body.angle = angle(collision_points[0].point_a - collision_points[0].point_b)
else:
self.robot.body.angle = angle(-previous_angle)
if center:
self.robot.body.position = self.CENTER
else:
self.robot.body.position = previous_position - (previous_angle * 25)
self.robot.body.angular_velocity = 0
self.robot.body.velocity = (0,0)