예제 #1
0
 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]))
예제 #2
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
예제 #3
0
파일: aambb.py 프로젝트: jstasiak/Pyrr
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 ]
            ]
        )
예제 #4
0
파일: aambb.py 프로젝트: jstasiak/Pyrr
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 ]
            ]
        )
예제 #5
0
 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]))
예제 #6
0
파일: quaternion.py 프로젝트: jstasiak/Pyrr
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 )
예제 #7
0
 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]))
예제 #8
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]))
예제 #9
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"
                )
예제 #10
0
파일: test_aambb.py 프로젝트: RazerM/Pyrr
 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]))
예제 #11
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"
                )
예제 #12
0
파일: quaternion.py 프로젝트: jstasiak/Pyrr
    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
예제 #13
0
    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"
            )
예제 #14
0
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
예제 #15
0
    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
예제 #16
0
파일: test_aambb.py 프로젝트: RazerM/Pyrr
 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]))
예제 #17
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
예제 #18
0
    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,
        )
예제 #19
0
 def distance_to(self, other) -> float:
     return vector.length(self.vector_to(other))