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