00001 #include "VirtualRobot.h"
00002
00030 DLL_EXPORT void STD AbrirMapaMIL (HWND WindowHandle,char *FICHERO,char *DIRECTORIO_OBJETOS,
00031 long *alto,long *ancho,double escala,double *PosX,double *PosY,
00032 double *Heading,MIL_ID sys,MIL_ID *disp,MIL_ID *mapa)
00033 {
00034 int n;
00035 Pt2D tmp;
00036 double tmp2;
00037
00038
00039 for (n=0;n<30;n++) Mundo[n]=NULL;
00040
00041 MatrizObjetos=IniMatrix(3);
00042 NuevoMapaMIL(WindowHandle,*alto,*ancho,escala,sys,disp,mapa);
00043 LeerMundo(Mundo,FICHERO,DIRECTORIO_OBJETOS,&tmp,&tmp2);
00044 *PosX=tmp.x;
00045 *PosY=tmp.y;
00046 *Heading=tmp2;
00047 PintarMundo(Mundo,ESTACIONARIO);
00048 MbufCopy(WorkingBuffer,RefreshBuffer);
00049 }
00050
00069 DLL_EXPORT void STD NuevoMapaMIL (HWND WindowHandle,long alto, long ancho ,
00070 double escala,MIL_ID sys,MIL_ID *disp,
00071 MIL_ID *mapa)
00072 {
00073
00074 MdispAlloc (sys,M_DEFAULT,"M_DEFAULT",M_DEFAULT,&*disp);
00075
00076
00077
00078 MbufAllocColor (sys,3,ancho,alto,8,M_IMAGE+M_DISP+M_PROC,&*mapa);
00079 MbufAllocColor (sys,3,ancho,alto,8,M_IMAGE+M_DISP+M_PROC,&WorkingBuffer);
00080 MbufAllocColor (sys,3,ancho,alto,8,M_IMAGE+M_DISP+M_PROC,&RefreshBuffer);
00081
00082 MbufClear (*mapa,255);
00083 MdispSelectWindow (*disp,*mapa,WindowHandle);
00084
00085
00086
00087 MdispControl (*disp,M_WINDOW_ZOOM,M_DISABLE);
00088 MdispControl (*disp,M_WINDOW_TITLE_BAR,M_DISABLE);
00089 MilUnderlayBuffer=*mapa;
00090 MgraColor (M_DEFAULT, 0);
00091
00092
00093 Buffer_Width=ancho;
00094 Buffer_Height=alto;
00095 MbufClear(RefreshBuffer,255);
00096 ESCALA_PixPorMM=1/escala;
00097
00098
00099
00100 ZonaAltaDensidad.EsqSupIzq.x =0;
00101 ZonaAltaDensidad.EsqSupIzq.y =Buffer_Height*escala;
00102 ZonaAltaDensidad.EsqInfDrch.x =(Buffer_Width*escala)/2;
00103 ZonaAltaDensidad.EsqInfDrch.y =(Buffer_Height*escala)/2;
00104
00105
00106 AlturaImagen=(double)alto*escala;
00107 Base=IniciarMatrizCambioBase (&Giro,&Traslacion,&Simetria_X,AlturaImagen,0,0,0,0,0);
00108 InversaBase=Inversa3X3(Base);
00109 }
00110
00121 DLL_EXPORT void STD SalvarMapaMIL (char *FileName)
00122 {
00123 MbufExport (FileName,M_BMP,RefreshBuffer);
00124 }
00125
00148 DLL_EXPORT void STD RecalcularMapaMIL (double escala,double DesplazamientoH,
00149 double DesplazamientoV,double AnguloGiro,
00150 long EjeGiroX,long EjeGiroY,int modo)
00151 {
00152 double alto;
00153
00154
00155 alto=AlturaImagen*ESCALA_PixPorMM;
00156 ESCALA_PixPorMM=1/escala;
00157 AlturaImagen=(double)alto*escala;
00158
00159 Base=IniciarMatrizCambioBase (&Giro,&Traslacion,&Simetria_X,AlturaImagen,
00160 DesplazamientoH,DesplazamientoV,AnguloGiro,
00161 EjeGiroX,EjeGiroY);
00162 InversaBase=Inversa3X3(Base);
00163
00164
00165 MbufClear (WorkingBuffer,255);
00166
00167
00168 PintarMundo (Mundo, modo);
00169 MbufCopy(WorkingBuffer,RefreshBuffer);
00170 PintaRobot (MatrizRobot);
00171 PintaCamaras (MatrizCamaras);
00172 PintaZona(ZonaAltaDensidad);
00173 ObtenerLecturaSonares();
00174 MbufCopy(WorkingBuffer,MilUnderlayBuffer);
00175 }
00176
00187 DLL_EXPORT void STD CerrarMapaMIL (MIL_ID disp)
00188 {
00189 MbufFree(MilUnderlayBuffer);
00190 MbufFree(WorkingBuffer);
00191 MbufFree(RefreshBuffer);
00192
00193 BorrarMundo(Mundo);
00194 MdispFree(disp);
00195 }
00203 DLL_EXPORT void STD UsarZonaAltaResolucion (void)
00204 {
00205 ZonaAltaDensidad.UsarZona=true;
00206 MbufCopy(RefreshBuffer,WorkingBuffer);
00207 PintaRobot(MatrizRobot);
00208 PintaCamaras(MatrizCamaras);
00209 PintaZona(ZonaAltaDensidad);
00210 MbufCopy(WorkingBuffer,MilUnderlayBuffer);
00211 }
00212
00227 DLL_EXPORT void STD AjustarZonaAltaResolucion (long XSupIzq,long YSupIzq,
00228 long XInfDrch,long YInfDrch)
00229 {
00230 ZonaAltaDensidad.EsqSupIzq.x=XSupIzq;
00231 ZonaAltaDensidad.EsqSupIzq.y=YSupIzq;
00232 ZonaAltaDensidad.EsqInfDrch.x=XInfDrch;
00233 ZonaAltaDensidad.EsqInfDrch.y=YInfDrch;
00234 ZonaAltaDensidad.UsarZona =true;
00235 MbufCopy(RefreshBuffer,WorkingBuffer);
00236 PintaRobot (MatrizRobot);
00237 PintaCamaras (MatrizCamaras);
00238 PintaZona(ZonaAltaDensidad);
00239 MbufCopy(WorkingBuffer,MilUnderlayBuffer);
00240 }
00241
00249 DLL_EXPORT void STD NoUsarZonaAltaResolucion (void)
00250 {
00251 ZonaAltaDensidad.UsarZona=false;
00252 MbufCopy(RefreshBuffer,WorkingBuffer);
00253 PintaRobot(MatrizRobot);
00254 PintaCamaras(MatrizCamaras);
00255 MbufCopy(WorkingBuffer,MilUnderlayBuffer);
00256 }
00257
00269 DLL_EXPORT void STD PosicionarRobotVirtual (double x, double y, double Angulo,
00270 double AnguloCamaras, long modo)
00271 {
00272
00273
00274
00275
00276 Robot.P1.x=99.8; Robot.P1.y=190;
00277 Robot.P2.x=190; Robot.P2.y=99.8;
00278 Robot.P3.x=190; Robot.P3.y=-99.8;
00279 Robot.P4.x=99.8; Robot.P4.y=-190;
00280 Robot.P5.x=-99.8; Robot.P5.y=-190;
00281 Robot.P6.x=-190; Robot.P6.y=-99.8;
00282 Robot.P7.x=-190; Robot.P7.y=99.8;
00283 Robot.P8.x=-99.8; Robot.P8.y=190;
00284 Robot.centro.x=0; Robot.centro.y=0;
00285 Robot.BaseFlecha.x=0; Robot.BaseFlecha.y=190;
00286 Robot.PuntaFlechaC.x=0;
00287 Robot.PuntaFlechaC.y=(30*(1/ESCALA_PixPorMM))+190;
00288
00289 Robot.PuntaFlechaL.x=-5*(1/ESCALA_PixPorMM);
00290 Robot.PuntaFlechaL.y=(25*(1/ESCALA_PixPorMM))+190;
00291 Robot.PuntaFlechaR.x=5*(1/ESCALA_PixPorMM);
00292 Robot.PuntaFlechaR.y=(25*(1/ESCALA_PixPorMM))+190;
00293
00294
00295 camaras.uno.x=0; camaras.uno.y = 49.9;
00296 camaras.dos.x=29.9; camaras.dos.y = -29.9;
00297 camaras.tres.x=0; camaras.tres.y = 0;
00298 camaras.cuatro.x=-29.9; camaras.cuatro.y = -29.9;
00299 camaras.Cono1L.x=-20*(1/ESCALA_PixPorMM);
00300 camaras.Cono1L.y=90*(1/ESCALA_PixPorMM);
00301 camaras.Cono1R.x=20*(1/ESCALA_PixPorMM);
00302 camaras.Cono1R.y=90*(1/ESCALA_PixPorMM);
00303
00304 IniciarMatrices (&Giro,&Traslacion,&MatrizRobot,&MatrizCamaras,
00305 &PosicionActual,Angulo, AnguloCamaras);
00306
00307 ActualizarVista(x,y,Angulo,AnguloCamaras,modo);
00308 }
00309
00323 DLL_EXPORT void STD ActualizarVista (double x, double y, double Angulo
00324 ,double AnguloCamaras, long modo)
00325 {
00326 PosicionActual.x=x;
00327 PosicionActual.y=y;
00328 switch (modo)
00329 {
00330 case 0:
00331
00332 IniTraslacion(Traslacion,PosicionActual.x,PosicionActual.y);
00333 IniGiro(Giro,Angulo);
00334 mulmat (Giro,Traslacion,MatrizRobot);
00335 IniMatrizCamaras(MatrizCamaras,Giro,Traslacion,AnguloCamaras);
00336 PintaRobot (MatrizRobot);
00337 PintaCamaras (MatrizCamaras);
00338 PintaZona(ZonaAltaDensidad);
00339 ObtenerLecturaSonares();
00340 MbufCopy(WorkingBuffer,MilUnderlayBuffer);
00341 break;
00342 case 1:
00343
00344 double CentradoX, CentradoY;
00345
00346 CentradoX=PosicionActual.x-(Buffer_Width/(2*ESCALA_PixPorMM));
00347 CentradoY=PosicionActual.y-(Buffer_Height/(2*ESCALA_PixPorMM));
00348
00349 Base=IniciarMatrizCambioBase (&Giro,&Traslacion,&Simetria_X,(Buffer_Height/ESCALA_PixPorMM),
00350 -CentradoX,-CentradoY,Angulo,
00351 PosicionActual.x,PosicionActual.y);
00352 InversaBase=Inversa3X3(Base);
00353
00354
00355 MbufClear (WorkingBuffer,255);
00356
00357
00358 PintarMundo (Mundo, MOVIMIENTO);
00359 MbufCopy(WorkingBuffer,RefreshBuffer);
00360
00361 IniTraslacion(Traslacion,PosicionActual.x,PosicionActual.y);
00362 IniGiro(Giro,Angulo);
00363 mulmat (Giro,Traslacion,MatrizRobot);
00364 IniMatrizCamaras(MatrizCamaras,Giro,Traslacion,AnguloCamaras);
00365 PintaRobot (MatrizRobot);
00366 PintaCamaras (MatrizCamaras);
00367 PintaZona(ZonaAltaDensidad);
00368 ObtenerLecturaSonares();
00369 MbufCopy(WorkingBuffer,MilUnderlayBuffer);
00370 break;
00371 }
00372 }
00373
00380 DLL_EXPORT void STD ObtenerLecturaSonares (void)
00381 {
00382 Pt2D PuntoPantalla;
00383 int j;
00384
00385
00386 s_actualitzar();
00387 MgraColor (M_DEFAULT,M_RGB888(17,140,255));
00388 for (j=0;j<LISTA_MAX;j++)
00389 {
00390 PuntoPantalla=MultVect(Base,ListaFIFO[j]);
00391 MgraArc (M_DEFAULT,WorkingBuffer,(long)PuntoPantalla.x,(long)PuntoPantalla.y,5,5,0,360);
00392 }
00393 MgraColor (M_DEFAULT,0);
00394 }
00395
00402 DLL_EXPORT void STD VaciarBufferLecturasSonares (void)
00403 {
00404 int i;
00405 Pt2D Lectura;
00406
00407 Lectura.x=0; Lectura.y=0;
00408 for (i=0;i<LISTA_MAX;i++)
00409 {
00410 ListaFIFO[i]=Lectura;
00411 }
00412 sfSetFrontBuffer (0);
00413 sfSetSideBuffer (0);
00414 }
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430
00431
00432
00433
00434
00445 DLL_EXPORT void STD TransfMundoDispositivo (double PosXMundo, double PosYMundo,
00446 double *PosXDisp, double *PosYDisp)
00447 {
00448 Pt2D PuntoMundo,PuntoDisp;
00449
00450 PuntoMundo.x=PosXMundo;
00451 PuntoMundo.y=PosYMundo;
00452 PuntoDisp=MultVect(Base,PuntoMundo);
00453 *PosXDisp=PuntoDisp.x;
00454 *PosYDisp=PuntoDisp.y;
00455 }
00456
00467 DLL_EXPORT void STD TransfDispositivoMundo (double PosXDisp, double PosYDisp,
00468 double *PosXMundo, double *PosYMundo)
00469 {
00470 Pt2D PuntoMundo,PuntoDisp;
00471
00472 PuntoDisp.x=PosXDisp;
00473 PuntoDisp.y=PosYDisp;
00474 PuntoMundo=MultVect(InversaBase,PuntoDisp);
00475 *PosXMundo=PuntoMundo.x;
00476 *PosYMundo=PuntoMundo.y;
00477 }
00478
00479
00491 DLL_EXPORT void STD SplineTracer (double x1, double y1, double x2, double y2)
00492 {
00493 Pt2D punto,P1,P2;
00494
00495 punto.x=x1;
00496 punto.y=y1;
00497 P1=MultVect(Base,punto);
00498 punto.x=x2;
00499 punto.y=y2;
00500 P2=MultVect(Base,punto);
00501
00502 MgraColor (M_DEFAULT,0);
00503 MgraLine(M_DEFAULT,RefreshBuffer,P1.x,P1.y,P2.x,P2.y);
00504 MgraLine(M_DEFAULT,MilUnderlayBuffer,P1.x,P1.y,P2.x,P2.y);
00505 }
00506
00516 DLL_EXPORT void STD PintarPuntoTrayectoria (double x1, double y1)
00517 {
00518 Pt2D punto, resultado;
00519
00520 punto.x=x1;
00521 punto.y=y1;
00522 resultado=MultVect(Base,punto);
00523
00524 MgraColor (M_DEFAULT,M_RGB888(200,0,0));
00525 MgraDot(M_DEFAULT,RefreshBuffer,resultado.x,resultado.y);
00526 MgraDot(M_DEFAULT,MilUnderlayBuffer,resultado.x,resultado.y);
00527 }
00528
00533 DLL_EXPORT void STD GrabarBuffer ()
00534 {
00535 FILE *fichero;
00536 int i;
00537 char nombre[15];
00538 char tmp[3];
00539 int u;
00540
00541 strcpy (nombre,"c:\\mio0");
00542 i=0;
00543 _itoa (i,tmp,10);
00544 strcat (nombre,tmp);
00545 strcat (nombre,".tif");
00546 while (fopen(nombre,"r")!=NULL)
00547 {
00548 i++;
00549 _itoa (i,tmp,10);
00550 if (i<10){ strcpy (nombre,"c:\\mio0"); }
00551 else if (i>=10){strcpy (nombre,"c:\\mio");}
00552 strcat (nombre,tmp);
00553 strcat (nombre,".tif");
00554 }
00555 MbufExport(nombre,M_TIFF,MilUnderlayBuffer);
00556 }
00557
00558
00559
00560
00561
00582 matriz IniciarMatrizCambioBase (matriz *giro,matriz *trasl,matriz *simetria_x
00583 ,double AlturaImagen, double DesplazamientoH,
00584 double DesplazamientoV,double AnguloGiro,
00585 long EjeGiroX,long EjeGiroY)
00586 {
00587 matriz a,b,c,d;
00588 matriz temp,MatrizEscalado,Resultado;
00589
00590 temp=IniMatrix (3); Resultado=IniMatrix(3);
00591 a=IniMatrix (3); b=IniMatrix (3);
00592 c=IniMatrix (3); d=IniMatrix (3);
00593 MatrizEscalado=IniMatrix(3);
00594
00595
00596 IniTraslacion (b,DesplazamientoH,AlturaImagen-DesplazamientoV);
00597 IniGiroRespectoPunto (a,AnguloGiro,-EjeGiroX,-EjeGiroY);
00598 IniSimetriaX (c);
00599 IniEscalado (ESCALA_PixPorMM,ESCALA_PixPorMM,MatrizEscalado);
00600
00601
00602 mulmat (c,a,temp);
00603 mulmat (temp,b,d);
00604 mulmat (d,MatrizEscalado,Resultado);
00605 *giro=a; *trasl=b;
00606 *simetria_x=c;
00607
00608 FreeMatrix(temp,3);
00609 FreeMatrix(d,3);
00610
00611 return(Resultado);
00612 }
00613
00632 void IniciarMatrices (matriz *giro,matriz *trasl,matriz *MatrizRobot,
00633 matriz *matrizcamaras, Pt2D *PosicionActual,
00634 double AnguloActual,double AnguloCamaras)
00635 {
00636 matriz e,f;
00637
00638 e=IniMatrix (3); f=IniMatrix (3);
00639
00640
00641
00642 IniTraslacion((*trasl),(*PosicionActual).x,(*PosicionActual).y);
00643 IniGiro((*giro),AnguloActual);
00644 mulmat ((*giro),(*trasl),e);
00645
00646
00647 IniMatrizCamaras(f,(*giro),(*trasl),AnguloCamaras);
00648
00649 *MatrizRobot=e; *matrizcamaras=f;
00650 }
00651
00663 void IniMatrizCamaras (matriz PosCamaras,matriz giro,matriz traslacion,double angulo)
00664 {
00665 matriz GiroCamara,TraslCamara,tmp,tmp2,tmp3;
00666 tmp=IniMatrix(3);
00667 tmp2=IniMatrix(3);
00668 tmp3=IniMatrix(3);
00669 GiroCamara=IniMatrix(3);
00670 TraslCamara=IniMatrix(3);
00671 angulo=(angulo*PI)/180;
00672
00673
00674
00675 GiroCamara[0][0]=GiroCamara[1][1]=cos(angulo);
00676 GiroCamara[0][1]=sin(angulo);
00677 GiroCamara[1][0]=-sin(angulo);
00678 GiroCamara[2][2]=1;
00679 TraslCamara[0][0]=TraslCamara[1][1]=TraslCamara[2][2]=1;
00680 TraslCamara[2][0]=-69.86;
00681 TraslCamara[2][1]=69.86;
00682
00683
00684
00685 mulmat(GiroCamara,TraslCamara,tmp);
00686 mulmat(tmp,giro,tmp2);
00687 mulmat(tmp2,traslacion,PosCamaras);
00688
00689 FreeMatrix(tmp,3);
00690 FreeMatrix(tmp2,3);
00691 FreeMatrix(tmp3,3);
00692 FreeMatrix(GiroCamara,3);
00693 FreeMatrix(TraslCamara,3);
00694 }
00695
00706 void PintaRobot (matriz MatrizRobot)
00707 {
00708 Coordenadas=CalculaPosRobot (Robot,MatrizRobot);
00709 Coordenadas=CalculaPosRobot(Coordenadas,Base);
00710 MbufCopy (RefreshBuffer,WorkingBuffer);
00711 MgraColor(M_DEFAULT,0);
00712 MgraLine(M_DEFAULT,WorkingBuffer,long(Coordenadas.P1.x),long(Coordenadas.P1.y),long(Coordenadas.P2.x),long(Coordenadas.P2.y));
00713 MgraLine(M_DEFAULT,WorkingBuffer,long(Coordenadas.P2.x),long(Coordenadas.P2.y),long(Coordenadas.P3.x),long(Coordenadas.P3.y));
00714 MgraLine(M_DEFAULT,WorkingBuffer,long(Coordenadas.P3.x),long(Coordenadas.P3.y),long(Coordenadas.P4.x),long(Coordenadas.P4.y));
00715 MgraLine(M_DEFAULT,WorkingBuffer,long(Coordenadas.P4.x),long(Coordenadas.P4.y),long(Coordenadas.P5.x),long(Coordenadas.P5.y));
00716 MgraLine(M_DEFAULT,WorkingBuffer,long(Coordenadas.P5.x),long(Coordenadas.P5.y),long(Coordenadas.P6.x),long(Coordenadas.P6.y));
00717 MgraLine(M_DEFAULT,WorkingBuffer,long(Coordenadas.P6.x),long(Coordenadas.P6.y),long(Coordenadas.P7.x),long(Coordenadas.P7.y));
00718 MgraLine(M_DEFAULT,WorkingBuffer,long(Coordenadas.P7.x),long(Coordenadas.P7.y),long(Coordenadas.P8.x),long(Coordenadas.P8.y));
00719 MgraLine(M_DEFAULT,WorkingBuffer,long(Coordenadas.P8.x),long(Coordenadas.P8.y),long(Coordenadas.P1.x),long(Coordenadas.P1.y));
00720 MgraColor(M_DEFAULT,M_RGB888(255,45,0));
00721 MgraLine(M_DEFAULT,WorkingBuffer,long(Coordenadas.BaseFlecha.x),long(Coordenadas.BaseFlecha.y),long(Coordenadas.PuntaFlechaC.x),long(Coordenadas.PuntaFlechaC.y));
00722 MgraLine(M_DEFAULT,WorkingBuffer,long(Coordenadas.PuntaFlechaC.x),long(Coordenadas.PuntaFlechaC.y),long(Coordenadas.PuntaFlechaL.x),long(Coordenadas.PuntaFlechaL.y));
00723 MgraLine(M_DEFAULT,WorkingBuffer,long(Coordenadas.PuntaFlechaC.x),long(Coordenadas.PuntaFlechaC.y),long(Coordenadas.PuntaFlechaR.x),long(Coordenadas.PuntaFlechaR.y));
00724
00725
00726
00727
00728 MgraColor(M_DEFAULT,0);
00729 }
00730
00742 void PintaCamaras (matriz MatrizCamaras)
00743 {
00744 vision coord;
00745
00746 coord=CalculaPosCamaras(camaras,MatrizCamaras);
00747 coord=CalculaPosCamaras(coord,Base);
00748
00749 MgraLine(M_DEFAULT,WorkingBuffer,long(coord.uno.x),long(coord.uno.y),long(coord.dos.x),long(coord.dos.y));
00750 MgraLine(M_DEFAULT,WorkingBuffer,long(coord.dos.x),long(coord.dos.y),long(coord.tres.x),long(coord.tres.y));
00751 MgraLine(M_DEFAULT,WorkingBuffer,long(coord.tres.x),long(coord.tres.y),long(coord.cuatro.x),long(coord.cuatro.y));
00752 MgraLine(M_DEFAULT,WorkingBuffer,long(coord.cuatro.x),long(coord.cuatro.y),long(coord.uno.x),long(coord.uno.y));
00753 MgraColor(M_DEFAULT,M_RGB888(103,162,249));
00754 MgraLine(M_DEFAULT,WorkingBuffer,long(coord.uno.x),long(coord.uno.y),long(coord.Cono1L.x),long(coord.Cono1L.y));
00755 MgraLine(M_DEFAULT,WorkingBuffer,long(coord.uno.x),long(coord.uno.y),long(coord.Cono1R.x),long(coord.Cono1R.y));
00756 MgraColor(M_DEFAULT,0);
00757 }
00758
00768 void PintaZona (zona ZonaAltaDensidad)
00769 {
00770 if (ZonaAltaDensidad.UsarZona==true) {
00771 Pt2D P1,P2,P3,P4;
00772 Pt2D temp1,temp2;
00773
00774 temp1.x=ZonaAltaDensidad.EsqInfDrch.x;
00775 temp1.y=ZonaAltaDensidad.EsqSupIzq.y;
00776 temp2.x=ZonaAltaDensidad.EsqSupIzq.x;
00777 temp2.y=ZonaAltaDensidad.EsqInfDrch.y;
00778 P1=MultVect(Base,ZonaAltaDensidad.EsqSupIzq);
00779 P2=MultVect(Base,ZonaAltaDensidad.EsqInfDrch);
00780 P3=MultVect(Base,temp1);
00781 P4=MultVect(Base,temp2);
00782 MgraColor(M_DEFAULT,M_RGB888(1,215,92));
00783 MgraRect(M_DEFAULT,WorkingBuffer,P1.x,P1.y,P2.x,P2.y);
00784 MgraColor(M_DEFAULT,0);
00785 }
00786 }
00787
00800 posicion CalculaPosRobot (posicion robot,matriz matrix)
00801 {
00802 posicion temp;
00803 temp.P1=MultVect(matrix,robot.P1);
00804 temp.P2=MultVect(matrix,robot.P2);
00805 temp.P3=MultVect(matrix,robot.P3);
00806 temp.P4=MultVect(matrix,robot.P4);
00807 temp.P5=MultVect(matrix,robot.P5);
00808 temp.P6=MultVect(matrix,robot.P6);
00809 temp.P7=MultVect(matrix,robot.P7);
00810 temp.P8=MultVect(matrix,robot.P8);
00811 temp.centro=MultVect(matrix,robot.centro);
00812 temp.BaseFlecha=MultVect(matrix,robot.BaseFlecha);
00813 temp.PuntaFlechaC=MultVect(matrix,robot.PuntaFlechaC);
00814 temp.PuntaFlechaL=MultVect(matrix,robot.PuntaFlechaL);
00815 temp.PuntaFlechaR=MultVect(matrix,robot.PuntaFlechaR);
00816 return temp;
00817 }
00818
00831 vision CalculaPosCamaras (vision camara, matriz matrix)
00832 {
00833 vision temp;
00834 temp.uno=MultVect(matrix,camara.uno);
00835 temp.dos=MultVect(matrix,camara.dos);
00836 temp.tres=MultVect(matrix,camara.tres);
00837 temp.cuatro=MultVect(matrix,camara.cuatro);
00838 temp.Cono1L=MultVect(matrix,camara.Cono1L);
00839 temp.Cono1R=MultVect(matrix,camara.Cono1R);
00840 return temp;
00841 }
00842
00852 void s_actualitzar (void)
00853 {
00854 int limit, i;
00855 Pt2D Punto,PuntoMundo;
00856 bool ValorRepetido=false;
00857
00858 i = 0;
00859 SonarBufferIndex = 0;
00860 limit = sr_buf->limit;
00861 while (i<limit)
00862 {
00863 if (sr_buf->valid[i])
00864 {
00865 SonarBuffer[SonarBufferIndex].x = sr_buf->xbuf[i];
00866 SonarBuffer[SonarBufferIndex].y = sr_buf->ybuf[i];
00867 SonarBufferIndex++;
00868 }
00869 i++;
00870 }
00871
00872 i = 0;
00873 limit = sl_buf->limit;
00874 while (i<limit)
00875 {
00876 if (sl_buf->valid[i])
00877 {
00878 SonarBuffer[SonarBufferIndex].x = sl_buf->xbuf[i];
00879 SonarBuffer[SonarBufferIndex].y = sl_buf->ybuf[i];
00880 SonarBufferIndex++;
00881 }
00882 i++;
00883 }
00884
00885 i = 0;
00886 limit = sraw_buf->limit;
00887 while (i<limit)
00888 {
00889 if (sraw_buf->valid[i])
00890 {
00891 SonarBuffer[SonarBufferIndex].x = sraw_buf->xbuf[i];
00892 SonarBuffer[SonarBufferIndex].y = sraw_buf->ybuf[i];
00893 SonarBufferIndex++;
00894 }
00895 i++;
00896 }
00897
00898 i=0;
00899 while (i<SonarBufferIndex)
00900
00901 {
00902 Punto.x = -SonarBuffer[i].y;
00903 Punto.y = SonarBuffer[i].x;
00904 PuntoMundo=MultVect(MatrizRobot,Punto);
00905 i++;
00906 ValorRepetido=BuscarLectura(PuntoMundo);
00907 if (ValorRepetido==false)
00908 {
00909 ListaFIFO[Indice1]=PuntoMundo;
00910 Indice1++;
00911 }
00912 if (Indice1>=LISTA_MAX) {Indice1=0;}
00913 }
00914 }
00915
00923 bool BuscarLectura (Pt2D Valor)
00924 {
00925 int i;
00926
00927 for (i=0;i<LISTA_MAX;i++)
00928 {
00929 if (ListaFIFO[i].x==Valor.x && ListaFIFO[i].y==Valor.y) {
00930 return true;
00931 }
00932 }
00933 return false;
00934 }
00935
00953 void LeerMundo (lista *objetos,char *Mapa,char *DirectorioObjetos,Pt2D *lugar
00954 ,double *Horientacion)
00955 {
00956 char directorio[100];
00957 char fichero[100];
00958 int m,n,i,longitud,NumeroObjetos;
00959 char nombre[100];
00960 char aux[100];
00961 char *nombre2,*codigo;
00962 bool ObjetoMovil=false;
00963 long x,y,NumeroPuntos;
00964 Pt2D punto;
00965 double PosX,PosY,heading;
00966
00967 map=fopen(Mapa,"r");
00968 fscanf(map,"%d",&NumeroObjetos);
00969
00970 for (i=0;i<NumeroObjetos;i++)
00971 {
00972 for (m=0;objetos[m]!=NULL;m++);
00973
00974 fscanf(map,"%s",&fichero);
00975 strcat(fichero,".dat");
00976 sprintf (aux,"%s%s",DirectorioObjetos,fichero);
00977 in=fopen(aux,"r");
00978
00979 fscanf(in,"%s",&nombre);
00980 longitud=strlen(nombre);
00981 nombre2=(char *)calloc(longitud,sizeof(char));
00982 strcpy(nombre2,nombre);
00983
00984 fscanf(in,"%s",&nombre);
00985 longitud=strlen(nombre);
00986 codigo=(char *)calloc(longitud,sizeof(char));
00987 strcpy(codigo,nombre);
00988
00989 if (strncmp(codigo,"001",3)!=0) {ObjetoMovil=true;}
00990
00991 fscanf(in,"%d",&NumeroPuntos);
00992
00993 for (n=0;n<NumeroPuntos;n++)
00994 {
00995 fscanf(in,"%d %d",&x,&y);
00996 punto.x=x; punto.y=y;
00997 objetos[m]=AnadirNodo(objetos[m],punto);
00998 }
00999 objetos[m]->nombre=nombre2;
01000 objetos[m]->codigo=codigo;
01001 fclose(in);
01002
01003 if (ObjetoMovil==true)
01004 {
01005 fscanf (map,"%lf %lf %lf",&PosX,&PosY,&heading);
01006 punto.x=PosX; punto.y=PosY;
01007 objetos[m]->Posicion =punto;
01008 objetos[m]->Horientacion =heading;
01009 ObjetoMovil=false;
01010 }
01011 }
01012
01013 fscanf (map,"%lf %lf %lf",&PosX,&PosY,&heading);
01014 (*lugar).x=PosX; (*lugar).y=PosY;
01015 *Horientacion=heading;
01016 fclose(map);
01017 }
01018
01026 void BorrarMundo (lista *objetos)
01027 {
01028 int i=0;
01029 while (objetos[i]!=NULL)
01030 {
01031 BorrarLista (objetos[i]);
01032 objetos[i]=NULL;
01033 i++;
01034 }
01035 }
01036
01043 lista CrearLista ()
01044 {
01045 return (lista)malloc(sizeof(poligono));
01046 }
01047
01055 void BorrarLista (lista Lista_a_Borrar)
01056 {
01057 nodo_ptr actual,siguiente;
01058
01059 actual=Lista_a_Borrar->ultimo;
01060 while (actual!=NULL)
01061 {
01062 siguiente=actual->next;
01063 BorrarNodo(actual);
01064 actual=siguiente;
01065 }
01066 free (Lista_a_Borrar);
01067 }
01068
01082 lista AnadirNodo (lista l,Pt2D valor)
01083 {
01084 nodo_ptr nodo;
01085 lista temp;
01086
01087 nodo=CrearNodo();
01088
01089 if (l==NULL){
01090 temp=CrearLista();
01091 l=temp;
01092 l->ultimo=nodo;
01093 l->ultimo->next=NULL;
01094 l->ultimo->value=valor;
01095 }
01096 else {
01097 nodo->next=l->ultimo;
01098 nodo->value=valor;
01099 l->ultimo=nodo;
01100 }
01101 return l;
01102 }
01103
01111 nodo_ptr CrearNodo ()
01112 {
01113 return (nodo_ptr)malloc(sizeof(nodo));
01114 }
01115
01123 void BorrarNodo (nodo_ptr Nodo_a_Borrar)
01124 {
01125 free (Nodo_a_Borrar);
01126 }
01127
01136 void PintarMundo (lista *objetos,int modo)
01137 {
01138 int m=0;
01139
01140 while (objetos[m]!=NULL)
01141 {
01142 PintarObjeto(objetos[m],modo);
01143 m++;
01144 }
01145
01146 }
01147
01158 void PintarObjeto (lista objeto,int modo)
01159 {
01160 nodo_ptr tmp;
01161 Pt2D anterior,actual,PuntoMundo;
01162 lista ObjetoMundo=NULL;
01163
01164 if (strncmp(objeto->codigo,"001",3)==0)
01165 {
01166 if (modo==ESTACIONARIO) {
01167 MbufClear(WorkingBuffer,0);
01168 MgraColor (M_DEFAULT,255);
01169 }
01170 else {
01171 MbufClear(WorkingBuffer,255);
01172 MgraColor (M_DEFAULT,0);
01173 }
01174
01175 tmp=objeto->ultimo;
01176 anterior=MultVect(Base,tmp->value);
01177 ObjetoMundo=AnadirNodo(ObjetoMundo,anterior);
01178 tmp=tmp->next;
01179 while (tmp!=NULL){
01180 actual=MultVect(Base,tmp->value);
01181 ObjetoMundo=AnadirNodo(ObjetoMundo,actual);
01182 MgraLine (M_DEFAULT, WorkingBuffer, anterior.x, anterior.y, actual.x, actual.y);
01183 anterior=actual;
01184 tmp=tmp->next;
01185 }
01186 actual=MultVect(Base,objeto->ultimo->value);
01187 ObjetoMundo=AnadirNodo(ObjetoMundo,actual);
01188 MgraLine (M_DEFAULT, WorkingBuffer, anterior.x, anterior.y, actual.x, actual.y);
01189
01190
01191 if (modo==ESTACIONARIO){ RellenarObjeto (ObjetoMundo,WorkingBuffer,255);}
01192 }
01193 else if (strncmp(objeto->codigo,"002",3)==0)
01194 {
01195
01196 IniTraslacion(Traslacion,objeto->Posicion.x,objeto->Posicion.y);
01197 IniGiro(Giro,objeto->Horientacion);
01198 mulmat (Giro,Traslacion,MatrizObjetos);
01199
01200 tmp=objeto->ultimo;
01201 MgraColor (M_DEFAULT,100);
01202
01203 PuntoMundo=MultVect(MatrizObjetos,tmp->value);
01204 anterior=MultVect(Base,PuntoMundo);
01205 ObjetoMundo=AnadirNodo(ObjetoMundo,anterior);
01206 tmp=tmp->next;
01207
01208 while (tmp!=NULL){
01209 PuntoMundo=MultVect(MatrizObjetos,tmp->value);
01210 actual=MultVect(Base,PuntoMundo);
01211 ObjetoMundo=AnadirNodo(ObjetoMundo,actual);
01212 MgraLine (M_DEFAULT, WorkingBuffer, anterior.x, anterior.y, actual.x, actual.y);
01213 anterior=actual;
01214 tmp=tmp->next;
01215 }
01216 PuntoMundo=MultVect(MatrizObjetos,objeto->ultimo->value);
01217 actual=MultVect(Base,PuntoMundo);
01218 ObjetoMundo=AnadirNodo(ObjetoMundo,actual);
01219 MgraLine (M_DEFAULT, WorkingBuffer, anterior.x, anterior.y, actual.x, actual.y);
01220
01221
01222 if (modo==ESTACIONARIO){
01223 RellenarObjeto (ObjetoMundo,WorkingBuffer,0);
01224 }
01225 }
01226 BorrarLista(ObjetoMundo);
01227 }
01228
01240 void RellenarObjeto (lista objeto,MIL_ID buffer,int color)
01241 {
01242 polygon_ptr poligono;
01243 nodo_ptr tmp;
01244 long punto_x=0,punto_y=0;
01245 long Tolerancia=10;
01246 Pt2D punto;
01247 vtx_ptr new_vertex;
01248 pt_poly_relation posicion_del_punto=FUERA;
01249
01250
01251
01252 polygon_create(poligono);
01253
01254 tmp=objeto->ultimo;
01255 while (tmp!=NULL){
01256 polygon_new_vertex(poligono,tmp->value.x,tmp->value.y,new_vertex);
01257 tmp=tmp->next;
01258 }
01259
01260
01261
01262 while (Tolerancia!=NULL && posicion_del_punto==FUERA){
01263 tmp=objeto->ultimo;
01264 while (posicion_del_punto==FUERA && tmp!=NULL){
01265 punto_x=tmp->value.x+Tolerancia;
01266 punto_y=tmp->value.y;
01267 posicion_del_punto=point_in_poly(poligono,punto_x,punto_y);
01268 if (posicion_del_punto==FUERA){
01269 punto_y=punto_y+Tolerancia;
01270 posicion_del_punto=point_in_poly(poligono,punto_x,punto_y);
01271 if (posicion_del_punto==FUERA){
01272 punto_x=punto_x-Tolerancia;
01273 posicion_del_punto=point_in_poly(poligono,punto_x,punto_y);
01274 if (posicion_del_punto==FUERA){
01275 punto_x=punto_x-Tolerancia;
01276 posicion_del_punto=point_in_poly(poligono,punto_x,punto_y);
01277 if (posicion_del_punto==FUERA){
01278 punto_y=punto_y-Tolerancia;
01279 posicion_del_punto=point_in_poly(poligono,punto_x,punto_y);
01280 if (posicion_del_punto==FUERA){
01281 punto_y=punto_y-Tolerancia;
01282 posicion_del_punto=point_in_poly(poligono,punto_x,punto_y);
01283 if (posicion_del_punto==FUERA){
01284 punto_x=punto_x+Tolerancia;
01285 posicion_del_punto=point_in_poly(poligono,punto_x,punto_y);
01286 if (posicion_del_punto==FUERA){
01287 punto_x=punto_x+Tolerancia;
01288 posicion_del_punto=point_in_poly(poligono,punto_x,punto_y);
01289 }
01290 }
01291 }
01292 }
01293 }
01294 }
01295 }
01296
01297 tmp=tmp->next;
01298 punto.x=punto_x; punto.y=punto_y;
01299 if (!Punto_en_Buffer(punto)) posicion_del_punto=FUERA;
01300 }
01301 if (posicion_del_punto==FUERA){
01302 switch (Tolerancia)
01303 {
01304 case 10:
01305 Tolerancia=2;
01306 break;
01307 case 2:
01308 Tolerancia=NULL;
01309 }
01310 }
01311 }
01312 if (posicion_del_punto==DENTRO) {
01313 MgraColor(M_DEFAULT,(double)color);
01314 MgraFill(M_DEFAULT,buffer,punto.x,punto.y);
01315
01316
01317
01318 }
01319 }
01320
01327 bool Punto_en_Buffer (Pt2D Punto)
01328 {
01329 if (Punto.x>=0 && Punto.x<=Buffer_Width)
01330 {
01331 if (Punto.y>=0 && Punto.y<=Buffer_Height) return TRUE;
01332 }
01333 return FALSE;
01334 }