def on_ball_paddle_collision(self, ball_body, paddle_body, normal):
# Adjusts the ball direction if the paddle is moving when the ball collides with it
angle = math.acos(dot(normal, ball_body.direction)) # Angle between the reflected direction and the normal
delta_angle = abs(((math.pi * 0.5) - angle) * 0.5) # Half the angle that remains if were to perform a 90 degree reflection
if paddle_body.direction.x > 0: # Clockwise rotation because the paddle is moving to the right
ball_body.direction = normalize(rotate(ball_body.direction, delta_angle))
elif paddle_body.direction.x < 0: # Counter-clockwise rotation because the paddle is moving to the left
ball_body.direction = normalize(rotate(ball_body.direction, -delta_angle))
def on_ball_left_right_collision(self, ball_body, wall_body, normal):
angle = math.acos(dot(normal, ball_body.direction)) # Angle between the reflected direction and the normal
# If the angle is too flat, add a small rotation to the reflected direction
if angle < 0.1:
delta_angle = 0.2
if ball_body.direction.y > 0: # Counter-clockwise rotation because the ball is moving downwards
ball_body.direction = normalize(rotate(ball_body.direction, -delta_angle))
elif ball_body.direction.y <= 0: # Clockwise rotation because the ball is moving upwards
ball_body.direction = normalize(rotate(ball_body.direction, delta_angle))
def dir_from_segment(point, a, b):
if dot(b - a, point - a) <= 0:
return normalize(point - a)
if dot(a - b, point - b) <= 0:
return normalize(point - b)
normal = normalize(b - a)
normal = Vector(-normal.y, normal.x)
if dot(normal, point - a) < 0:
normal = -normal
assert(distance_to_segment(point, a, b) < distance_to_segment(point + normal, a, b))
return normal
def orthovec(u):
"""return arbitrary orthogonal vectors to u"""
v = Vec([1., -u.x/u.y, 0.])
b = 1.
a = b*u.x/u.y
c = -b*(u.x**2 + u.y**2)/(u.y*u.z)
w = Vec([a, b, c])
print "u dot v", numpy.dot(u, v)
print "w dot v", numpy.dot(v, w)
print "w dot u", numpy.dot(u, w)
return normalize(v), normalize(w)
def __init__(self, rect, velocity, object_type, tag_ent, is_static=True):
self.rect = rect
self.set_velocity(vector.magnitude(velocity))
self.direction = vector.normalize(velocity)
self.object_type = object_type
self.is_static = is_static
self.tag_ent = tag_ent
def predict_puck_movement(self, bag): #ausgefuehrt
"""
Predicts the pucks movement.
:param bag: The parameter bag.
:return:
"""
# need at least 2 points
if len(self.buffer) < 2:
return
# loop over points
direction_sum = (0, 0)
for i in xrange(0, len(self.buffer) - 1):
# get tuples and time diff
current_tuple = self.buffer[i]
next_tuple = self.buffer[i+1]
time_diff = next_tuple[2] - current_tuple[2]
# get direction (length is velocity) and divide by time_diff
direction = vector.mul_by(vector.from_to(current_tuple, next_tuple), 1.0 / time_diff)
# sum up
direction_sum = vector.add(direction_sum, direction)
# averaging
direction = vector.mul_by(direction_sum, 1.0 / self.buffer_size)
# add puck direction (normalized) and velocity
bag.puck.velocity = vector.length(direction)
bag.puck.direction = vector.normalize(direction)
bag.next.puck.olddirection = bag.puck.direction
def move_stick(self, bag):
"""
Moves the stick.
:param bag: The parameter bag.
:return:
"""
if 'position' not in bag.stick:
bag.next.stick.position = strategy.SimpleStrategy.HOME_POSITION
return
if 'dest_position' not in bag.stick:
position = bag.stick.position
else:
# move stick towards destination
# ignore dest_direction and dest_velocity
dist_moved = strategy.SimpleStrategy.STICK_MAX_SPEED * bag.time_diff
direction = vector.from_to(bag.stick.position, bag.stick.dest_position)
if dist_moved > vector.length(direction):
position = bag.stick.dest_position
else:
direction = vector.mul_by(vector.normalize(direction), dist_moved * strategy.SimpleStrategy.STICK_MAX_SPEED)
position = vector.add(bag.stick.position, direction)
# set new position
bag.next.stick.position = position
def calculate_normal(self, b1, b2):
"""
Calculates the normal of b1 with respect to b2
"""
normal_x = 0.0
normal_y = 0.0
# Difference between body centers
center_dir = vector.normalize(vector.sub(b1.rect.center(), b2.rect.center()))
cos_threshold = b2.rect.w / math.sqrt(math.pow(b2.rect.h, 2) + math.pow(b2.rect.w, 2))
if vector.dot(center_dir, vector.Vector2(1, 0)) >= cos_threshold:
# b1 is at the right side of b2
normal_x = 1.0
elif vector.dot(center_dir, vector.Vector2(-1, 0)) >= cos_threshold:
# b1 is at the left side of b2
normal_x = -1.0
elif vector.dot(center_dir, vector.Vector2(0, -1)) >= math.cos(math.pi * 0.5 - math.acos(cos_threshold)):
# b1 is above b2
normal_y = -1.0
else:
# b1 is below b2
normal_y = 1.0
return vector.Vector2(normal_x, normal_y)
def Update(self, Gyroscope, Accelerometer, Magnetometer):
'''Calculate the best quaternion to the given measurement values.
Parameters
----------
Gyroscope : array, shape (N,3)
Angular velocity [rad/s]
Accelerometer : array, shape (N,3)
Linear acceleration (Only the direction is used, so units don't matter.)
Magnetometer : array, shape (N,3)
Orientation of local magenetic field.
(Again, only the direction is used, so units don't matter.)
'''
q = self.Quaternion; # short name local variable for readability
# Reference direction of Earth's magnetic field
h = vector.rotate_vector(Magnetometer, q)
b = np.hstack((0, np.sqrt(h[0]**2+h[1]**2), 0, h[2]))
# Gradient decent algorithm corrective step
F = [2*(q[1]*q[3] - q[0]*q[2]) - Accelerometer[0],
2*(q[0]*q[1] + q[2]*q[3]) - Accelerometer[1],
2*(0.5 - q[1]**2 - q[2]**2) - Accelerometer[2],
2*b[1]*(0.5 - q[2]**2 - q[3]**2) + 2*b[3]*(q[1]*q[3] - q[0]*q[2]) - Magnetometer[0],
2*b[1]*(q[1]*q[2] - q[0]*q[3]) + 2*b[3]*(q[0]*q[1] + q[2]*q[3]) - Magnetometer[1],
2*b[1]*(q[0]*q[2] + q[1]*q[3]) + 2*b[3]*(0.5 - q[1]**2 - q[2]**2) - Magnetometer[2]]
J = np.array([
[-2*q[2], 2*q[3], -2*q[0], 2*q[1]],
[ 2*q[1], 2*q[0], 2*q[3], 2*q[2]],
[0, -4*q[1], -4*q[2], 0],
[-2*b[3]*q[2], 2*b[3]*q[3], -4*b[1]*q[2]-2*b[3]*q[0], -4*b[1]*q[3]+2*b[3]*q[1]],
[-2*b[1]*q[3]+2*b[3]*q[1], 2*b[1]*q[2]+2*b[3]*q[0], 2*b[1]*q[1]+2*b[3]*q[3], -2*b[1]*q[0]+2*b[3]*q[2]],
[ 2*b[1]*q[2], 2*b[1]*q[3]-4*b[3]*q[1], 2*b[1]*q[0]-4*b[3]*q[2], 2*b[1]*q[1]]])
step = J.T.dot(F)
step = vector.normalize(step) # normalise step magnitude
# Compute rate of change of quaternion
qDot = 0.5 * quat.q_mult(q, np.hstack([0, Gyroscope])) - self.Beta * step
# Integrate to yield quaternion
q = q + qDot * self.SamplePeriod
self.Quaternion = vector.normalize(q).flatten()
def update(self):
self.sprite.update()
self.tick += 1
if self.tick == self.targ_time:
self.target_direction = vector.normalize(vector.get_direction(self.sprite.rect.center, self.target.rect.center))
if self.tick == self.shoot_time:
self.transition_to(self.manager.SHOOTING)
def camera(i, j):
# int, int -> vector a
alpha = math.tan(fovx/2)*((i - (width/2))/(width/2))
beta = math.tan(fovy/2)*(((height/2) - j)/(height/2))
dirn = vr.add_vector(vr.add_vector(vr.scale_mul(alpha, side(geo)),
vr.scale_mul(beta, up(geo))),
forward(geo))
dirn = vr.normalize(dirn)
return ry.make_ray(eye, dirn)
def update(self):
self.sprite.update()
self.tick += 1
if self.tick == self.targ_time:
self.target_direction = vector.normalize(
vector.get_direction(self.sprite.rect.center,
self.target.rect.center))
if self.tick == self.shoot_time:
self.transition_to(self.manager.SHOOTING)
def update():
global circle_velocity
global circle_position
if circle_following_mouse and states.game_state == GameState.PLAYING:
mouse_pos_vec = Vector(mouse_position[0], mouse_position[1])
diff_vec = mouse_pos_vec - circle_position
normalized_direction = vector.normalize(diff_vec)
circle_velocity += normalized_direction * 1000 * delta_time
circle_position += circle_velocity * delta_time
def update(self, step_time):
def change_dir_vel(entities, direction, velocity):
for entity in entities:
entity.body.direction = direction
entity.body.set_velocity(velocity)
if (self.moving_left or self.moving_right):
if self.moving_left:
change_dir_vel(self.paddles, Vector2(-1, 0), PADDLE_VELOCITY)
if self.game_status == GameLayer.INITIALIZATION:
change_dir_vel(self.balls, Vector2(-1, 0), PADDLE_VELOCITY)
else:
change_dir_vel(self.paddles, Vector2(1, 0), PADDLE_VELOCITY)
if self.game_status == GameLayer.INITIALIZATION:
change_dir_vel(self.balls, Vector2(1, 0), PADDLE_VELOCITY)
else:
change_dir_vel(self.paddles, ZERO2, magnitude(ZERO2))
if self.game_status == GameLayer.INITIALIZATION:
change_dir_vel(self.balls, ZERO2, magnitude(ZERO2))
if self.push_balls and self.game_status == GameLayer.INITIALIZATION:
for ball in self.balls:
if ball.body.is_static:
ball.body.is_static = False
v = Vector2(BALL_VELOCITY_X, BALL_VELOCITY_Y)
change_dir_vel(self.balls, normalize(v), magnitude(v))
self.push_balls = False
self.game_status = GameLayer.GAME_LOOP
# Remove bricks that have been destroyed
free_brick_list = []
for brick in self.bricks:
if brick.health_points == 0:
self.unregister_entity(brick)
free_brick_list.append(brick)
self.bricks = [b for b in self.bricks if free_brick_list.count(b) == 0]
for paddle in self.paddles:
# Integrate paddle
paddle.body.rect.position = sum(
paddle.body.rect.position,
mul(paddle.body.direction, paddle.body.velocity * step_time))
# Relocate paddle position to a valid position range
paddle.body.rect.position.x = utils.clamp(
paddle.body.rect.position.x, 0, WINDOW_WIDTH - PADDLE_WIDTH)
paddle.body.rect.position.y = WINDOW_HEIGHT - PADDLE_HEIGHT - PADDLE_LINE_SPACING
for ball in self.balls:
if ball.body.is_static:
pos_r = Vector2((PADDLE_WIDTH - BALL_WIDTH) * 0.5,
-BALL_HEIGHT)
ball.body.rect.position = sum(
self.paddles[0].body.rect.position, pos_r)
def f_quat(beta, x, sphere_fit):
sphere_fit = numpy.array(sphere_fit)
n = [x[0] - sphere_fit[0], x[1] - sphere_fit[1], x[2] - sphere_fit[2]]
q = [1 - vector.norm(beta[:3])] + list(beta[:3])
q = angvec2quat(vector.norm(beta[:3]), beta[:3])
m = lmap(lambda v: rotvecquat(vector.normalize(v), q),
list(zip(n[0], n[1], n[2])))
# m = lmap(lambda v : rot(v, beta), list(zip(n[0], n[1], n[2])))
m = numpy.array(list(zip(*m)))
d = m[0] * x[3] + m[1] * x[4] + m[2] * x[5]
return beta[3] - d
def random_puck_velocity():
speed_vector = np.array(
[np.random.uniform(-1, 1),
np.random.uniform(1, 2)])
# 单位化速度向量
speed_vector = V.normalize(speed_vector)
# 速度系数（让其不一定是最大速度）
speed_coefficient = np.random.uniform(0.5, 1)
# 初始速度
initial_speed = speed_vector.dot(
P.puck_maximum_speed).dot(speed_coefficient)
return initial_speed
def _load_normals(config: Dict) -> Tensor:
'''
Loads a normal map from the given flow configuration.
Args:
config: Flow configuration.
Returns:
Tensor [1, R, C, 3] of surface normals as specified by the normal map image.
'''
assert 'Normals' in config, 'Scope "Flow" in configuration file is missing key "Normals".'
return vector.normalize(2 *
image.load(config['Normals']).unsqueeze(0) - 1)
def intersect_lights(scene, lights, pos, normal):
# scene, light, vector -> float
color = 1
for light in lights:
dirn = vr.normalize(vr.sub_vector(lt.get_pos(light), pos))
new_ray = ry.make_ray(pos, dirn)
t, n = intersect_scene(scene, new_ray)
if not t:
dot_or_0 = max([vr.dot_prod(dirn, normal), 0])
incr = dot_or_0 * lt.get_brightness(light)
color += incr
if color:
return min([8, color])
def move(self):
if self.controlled:
self.v = normalize(self.v) * self.params.maxV
elif self.seeking:
self.seek()
else:
self.wander()
# New position is simply last position plus velocity (times simulation speed)
self.pos += self.v * self.gParams.speed
if self.seeking and distance(self.pos, self.target) < 10.0:
self.seeking = False
# Set current angle for drawing
self.orientation = self.v.angle() * 180 / pi
def update(self, step_time):
def change_dir_vel(entities, direction, velocity):
for entity in entities:
entity.body.direction = direction
entity.body.set_velocity(velocity)
if(self.moving_left or self.moving_right):
if self.moving_left:
change_dir_vel(self.paddles, Vector2(-1,0), PADDLE_VELOCITY)
if self.game_status == GameLayer.INITIALIZATION:
change_dir_vel(self.balls, Vector2(-1,0), PADDLE_VELOCITY)
else:
change_dir_vel(self.paddles, Vector2(1,0), PADDLE_VELOCITY)
if self.game_status == GameLayer.INITIALIZATION:
change_dir_vel(self.balls, Vector2(1,0), PADDLE_VELOCITY)
else:
change_dir_vel(self.paddles, ZERO2, magnitude(ZERO2))
if self.game_status == GameLayer.INITIALIZATION:
change_dir_vel(self.balls, ZERO2, magnitude(ZERO2))
if self.push_balls and self.game_status == GameLayer.INITIALIZATION:
for ball in self.balls:
if ball.body.is_static:
ball.body.is_static = False
v = Vector2(BALL_VELOCITY_X, BALL_VELOCITY_Y)
change_dir_vel(self.balls, normalize(v), magnitude(v))
self.push_balls = False
self.game_status = GameLayer.GAME_LOOP
# Remove bricks that have been destroyed
free_brick_list = []
for brick in self.bricks:
if brick.health_points == 0:
self.unregister_entity(brick)
free_brick_list.append(brick)
self.bricks = [ b for b in self.bricks if free_brick_list.count(b) == 0 ]
for paddle in self.paddles:
# Integrate paddle
paddle.body.rect.position = sum(paddle.body.rect.position,
mul(paddle.body.direction, paddle.body.velocity * step_time))
# Relocate paddle position to a valid position range
paddle.body.rect.position.x = utils.clamp(paddle.body.rect.position.x, 0,
WINDOW_WIDTH - PADDLE_WIDTH)
paddle.body.rect.position.y = WINDOW_HEIGHT - PADDLE_HEIGHT - PADDLE_LINE_SPACING
for ball in self.balls:
if ball.body.is_static:
pos_r = Vector2((PADDLE_WIDTH - BALL_WIDTH) * 0.5, - BALL_HEIGHT)
ball.body.rect.position = sum(self.paddles[0].body.rect.position, pos_r)
def wander(self):
# Get wander force with random angle
wForce = normalize(self.v) * self.cDist
rForce = normalize(self.v) * self.cRad
self.wAngle += (random.random() * 2 - 1) * self.wChange
rForce.rotate(self.wAngle)
wForce += rForce
# Turn back at the window limits
if self.pos.x > 1100:
wForce += Vector2f(-1.0, 0.0) * self.pos.x / 100 * self.params.maxF
elif self.pos.x < 100:
wForce += Vector2f(
1.0, 0.0) * (1280 - self.pos.x) / 100 * self.params.maxF
if self.pos.y > 650:
wForce += Vector2f(0.0, -1.0) * self.pos.y / 100 * self.params.maxF
elif self.pos.y < 70:
wForce += Vector2f(
0.0, 1.0) * (720 - self.pos.y) / 100 * self.params.maxF
# Truncate to maxF
wForce.trunc(self.params.maxF)
# Get final velocity
steering = wForce / self.params.mass * self.gParams.speed
self.v = trunc(self.v + steering, self.params.maxV)
def evaluate(self, normals: Tensor, incident_directions: Tensor,
outbound_directions: Tensor) -> Tensor:
'''See SVBRDF.evaluate().'''
colours, glossiness = SVBRDF._split_parameters(self.parameters, [3, 1])
# This Phong model is based on the halfway vector parameterization.
halfways = vector.normalize(incident_directions + outbound_directions)
# The macrosurface normal is assumed to be (0, 0, 1).
zeniths = vector.dot(halfways, normals).clamp(0, 1)
# The parameterization of the specular exponent is taken from the Substance Blinn-Phong shader.
exponents = 4 * torch.pow(2, glossiness * 11)
# The Phong normalization factor is derived in http://www.farbrausch.de/~fg/stuff/phong.pdf.
factors = (exponents + 2) * (exponents + 4) / (
8 * math.pi * (torch.pow(1 / math.sqrt(2), exponents) + exponents))
return colours * factors * zeniths.pow(exponents)
def Update(self, Gyroscope, Accelerometer, Magnetometer):
'''Calculate the best quaternion to the given measurement values.
Parameters
----------
Gyroscope : array, shape (N,3)
Angular velocity [rad/s]
Accelerometer : array, shape (N,3)
Linear acceleration (Only the direction is used, so units don't matter.)
Magnetometer : array, shape (N,3)
Orientation of local magenetic field.
(Again, only the direction is used, so units don't matter.)
'''
q = self.Quaternion; # short name local variable for readability
# Reference direction of Earth's magnetic field
h = vector.rotate_vector(Magnetometer, q)
b = np.hstack((0, np.sqrt(h[0]**2+h[1]**2), 0, h[2]))
# Estimated direction of gravity and magnetic field
v = np.array([
2*(q[1]*q[3] - q[0]*q[2]),
2*(q[0]*q[1] + q[2]*q[3]),
q[0]**2 - q[1]**2 - q[2]**2 + q[3]**2])
w = np.array([
2*b[1]*(0.5 - q[2]**2 - q[3]**2) + 2*b[3]*(q[1]*q[3] - q[0]*q[2]),
2*b[1]*(q[1]*q[2] - q[0]*q[3]) + 2*b[3]*(q[0]*q[1] + q[2]*q[3]),
2*b[1]*(q[0]*q[2] + q[1]*q[3]) + 2*b[3]*(0.5 - q[1]**2 - q[2]**2)])
# Error is sum of cross product between estimated direction and measured direction of fields
e = np.cross(Accelerometer, v) + np.cross(Magnetometer, w)
if self.Ki > 0:
self._eInt += e * self.SamplePeriod
else:
self._eInt = np.array([0, 0, 0], dtype=np.float)
# Apply feedback terms
Gyroscope += self.Kp * e + self.Ki * self._eInt;
# Compute rate of change of quaternion
qDot = 0.5 * quat.q_mult(q, np.hstack([0, Gyroscope])).flatten()
# Integrate to yield quaternion
q += qDot * self.SamplePeriod
self.Quaternion = vector.normalize(q)
def Update(self, Gyroscope, Accelerometer, Magnetometer):
'''Calculate the best quaternion to the given measurement values.
Parameters
----------
Gyroscope : array, shape (N,3)
Angular velocity [rad/s]
Accelerometer : array, shape (N,3)
Linear acceleration (Only the direction is used, so units don't matter.)
Magnetometer : array, shape (N,3)
Orientation of local magenetic field.
(Again, only the direction is used, so units don't matter.)
'''
q = self.Quaternion; # short name local variable for readability
# Reference direction of Earth's magnetic field
h = vector.rotate_vector(Magnetometer, q)
b = np.hstack((0, np.sqrt(h[0]**2+h[1]**2), 0, h[2]))
# Estimated direction of gravity and magnetic field
v = np.array([
2*(q[1]*q[3] - q[0]*q[2]),
2*(q[0]*q[1] + q[2]*q[3]),
q[0]**2 - q[1]**2 - q[2]**2 + q[3]**2])
w = np.array([
2*b[1]*(0.5 - q[2]**2 - q[3]**2) + 2*b[3]*(q[1]*q[3] - q[0]*q[2]),
2*b[1]*(q[1]*q[2] - q[0]*q[3]) + 2*b[3]*(q[0]*q[1] + q[2]*q[3]),
2*b[1]*(q[0]*q[2] + q[1]*q[3]) + 2*b[3]*(0.5 - q[1]**2 - q[2]**2)])
# Error is sum of cross product between estimated direction and measured direction of fields
e = np.cross(Accelerometer, v) + np.cross(Magnetometer, w)
if self.Ki > 0:
self._eInt += e * self.SamplePeriod
else:
self._eInt = np.array([0, 0, 0], dtype=np.float)
# Apply feedback terms
Gyroscope += self.Kp * e + self.Ki * self._eInt;
# Compute rate of change of quaternion
qDot = 0.5 * quat.q_mult(q, np.hstack([0, Gyroscope])).flatten()
# Integrate to yield quaternion
q += qDot * self.SamplePeriod
self.Quaternion = vector.normalize(q)
def draw():
glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT)
#glLoadIdentity()
#glTranslatef(-10.0, -10.0, -36.0)
#glTranslatef(0.,0.,-6.)
gl.draw_axes()
glColor3f(1,1,1)
glPointSize(5)
#glBegin(GL_POINTS)
#glVertex3f(0,0,0)
#glVertex3f(1,1,1)
#glVertex3f(0, 1, 0)
#glEnd()
#origin
o = Vec([0.,0.,0.])
#direction
u = normalize(Vec([1., 1., 1.]))
#orthogonal vec
v,w = orthovec(u)
#w = rotateaxis(u, v, 90)
#print u
#print v
#print w
glColor3f(.5, 1, 0)
gl.draw_line(o, u)
glColor3f(0, 1, .5)
gl.draw_line(o, v)
glColor3f(.5, 0, .5)
gl.draw_line(o, w)
particles = distribute_disc(u, v, w, 1, 300, .1)
print len(particles)
glBegin(GL_POINTS)
glColor3f(1, 1, 1)
glVertex3f(u.x, u.y, u.z)
glColor3f(.5, .5, .5)
for p in particles:
glVertex3f(p.x, p.y, p.z)
glEnd()
pygame.display.flip()
def evaluate(self, normals: Tensor, incident_directions: Tensor,
outbound_directions: Tensor) -> Tensor:
'''See SVBRDF.evaluate().'''
D_parameters = self.parameters[:, :, :, self._D_indices]
F_parameters = self.parameters[:, :, :, self._F_indices]
G_parameters = self.parameters[:, :, :, self._G_indices]
# The halfway directions parameterize each of the D, F, and G functions so it's worth computing only once.
halfways = vector.normalize(incident_directions + outbound_directions)
# Evaluate the remainder of the microfacet SVBRDF in the usual way, taking into account that the value returned
# by G is already divided by a factor of the denominator in the original microfacet model.
D_terms = self._D_function(D_parameters, normals, halfways)
F_terms = self._F_function(F_parameters, halfways, incident_directions)
G_terms = self._G_function(G_parameters, normals, incident_directions, halfways) * \
self._G_function(G_parameters, normals, outbound_directions, halfways)
return F_terms * D_terms * G_terms
def _fire(self):
"""Fire if the mouse button is held down."""
buttons = pygame.mouse.get_pressed()
if buttons[0] and self.ammo and self.fire_wait_tick <= 0:
pos = pygame.mouse.get_pos()
velocity = vector.subtract(pos, self.rect.center)
velocity = vector.normalize(velocity)
velocity = vector.scalar_multiply(velocity, 10)
velocity = vector.add(velocity, vector.intvector(self.velocity))
self.level.add(PlayerShot(velocity=list(velocity)),
self.maprect.center)
self.fire_wait_tick = 10
self.ammo -= 1
else:
self.fire_wait_tick -= 1
def _load_normals(self, texture: Texture) -> Tensor:
'''
Loads the normal map associated with the given texture.
Args:
texture: Texture of interest.
Returns:
Tensor [1, R, C, 3] of texture normals.
'''
path_to_texture = self._derive_path_to_texture_directory(texture)
path_to_normals = os.path.join(path_to_texture,
self._layout['Normals'])
return vector.normalize(2 * image.load(path_to_normals).unsqueeze(0) -
1)
def getRoll(self, degrees=False):
ref = None
if self.getPitch() > pi / 2 - 0.01:
ref = self._getRawRollRef()
elif self.getPitch() < 0.01 - pi / 2:
self._getRawRollRef() * -1
else:
Q = Quaternion()
Q.fromTwoVectors(self._getPointing(), Flight.up)
ref = normalize(
Q.rotateVector(self.q.rotateVector(self._getRawRollRef())) *
(1 if self.getPitch() > 0 else -1))
return (acos(not Flight.north.dot(ref)) +
(pi if Flight.east().dot(ref) <= 0 else 0)) * (180 / pi if
degrees else 1)
def build_puck_path(self, bag, remaining_forecast_time=None): #ausgefuehrt
"""
Checks for a collision with the table boundaries.
:param bag: The parameter bag.
:param remaining_forecast_time:
:return:
"""
if 'direction' not in bag.puck:
return
if remaining_forecast_time is None:
remaining_forecast_time = const.CONST.TABLE_BOUNDARIES_COLLISION_FORECAST_TIME
if 'path' not in bag.puck:
bag.puck.path = []
if len(bag.puck.path) == 0:
bag.puck.path.append((bag.puck.position, bag.puck.direction, bag.puck.velocity, bag.puck.detection_time))
# get position and direction for the current path part
position = bag.puck.path[-1][0]
direction = bag.puck.path[-1][1]
velocity = bag.puck.path[-1][2]
# follow puck movement for remaining time
movement = vector.mul_by(direction, velocity)
predicted_position = vector.add(position, vector.mul_by(movement, remaining_forecast_time))
# check if position is out of bounds
collision = self._check_collision(position, predicted_position, movement)
if collision is not None:
# add to path
bag.puck.path.append((collision[0], vector.normalize(collision[1]), velocity, bag.puck.path[-1][3] + collision[2]))
# continue building path
remaining_forecast_time -= collision[2]
#print("Velocity: " + str(velocity))
#print(remaining_forecast_time)
#if len(bag.puck.path) > 20:
# exit()
if remaining_forecast_time > 0:
self.build_puck_path(bag, remaining_forecast_time)
# save path
#print("done, path: " + str(bag.puck.path))
self.last_path = bag.puck.path
def create_source_ray(x, y):
# https://www.scratchapixel.com/lessons/3d-basic-rendering/ray-tracing-generating-camera-rays/generating-camera-rays
cameraToWorld = scene['camera']
width = WIDTH / PIXEL_SCALE
height = HEIGHT / PIXEL_SCALE
a = math.tan(FOV / 2.0 * math.pi / 180.0)
Px = (2.0 * ((x + 0.5) / width) - 1) * a * ASPECT_RATIO
Py = (1.0 - 2.0 * ((y + 0.5) / height)) * a
Rp = [Px, Py, -1.0]
Ro = vector.origin()
Row = matrix.apply(cameraToWorld, Ro)
Rpw = matrix.apply(cameraToWorld, Rp)
Rdw = vector.subtract(Rpw, Row)
Rdw = vector.normalize(Rdw)
return [Row, Rdw]
def run(self, shape, vector): self.shape = shape r,g,b,a = shape.color shape.color = r, g, b, int(a * .3) dt = float(self.expire_time) / self.time_slice nv = vector.normalize() def _move(): self.duration += self.time_slice if self.expire_time <= self.duration: self.run_tag = None shape.destroy() self.finished_cb() return False dx, dy = nv * 5 x, y = shape.pos shape.pos = x + dx, y + dy return True self.run_tag = eloop.Timer(self.time_slice, _move)
def rot(v, beta):
sin, cos = math.sin, math.cos
v = vector.normalize(v)
# q = angvec2quat(beta[0], [0, 1, 0])
# return rotvecquat(v, q)
v1 = [v[0]*cos(beta[0]) + v[2]*sin(beta[0]),
v[1],
v[2]*cos(beta[0]) - v[0]*sin(beta[0])]
v2 = [v1[0],
v1[1]*cos(beta[1]) + v1[2]*sin(beta[1]),
v1[2]*cos(beta[1]) - v1[1]*sin(beta[1])]
v3 = [v2[0]*cos(beta[2]) + v2[1]*sin(beta[2]),
v2[1]*cos(beta[2]) - v2[0]*sin(beta[1]),
v2[2]]
return v3
def run(self, shape, vector): self.shape = shape dt = float(self.expire_time) / self.time_slice nv = vector.normalize() def _move(): self.duration += self.time_slice if self.expire_time <= self.duration: self.run_tag = None self.finished_cb(shape) return False dx, dy = nv x, y = shape.pos shape.pos = x + dx, y + dy if self.rotation == 'left': shape.angle = (shape.angle - 20) % 360 else: shape.angle = (shape.angle + 20) % 360 return True self.run_tag = eloop.Timer(self.time_slice, _move)
def rotateaxis(u,v,theta):
"""rotate v about u by angle theta (in degrees)"""
theta *= numpy.pi/180.
#matrix gotten from http://www.fastgraph.com/makegames/3drotation/ and originally from Graphics Gems 1990
s = sin(theta)
c = cos(theta)
t = 1. - cos(theta)
x = u.x
y = u.y
z = u.z
R = numpy.array([[t*x**2 + c, t*x*y - s*z, t*x*z + s*y, 0.],
[t*x*y + s*z, t*y**2 + c, t*y*z - s*x, 0.],
[t*x*z - s*y, t*y*z + s*x, t*z**2 + c, 0.],
[0., 0., 0., 1.]])
v = Vec([v.x, v.y, v.z, 1.])
r = Vec(numpy.dot(R, v))
v = normalize(Vec([r.x, r.y, r.z]))
return v
def update(self):
if not self.manipulator.is_alive(): return
main_blob = self.manipulator.get_largest_blob()
others = self.manipulator.get_other_items()
closest = None
for item in others:
if isinstance(item, PelletProxy) or isinstance(item, LargePelletProxy):
distance = vector.squared_distance(main_blob.get_position(), item.get_position())
if closest is None or distance < closest[1]:
closest = (item.get_position(), distance)
if closest is None:
position = self.manipulator.get_largest_blob().get_position()
middle = vector.divide(self.manipulator.get_board_size(), 2)
self.manipulator.set_velocity(vector.substract(middle, position))
else:
self.manipulator.set_velocity(vector.normalize(vector.substract(closest[0], main_blob.get_position())))
def intersectsWall(self, wall1):
# Get the directional vector
# representing the wall
wallDir = vector.normalize(wall1.line)
# Get the length of the line
# from the start of the wall
# to the nearest point to the ball
projectedMag = vector.dot(wallDir, vector.subtract(self.position, wall1.start))
# Scale the directional vector
# by the projected magnitude
wallDir.scalarMultiply(projectedMag)
# Get the line between the
# ball and the nearest point on the wall
wallToBall = vector.subtract(vector.add(wallDir, wall1.start), self.position)
# Return the amount the wall
# is intersecting with the ball
return self.size - vector.magnitude(wallToBall)
def calcRepulsion(self, followers):
self.repulsionF = Vector2f(0.0, 0.0)
collisionChecks = 0
# Calculate evasion from neighbours that are too close
for i in self.grid.getNeighbours(self.loc.x, self.loc.y):
d = distance(self.pos, i.pos)
collisionChecks += 1
if d < self.params.separationD and d != 0.0:
self.repulsionF += (self.pos -
i.getPos()) * self.params.separationD / d
# Evade leader if necessary
fLeader = self.leader.getPos() + normalize(
self.leader.getV()) * self.params.followDist
d = distance(self.pos, fLeader)
if d < self.params.followDist and d != 0.0:
self.repulsionF += (self.pos -
fLeader) * self.params.followDist / d
d = distance(self.pos, self.leader.getPos())
if d < self.params.followDist and d != 0.0:
self.repulsionF += (self.pos - self.leader.getPos()
) * 2 * self.params.followDist / d
return collisionChecks
def circle_line(body, line, screen=None):
# 计算circle与直线最近的点
relative_position = body.position - line.p1
projected_vector = line.direction * relative_position.dot(
line.direction)
closest_point = line.p1 + projected_vector
# 记录参数
cx, cy = closest_point
lx1, ly1 = line.p1
lx2, ly2 = line.p2
# Make sure that closest point lies on the line
if lx1 - lx2 > 0:
cx = max(min(cx, lx1), lx2)
else:
cx = min(max(cx, lx1), lx2)
if ly1 - ly2 > 0:
cy = max(min(cy, ly1), ly2)
else:
cy = min(max(cy, ly1), ly2)
closest_point[:] = cx, cy
distance = V.magnitude(body.position - closest_point)
collided = distance < body.radius
if collided:
# Resolve interpenetration
orthogonal_vector = relative_position - projected_vector
penetration = body.radius - V.magnitude(orthogonal_vector)
disposition = V.normalize(orthogonal_vector) * penetration
body.position += disposition
# Resolve Velocity
velocity = body.get_velocity() - line.normal * body.get_velocity(
).dot(line.normal) * (1 + body.wall_restitution)
body.set_velocity(velocity)
return collided
def fromTwoVectors(self,orig,target):
o=normalize(orig)
t=normalize(target)
self.fromAxisAngle(o.cross(t),acos(o.dot(t)))
def test_normalize(self):
result = vector.normalize([3, 0, 0])
correct = array([[ 1., 0., 0.]])
error = norm(result-correct)
self.assertAlmostEqual(error, 0)
def normalize(ray):
# ray -> ray
dirn = vr.normalize(get_dirn(ray))
return make_ray(get_pos(ray), dirn)
#!/usr/bin/env python2
import vector
import math
v1 = 1,2,3
v2 = 3,4,5
n1 = vector.normalize(v1)
assert vector.add(v1, v2) == (4, 6, 8)
assert vector.subtract(v1, v2) == (-2, -2, -2)
assert abs(n1[0] - 0.27) < 0.01 and \
abs(n1[1] - 0.53) < 0.01 and \
abs(n1[2] - 0.80) < 0.01
assert vector.dot(v1, v2) == 26
assert vector.cross(v1, v2) == (-2, 4, -2)
print "All tests passed!"
def make_view_geometry(eye, up, center):
# (vector a) ** 3 -> seq (vector a)
forward = vr.normalize(vr.sub_vector(center, eye))
side = vr.normalize(vr.cross_prod(forward, up))
up = vr.normalize(vr.cross_prod(side, forward))
return [forward, side, up]
def sphere_normal(sphere, pt):
return vr.normalize(vr.sub_vector(pt, get_center(sphere)))
def Update(self, Gyroscope, Accelerometer, Magnetometer):
'''Calculate the best quaternion to the given measurement values.
Parameters
----------
Gyroscope : array, shape (N,3)
Angular velocity [rad/s]
Accelerometer : array, shape (N,3)
Linear acceleration (Only the direction is used, so units don't matter.)
Magnetometer : array, shape (N,3)
Orientation of local magenetic field.
(Again, only the direction is used, so units don't matter.)
'''
q = self.Quaternion
# short name local variable for readability
# Reference direction of Earth's magnetic field
h = vector.rotate_vector(Magnetometer, q)
b = np.hstack((0, np.sqrt(h[0]**2 + h[1]**2), 0, h[2]))
# Gradient decent algorithm corrective step
F = [
2 * (q[1] * q[3] - q[0] * q[2]) - Accelerometer[0],
2 * (q[0] * q[1] + q[2] * q[3]) - Accelerometer[1],
2 * (0.5 - q[1]**2 - q[2]**2) - Accelerometer[2],
2 * b[1] * (0.5 - q[2]**2 - q[3]**2) + 2 * b[3] *
(q[1] * q[3] - q[0] * q[2]) - Magnetometer[0],
2 * b[1] * (q[1] * q[2] - q[0] * q[3]) + 2 * b[3] *
(q[0] * q[1] + q[2] * q[3]) - Magnetometer[1],
2 * b[1] * (q[0] * q[2] + q[1] * q[3]) + 2 * b[3] *
(0.5 - q[1]**2 - q[2]**2) - Magnetometer[2]
]
J = np.array([[-2 * q[2], 2 * q[3], -2 * q[0], 2 * q[1]],
[2 * q[1], 2 * q[0], 2 * q[3], 2 * q[2]],
[0, -4 * q[1], -4 * q[2], 0],
[
-2 * b[3] * q[2], 2 * b[3] * q[3],
-4 * b[1] * q[2] - 2 * b[3] * q[0],
-4 * b[1] * q[3] + 2 * b[3] * q[1]
],
[
-2 * b[1] * q[3] + 2 * b[3] * q[1],
2 * b[1] * q[2] + 2 * b[3] * q[0],
2 * b[1] * q[1] + 2 * b[3] * q[3],
-2 * b[1] * q[0] + 2 * b[3] * q[2]
],
[
2 * b[1] * q[2], 2 * b[1] * q[3] - 4 * b[3] * q[1],
2 * b[1] * q[0] - 4 * b[3] * q[2], 2 * b[1] * q[1]
]])
step = J.T.dot(F)
step = vector.normalize(step) # normalise step magnitude
# Compute rate of change of quaternion
qDot = 0.5 * quat.q_mult(q, np.hstack([0, Gyroscope
])) - self.Beta * step
# Integrate to yield quaternion
q = q + qDot * self.SamplePeriod
self.Quaternion = vector.normalize(q).flatten()
def _calc_orientation(self):
'''
Calculate the current orientation
Parameters
----------
type : string
- 'analytical' .... quaternion integration of angular velocity
- 'kalman' ..... quaternion Kalman filter
- 'madgwick' ... gradient descent method, efficient
- 'mahony' .... formula from Mahony, as implemented by Madgwick
'''
initialPosition = np.r_[0, 0, 0]
method = self.q_type
if method == 'analytical':
(quaternion, position,
velocity) = analytical(self.R_init, self.omega, initialPosition,
self.acc, self.rate)
elif method == 'kalman':
self._checkRequirements()
quaternion = kalman(self.rate, self.acc, np.deg2rad(self.omega),
self.mag)
elif method == 'madgwick':
self._checkRequirements()
# Initialize object
AHRS = Madgwick(SamplePeriod=1. / self.rate,
Beta=0.5) # previously: Beta=1.5
quaternion = np.zeros((self.totalSamples, 4))
# The "Update"-function uses angular velocity in radian/s, and only the directions of acceleration and magnetic field
Gyr = self.omega
Acc = vector.normalize(self.acc)
Mag = vector.normalize(self.mag)
#for t in range(len(time)):
for t in misc.progressbar(range(self.totalSamples),
'Calculating the Quaternions ', 25):
AHRS.Update(Gyr[t], Acc[t], Mag[t])
quaternion[t] = AHRS.Quaternion
elif method == 'mahony':
self._checkRequirements()
# Initialize object
AHRS = Mahony(SamplePeriod=1. / np.float(self.rate),
Kp=0.4) # previously: Kp=0.5
quaternion = np.zeros((self.totalSamples, 4))
# The "Update"-function uses angular velocity in radian/s, and only the directions of acceleration and magnetic field
Gyr = self.omega
Acc = vector.normalize(self.acc)
Mag = vector.normalize(self.mag)
#for t in range(len(time)):
for t in misc.progressbar(range(self.totalSamples),
'Calculating the Quaternions ', 25):
AHRS.Update(Gyr[t], Acc[t], Mag[t])
quaternion[t] = AHRS.Quaternion
else:
print('Unknown orientation type: {0}'.format(method))
return
self.quat = quaternion
def analyze_3Dmarkers(MarkerPos, ReferencePos):
'''
Take recorded positions from 3 markers, and calculate center-of-mass (COM) and orientation
Can be used e.g. for the analysis of Optotrac data.
Parameters
----------
MarkerPos : ndarray, shape (N,9)
x/y/z coordinates of 3 markers
ReferencePos : ndarray, shape (1,9)
x/y/z coordinates of markers in the reference position
Returns
-------
Position : ndarray, shape (N,3)
x/y/z coordinates of COM, relative to the reference position
Orientation : ndarray, shape (N,3)
Orientation relative to reference orientation, expressed as quaternion
Example
-------
>>> (PosOut, OrientOut) = analyze_3Dmarkers(MarkerPos, ReferencePos)
'''
# Specify where the x-, y-, and z-position are located in the data
cols = {'x' : r_[(0,3,6)]}
cols['y'] = cols['x'] + 1
cols['z'] = cols['x'] + 2
# Calculate the position
cog = np.vstack(( sum(MarkerPos[:,cols['x']], axis=1),
sum(MarkerPos[:,cols['y']], axis=1),
sum(MarkerPos[:,cols['z']], axis=1) )).T/3.
cog_ref = np.vstack(( sum(ReferencePos[cols['x']]),
sum(ReferencePos[cols['y']]),
sum(ReferencePos[cols['z']]) )).T/3.
position = cog - cog_ref
# Calculate the orientation
numPoints = len(MarkerPos)
orientation = np.ones((numPoints,3))
refOrientation = vector.plane_orientation(ReferencePos[:3], ReferencePos[3:6], ReferencePos[6:])
for ii in range(numPoints):
'''The three points define a triangle. The first rotation is such
that the orientation of the reference-triangle, defined as the
direction perpendicular to the triangle, is rotated along the shortest
path to the current orientation.
In other words, this is a rotation outside the plane spanned by the three
marker points.'''
curOrientation = vector.plane_orientation( MarkerPos[ii,:3], MarkerPos[ii,3:6], MarkerPos[ii,6:])
alpha = vector.angle(refOrientation, curOrientation)
if alpha > 0:
n1 = vector.normalize( np.cross(refOrientation, curOrientation) )
q1 = n1 * np.sin(alpha/2)
else:
q1 = r_[0,0,0]
# Now rotate the triangle into this orientation ...
refPos_after_q1 = vector.rotate_vector(np.reshape(ReferencePos,(3,3)), q1)
'''Find which further rotation in the plane spanned by the three marker points
is necessary to bring the data into the measured orientation.'''
Marker_0 = MarkerPos[ii,:3]
Marker_1 = MarkerPos[ii,3:6]
Vector10 = Marker_0 - Marker_1
vector10_ref = refPos_after_q1[0]-refPos_after_q1[1]
beta = vector.angle(Vector10, vector10_ref)
q2 = curOrientation * np.sin(beta/2)
if np.cross(vector10_ref,Vector10).dot(curOrientation)<=0:
q2 = -q2
orientation[ii,:] = quat.q_mult(q2, q1)
return (position, orientation)
def distance_to_segment(point, a, b):
if dot(b - a, point - a) <= 0 or dot(a - b, point - b) <= 0:
return min(dist(point, a), dist(point, b))
return abs(cross(point - a, normalize(b - a)))
def __init__(self, _orig, _dir):
self.orig = _orig
self.dir = normalize(_dir)
def __init__(self, _orig, _dir):
self.orig = _orig
self.dir = normalize(_dir)
def _calc_orientation(self):
'''
Calculate the current orientation
Parameters
----------
type : string
- 'analytical' .... quaternion integration of angular velocity
- 'kalman' ..... quaternion Kalman filter
- 'madgwick' ... gradient descent method, efficient
- 'mahony' .... formula from Mahony, as implemented by Madgwick
'''
initialPosition = np.r_[0,0,0]
method = self.q_type
if method == 'analytical':
(quaternion, position, velocity) = analytical(self.R_init, self.omega, initialPosition, self.acc, self.rate)
elif method == 'kalman':
self._checkRequirements()
quaternion = kalman(self.rate, self.acc, np.deg2rad(self.omega), self.mag)
elif method == 'madgwick':
self._checkRequirements()
# Initialize object
AHRS = Madgwick(SamplePeriod=1./self.rate, Beta=0.5) # previously: Beta=1.5
quaternion = np.zeros((self.totalSamples, 4))
# The "Update"-function uses angular velocity in radian/s, and only the directions of acceleration and magnetic field
Gyr = self.omega
Acc = vector.normalize(self.acc)
Mag = vector.normalize(self.mag)
#for t in range(len(time)):
for t in misc.progressbar(range(self.totalSamples), 'Calculating the Quaternions ', 25) :
AHRS.Update(Gyr[t], Acc[t], Mag[t])
quaternion[t] = AHRS.Quaternion
elif method == 'mahony':
self._checkRequirements()
# Initialize object
AHRS = Mahony(SamplePeriod=1./np.float(self.rate), Kp=0.4) # previously: Kp=0.5
quaternion = np.zeros((self.totalSamples, 4))
# The "Update"-function uses angular velocity in radian/s, and only the directions of acceleration and magnetic field
Gyr = self.omega
Acc = vector.normalize(self.acc)
Mag = vector.normalize(self.mag)
#for t in range(len(time)):
for t in misc.progressbar(range(self.totalSamples), 'Calculating the Quaternions ', 25) :
AHRS.Update(Gyr[t], Acc[t], Mag[t])
quaternion[t] = AHRS.Quaternion
else:
print('Unknown orientation type: {0}'.format(method))
return
self.quat = quaternion
def _calculate_collision_point(self, bag):
"""
Calculates the collision point between the puck and the stick.
:param bag: The parameter bag.
:return:
"""
# NOTE: First attempt, ignoring stick and puck acceleration, puck radius and stick radius here
# OK, math first:
#
# S = stick position
# C = point of collision
# v = max speed of the stick
# P = puck position
# p = puck direction * velocity
#
# Points which the stick can reach in time t build a circle around him:
# 1. (Cx - Sx)^2 + (Cy - Sy)^2 = r = v * t
#
# Points which the puck reaches in time t are on the line:
# 2. Px + px * t = Cx
# 3. Py + py * t = Cy
#
# Approach:
# - replace Cx and Cy in 1. with 2. and 3.
# - bracket the square terms (Px + px * t - Sx)^2 and (Py + py * t - Sy)^2
# - outline t^2 and t to get a formula like t^2*(...) + t*(...) + (...)
# - name (...) a, b and c
# - use the quadratic formula to get t
# - use puck movement and t to get collision point
# - use stick position and collision point
# - gg
Sx = bag.stick.position[0]
Sy = bag.stick.position[1]
Px = bag.puck.position[0]
Py = bag.puck.position[1]
px = bag.puck.direction[0] * bag.puck.velocity
py = bag.puck.direction[1] * bag.puck.velocity
v = self.STICK_MAX_SPEED
# calculate a, b and c
a = px*px + py*py - v*v
b = 2*Px*px - 2*Sx*px + 2*Py*py - 2*Sy*py
c = Px*Px - 2*Px*Sx + Sx*Sx + Py*Py - 2*Py*Sy + Sy*Sy
# calculate t
inner_sqrt = b*b - 4*a*c
if inner_sqrt < 0:
print("Going home, inner sqrt: " + str(inner_sqrt))
# no chance to get that thing, go home
self.go_home(bag)
return
# use + first, since a is negative (Vpuck - Vstick)
# (... / 2 * a) -> shortest time needed
sqrt_b2_4ac = math.sqrt(inner_sqrt)
t = (-b + sqrt_b2_4ac) / 2*a
print("Vp: "+str(bag.puck.velocity))
print("t: "+str(t))
if t < 0:
# too late for that chance, use other result
t = (-b - sqrt_b2_4ac) / 2*a
print("t2: "+str(t))
# get collision point
C = vector.add((Px, Py), vector.mul_by((px, py), t))
s = vector.from_to((Sx, Sy), C)
# save
bag.stick.dest_position = C
bag.stick.dest_direction = vector.normalize(s)
bag.stick.dest_velocity = self.STICK_MAX_SPEED
bag.stick.dest_time = time.time() + t
def create_obstacle_circle(center, radius):
o = Obstacle(lambda r: dist(r, center) - radius,
lambda r: normalize(r - center),
shapes.Circle(center, radius))
o.center = center
return o
def kalman(rate, acc, omega, mag):
'''
Calclulate the orientation from IMU magnetometer data.
Parameters
----------
rate : float
sample rate [Hz]
acc : (N,3) ndarray
linear acceleration [m/sec^2]
omega : (N,3) ndarray
angular velocity [rad/sec]
mag : (N,3) ndarray
magnetic field orientation
Returns
-------
qOut : (N,4) ndarray
unit quaternion, describing the orientation relativ to the coordinate system spanned by the local magnetic field, and gravity
Notes
-----
Based on "Design, Implementation, and Experimental Results of a Quaternion-Based Kalman Filter for Human Body Motion Tracking" Yun, X. and Bachman, E.R., IEEE TRANSACTIONS ON ROBOTICS, VOL. 22, 1216-1227 (2006)
'''
numData = len(acc)
# Set parameters for Kalman Filter
tstep = 1. / rate
tau = [0.5, 0.5, 0.5] # from Yun, 2006
# Initializations
x_k = np.zeros(7) # state vector
z_k = np.zeros(7) # measurement vector
z_k_pre = np.zeros(7)
P_k = np.matrix(np.eye(7)) # error covariance matrix P_k
Phi_k = np.matrix(np.zeros(
(7, 7))) # discrete state transition matrix Phi_k
for ii in range(3):
Phi_k[ii, ii] = np.exp(-tstep / tau[ii])
H_k = np.eye(7) # Identity matrix
Q_k = np.zeros((7, 7)) # process noise matrix Q_k
#D = 0.0001*np.r_[0.4, 0.4, 0.4] # [rad^2/sec^2]; from Yun, 2006
D = np.r_[0.4, 0.4, 0.4] # [rad^2/sec^2]; from Yun, 2006
for ii in range(3):
Q_k[ii,
ii] = D[ii] / (2 * tau[ii]) * (1 - np.exp(-2 * tstep / tau[ii]))
# Evaluate measurement noise covariance matrix R_k
R_k = np.zeros((7, 7))
r_angvel = 0.01
# [rad**2/sec**2]; from Yun, 2006
r_quats = 0.0001
# from Yun, 2006
for ii in range(7):
if ii < 3:
R_k[ii, ii] = r_angvel
else:
R_k[ii, ii] = r_quats
# Calculation of orientation for every time step
qOut = np.zeros((numData, 4))
for ii in range(numData):
accelVec = acc[ii, :]
magVec = mag[ii, :]
angvelVec = omega[ii, :]
z_k_pre = z_k.copy(
) # watch out: by default, Python passes the reference!!
# Evaluate quaternion based on acceleration and magnetic field data
accelVec_n = vector.normalize(accelVec)
magVec_hor = magVec - accelVec_n * (accelVec_n @ magVec)
magVec_n = vector.normalize(magVec_hor)
basisVectors = np.vstack((magVec_n, np.cross(accelVec_n,
magVec_n), accelVec_n)).T
quatRef = quat.q_inv(rotmat.convert(basisVectors, to='quat')).flatten()
# Update measurement vector z_k
z_k[:3] = angvelVec
z_k[3:] = quatRef
# Calculate Kalman Gain
# K_k = P_k * H_k.T * inv(H_k*P_k*H_k.T + R_k)
K_k = P_k @ np.linalg.inv(P_k + R_k)
# Update state vector x_k
x_k += np.array(K_k.dot(z_k - z_k_pre)).ravel()
# Evaluate discrete state transition matrix Phi_k
Phi_k[3, :] = np.r_[-x_k[4] * tstep / 2, -x_k[5] * tstep / 2,
-x_k[6] * tstep / 2, 1, -x_k[0] * tstep / 2,
-x_k[1] * tstep / 2, -x_k[2] * tstep / 2]
Phi_k[4, :] = np.r_[x_k[3] * tstep / 2, -x_k[6] * tstep / 2,
x_k[5] * tstep / 2, x_k[0] * tstep / 2, 1,
x_k[2] * tstep / 2, -x_k[1] * tstep / 2]
Phi_k[5, :] = np.r_[x_k[6] * tstep / 2, x_k[3] * tstep / 2,
-x_k[4] * tstep / 2, x_k[1] * tstep / 2,
-x_k[2] * tstep / 2, 1, x_k[0] * tstep / 2]
Phi_k[6, :] = np.r_[-x_k[5] * tstep / 2, x_k[4] * tstep / 2,
x_k[3] * tstep / 2, x_k[2] * tstep / 2,
x_k[1] * tstep / 2, -x_k[0] * tstep / 2, 1]
# Update error covariance matrix
#P_k = (eye(7)-K_k*H_k)*P_k
P_k = (np.eye(7) - K_k) @ P_k
# Projection of state quaternions
x_k[3:] += 0.5 * quat.q_mult(x_k[3:], np.r_[0, x_k[:3]]).flatten()
x_k[3:] = vector.normalize(x_k[3:])
x_k[:3] = np.zeros(3)
x_k[:3] += tstep * (-x_k[:3] + z_k[:3])
qOut[ii, :] = x_k[3:]
# Projection of error covariance matrix
P_k = Phi_k @ P_k @ Phi_k.T + Q_k
# Make the first position the reference position
qOut = quat.q_mult(qOut, quat.q_inv(qOut[0]))
return qOut
#!/usr/bin/env python2 import vector import math v1 = [36.886, 53.177, 21.887] v2 = [38.323, 52.817, 21.996] v3 = [38.493, 51.553, 22.830] v4 = [39.483, 50.748, 22.463] n1 = vector.normalize(v1) n2 = vector.normalize(v2) v1 = n1 = 1,2,3 v2 = n2 = 3,4,5 print vector.add(v1, v2) print vector.subtract(v1, v2) print vector.normalize(v1) print vector.dot(n1, n2) print vector.cross(n1, n2) print vector.torsion(v1, v2, v3, v4)
def test_normalize(self):
result = vector.normalize([3, 0, 0])
correct = array([[1., 0., 0.]])
error = norm(result - correct)
self.assertAlmostEqual(error, 0)
def kalman(rate, acc, omega, mag):
'''
Calclulate the orientation from IMU magnetometer data.
Parameters
----------
rate : float
sample rate [Hz]
acc : (N,3) ndarray
linear acceleration [m/sec^2]
omega : (N,3) ndarray
angular velocity [rad/sec]
mag : (N,3) ndarray
magnetic field orientation
Returns
-------
qOut : (N,4) ndarray
unit quaternion, describing the orientation relativ to the coordinate system spanned by the local magnetic field, and gravity
Notes
-----
Based on "Design, Implementation, and Experimental Results of a Quaternion-Based Kalman Filter for Human Body Motion Tracking" Yun, X. and Bachman, E.R., IEEE TRANSACTIONS ON ROBOTICS, VOL. 22, 1216-1227 (2006)
'''
numData = len(acc)
# Set parameters for Kalman Filter
tstep = 1./rate
tau = [0.5, 0.5, 0.5] # from Yun, 2006
# Initializations
x_k = np.zeros(7) # state vector
z_k = np.zeros(7) # measurement vector
z_k_pre = np.zeros(7)
P_k = np.matrix( np.eye(7) ) # error covariance matrix P_k
Phi_k = np.matrix( np.zeros((7,7)) ) # discrete state transition matrix Phi_k
for ii in range(3):
Phi_k[ii,ii] = np.exp(-tstep/tau[ii])
H_k = np.eye(7) # Identity matrix
Q_k = np.zeros((7,7)) # process noise matrix Q_k
#D = 0.0001*np.r_[0.4, 0.4, 0.4] # [rad^2/sec^2]; from Yun, 2006
D = np.r_[0.4, 0.4, 0.4] # [rad^2/sec^2]; from Yun, 2006
for ii in range(3):
Q_k[ii,ii] = D[ii]/(2*tau[ii]) * ( 1-np.exp(-2*tstep/tau[ii]) )
# Evaluate measurement noise covariance matrix R_k
R_k = np.zeros((7,7))
r_angvel = 0.01; # [rad**2/sec**2]; from Yun, 2006
r_quats = 0.0001; # from Yun, 2006
for ii in range(7):
if ii<3:
R_k[ii,ii] = r_angvel
else:
R_k[ii,ii] = r_quats
# Calculation of orientation for every time step
qOut = np.zeros( (numData,4) )
for ii in range(numData):
accelVec = acc[ii,:]
magVec = mag[ii,:]
angvelVec = omega[ii,:]
z_k_pre = z_k.copy() # watch out: by default, Python passes the reference!!
# Evaluate quaternion based on acceleration and magnetic field data
accelVec_n = vector.normalize(accelVec)
magVec_hor = magVec - accelVec_n * (accelVec_n @ magVec)
magVec_n = vector.normalize(magVec_hor)
basisVectors = np.vstack( (magVec_n, np.cross(accelVec_n, magVec_n), accelVec_n) ).T
quatRef = quat.q_inv(rotmat.convert(basisVectors, to='quat')).flatten()
# Update measurement vector z_k
z_k[:3] = angvelVec
z_k[3:] = quatRef
# Calculate Kalman Gain
# K_k = P_k * H_k.T * inv(H_k*P_k*H_k.T + R_k)
K_k = P_k @ np.linalg.inv(P_k + R_k)
# Update state vector x_k
x_k += np.array( K_k.dot(z_k-z_k_pre) ).ravel()
# Evaluate discrete state transition matrix Phi_k
Phi_k[3,:] = np.r_[-x_k[4]*tstep/2, -x_k[5]*tstep/2, -x_k[6]*tstep/2, 1, -x_k[0]*tstep/2, -x_k[1]*tstep/2, -x_k[2]*tstep/2]
Phi_k[4,:] = np.r_[ x_k[3]*tstep/2, -x_k[6]*tstep/2, x_k[5]*tstep/2, x_k[0]*tstep/2, 1, x_k[2]*tstep/2, -x_k[1]*tstep/2]
Phi_k[5,:] = np.r_[ x_k[6]*tstep/2, x_k[3]*tstep/2, -x_k[4]*tstep/2, x_k[1]*tstep/2, -x_k[2]*tstep/2, 1, x_k[0]*tstep/2]
Phi_k[6,:] = np.r_[-x_k[5]*tstep/2, x_k[4]*tstep/2, x_k[3]*tstep/2, x_k[2]*tstep/2, x_k[1]*tstep/2, -x_k[0]*tstep/2, 1]
# Update error covariance matrix
#P_k = (eye(7)-K_k*H_k)*P_k
P_k = (np.eye(7) - K_k) @ P_k
# Projection of state quaternions
x_k[3:] += 0.5 * quat.q_mult(x_k[3:], np.r_[0, x_k[:3]]).flatten()
x_k[3:] = vector.normalize( x_k[3:] )
x_k[:3] = np.zeros(3)
x_k[:3] += tstep * (-x_k[:3]+z_k[:3])
qOut[ii,:] = x_k[3:]
# Projection of error covariance matrix
P_k = Phi_k @ P_k @ Phi_k.T + Q_k
# Make the first position the reference position
qOut = quat.q_mult(qOut, quat.q_inv(qOut[0]))
return qOut
