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