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)
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)
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))
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)
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)
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.]))
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]]))
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)
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.]))
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)
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 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)
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)
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)
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))
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))
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))
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))
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)
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))
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)
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]]))