def test_create_from_bounds(self): bounds = [[-1., 1., -1.], [2., 1., 0.]] result = aambb.create_from_bounds(*bounds) length = max(vector.length(bounds[0]), vector.length(bounds[1])) self.assertTrue( np.array_equal( result, [[-length, -length, -length], [length, length, length]])) self.assertTrue( np.array_equal(aambb.centre_point(result), [0.0, 0.0, 0.0]))
def heading_to_point( start_x: float, start_y: float, vel_x: float, vel_y: float, point_x: float, point_y: float, ): """ Return a heading, in 2D RH coordinate system. x,y: the current position of the object vel_x, vel_y: the current velocity vector of motion for the object point_x, point_y: the destination point to head towards returns: offset angle in radians in the range [-pi .. pi] where: 0.0: object is moving directly towards the point [-pi .. <0]: object is moving to the "right" of the point [>0 .. -pi]: object is moving to the "left" of the point [-pi, pi]: object is moving directly away from the point """ # vector to point dx = point_x - start_x dy = point_y - start_y # if the ball is already at the target location or # is not moving, return a heading of 0 so we don't # attempt to normalize a zero-length vector if dx == 0 and dy == 0: return 0 if vel_x == 0 and vel_y == 0: return 0 # vectors and lengths u = vector.normalize([dx, dy, 0.0]) v = vector.normalize([vel_x, vel_y, 0.0]) ul = vector.length(u) vl = vector.length(v) # no velocity? already on the target? angle = 0.0 if (ul != 0.0) and (vl != 0.0): # angle between vectors uv_dot = vector.dot(u, v) # signed angle x = u[0] y = u[1] angle = math.atan2(vector.dot([-y, x, 0.0], v), uv_dot) if math.isnan(angle): angle = 0.0 return angle
def create_from_points( points ): """Creates an AAMBB from the list of specified points. Points must be a 2D list. Ie:: numpy.array([ [ x, y, z ], [ x, y, z ], ]) """ # convert any negative values to positive abs_points = numpy.absolute( points ) # extract the maximum extent as a vector vec = numpy.amax( abs_points, axis = 0 ) # find the length of this vector length = vector.length( vec ) # our AAMBB extends from +length to -length # in all directions return numpy.array( [ [-length,-length,-length ], [ length, length, length ] ] )
def add_points( bb, points ): """Extends an AAMBB to encompass a list of points. It should be noted that this ensures that the encompassed points can rotate freely. Calling this using the min / max points from the AAMBB will create an even bigger AAMBB. """ # add our AABB to the list of points values = numpy.vstack( points, bb[ 0 ], bb[ 1 ] ) # convert any negative values to positive abs_points = numpy.absolute( values ) # extract the maximum extent as a vector vec = numpy.amax( abs_points, axis = 0 ) # find the length of this vector length = vector.length( vec ) # our AAMBB extends from +length to -length # in all directions return numpy.array( [ [-length,-length,-length ], [ length, length, length ] ] )
def test_add_aabbs(self): a1 = aambb.create_from_bounds([-0.5,-0.5,-0.5], [0.5,0.5,0.5]) a2 = aambb.create_from_bounds([1.0,-2.0, 1.0], [2.0,-1.0, 1.0]) result = aambb.add_aabbs(a1, [a2]) length = np.amax(vector.length([a1, a2])) self.assertTrue(np.array_equal(result, [[-length,-length,-length],[length,length,length]]), (result,)) self.assertTrue(np.array_equal(aambb.centre_point(result), [0.0,0.0,0.0]))
def length( quat ): """Calculates the length of a quaternion. :param numpy.array quat: The quaternion to measure. :rtype: float, numpy.array :return: If a 1d array was passed, it will be a scalar. Otherwise the result will be an array of scalars with shape vec.ndim with the last dimension being size 1. """ return vector.length( quat )
def test_add_point(self): a = aambb.create_from_bounds([-0.5,-0.5,-0.5], [0.5,0.5,0.5]) points = np.array([ [ 2.0,-1.0,-1.0], [ 1.0, 3.0,-1.0], ]) result = aambb.add_points(a, points) length = np.amax(vector.length([a, points])) self.assertTrue(np.array_equal(result, [[-length,-length,-length],[length,length,length]]), (result,)) self.assertTrue(np.array_equal(aambb.centre_point(result), [0.0,0.0,0.0]))
def test_create_from_aabbs(self): a1 = aambb.create_from_points([[0.0, 0.0, 0.0], [1.0, 1.0, -1.0]]) a2 = aambb.create_from_points([[0.0, 0.0, 2.0], [-1.0, -1.0, 1.0]]) result = aambb.create_from_aabbs([a1, a2]) length = np.amax(vector.length([a1, a2])) self.assertTrue( np.array_equal( result, [[-length, -length, -length], [length, length, length]]), (result, )) self.assertTrue( np.array_equal(aambb.centre_point(result), [0.0, 0.0, 0.0]))
def single_vector(): vec = numpy.array( [ 1.0, 1.0, 1.0 ] ) result = vector.length( vec ) expected = math.sqrt( numpy.sum( vec ** 2 ) ) # ensure the calculated length is what we expect self.assertEqual( result, expected, "Vector length calculation incorrect" )
def test_create_from_aabbs(self): a1 = aambb.create_from_points([ [ 0.0, 0.0, 0.0], [ 1.0, 1.0,-1.0] ]) a2 = aambb.create_from_points([ [ 0.0, 0.0, 2.0], [-1.0,-1.0, 1.0] ]) result = aambb.create_from_aabbs([a1, a2]) length = np.amax(vector.length([a1, a2])) self.assertTrue(np.array_equal(result, [[-length,-length,-length],[length,length,length]]), (result,)) self.assertTrue(np.array_equal(aambb.centre_point(result), [0.0,0.0,0.0]))
def batch_lengths(): vec = numpy.array( [ 1.0, 1.0, 1.0 ] ) batch = numpy.tile( vec, (3,1) ) result = vector.length( batch ) expected = numpy.array( [ math.sqrt( numpy.sum( vec ** 2 ) ) ] ) expected = numpy.tile( expected, (3) ) self.assertTrue( numpy.array_equal( result, expected ), "Vector batch length calculation incorrect" )
def apply( quat, vec3 ): """ v = numpy.array( vec ) return v + 2.0 * vector.cross( quat[:-1], vector.cross( quat[:-1], v ) + (quat[-1] * v) ) """ length = vector.length( vec3 ) vec3 = vector.normalise( vec3 ) # use the vector to create a new quaternion # this is basically the vector3 to vector4 conversion with W = 0 vec_quat = numpy.array( [ vec3[ 0 ], vec3[ 1 ], vec3[ 2 ], 0.0 ] ) # quat * vec * quat^-1 result = cross( quat, cross( vec_quat, conjugate( quat ) ) ) return result[ :-1 ] * length
def test_create_from_points( self ): vecs = numpy.array( [ [ 0.0, 0.0, 0.0 ], [ 5.0, 5.0, 5.0 ], [ 0.0, 0.0, 5.0 ], [-5.0, 0.0, 0.0 ], ], dtype = numpy.float ) # the biggest should be 5,5,5 result = sphere.create_from_points( vecs ) # centred around 0,0,0 # with MAX LENGTH as radius lengths = vector.length( vecs ) expected = numpy.array( [ 0.0, 0.0, 0.0, lengths.max() ] ) self.assertTrue( numpy.array_equal( result, expected ), "Sphere not calculated correctly" )
def sphere_penetration_sphere( s1, s2 ): """Calculates the distance two spheres have penetrated into one another. :param numpy.array s1: The first circle. :param numpy.array s2: The second circle. :rtype: float :return: The total overlap of the two spheres. This is essentially: r1 + r2 - distance Where r1 and r2 are the radii of circle 1 and 2 and distance is the length of the vector p2 - p1. Will return 0.0 if the circles do not overlap. """ delta = s2[ :3 ] - s1[ :3 ] distance = vector.length( delta ) combined_radii = s1[ 3 ] + s2[ 3 ] penetration = combined_radii - distance if penetration <= 0.0: return 0.0 return penetration
def update_camera_vectors(self): if not self.rotate_around_base: front: Vector3 = Vector3([0.0, 0.0, 0.0]) front.x = cos(radians(self.yaw + self.yaw_offset)) * cos( radians(self.pitch)) front.y = sin(radians(self.pitch)) front.z = sin(radians(self.yaw + self.yaw_offset)) * cos( radians(self.pitch)) self.camera_front = vector.normalize(front) self.camera_right = vector.normalize( vector3.cross(self.camera_front, Vector3([0.0, 1.0, 0.0]))) self.camera_up = vector.normalise( vector3.cross(self.camera_right, self.camera_front)) self.camera_pos = self.camera_pos + self.camera_right * self.move_vector.x * self.move_speed self.camera_pos = self.camera_pos + self.camera_up * self.move_vector.y * self.move_speed self.camera_pos = self.camera_pos + self.camera_front * self.move_vector.z * self.move_speed else: self.yaw_offset += self.rotation_speed front: Vector3 = Vector3([0.0, 0.0, 0.0]) front.x = cos(radians(self.yaw + self.yaw_offset)) * cos( radians(self.pitch)) front.y = sin(radians(self.pitch)) front.z = sin(radians(self.yaw + self.yaw_offset)) * cos( radians(self.pitch)) front_offset = vector.normalize(front) - self.camera_front self.camera_pos -= front_offset * vector.length(self.camera_pos - self.base) self.camera_front = vector.normalize(front) self.camera_right = vector.normalize( vector3.cross(self.camera_front, Vector3([0.0, 1.0, 0.0]))) self.camera_up = vector.normalise( vector3.cross(self.camera_right, self.camera_front)) self.camera_pos = self.camera_pos + self.camera_right * self.move_vector.x * self.move_speed self.camera_pos = self.camera_pos + self.camera_up * self.move_vector.y * self.move_speed self.camera_pos = self.camera_pos + self.camera_front * self.move_vector.z * self.move_speed
def test_create_from_bounds(self): bounds = [[-1.,1.,-1.], [2.,1.,0.]] result = aambb.create_from_bounds(*bounds) length = max(vector.length(bounds[0]), vector.length(bounds[1])) self.assertTrue(np.array_equal(result, [[-length,-length,-length],[length,length,length]])) self.assertTrue(np.array_equal(aambb.centre_point(result), [0.0,0.0,0.0]))
def test_plate_to_world_to_plate(): vec_world = model.plate_to_world(test_vector.x, test_vector.y, test_vector.z) result = model.world_to_plate(vec_world.x, vec_world.y, vec_world.z) delta = vector.length(result - test_vector) assert delta < TOLERANCE
def _update_estimated_ball(self, ball: Vector3): """ Ray trace the ball position and an edge of the ball back to the camera origin and use the collision points with the tilted plate to estimate what a camera might perceive the ball position and size to be. """ # contact ray from camera to plate camera = self._camera_pos() displacement = camera - self.ball displacement_radius = camera - (self.ball + Vector3([self.ball_radius, 0, 0])) ball_ray = ray.create(camera, displacement) ball_radius_ray = ray.create(camera, displacement_radius) surface_plane = self._surface_plane() contact = Vector3(ray_intersect_plane(ball_ray, surface_plane, False)) radius_contact = Vector3( ray_intersect_plane(ball_radius_ray, surface_plane, False)) x, y = contact.x, contact.y r = math.fabs(contact.x - radius_contact.x) # add the noise in self.estimated_x = x + MoabModel.random_noise(self.ball_noise) self.estimated_y = y + MoabModel.random_noise(self.ball_noise) self.estimated_radius = r + MoabModel.random_noise(self.ball_noise) # Use n-1 states to calculate an estimated velocity. self.estimated_vel_x = (self.estimated_x - self.prev_estimated_x) / self.step_time self.estimated_vel_y = (self.estimated_y - self.prev_estimated_y) / self.step_time # distance to target self.estimated_distance = MoabModel.distance_to_point( self.estimated_x, self.estimated_y, self.target_x, self.target_y) # update the derived states self.estimated_speed = cast( float, vector.length([self.ball_vel.x, self.ball_vel.y, self.ball_vel.z])) self.estimated_direction = MoabModel.heading_to_point( self.estimated_x, self.estimated_y, self.estimated_vel_x, self.estimated_vel_y, self.target_x, self.target_y, ) # update for next time self.prev_estimated_x = self.estimated_x self.prev_estimated_y = self.estimated_y # update ball position in plate origin coordinates, and obstacle distance and direction self.ball_on_plate = self.world_to_plate(self.ball.x, self.ball.y, self.ball.z) self.obstacle_distance = self._get_obstacle_distance() self.obstacle_direction = MoabModel.heading_to_point( self.ball.x, self.ball.y, self.ball_vel.x, self.ball_vel.y, self.obstacle_x, self.obstacle_y, )
def distance_to(self, other) -> float: return vector.length(self.vector_to(other))