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);
}
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_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);
}
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);
}
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);
}
}
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
}