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