static void update_gl_matrices(BotViewer *viewer, BotViewHandler *vhandler)
{
    BotDefaultViewHandler *dvh = (BotDefaultViewHandler*) vhandler->user;

    glGetIntegerv(GL_VIEWPORT, dvh->viewport);
    dvh->width = dvh->viewport[2];
    dvh->height = dvh->viewport[3];
    dvh->aspect_ratio = ((double) dvh->width) / dvh->height;

    glMatrixMode (GL_PROJECTION);
    glLoadIdentity ();

    if (dvh->projection_type == BOT_VIEW_ORTHOGRAPHIC) {
        double le[3];
        bot_vector_subtract_3d (dvh->eye, dvh->lookat, le);
        double dist = bot_vector_magnitude_3d (le) *
                      tan (dvh->fov_degrees * M_PI / 180 / 2);
        glOrtho (-dist*dvh->aspect_ratio, dist*dvh->aspect_ratio,
                 -dist, dist, -10*dist, EYE_MAX_DIST * 2);
    } else {
        gluPerspective (dvh->fov_degrees, dvh->aspect_ratio, 0.1, EYE_MAX_DIST * 2);
    }

    glGetDoublev(GL_PROJECTION_MATRIX, dvh->projection_matrix);

    glMatrixMode (GL_MODELVIEW);
    glLoadIdentity ();
    glMultMatrixd(dvh->model_matrix);
}
Beispiel #2
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;
}
Beispiel #3
0
void
bot_trans_set_from_velocities(BotTrans *dest, const double angular_rate[3],
    const double velocity[3], double dt)
{
  //see Frazolli notes, Aircraft Stability and Control, lecture 2, page 15
  if (dt == 0) {
    bot_trans_set_identity(dest);
    return;
  }

  double identity_quat[4] = { 1, 0, 0, 0 };
  double norm_angular_rate = bot_vector_magnitude_3d(angular_rate);

  if (norm_angular_rate == 0) {
    double delta[3];
    memcpy(delta, velocity, 3 * sizeof(double));
    bot_vector_scale_3d(delta, dt);
    bot_trans_set_from_quat_trans(dest, identity_quat, delta);
    return;
  }

  //"exponential of a twist: R=exp(skew(omega*t));
  //rescale vel, omega, t so ||omega||=1
  //trans =  (I-R)*(omega \cross v) + (omega \dot v) *omega* t
  //                term2                       term3
  // R*(omega \cross v)
  //     term1
  //rescale
  double t = dt * norm_angular_rate;
  double omega[3], vel[3];
  memcpy(omega, angular_rate, 3 * sizeof(double));
  memcpy(vel, velocity, 3 * sizeof(double));
  bot_vector_scale_3d(omega, 1.0/norm_angular_rate);
  bot_vector_scale_3d(vel, 1.0/norm_angular_rate);

  //compute R (quat in our case)
  bot_angle_axis_to_quat(t, omega, dest->rot_quat);

  //cross and dot products
  double omega_cross_vel[3];
  bot_vector_cross_3d(omega, vel, omega_cross_vel);
  double omega_dot_vel = bot_vector_dot_3d(omega, vel);


  double term1[3];
  double term2[3];
  double term3[3];

  //(I-R)*(omega \cross v) = term2
  memcpy(term1, omega_cross_vel, 3 * sizeof(double));
  bot_quat_rotate(dest->rot_quat, term1);
  bot_vector_subtract_3d(omega_cross_vel, term1, term2);

  //(omega \dot v) *omega* t
  memcpy(term3, omega, 3 * sizeof(double));
  bot_vector_scale_3d(term3, omega_dot_vel * t);

  bot_vector_add_3d(term2, term3, dest->trans_vec);
}
Beispiel #4
0
static void
on_pose(const lcm_recv_buf_t *rbuf, const char *channel, 
        const botlcm_pose_t *pose, void *user_data)
{
    RendererCar *self = (RendererCar*) user_data;
    ViewHandler *vhandler = self->viewer->view_handler;

    double lastpos[3] = {0,0,0};
    if (bot_ptr_circular_size(self->path))
        memcpy(lastpos, bot_ptr_circular_index(self->path, 0),
               3 * sizeof(double));

    double diff[3];
    bot_vector_subtract_3d(pose->pos, lastpos, diff);

    if (bot_vector_magnitude_3d(diff) > 2.0) {
        // clear the buffer if we jump
        bot_ptr_circular_clear(self->path);
    }

    if (bot_vector_magnitude_3d(diff) > 0.1 ||
            bot_ptr_circular_size(self->path)==0) {
        double *p = (double*) calloc(3, sizeof(double));
        memcpy(p, pose->pos, sizeof(double)*3);
        bot_ptr_circular_add(self->path, p);
    }

    if (vhandler && vhandler->update_follow_target && !self->teleport_car) {
        vhandler->update_follow_target(vhandler, pose->pos, pose->orientation);
    }

    if (!self->did_teleport)
        on_find_button(NULL, self);
    self->did_teleport = 1;

    int64_t dt = pose->utime - self->last_pose.utime;
    double r = bot_conf_get_double_or_default (self->config,
            "renderer_car.wheel_radius", 0.3);

    if (self->last_pose.utime) {
        int i;
        for (i = 0; i < 4; i++) {
            self->wheelpos[i] += self->wheelspeeds[i] * dt * 1e-6 / r;
            self->wheelpos[i] = bot_mod2pi (self->wheelpos[i]);
        }
    }
    memcpy (&self->last_pose, &pose, sizeof (botlcm_pose_t));

    viewer_request_redraw(self->viewer);
}
static void zoom_ratio(BotDefaultViewHandler *dvh, double ratio)
{
    double le[3];
    bot_vector_subtract_3d (dvh->eye, dvh->lookat, le);
    double eye_dist = bot_vector_magnitude_3d (le);

    eye_dist *= ratio;
    if (eye_dist < EYE_MIN_DIST) return;
    if (eye_dist > EYE_MAX_DIST) return;

    bot_vector_normalize_3d (le);
    bot_vector_scale_3d (le, eye_dist);

    bot_vector_add_3d (le, dvh->lookat, dvh->eye);

    look_at_changed(dvh);
}
// column-major (opengl compatible) order.  We need this because we
// need to be able to recompute the model matrix without requiring the
// correct GL context to be current.
static void look_at_to_matrix(const double eye[3], const double lookat[3], const double _up[3], double M[16])
{
    double up[3];
    memcpy(up, _up, 3 * sizeof(double));
    bot_vector_normalize_3d(up);

    double f[3];
    bot_vector_subtract_3d(lookat, eye, f);
    bot_vector_normalize_3d(f);

    double s[3], u[3];
    bot_vector_cross_3d(f, up, s);
    bot_vector_cross_3d(s, f, u);

    // row-major
    double R[16];
    memset(R, 0, sizeof(R));
    R[0] = s[0];
    R[1] = s[1];
    R[2] = s[2];
    R[4] = u[0];
    R[5] = u[1];
    R[6] = u[2];
    R[8] = -f[0];
    R[9] = -f[1];
    R[10] = -f[2];
    R[15] = 1;

    double T[16];
    memset(T, 0, sizeof(T));
    T[0] = 1;
    T[3] = -eye[0];
    T[5] = 1;
    T[7] = -eye[1];
    T[10] = 1;
    T[11] = -eye[2];
    T[15] = 1;

    double MT[16];
    bot_matrix_multiply_4x4_4x4(R, T, MT);
    bot_matrix_transpose_4x4d(MT, M);
}
Beispiel #7
0
static void on_find_button(GtkWidget *button, RendererCar *self)
{
    ViewHandler *vhandler = self->viewer->view_handler;

    double eye[3];
    double lookat[3];
    double up[3];

    vhandler->get_eye_look(vhandler, eye, lookat, up);
    double diff[3];
    bot_vector_subtract_3d(eye, lookat, diff);

    double pos[3] = { 0, 0, 0 };
    atrans_vehicle_pos_local(self->atrans, pos);

    bot_vector_add_3d(pos, diff, eye);

    vhandler->set_look_at(vhandler, eye, pos, up);

    viewer_request_redraw(self->viewer);
}
static void update_follow_target(BotViewHandler *vhandler, const double pos[3], const double quat[4])
{
    BotDefaultViewHandler *dvh = (BotDefaultViewHandler*) vhandler->user;

    if (dvh->have_last && (vhandler->follow_mode & BOT_FOLLOW_YAW)) {

        // compute the vectors from the vehicle to the lookat and eye
        // point and then project them given the new position of the car/
        double v2eye[3];
        bot_vector_subtract_3d(dvh->lastpos, dvh->eye, v2eye);
        double v2look[3];
        bot_vector_subtract_3d(dvh->lastpos, dvh->lookat, v2look);

        double vxy[3] = { 1, 0, 0 };
        bot_quat_rotate (quat, vxy);
        vxy[2] = 0;

        // where was the car pointing last time?
        double oxy[3] = { 1, 0, 0 };
        bot_quat_rotate (dvh->lastquat, oxy);
        oxy[2] = 0;
        double theta = bot_vector_angle_2d (oxy, vxy);

        double zaxis[3] = { 0, 0, 1 };
        double q[4];
        bot_angle_axis_to_quat (theta, zaxis, q);

        bot_quat_rotate(q, v2look);
        bot_vector_subtract_3d(pos, v2look, dvh->lookat);

        bot_quat_rotate(q, v2eye);
        bot_vector_subtract_3d(pos, v2eye, dvh->eye);
        bot_quat_rotate (q, dvh->up);

        // the above algorithm "builds in" a BOT_FOLLOW_POS behavior.

    } else if (dvh->have_last && (vhandler->follow_mode & BOT_FOLLOW_POS)) {

        double dpos[3];
        for (int i = 0; i < 3; i++)
            dpos[i] = pos[i] - dvh->lastpos[i];

        for (int i = 0; i < 3; i++) {
            dvh->eye[i] += dpos[i];
            dvh->lookat[i] += dpos[i];
        }
    } else {

        // when the follow target moves, we want rotations to still behave
        // correctly. Rotations use the lookat point as the center of rotation,
        // so adjust the lookat point so that it is on the same plane as the
        // follow target.

        double look_dir[3];
        bot_vector_subtract_3d(dvh->lookat, dvh->eye, look_dir);
        bot_vector_normalize_3d(look_dir);

        if (fabs(look_dir[2]) > 0.0001) {
            double dist = (dvh->lastpos[2] - dvh->lookat[2]) / look_dir[2];
            for (int i = 0; i < 3; i++)
                dvh->lookat[i] += dist * look_dir[i];
        }
    }

    look_at_changed(dvh);

    dvh->have_last = 1;
    memcpy(dvh->lastpos, pos, 3 * sizeof(double));
    memcpy(dvh->lastquat, quat, 4 * sizeof(double));
}
static int mouse_motion  (BotViewer *viewer, BotEventHandler *ehandler,
                          const double ray_start[3], const double ray_dir[3], const GdkEventMotion *event)
{
    BotDefaultViewHandler *dvh = (BotDefaultViewHandler*) ehandler->user;

    double dy = event->y - dvh->last_mouse_y;
    double dx = event->x - dvh->last_mouse_x;
    double look[3];
    bot_vector_subtract_3d (dvh->lookat, dvh->eye, look);

    if (event->state & GDK_BUTTON3_MASK) {
        double xy_len = bot_vector_magnitude_2d (look);
        double init_elevation = atan2 (look[2], xy_len);

        double left[3];
        bot_vector_cross_3d (dvh->up, look, left);
        bot_vector_normalize_3d (left);

        double delevation = -dy * 0.005;
        if (delevation + init_elevation < -M_PI/2) {
            delevation = -M_PI/2 - init_elevation;
        }
        if (delevation + init_elevation > M_PI/8) {
            delevation = M_PI/8 - init_elevation;
        }

        double q[4];
        bot_angle_axis_to_quat(-delevation, left, q);
        double newlook[3];
        memcpy (newlook, look, sizeof (newlook));
        bot_quat_rotate (q, newlook);

        double dazimuth = -dx * 0.01;
        double zaxis[] = { 0, 0, 1 };
        bot_angle_axis_to_quat (dazimuth, zaxis, q);
        bot_quat_rotate (q, newlook);
        bot_quat_rotate (q, left);

        bot_vector_subtract_3d (dvh->lookat, newlook, dvh->eye);
        bot_vector_cross_3d (newlook, left, dvh->up);
        look_at_changed(dvh);
    }
    else if (event->state & GDK_BUTTON2_MASK) {
        double init_eye_dist = bot_vector_magnitude_3d (look);

        double ded = pow (10, dy * 0.01);
        double eye_dist = init_eye_dist * ded;
        if (eye_dist > EYE_MAX_DIST)
            eye_dist = EYE_MAX_DIST;
        else if (eye_dist < EYE_MIN_DIST)
            eye_dist = EYE_MIN_DIST;

        double le[3];
        memcpy (le, look, sizeof (le));
        bot_vector_normalize_3d (le);
        bot_vector_scale_3d (le, eye_dist);

        bot_vector_subtract_3d (dvh->lookat, le, dvh->eye);
        look_at_changed(dvh);
    }
    else if (event->state & GDK_BUTTON1_MASK) {

        double dx = event->x - dvh->last_mouse_x;
        double dy = event->y - dvh->last_mouse_y;

        window_space_pan(dvh, dvh->manipulation_point, dx, dy, 1);
    }
    dvh->last_mouse_x = event->x;
    dvh->last_mouse_y = event->y;

    bot_viewer_request_redraw(viewer);
    return 1;
}
/** Given a coordinate in scene coordinates dq, modify the camera
	such that the screen projection of point dq moves (x,y) pixels.
**/
static void window_space_pan(BotDefaultViewHandler *dvh, double dq[], double x, double y, int preserveZ)
{
    double orig_eye[3], orig_lookat[3];
    memcpy(orig_eye, dvh->eye, 3 * sizeof(double));
    memcpy(orig_lookat, dvh->lookat, 3 * sizeof(double));

    y *= -1;   // y is upside down...

    double left[3], up[3];

    // compute left and up vectors
    if (!preserveZ) {
        // constraint translation such that the distance between the
        // eye and the lookat does not change; i.e., that the motion
        // is upwards and leftwards only.
        double look_vector[3];
        bot_vector_subtract_3d(dvh->lookat, dvh->eye, look_vector);
        bot_vector_normalize_3d(look_vector);
        bot_vector_cross_3d(dvh->up, look_vector, left);

        memcpy(up, dvh->up, 3 * sizeof(double));
    } else {
        // abuse the left and up vectors: change them to xhat, yhat... this ensures
        // that pan does not affect the camera Z height.
        left[0] = 1;
        left[1] = 0;
        left[2] = 0;
        up[0] = 0;
        up[1] = 1;
        up[2] = 0;
    }

    double A[4];
    double B[2] = { x, y };

    build_pan_jacobian(dvh, dq, up, left, A);
    double detOriginal = A[0]*A[3] - A[1]*A[2];

    // Ax = b, where x = how much we should move in the up and left directions.
    double Ainverse[4] = { 0, 0, 0, 0 };
    bot_matrix_inverse_2x2d(A, Ainverse);

    double sol[2];
    bot_matrix_vector_multiply_2x2_2d(Ainverse, B, sol);

    double motionup[3], motionleft[3], motion[3];
    memcpy(motionup, up, 3 * sizeof(double));
    bot_vector_scale_3d(motionup, sol[0]);

    memcpy(motionleft, left, 3 * sizeof(double));
    bot_vector_scale_3d(motionleft, sol[1]);

    bot_vector_add_3d(motionup, motionleft, motion);


    double magnitude = bot_vector_magnitude_3d(motion);
    double new_magnitude = fmax(fmin(magnitude,MAX_MOTION_MAGNITUDE),MIN_MOTION_MAGNITUDE);
    //bot_vector_normalize_3d(motion); // if magnitude is zero it will return nan's
    bot_vector_scale_3d(motion,new_magnitude/fmax(magnitude,MIN_MOTION_MAGNITUDE));

    double neweye[3], newlookat[3];
    bot_vector_subtract_3d(dvh->eye, motion, neweye);
    bot_vector_subtract_3d(dvh->lookat, motion, newlookat);


    memcpy(dvh->eye, neweye, sizeof(double)*3);
    memcpy(dvh->lookat, newlookat, sizeof(double)*3);

    look_at_changed(dvh);
    build_pan_jacobian(dvh, dq, up, left, A);
    // if the projection is getting sketchy, and it's getting worse,
    // then just reject it.  this is better than letting the
    // projection become singular! (This only happens with preserveZ?)
    double detNew = A[0]*A[3] - A[1]*A[2];
    //printf(" %15f %15f\n", detOriginal, detNew);
    //if (fabs(detNew) < 0.01 && fabs(detNew) <= fabs(detOriginal)) {
    if ((fabs(detNew) < 25 )||(fabs(detOriginal) < 25 )) {
        memcpy(dvh->eye, orig_eye, 3 * sizeof(double));
        memcpy(dvh->lookat, orig_lookat, 3 * sizeof(double));
        look_at_changed(dvh);
        printf("skipping pan: %15f %15f\n", detOriginal, detNew);
    }


}
Beispiel #11
0
void
bot_rwx_model_gl_draw( BotRwxModel *model ) 
{
    GList *citer;

    double a[3], b[3];
    double n[3];

    float color[4];
#if 0
    float minv[3] = {INFINITY, INFINITY, INFINITY };
    float maxv[3] = {-INFINITY, -INFINITY, -INFINITY };
#endif

    for( citer = model->clumps; citer != NULL; citer = citer->next ) {
        BotRwxClump *clump = (BotRwxClump*)citer->data;
        int i;

        /*glColor4f( clump->color[0], clump->color[1], clump->color[2],
          clump->opacity ); */

        // ambient
        if (clump->ambient < .5)
            clump->ambient = 0.5;

        color[0] = clump->color[0] * clump->ambient;
        color[1] = clump->color[1] * clump->ambient;
        color[2] = clump->color[2] * clump->ambient;
        color[3] = 1;
        glMaterialfv( GL_FRONT, GL_AMBIENT, color );

        // diffuse
        //	clump->diffuse = 1.0;
        color[0] = clump->color[0] * clump->diffuse;
        color[1] = clump->color[1] * clump->diffuse;
        color[2] = clump->color[2] * clump->diffuse;
        color[3] = clump->opacity;
        glMaterialfv( GL_FRONT, GL_DIFFUSE, color );

        // emission
        color[0] = color[1] = color[2] = 0;
        color[3] = 1;
        glMaterialfv( GL_FRONT, GL_EMISSION, color );

        // specular
        color[0] = color[1] = color[2] = clump->specular;
        color[3] = 1;
        glMaterialfv( GL_FRONT, GL_SPECULAR, color );

        // XXX hard-code shininess for lack of information
        glMateriali( GL_FRONT, GL_SHININESS, 20 );

        glBegin( GL_TRIANGLES );

        // For every single vertex, average out the normal vectors for every
        // triangle that the vertex participates in.  Set that averaged vector
        // as the normal vector for that vertex.  This results in a much
        // smoother rendered model than the simple way (which is to just have
        // a single normal vector for all three vertices of a triangle when
        // the triangle is drawn).
        int *vertex_counts = (int*)calloc(1,clump->nvertices*sizeof(int));
        double *normals = (double*)calloc(1,clump->nvertices*sizeof(double)*3);

#if 0
        for (i = 1; i < clump->nvertices; i++) {
            BotRwxVertex * v = clump->vertices + i;
            int j;
            for (j = 0; j < 3; j++) {
                if (v->pos[j] < minv[j])
                    minv[j] = v->pos[j];
                if (v->pos[j] > maxv[j])
                    maxv[j] = v->pos[j];
            }
        }
#endif

        // account for the normal vector of every triangle.
        for( i=1; i<clump->ntriangles; i++ ) {
            // find the vertex indices
            int vid1, vid2, vid3;
            vid1 = clump->triangles[i].vertices[0];
            vid2 = clump->triangles[i].vertices[1];
            vid3 = clump->triangles[i].vertices[2];
            // load the vertices
            BotRwxVertex *v1, *v2, *v3;
            v1 = &clump->vertices[vid1];
            v2 = &clump->vertices[vid2];
            v3 = &clump->vertices[vid3];
            // compute and average in the normal
            bot_vector_subtract_3d( v2->pos, v1->pos, a );
            bot_vector_subtract_3d( v3->pos, v1->pos, b );
            bot_vector_cross_3d( a, b, n );
            double nmag = sqrt( n[0]*n[0] + n[1]*n[1] + n[2]*n[2] );
            n[0] /= nmag;
            n[1] /= nmag;
            n[2] /= nmag;

            vertex_counts[vid1]++;
            vertex_counts[vid2]++;
            vertex_counts[vid3]++;

            normals[vid1*3 + 0] += n[0];
            normals[vid1*3 + 1] += n[1];
            normals[vid1*3 + 2] += n[2];
            normals[vid2*3 + 0] += n[0];
            normals[vid2*3 + 1] += n[1];
            normals[vid2*3 + 2] += n[2];
            normals[vid3*3 + 0] += n[0];
            normals[vid3*3 + 1] += n[1];
            normals[vid3*3 + 2] += n[2];
        }

        // scale the resulting vectors to be of unit length
        for( i=1; i<clump->nvertices; i++ ) {
            normals[i*3 + 0] /= vertex_counts[i];
            normals[i*3 + 1] /= vertex_counts[i];
            normals[i*3 + 2] /= vertex_counts[i];

            // re-normalize, because averaging out unit-length vectors doesn't
            // always result in a unit-length vector.
            double *n = normals + i*3;
            double nmag = sqrt( n[0]*n[0] + n[1]*n[1] + n[2]*n[2] );
            n[0] /= nmag;
            n[1] /= nmag;
            n[2] /= nmag;
        }

        free( vertex_counts );

        for( i=0; i<clump->ntriangles; i++ ) {
            // find the vertex indices
            int vid1, vid2, vid3;
            vid1 = clump->triangles[i].vertices[0];
            vid2 = clump->triangles[i].vertices[1];
            vid3 = clump->triangles[i].vertices[2];

            // load the vertices
            BotRwxVertex *v1, *v2, *v3;
            v1 = &clump->vertices[vid1];
            v2 = &clump->vertices[vid2];
            v3 = &clump->vertices[vid3];

#if 0
            // compute and set the normal
            vector_subtract_3d( v2->pos, v1->pos, a );
            vector_subtract_3d( v3->pos, v1->pos, b );
            vector_cross_3d( a, b, n );
            double nmag = sqrt( n[0]*n[0] + n[1]*n[1] + n[2]*n[2] );
            n[0] /= nmag;
            n[1] /= nmag;
            n[2] /= nmag;
            glNormal3d( n[0], n[1], n[2] );
#endif

            // render the triangle
            glNormal3d( normals[vid1*3+ 0], 
                    normals[vid1*3 + 1],
                    normals[vid1*3 + 2]);
            glVertex3f( v1->pos[0], v1->pos[1], v1->pos[2] );
            glNormal3d( normals[vid2*3+ 0], 
                    normals[vid2*3 + 1],
                    normals[vid2*3 + 2]);
            glVertex3f( v2->pos[0], v2->pos[1], v2->pos[2] );
            glNormal3d( normals[vid3*3 + 0], 
                    normals[vid3*3 + 1],
                    normals[vid3*3 + 2]);
            glVertex3f( v3->pos[0], v3->pos[1], v3->pos[2] );
        }

        free( normals );
        glEnd();
    }

#if 0
    printf ("max-min %f %f %f\n", maxv[0] - minv[0], maxv[1] - minv[1],
            maxv[2] - minv[2]);
    printf ("min %f %f %f\n", minv[0], minv[1], minv[2]);
#endif
    
#if 0
    glLineWidth( 4.0 );
    glBegin( GL_LINES );
    glColor4f( 1, 1, 1, 0.5 );

    for( citer = model->clumps; citer != NULL; citer = citer->next ) {
        BotRwxClump *clump = (BotRwxClump*)citer->data;
        int i;
        for( i=0; i<clump->ntriangles; i++ ) {
            // find the vertex indices
            int vid1, vid2, vid3;
            vid1 = clump->triangles[i].vertices[0];
            vid2 = clump->triangles[i].vertices[1];
            vid3 = clump->triangles[i].vertices[2];

            // load the vertices
            BotRwxVertex *v1, *v2, *v3;
            v1 = &clump->vertices[vid1];
            v2 = &clump->vertices[vid2];
            v3 = &clump->vertices[vid3];

            // compute and set the normal
            vector_subtract_3d( v2->pos, v1->pos, a );
            vector_subtract_3d( v3->pos, v1->pos, b );
            vector_cross_3d( a, b, n );
            double nmag = sqrt( n[0]*n[0] + n[1]*n[1] + n[2]*n[2] );
            n[0] /= nmag;
            n[1] /= nmag;
            n[2] /= nmag;

            n[0] *= 5;
            n[1] *= 5;
            n[2] *= 5;

            // midpoint of the triangle
            double m[3];
            m[0] = (v1->pos[0] + v2->pos[0] + v3->pos[0]) / 3;
            m[1] = (v1->pos[1] + v2->pos[1] + v3->pos[1]) / 3;
            m[2] = (v1->pos[2] + v2->pos[2] + v3->pos[2]) / 3;

            // render the normal vector
            glVertex3f( m[0], m[1], m[2] );
            glVertex3f( m[0] + n[0], m[1] + n[1], m[2] + n[2] );
        }
    }
    glEnd();
#endif
}