def move(self, direction, speed, dt):
if any(direction):
ix, iy = direction
rads = self.app.renderer.cam_yaw.angle / 57.3
cos = math.cos(rads)
sin = math.sin(rads)
dx = cos * ix + -sin * iy
dy = sin * ix + cos * iy
mag_sqrd = dx**2 + dy**2
if mag_sqrd > 1:
s = speed / (mag_sqrd**.5)
else:
s = speed
self.collision.body.apply_impulse_at_local_point((dx * s, dy * s))
if any(self.collision.body.velocity):
target_angle = 90 - self.app.renderer.cam_yaw.angle
da = util.angle_diff(self.graphics.rot.angle, target_angle)
turn_speed = self.turn_speed * dt
if abs(da) > turn_speed:
self.graphics.rot.angle += da * turn_speed
else:
self.graphics.rot.angle = target_angle
def update(self, dt):
# print (self.throttle_up - self.throttle_down - .2)
accel = self.throttle_up - self.throttle_down
if not self.collider.platform:
accel -= .2
if accel:
self.collider.sleeping = False
self.collider.force.y += accel * self.vertical_force
self.animate(self.ix, self.iy, (self.throttle_up - self.throttle_down),
self.collider.platform, dt)
if not self.collider.platform and 0 in self.occupied_seats:
if abs(self.collider.vel.y) > self.max_y_vel:
self.collider.vel.y = cmp(self.collider.vel.y,
0) * self.max_y_vel
rot_mat = glm.rotate(glm.mat4(1.0), glm.radians(90 - self.angle),
glm.vec3(0, 1, 0))
local_vel = rot_mat * glm.vec4(self.collider.vel, 1)
local_vel.x += self.ix * self.accel_force * dt
local_vel.z += self.iy * self.turn_force * dt
turn_input = util.angle_diff(
self.angle, 90 - self.engine.renderer.camera.yaw) * .1
turn_force = self.turn_force * turn_input
self.angle += turn_force * dt
local_vel.z *= 1 - self.side_friction * dt
local_vel.x = min(self.max_speed, max(self.min_speed, local_vel.x))
self.collider.vel = glm.vec3(glm.inverse(rot_mat) * local_vel).xyz
def compute_avoids(self, avoids):
"""
Return avoid component.
This rule consists of two components. The first is active for all
obstacles within range and depends on agent's distance from obstacle as
well as its aproach angle. Force is maximum when agent approaches the
obstacle head-on and minimum if obstacle is to the side of an agent.
Second component depends only on distance and only obstacles closer than
avoid_radius are taken into account. This is to ensure minimal distance
from obstacles at all times.
"""
main_direction = Vector2()
safety_direction = Vector2()
count = 0
# Calculate repulsive force for each obstacle in sight.
for obst in avoids:
obst_position = get_obst_position(obst)
d = obst_position.norm()
obst_position *= -1 # Make vector point away from obstacle.
obst_position.normalize() # Normalize to get only direction.
# Additionally, if obstacle is very close...
if d < self.avoid_radius:
# Scale lineary so that there is no force when agent is on the
# edge of minimum avoiding distance and force is maximum if the
# distance from the obstacle is zero.
safety_scaling = -2 * self.max_force / self.avoid_radius * d + 2 * self.max_force
safety_direction += obst_position * safety_scaling
count += 1
# For all other obstacles: scale with inverse square law.
obst_position = obst_position / (self.avoid_scaling * d**2)
main_direction += obst_position
if avoids:
# Calculate the approach vector.
a = angle_diff(self.old_heading, main_direction.arg() + 180)
# We mustn't allow scaling to be negative.
side_scaling = max(math.cos(math.radians(a)), 0)
# Divicde by number of obstacles to get average.
main_direction = main_direction / len(avoids) * side_scaling
safety_direction /= count
rospy.logdebug("avoids*: %s", main_direction)
# Final force is sum of two componets.
# Force is not limited so this rule has highest priority.
return main_direction + safety_direction