00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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
00032 #define PI 3.141593
00033 #define SQR3DIV2 0.866025
00034
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
00048
00049 #define ALTO_PARED 270
00050 #define ALTO_RODAPIE 20
00051
00052
00053
00054 #define ALTO_PUERTA 210
00055 #define ANCHO_MARCO 10
00056 #define RADIO_MANIVELA 6
00057
00058
00059 #define RADIO_ROBOT 25
00060 #define RADIO_TAPA_ROBOT 28
00061 #define ALTO_ROBOT 20
00062 #define ELEVACION_ROBOT 5
00063 #define ALTO_TRIPODE 70
00064 #define RADIO_INT_RUEDA_ROBOT 5
00065 #define RADIO_EXT_RUEDA_ROBOT 10
00066 #define CAM_DEPTH 15
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
00079 glColor3ub(COLOR_MARCO);
00080
00081
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
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
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
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
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
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
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
00158
00159
00160
00161
00162
00163 glPushMatrix();
00164
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
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
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
00216
00217 obj = gluNewQuadric();
00218 gluQuadricDrawStyle(obj,GLU_FILL);
00219 gluQuadricNormals(obj,GLU_SMOOTH);
00220 gluQuadricOrientation(obj,GLU_OUTSIDE);
00221
00222
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
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
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
00259
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
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
00302
00303 geobot3D_internal();
00304 glPopMatrix();
00305 }
00306
00307
00308
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401