static double pick_query(Viewer *viewer, EventHandler *ehandler, const double ray_start[3], const double ray_dir[3])
{
RendererCar *self = (RendererCar*) ehandler->user;
botlcm_pose_t pose;
if (atrans_get_local_pose (self->atrans, &pose) < 0)
return -1;
double ray_start_body[3];
bot_vector_subtract_3d (ray_start, pose.pos, ray_start_body);
bot_quat_rotate_rev (pose.orientation, ray_start_body);
double ray_dir_body[3] = { ray_dir[0], ray_dir[1], ray_dir[2] };
bot_quat_rotate_rev (pose.orientation, ray_dir_body);
bot_vector_normalize_3d (ray_dir_body);
point3d_t car_pos_body = { 1.3, 0, 1 };
point3d_t box_size = { 4.6, 2, 1.4 };
double t = geom_ray_axis_aligned_box_intersect_3d (POINT3D(ray_start_body),
POINT3D (ray_dir_body), &car_pos_body, &box_size, NULL);
if (isfinite (t)) return t;
self->ehandler.hovering = 0;
return -1;
}
void
bot_trans_invert(BotTrans * btrans)
{
btrans->trans_vec[0] = -btrans->trans_vec[0];
btrans->trans_vec[1] = -btrans->trans_vec[1];
btrans->trans_vec[2] = -btrans->trans_vec[2];
bot_quat_rotate_rev(btrans->rot_quat, btrans->trans_vec);
btrans->rot_quat[1] = -btrans->rot_quat[1];
btrans->rot_quat[2] = -btrans->rot_quat[2];
btrans->rot_quat[3] = -btrans->rot_quat[3];
}