Exemplo n.º 1
0
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;
}
Exemplo n.º 2
0
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];
}