void init_OpenGL(){
glMatrixMode( GL_PROJECTION );
glLoadIdentity( );
gluPerspective(50,(double)SCREEN_WIDTH/SCREEN_HEIGHT,0.1,1000);
t3dInit();
ship_color.x=100;ship_color.y=0;ship_color.z=0;
}
int InitGL() // All Setup For OpenGL goes here
{
glShadeModel(GL_SMOOTH); // Enables Smooth Shading
glClearColor(0.0, 0.0, 0.0, 1.0);
glClearDepth(1.0f); // Depth Buffer Setup
glEnable(GL_DEPTH_TEST); // Enables Depth Testing
glDepthFunc(GL_LEQUAL); // The Type Of Depth Test To Do
glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST); // Really Nice Perspective Calculation
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0); // Uses default lighting parameters
glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, GL_TRUE);
glEnable(GL_NORMALIZE);
glLightfv(GL_LIGHT1, GL_AMBIENT, LightAmbient);
glLightfv(GL_LIGHT1, GL_DIFFUSE, LightDiffuse);
glLightfv(GL_LIGHT1, GL_POSITION, LightPosition);
glEnable(GL_LIGHT1);
glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);
// and now some init I need to do for other stuff
ilInit(); /* Initialization of DevIL */
// mouse stuff
pointer = toGLTexture("./dati/models/textures/mirino.png");
lastx = middleX;
lasty = middleY;
relativeX = 0.0;
relativeY = 0.0;
glutWarpPointer(middleX, middleY); // put the cursor in the middle of the screen
//for the menu
InitMenu();
// for the text
t3dInit();
// for the skybox
initSkybox();
// for the blend
initAssets();
// for the sounds
if (!sound_hit.loadFromFile("./dati/audio/hit.wav")) return -1;
if (!sound_miss.loadFromFile("./dati/audio/miss.wav")) return -1;
if (!sound_youreempty.loadFromFile("./dati/audio/youreempty.wav")) return -1;
if (!sound_ohno.loadFromFile("./dati/audio/ohno.wav")) return -1;
music.setLoop(true);
if (!music.openFromFile("./dati/audio/Rango_Theme.ogg")) return -1;
music.play();
return TRUE; // Initialization Went OK
}
int
main (int argc, char **argv)
{
RunAllTests();
GtkWidget *window;
GtkWidget *da;
GdkGLConfig *glconfig;
gtk_init (&argc, &argv);
gtk_gl_init (&argc, &argv);
window = gtk_window_new (GTK_WINDOW_TOPLEVEL);
gtk_window_set_default_size (GTK_WINDOW (window), 800, 600);
da = gtk_drawing_area_new ();
gtk_container_add (GTK_CONTAINER (window), da);
g_signal_connect_swapped (window, "destroy",
G_CALLBACK (gtk_main_quit), NULL);
gtk_widget_set_events (da, GDK_EXPOSURE_MASK);
gtk_widget_show (window);
/* prepare GL */
glconfig = gdk_gl_config_new_by_mode ((GdkGLConfigMode)
(GDK_GL_MODE_RGB |
GDK_GL_MODE_DEPTH |
GDK_GL_MODE_DOUBLE));
if (!glconfig)
{
g_assert_not_reached ();
}
if (!gtk_widget_set_gl_capability (da, glconfig, NULL, TRUE,
GDK_GL_RGBA_TYPE))
{
g_assert_not_reached ();
}
g_signal_connect (da, "configure-event",
G_CALLBACK (configure), NULL);
g_signal_connect (da, "expose-event",
G_CALLBACK (expose), NULL);
gtk_widget_show_all (window);
g_timeout_add (1000 / 30, rotate, da);
t3dInit();
gtk_main ();
}
static PyObject*
Velodyne_post(Velodyne* self, PyObject* args)
{
VelodyneSimu* info;
PyObject* data;
Py_buffer buf;
size_t i;
if (!PyArg_ParseTuple(args, "OO", &info, &data))
return NULL;
velodyne3DImage* im3d = &self->img;
PyObject_GetBuffer(data, &buf, PyBUF_SIMPLE);
float* p = (float*)buf.buf;
if (self->current_rot == 0) {
// clean the whole point array
memset(im3d->points, 0, sizeof(im3d->points));
// store the position
POM_EULER* local_stm_euler = & (im3d->position.sensorToMain.euler);
local_stm_euler->yaw = info->yaw_cam;
local_stm_euler->pitch = info->pitch_cam;
local_stm_euler->roll = info->roll_cam;
local_stm_euler->x = info->x_cam;
local_stm_euler->y = info->y_cam;
local_stm_euler->z = info->z_cam;
// Fill in the Main to Origin
POM_EULER* local_mto_euler = &(im3d->position.mainToOrigin.euler);
local_mto_euler->yaw = info->yaw_rob;
local_mto_euler->pitch = info->pitch_rob;
local_mto_euler->roll = info->roll_rob;
local_mto_euler->x = info->x_rob;
local_mto_euler->y = info->y_rob;
local_mto_euler->z = info->z_rob;
memcpy( &im3d->position.mainToBase.euler,
&im3d->position.mainToOrigin.euler, sizeof(POM_EULER));
im3d->position.date = info->pom_tag;
// fill the poster
velodyne3DPoint* points = im3d->points;
for (i = 0; i < info->nb_pts; i++)
{
points->coordinates[0] = *p;
points->coordinates[1] = *(p+1);
points->coordinates[2] = *(p+2);
if (points->coordinates[0] == 0.0 &&
points->coordinates[1] == 0.0 &&
points->coordinates[2] == 0.0)
points->status = VELODYNE_BAD_3DPOINT;
else
points->status = VELODYNE_GOOD_3DPOINT;
points++;
p += 3;
}
for (;i < self->size; i++, points++)
points->status = VELODYNE_BAD_3DPOINT;
} else {
// fill the poster, but we need to transform the position of each point
// in the frame of the first scan
T3D sensor2ToMain2;
T3D main2ToOrigin;
T3D sensor2ToOrigin;
T3D sensor1ToMain1;
T3D main1ToOrigin;
T3D sensor1ToOrigin;
T3D originToSensor1;
T3D sensor2ToSensor1;
t3dInit(& sensor2ToMain2, T3D_BRYAN, T3D_ALLOW_CONVERSION);
t3dInit(& main2ToOrigin, T3D_BRYAN, T3D_ALLOW_CONVERSION);
t3dInit(& sensor2ToOrigin, T3D_BRYAN, T3D_ALLOW_CONVERSION);
t3dInit(& sensor1ToMain1, T3D_BRYAN, T3D_ALLOW_CONVERSION);
t3dInit(& main1ToOrigin, T3D_BRYAN, T3D_ALLOW_CONVERSION);
t3dInit(& sensor1ToOrigin, T3D_BRYAN, T3D_ALLOW_CONVERSION);
t3dInit(& originToSensor1, T3D_BRYAN, T3D_ALLOW_CONVERSION);
t3dInit(& sensor2ToSensor1, T3D_BRYAN, T3D_ALLOW_CONVERSION);
sensor2ToMain2.euler.euler[0] = info->yaw_cam;
sensor2ToMain2.euler.euler[1] = info->pitch_cam;
sensor2ToMain2.euler.euler[2] = info->roll_cam;
sensor2ToMain2.euler.euler[3] = info->x_cam;
sensor2ToMain2.euler.euler[4] = info->y_cam;
sensor2ToMain2.euler.euler[5] = info->z_cam;
main2ToOrigin.euler.euler[0] = info->yaw_rob;
main2ToOrigin.euler.euler[1] = info->pitch_rob;
main2ToOrigin.euler.euler[2] = info->roll_rob;
main2ToOrigin.euler.euler[3] = info->x_rob;
main2ToOrigin.euler.euler[4] = info->y_rob;
main2ToOrigin.euler.euler[5] = info->z_rob;
main1ToOrigin.euler.euler[0] = self->img.position.mainToOrigin.euler.yaw;
main1ToOrigin.euler.euler[1] = self->img.position.mainToOrigin.euler.pitch;
main1ToOrigin.euler.euler[2] = self->img.position.mainToOrigin.euler.roll;
main1ToOrigin.euler.euler[3] = self->img.position.mainToOrigin.euler.x;
main1ToOrigin.euler.euler[4] = self->img.position.mainToOrigin.euler.y;
main1ToOrigin.euler.euler[5] = self->img.position.mainToOrigin.euler.z;
sensor1ToMain1.euler.euler[0] = self->img.position.sensorToMain.euler.yaw;
sensor1ToMain1.euler.euler[1] = self->img.position.sensorToMain.euler.pitch;
sensor1ToMain1.euler.euler[2] = self->img.position.sensorToMain.euler.roll;
sensor1ToMain1.euler.euler[3] = self->img.position.sensorToMain.euler.x;
sensor1ToMain1.euler.euler[4] = self->img.position.sensorToMain.euler.y;
sensor1ToMain1.euler.euler[5] = self->img.position.sensorToMain.euler.z;
t3dCompIn (&sensor2ToOrigin, &sensor2ToMain2, &main2ToOrigin);
t3dCompIn (&sensor1ToOrigin, &sensor1ToMain1, &main1ToOrigin);
t3dInvertIn (&originToSensor1, &sensor1ToOrigin);
t3dCompIn (&sensor2ToSensor1, &sensor2ToOrigin, &originToSensor1);
if (!t3dOk (&sensor2ToSensor1)) {
fprintf (stderr, "Failed to compute transformation\n");
Py_RETURN_NONE;
}
velodyne3DPoint* points = im3d->points + self->current_rot * self->size;
for (i = 0; i < info->nb_pts; i++)
{
double *c;
double x = *p;
double y = *(p+1);
double z = *(p+2);
if (x == 0.0 && y == 0.0 && z == 0.0) {
points->status = VELODYNE_BAD_3DPOINT;
} else {
points->status = VELODYNE_GOOD_3DPOINT;
c = sensor2ToSensor1.matrix.matrix;
// matrix mulplication inlined
points->coordinates[0]
= (float) (x * *c + y * *(c + 1) + z * *(c + 2) + *(c + 3));
c += 4;
points->coordinates[1]
= (float) (x * *c + y * *(c + 1) + z * *(c + 2) + *(c + 3));
c += 4;
points->coordinates[2]
= (float) (x * *c + y * *(c + 1) + z * *(c + 2) + *(c + 3));
}
points++;
p += 3;
}
for (;i < self->size; i++, points++)
points->status = VELODYNE_BAD_3DPOINT;
}
PyBuffer_Release(&buf);
self->current_rot = (self->current_rot + 1) % self->nb_rot;
// we have do one turn, just fill the poster
if (self->current_rot == 0) {
velodyne3DImage* p_im3d = posterAddr(self->id);
posterTake(self->id, POSTER_WRITE);
memcpy(p_im3d, im3d, sizeof(*im3d));
posterGive(self->id);
}
Py_RETURN_NONE;
}
