Esempio n. 1
0
 def test_ray_intersect_sphere_two_solutions_2(self):
     r = ray.create([2.48, 1.45, 1.78], [-3.1, 0.48, -3.2])
     s = sphere.create([1, 1, 0], 1)
     intersections = ray_intersect_sphere(r, s)
     self.assertEqual(len(intersections), 2)
     np.testing.assert_array_almost_equal(intersections[0], np.array([0.44, 1.77, -0.32]), decimal=2)
     np.testing.assert_array_almost_equal(intersections[1], np.array([1.41, 1.62, 0.67]), decimal=2)
Esempio n. 2
0
 def test_ray_intersect_sphere_two_solutions_1(self):
     r = ray.create([-2, 0, 0], [1, 0, 0])
     s = sphere.create([0, 0, 0], 1)
     intersections = ray_intersect_sphere(r, s)
     self.assertEqual(len(intersections), 2)
     np.testing.assert_array_almost_equal(intersections[0], np.array([1, 0, 0]), decimal=2)
     np.testing.assert_array_almost_equal(intersections[1], np.array([-1, 0, 0]), decimal=2)
Esempio n. 3
0
 def test_ray_parallel_ray_3(self):
     r1 = ray.create([0.,0.,0.],[1.,0.,0.])
     r2 = ray.create([0.,1.,0.],[1.,0.,0.])
     self.assertTrue(gt.ray_parallel_ray(r1,r2))
Esempio n. 4
0
 def test_ray_intersect_plane_invalid(self):
     r = ray.create([0.,-1.,0.],[1.,0.,0.])
     p = plane.create([0.,1.,0.], 0.)
     result = gt.ray_intersect_plane(r, p)
     self.assertEqual(result, None)
Esempio n. 5
0
 def test_ray_intersect_plane_front_only(self):
     r = ray.create([0.,-1.,0.],[0.,1.,0.])
     p = plane.create([0.,1.,0.], 0.)
     result = gt.ray_intersect_plane(r, p, front_only=True)
     self.assertEqual(result, None)
Esempio n. 6
0
 def test_ray_intersect_plane(self):
     r = ray.create([0.,-1.,0.],[0.,1.,0.])
     p = plane.create([0.,1.,0.], 0.)
     result = gt.ray_intersect_plane(r, p)
     self.assertFalse(np.array_equal(result, [0.,1.,0.]))
Esempio n. 7
0
 def test_create_from_ray(self):
     r = ray.create([0., 0., 0.], [1., 0., 0.])
     result = line.create_from_ray(r)
     self.assertTrue(np.allclose(result, [[0, 0, 0], [1, 0, 0]]))
Esempio n. 8
0
 def test_create(self):
     result = ray.create([0.,0.,0.],[0.,0.,1.])
     np.testing.assert_almost_equal(result, [[0.,0.,0.],[0.,0.,1.]], decimal=5)
     self.assertTrue(result.dtype == np.float)
Esempio n. 9
0
 def test_ray_intersect_plane(self):
     r = ray.create([0.,-1.,0.],[0.,1.,0.])
     p = plane.create([0.,1.,0.], 0.)
     result = gt.ray_intersect_plane(r, p)
     self.assertFalse(np.array_equal(result, [0.,1.,0.]))
Esempio n. 10
0
 def test_ray_intersect_plane_invalid(self):
     r = ray.create([0.,-1.,0.],[1.,0.,0.])
     p = plane.create([0.,1.,0.], 0.)
     result = gt.ray_intersect_plane(r, p)
     self.assertEqual(result, None)
Esempio n. 11
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,
        )
Esempio n. 12
0
 def test_create_dtype(self):
     result = ray.create([0, 0, 0], [0, 0, 1], dtype=np.int)
     np.testing.assert_almost_equal(result, [[0, 0, 0], [0, 0, 1]],
                                    decimal=5)
     self.assertTrue(result.dtype == np.int)
Esempio n. 13
0
 def test_create(self):
     result = ray.create([0., 0., 0.], [0., 0., 1.])
     np.testing.assert_almost_equal(result, [[0., 0., 0.], [0., 0., 1.]],
                                    decimal=5)
     self.assertTrue(result.dtype == np.float)
Esempio n. 14
0
 def test_ray_intersect_sphere_no_solution_2(self):
     r = ray.create([0, 0, 0], [1, 0, 0])
     s = sphere.create([0, 2, 0], 1)
     intersections = ray_intersect_sphere(r, s)
     self.assertEqual(len(intersections), 0)
Esempio n. 15
0
 def test_ray_coincident_ray(self):
     r1 = ray.create([0.,0.,0.],[1.,0.,0.])
     r2 = ray.create([1.,0.,0.],[2.,0.,0.])
     self.assertTrue(gt.ray_coincident_ray(r1,r2))
Esempio n. 16
0
 def test_ray_parallel_ray_3(self):
     r1 = ray.create([0.,0.,0.],[1.,0.,0.])
     r2 = ray.create([0.,1.,0.],[1.,0.,0.])
     self.assertTrue(gt.ray_parallel_ray(r1,r2))
Esempio n. 17
0
 def test_ray_coincident_ray_3(self):
     r1 = ray.create([0.,0.,0.],[1.,0.,0.])
     r2 = ray.create([0.,1.,0.],[1.,0.,0.])
     self.assertTrue(False == gt.ray_coincident_ray(r1,r2))
Esempio n. 18
0
 def test_ray_coincident_ray(self):
     r1 = ray.create([0.,0.,0.],[1.,0.,0.])
     r2 = ray.create([1.,0.,0.],[2.,0.,0.])
     self.assertTrue(gt.ray_coincident_ray(r1,r2))
Esempio n. 19
0
 def test_ray_intersect_plane_front_only(self):
     r = ray.create([0.,-1.,0.],[0.,1.,0.])
     p = plane.create([0.,1.,0.], 0.)
     result = gt.ray_intersect_plane(r, p, front_only=True)
     self.assertEqual(result, None)
Esempio n. 20
0
 def test_ray_coincident_ray_3(self):
     r1 = ray.create([0.,0.,0.],[1.,0.,0.])
     r2 = ray.create([0.,1.,0.],[1.,0.,0.])
     self.assertTrue(False == gt.ray_coincident_ray(r1,r2))
Esempio n. 21
0
 def test_create_dtype(self):
     result = ray.create([0,0,0],[0,0,1], dtype=np.int)
     np.testing.assert_almost_equal(result, [[0,0,0],[0,0,1]], decimal=5)
     self.assertTrue(result.dtype == np.int)
Esempio n. 22
0
 def test_create_from_ray(self):
     r = ray.create([0.,0.,0.], [1., 0.,0.])
     result = line.create_from_ray(r)
     self.assertTrue(np.allclose(result, [[0,0,0],[1,0,0]]))