examples/openGL/models3D.cpp

00001 /*
00002  *      Copyright (C) 2007. PARP Research Group.
00003  *      <http://perception.inf.um.es>
00004  *      University of Murcia, Spain.
00005  *
00006  *      This file is part of the QVision library.
00007  *
00008  *      QVision is free software: you can redistribute it and/or modify
00009  *      it under the terms of the GNU Lesser General Public License as
00010  *      published by the Free Software Foundation, version 3 of the License.
00011  *
00012  *      QVision is distributed in the hope that it will be useful,
00013  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  *      GNU Lesser General Public License for more details.
00016  *
00017  *      You should have received a copy of the GNU Lesser General Public
00018  *      License along with QVision. If not, see <http://www.gnu.org/licenses/>.
00019  */
00020 
00024 
00025 #include <GL/gl.h>
00026 #include <GL/glu.h>
00027 #include <math.h>
00028 
00029 #include <models3D.h>
00030 
00031 #define CIRCLE_NUM_SECTORS  9 /* Nmero de sectores por c�culo. */
00032 #define PI                  3.141593
00033 #define SQR3DIV2            0.866025
00034 /* Colores b�icos: */
00035 #define COLOR_PARED            220,220,220
00036 #define COLOR_PUERTA           0,200,0
00037 #define COLOR_MARCO            200,200,0
00038 #define COLOR_MANIVELA         255,255,0
00039 #define COLOR_RODAPIE          255,100,100
00040 #define COLOR_SUELO            200,80,80
00041 #define COLOR_CUERPO_ROBOT     255,0,0
00042 #define COLOR_TAPA_ROBOT       10,10,10
00043 #define COLOR_RUEDAS_ROBOT     20,20,20
00044 #define COLOR_TRIPODE_ROBOT    100,100,100
00045 #define COLOR_CAMARA           0,0,0
00046 
00047 /* (Unidades en cm): */
00048 /* Pasillo: */
00049 #define ALTO_PARED    270 
00050 #define ALTO_RODAPIE  20 
00051 
00052 /* Puertas: */
00053 //Ahora es variable#define ANCHO_PUERTA      80  /* Sin contar el marco */ 
00054 #define ALTO_PUERTA       210 /* Sin contar el marco */
00055 #define ANCHO_MARCO       10
00056 #define RADIO_MANIVELA    6
00057 
00058 /* Robot: */
00059 #define RADIO_ROBOT            25
00060 #define RADIO_TAPA_ROBOT       28
00061 #define ALTO_ROBOT             20 /* Del cuerpo, sin contar elevaci�. */
00062 #define ELEVACION_ROBOT         5 /* Elevaci� sobre el suelo. */
00063 #define ALTO_TRIPODE           70 /* Sin contar elevaci� ni cuerpo. */
00064 #define RADIO_INT_RUEDA_ROBOT   5 
00065 #define RADIO_EXT_RUEDA_ROBOT  10 
00066 #define CAM_DEPTH              15 /* Medidas de la c�ara. */
00067 #define CAM_WIDTH               5
00068 #define CAM_HEIGHT              8
00069 
00070 void puerta3D_internal(float ancho)
00071 {
00072     int i;
00073     float angle;
00074     float ancho_puerta=ancho-2*ANCHO_MARCO;
00075     
00076     
00077     glBegin(GL_QUADS); 
00078       /* 3 rect�gulos para el marco: */    
00079       glColor3ub(COLOR_MARCO);
00080 
00081       /* Marco izquierdo: */
00082       glVertex3f(0,0,0);
00083       glVertex3f(0,0,ALTO_PUERTA);
00084       glVertex3f(ANCHO_MARCO,0,ALTO_PUERTA);
00085       glVertex3f(ANCHO_MARCO,0,0);
00086 
00087       /* Marco derecho: */
00088       glVertex3f(ancho_puerta+ANCHO_MARCO,0,0);
00089       glVertex3f(ancho_puerta+ANCHO_MARCO,0,ALTO_PUERTA);
00090       glVertex3f(ancho_puerta+2*ANCHO_MARCO,0,ALTO_PUERTA);
00091       glVertex3f(ancho_puerta+2*ANCHO_MARCO,0,0);
00092      
00093       /* Marco superior: */
00094       glVertex3f(0,0,ALTO_PUERTA);
00095       glVertex3f(0,0,ALTO_PUERTA+ANCHO_MARCO);
00096       glVertex3f(ancho_puerta+2*ANCHO_MARCO,0,ALTO_PUERTA+ANCHO_MARCO);
00097       glVertex3f(ancho_puerta+2*ANCHO_MARCO,0,ALTO_PUERTA);
00098       
00099       /* Puerta: */
00100       glColor3ub(COLOR_PUERTA);
00101 
00102       glVertex3f(ANCHO_MARCO,0,0);
00103       glVertex3f(ANCHO_MARCO,0,ALTO_PUERTA);
00104       glVertex3f(ANCHO_MARCO+ancho_puerta,0,ALTO_PUERTA);
00105       glVertex3f(ANCHO_MARCO+ancho_puerta,0,0);
00106       
00107       /* Cuadrado de pared encima de la puerta: */
00108       glColor3ub(COLOR_PARED);
00109 
00110       glVertex3f(0,0,ALTO_PUERTA+ANCHO_MARCO);
00111       glVertex3f(0,0,ALTO_PARED);
00112       glVertex3f(2*ANCHO_MARCO+ancho_puerta,0,ALTO_PARED);
00113       glVertex3f(2*ANCHO_MARCO+ancho_puerta,0,ALTO_PUERTA+ANCHO_MARCO);
00114 
00115     glEnd();
00116 
00117     
00118     glBegin(GL_TRIANGLE_FAN);
00119       /* Manivela: */
00120       glColor3ub(COLOR_MANIVELA);
00121 
00122       glVertex3f(ANCHO_MARCO+2*RADIO_MANIVELA,-1,ALTO_PUERTA/2);
00123       for(i=0;i<=CIRCLE_NUM_SECTORS;i++) {
00124           angle = i*2*PI/CIRCLE_NUM_SECTORS;
00125           glVertex3f(ANCHO_MARCO+2*RADIO_MANIVELA+RADIO_MANIVELA*cos(angle),
00126                      -1,
00127                      ALTO_PUERTA/2+RADIO_MANIVELA*sin(angle));
00128       }
00129     glEnd();       
00130 
00131 
00132     glBegin(GL_TRIANGLE_FAN);
00133       /* Manivela (otra cara): */
00134       glColor3ub(COLOR_MANIVELA);
00135 
00136       glVertex3f(ANCHO_MARCO+2*RADIO_MANIVELA,1,ALTO_PUERTA/2);
00137       for(i=0;i<=CIRCLE_NUM_SECTORS;i++) {
00138           angle = i*2*PI/CIRCLE_NUM_SECTORS;
00139           glVertex3f(ANCHO_MARCO+2*RADIO_MANIVELA+RADIO_MANIVELA*cos(angle),
00140                      1,
00141                      ALTO_PUERTA/2+RADIO_MANIVELA*sin(angle));
00142       }
00143     glEnd();       
00144 
00145 }
00146 
00147 
00148 void puerta3D(float x1,float y1,float x2,float y2)
00149 {
00150     float angle,posx,posy,ancho;
00151     
00152     angle = atan2(y2-y1,x2-x1);
00153     posx = (x1+x2)/2;
00154     posy = (y1+y2)/2;
00155     ancho = sqrt((y2-y1)*(y2-y1)+(x2-x1)*(x2-x1));
00156 
00157     /*glPushMatrix();
00158     glRotatef(angle*180/PI,0,0,1);
00159     glTranslatef(posx*cos(angle)+posy*sin(angle),
00160                  -posx*sin(angle)+posy*cos(angle),0);
00161     puerta3D_internal(ancho); */
00162 
00163     glPushMatrix();
00164     //glTranslatef(-posx,0,0);
00165     glRotatef(angle*180/PI,0,0,1);
00166     glTranslatef(x1*cos(angle)+y1*sin(angle),
00167                  -x1*sin(angle)+y1*cos(angle),0);
00168     puerta3D_internal(ancho);
00169 
00170 
00171     glPopMatrix();
00172 }
00173 
00174 
00175 void pared3D(float x1,float y1,float x2,float y2)
00176 {
00177     glBegin(GL_QUADS);       
00178       /* Cuadrado de pared encima del rodapie: */
00179       glColor3ub(COLOR_PARED);
00180 
00181       glVertex3f(x1,y1,ALTO_RODAPIE);
00182       glVertex3f(x1,y1,ALTO_PARED);
00183       glVertex3f(x2,y2,ALTO_PARED);
00184       glVertex3f(x2,y2,ALTO_RODAPIE);
00185 
00186       /* Rodapie: */
00187       glColor3ub(COLOR_RODAPIE);
00188 
00189       glVertex3f(x1,y1,0);
00190       glVertex3f(x1,y1,ALTO_RODAPIE);
00191       glVertex3f(x2,y2,ALTO_RODAPIE);
00192       glVertex3f(x2,y2,0);
00193 
00194     glEnd();
00195 }
00196 
00197 
00198 void suelo3D(float x1,float y1,float x2,float y2)
00199 {
00200     glBegin(GL_QUADS);       
00201       glColor3ub(COLOR_SUELO);
00202 
00203       glVertex3f(x1,y1,-1);
00204       glVertex3f(x1,y2,-1);
00205       glVertex3f(x2,y2,-1);
00206       glVertex3f(x2,y1,-1);
00207     glEnd();
00208 }
00209 
00210 
00211 void geobot3D_internal(void)
00212 {
00213     GLUquadricObj *obj;
00214 
00215     /* Creamos una cuadrica para el cilindro del cuerpo del robot, y para
00216      * las ruedas. */          
00217     obj = gluNewQuadric();
00218     gluQuadricDrawStyle(obj,GLU_FILL);
00219     gluQuadricNormals(obj,GLU_SMOOTH);
00220     gluQuadricOrientation(obj,GLU_OUTSIDE);    
00221     
00222     /* Cuerpo y tapa del robot: */
00223     glPushMatrix();
00224     glTranslatef(0,0,ELEVACION_ROBOT);
00225     glColor3ub(COLOR_CUERPO_ROBOT);
00226     gluCylinder(obj,RADIO_ROBOT,RADIO_ROBOT,ALTO_ROBOT,CIRCLE_NUM_SECTORS,1);
00227     glTranslatef(0,0,ALTO_ROBOT+1);
00228     glColor3ub(COLOR_TAPA_ROBOT);
00229     gluDisk(obj,0,RADIO_TAPA_ROBOT,CIRCLE_NUM_SECTORS,1);
00230     glPopMatrix();
00231 
00232     /* Ruedas: */
00233     glPushMatrix();
00234     glRotatef(90,0,1,0);
00235     glTranslatef(-RADIO_EXT_RUEDA_ROBOT,0,RADIO_ROBOT+1);
00236     glColor3ub(COLOR_RUEDAS_ROBOT);
00237     gluDisk(obj,RADIO_INT_RUEDA_ROBOT,RADIO_EXT_RUEDA_ROBOT,
00238                 CIRCLE_NUM_SECTORS,1);
00239     glTranslatef(0,0,-2*(RADIO_ROBOT+1));
00240     gluDisk(obj,RADIO_INT_RUEDA_ROBOT,RADIO_EXT_RUEDA_ROBOT,
00241                 CIRCLE_NUM_SECTORS,1);
00242     glPopMatrix();
00243     gluDeleteQuadric(obj);
00244 
00245     /* Tr�ode del robot: */
00246     glBegin(GL_LINES);
00247       glColor3ub(COLOR_TRIPODE_ROBOT);
00248       glVertex3f(0,-RADIO_ROBOT,ALTO_ROBOT+ELEVACION_ROBOT);
00249       glVertex3f(0,0,ALTO_ROBOT+ELEVACION_ROBOT+ALTO_TRIPODE);
00250      
00251       glVertex3f(SQR3DIV2*RADIO_ROBOT,RADIO_ROBOT/2,ALTO_ROBOT+ELEVACION_ROBOT);
00252       glVertex3f(0,0,ALTO_ROBOT+ELEVACION_ROBOT+ALTO_TRIPODE);
00253 
00254       glVertex3f(-SQR3DIV2*RADIO_ROBOT,RADIO_ROBOT/2,ALTO_ROBOT+ELEVACION_ROBOT);
00255       glVertex3f(0,0,ALTO_ROBOT+ELEVACION_ROBOT+ALTO_TRIPODE);    
00256     glEnd();    
00257     
00258     /* C�ara del robot: */
00259     /* Cuerpo de la c�ara: */
00260     glPushMatrix();
00261     glTranslatef(0,0,ALTO_ROBOT+ELEVACION_ROBOT+ALTO_TRIPODE);
00262     glRotatef(-90,1,0,0);
00263     glBegin(GL_QUAD_STRIP);
00264       glColor3ub(COLOR_CAMARA);
00265       glVertex3i(-CAM_WIDTH/2,-CAM_HEIGHT/2,-CAM_DEPTH/2);
00266       glVertex3i(-CAM_WIDTH/2,-CAM_HEIGHT/2,CAM_DEPTH/2);
00267       glVertex3i(CAM_WIDTH/2,-CAM_HEIGHT/2,-CAM_DEPTH/2);
00268       glVertex3i(CAM_WIDTH/2,-CAM_HEIGHT/2,CAM_DEPTH/2);
00269       glVertex3i(CAM_WIDTH/2,CAM_HEIGHT/2,-CAM_DEPTH/2);
00270       glVertex3i(CAM_WIDTH/2,CAM_HEIGHT/2,CAM_DEPTH/2);
00271       glVertex3i(-CAM_WIDTH/2,CAM_HEIGHT/2,-CAM_DEPTH/2);
00272       glVertex3i(-CAM_WIDTH/2,CAM_HEIGHT/2,CAM_DEPTH/2);
00273       glVertex3i(-CAM_WIDTH/2,-CAM_HEIGHT/2,-CAM_DEPTH/2);
00274       glVertex3i(-CAM_WIDTH/2,-CAM_HEIGHT/2,CAM_DEPTH/2);
00275     glEnd();     
00276     /* Ejes de la c�ara: */      
00277     glBegin(GL_LINES);
00278       glColor3ub(255,0,0);
00279       glVertex3i(0,0,0);
00280       glVertex3i((int)1.5*CAM_DEPTH,0,0);
00281 
00282       glColor3ub(0,255,0);
00283       glVertex3i(0,0,0);
00284       glVertex3i(0,(int)1.5*CAM_DEPTH,0); 
00285 
00286       glColor3ub(0,0,255);
00287       glVertex3i(0,0,0);
00288       glVertex3i(0,0,(int)1.5*CAM_DEPTH);                    
00289     glEnd(); 
00290 
00291     glPopMatrix();
00292 }
00293 
00294 
00295 void geobot3D(float posx, float posy, float angle)
00296 {
00297     glPushMatrix();
00298     glRotatef(angle,0,0,1);
00299     glTranslatef(posx*cos(angle*PI/180)+posy*sin(angle*PI/180),
00300                  -posx*sin(angle*PI/180)+posy*cos(angle*PI/180),0);
00301     //glTranslatef(posx,posy,0);
00302     
00303     geobot3D_internal();
00304     glPopMatrix();
00305 }
00306 
00307 
00308 
00310 
00311 
00312 // devuelve un punto en el rayo que pasa por un pixel y a una distancia d del centro          
00313 /*Matrix rayAt(VMatrix PIA, VMatrix c, VMatrix dir, double x, double y, double d) {
00314     Matrix r = PIA*CL(x,y,1.);
00315     r = CL(r(1)/r(4),r(2)/r(4),r(3)/r(4));
00316     r = r - c;
00317     if(DotProduct(r,dir)<0) {
00318         r = -1.*r;
00319     }
00320     r = r*(d/Norm(r));
00321     return c+r;
00322 }*/
00323 
00324 
00325  
00326 /*void parameterized_camera(LIST<double> p, float size, bool trip) {
00327     Requiere(p.size()==7,"problem in show_parameterized_camera");
00328     float x = p[4];
00329     float y = p[5];
00330     float z = p[6];
00331     Matrix A = SyntheticCamera(p);
00332     Matrix IA = PseudoInverse(A);
00333     if(z<0) {
00334         Debug("z negativo");
00335         return;
00336     }
00337     glPointSize(5.0);
00338     glBegin(GL_POINTS);
00339     glColor3ub(255,0,0);
00340     glVertex3f(x,y,z);                       
00341     glEnd();
00342     
00343     float d = Det3(ExtractArray(A,CL(1,2,3),CL(1,2,3))); // por si acaso
00344     float dx = d*A(3,1)/A(3,4);
00345     float dy = d*A(3,2)/A(3,4);                 
00346     float dz = d*A(3,3)/A(3,4);
00347     float n = sqrt(sqr(dx)+sqr(dy)+sqr(dz));
00348     Matrix dir = CL(double(dx/n),double(dy/n),double(dz/n));
00349     //glBegin(GL_LINE);
00350     //    glVertex3f(x,y,z);                 
00351     //    glVertex3f(x+size*dx/n,y+size*dy/n,z+size*dz/n);
00352     //glEnd();
00353     Matrix c = CL(double(x),double(y),double(z));
00354     
00355     LIST<Matrix> v;
00356     v.push_back(c);
00357     v.push_back(rayAt(IA,c,dir,-1.,-1.*3/4,size));
00358     v.push_back(rayAt(IA,c,dir,-1.,+1.*3/4,size));
00359     v.push_back(rayAt(IA,c,dir,+1.,+1.*3/4,size));
00360     v.push_back(rayAt(IA,c,dir,+1.,-1.*3/4,size));
00361     
00362 #define VER(k) glVertex3f(v[k](1),v[k](2),v[k](3));
00363     
00364     //glPolygonMode(GL_FRONT_AND_BACK,GL_LINE);
00365     glColor3ub(255,255,255);
00366     glBegin(GL_POLYGON);    
00367     VER(1);
00368     VER(2);
00369     VER(3);
00370     VER(4);
00371     VER(1);
00372     glEnd();
00373     glColor3ub(255,0,0);
00374     glBegin(GL_LINE);
00375     VER(0); VER(1);
00376     VER(0); VER(2);
00377     VER(0); VER(3);
00378     VER(0); VER(4);
00379     VER(1); VER(2);
00380     VER(2); VER(3);
00381     VER(3); VER(4);
00382     VER(4); VER(1);
00383     glEnd();
00384 
00385     if(not trip) {
00386         return;
00387     }
00388     //dibujamos el trípode
00389 
00390     LIST<HPoint> T = Tripod(HomographyZ0(A));
00391 
00392     glColor3ub(40,40,0);
00393     glBegin(GL_LINE);
00394     VER(0); glVertex3f(T[0].x/T[0].w,T[0].y/T[0].w,0); 
00395     VER(0); glVertex3f(T[1].x/T[1].w,T[1].y/T[1].w,0); 
00396     VER(0); glVertex3f(T[2].x/T[2].w,T[2].y/T[2].w,0); 
00397     glEnd();
00398 
00399 }
00400 
00401 */

Generated on Thu Mar 13 19:18:16 2008 for QVision by  doxygen 1.5.3