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 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 _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