void bot_gl_multTrans(BotTrans * trans){
double trans_m[16];
bot_trans_get_mat_4x4(trans, trans_m);
// opengl expects column-major matrices
double trans_m_opengl[16];
bot_matrix_transpose_4x4d(trans_m, trans_m_opengl);
glMultMatrixd(trans_m_opengl);
}
// 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
car_draw (Viewer *viewer, Renderer *super)
{
RendererCar *self = (RendererCar*) super->user;
botlcm_pose_t pose;
if (!atrans_get_local_pose (self->atrans, &pose))
return;
BotGtkParamWidget *pw = self->pw;
int bling = self->chassis_model ?
bot_gtk_param_widget_get_bool (pw, PARAM_NAME_BLING) : 0;
int wheels = self->wheel_model ?
bot_gtk_param_widget_get_bool (pw, PARAM_NAME_WHEELS) : 0;
if ((bling || wheels) && !self->display_lists_ready) {
load_bling (self);
}
glColor4f(0,1,0,0.75);
glLineWidth (2);
glBegin(GL_LINE_STRIP);
glVertex3dv (self->last_pose.pos);
for (unsigned int i = 0;
i < MIN (bot_ptr_circular_size(self->path), self->max_draw_poses);
i++) {
glVertex3dv(bot_ptr_circular_index(self->path, i));
}
glEnd();
glPushMatrix();
// compute the rotation matrix to orient the vehicle in world
// coordinates
double body_quat_m[16];
bot_quat_pos_to_matrix(pose.orientation, pose.pos, body_quat_m);
// opengl expects column-major matrices
double body_quat_m_opengl[16];
bot_matrix_transpose_4x4d (body_quat_m, body_quat_m_opengl);
// rotate and translate the vehicle
glMultMatrixd (body_quat_m_opengl);
glEnable (GL_DEPTH_TEST);
if (bling && self->display_lists_ready && self->chassis_dl)
draw_chassis_model (self);
else
draw_footprint (self);
if (wheels && self->display_lists_ready && self->wheel_dl)
draw_wheels (self);
glPopMatrix();
if (self->display_detail) {
char buf[256];
switch (self->display_detail)
{
case DETAIL_SPEED:
sprintf(buf, "%.2f m/s",
sqrt(sq(pose.vel[0]) + sq(pose.vel[1]) + sq(pose.vel[2])));
break;
case DETAIL_RPY:
{
double rpy[3];
bot_quat_to_roll_pitch_yaw(pose.orientation, rpy);
sprintf(buf, "r: %6.2f\np: %6.2f\ny: %6.2f", to_degrees(rpy[0]), to_degrees(rpy[1]), to_degrees(rpy[2]));
break;
}
}
glColor3f(1,1,1);
bot_gl_draw_text(pose.pos, GLUT_BITMAP_HELVETICA_12, buf,
BOT_GL_DRAW_TEXT_DROP_SHADOW);
}
}
