static double pick_query(Viewer *viewer, EventHandler *ehandler, const double ray_start[3], const double ray_dir[3])
{
RendererCar *self = (RendererCar*) ehandler->user;
lcmtypes_pose_t pose;
if (ctrans_local_pose (self->ctrans, &pose) < 0)
return -1;
double ray_start_body[3];
vector_subtract_3d (ray_start, pose.pos, ray_start_body);
rot_quat_rotate_rev (pose.orientation, ray_start_body);
double ray_dir_body[3] = { ray_dir[0], ray_dir[1], ray_dir[2] };
rot_quat_rotate_rev (pose.orientation, ray_dir_body);
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;
}
static int
pose_listener (CTrans * ctrans, ctrans_update_type_t type, void *user)
{
if (type != CTRANS_POSE_UPDATE)
return 0;
RendererCar *self = (RendererCar*) user;
ViewHandler *vhandler = self->viewer->view_handler;
lcmtypes_pose_t pose;
ctrans_local_pose (self->ctrans, &pose);
double lastpos[3] = {0,0,0};
if (gu_ptr_circular_size(self->path))
memcpy(lastpos, gu_ptr_circular_index(self->path, 0),
3 * sizeof(double));
double diff[3];
vector_subtract_3d(pose.pos, lastpos, diff);
if (vector_magnitude_3d(diff) > 2.0) {
// clear the buffer if we jump
gu_ptr_circular_clear(self->path);
}
if (vector_magnitude_3d(diff) > 0.1 ||
gu_ptr_circular_size(self->path)==0) {
double *p = (double*) calloc(3, sizeof(double));
memcpy(p, pose.pos, sizeof(double)*3);
gu_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 = config_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] = mod2pi (self->wheelpos[i]);
}
}
memcpy (&self->last_pose, &pose, sizeof (lcmtypes_pose_t));
viewer_request_redraw(self->viewer);
return 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];
vector_subtract_3d(eye, lookat, diff);
double pos[3] = { 0, 0, 0 };
ctrans_local_pos (self->ctrans, pos);
vector_add_3d(pos, diff, eye);
vhandler->set_look_at(vhandler, eye, pos, up);
viewer_request_redraw(self->viewer);
}
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
}
