Página principal   Lista alfabética   Lista de componentes   Lista de archivos   Miembros de las clases   Archivos de los miembros  

VirtualRobot.cpp

Ir a la documentación de este archivo.
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 // Inicializamos todos los punteros a objetos como NULL
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 //      Alocatamos los displays y buffers necesarios
00074         MdispAlloc (sys,M_DEFAULT,"M_DEFAULT",M_DEFAULT,&*disp);
00075 //      MdispAlloc (sys,M_DEFAULT,"M_DEFAULT",M_DEFAULT,&WorkingDisplay);       //Muestra el Buffer de trabajo
00076 //      MdispAlloc (sys,M_DEFAULT,"M_DEFAULT",M_DEFAULT,&RefreshDisplay);       //Muestra el Buffer de refresco
00077 //      MdispAlloc (sys,M_DEFAULT,"M_DEFAULT",M_DEFAULT,&SonaresDisplay);       //Muestra el Buffer del sonar
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 //      MbufAllocColor (sys,3,ancho,alto,8,M_IMAGE+M_DISP+M_PROC,&SonaresBuffer);
00082         MbufClear (*mapa,255);
00083         MdispSelectWindow (*disp,*mapa,WindowHandle);
00084 //      MdispSelect (WorkingDisplay,WorkingBuffer);
00085 //      MdispSelect (RefreshDisplay,RefreshBuffer);
00086 //      MdispSelect (SonaresDisplay,SonaresBuffer);
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 //      Definimos una Zona de Alta densidad por defecto en caso de que
00099 //      el usuario habilite el uso de esta antes de definir una el mismo
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 //      Calculamos la matriz de cambio de base y su inversa
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 //      Recalculamos la Matriz de cambio de base y su inversa
00155         alto=AlturaImagen*ESCALA_PixPorMM;
00156         ESCALA_PixPorMM=1/escala;
00157         AlturaImagen=(double)alto*escala;
00158 //      modo=ESTACIONARIO;      esta linea esta aqui para pruebas
00159         Base=IniciarMatrizCambioBase (&Giro,&Traslacion,&Simetria_X,AlturaImagen,
00160                                                                         DesplazamientoH,DesplazamientoV,AnguloGiro,
00161                                                                         EjeGiroX,EjeGiroY);
00162         InversaBase=Inversa3X3(Base);
00163 
00164 //      Repintamos la toda escena
00165         MbufClear (WorkingBuffer,255);  //Pintamos el WorkingBuffer de blanco
00166                                                                         //para que en caso de no pintar ningun mundo
00167                                                                         //no se produzcan errores
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 //      MbufFree(SonaresBuffer);
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);      //Copiamos al buffer que se muestra
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 //      PosicionActual.x=x;
00273 //      PosicionActual.y=y;
00274 
00275 //      Definimos físicamente el robot
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 //      Definimos físicamente las camaras
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                         //      Centrado en el Mundo
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                         //      Centrado en el Robot
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);  //Pintamos el WorkingBuffer de blanco
00356                                                                                         //para que en caso de no pintar ningun mundo
00357                                                                                         //no se produzcan errores
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 //      Actualizamos la lista de lecturas con otras nuevas
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);   //vacia el buffer frontal y trasero de Saphira
00413         sfSetSideBuffer (0);    //vacia los buffers laterales de Saphira
00414 }
00415 /*
00416 DLL_EXPORT void STD Tratar_Lecturas_Sonares (void)
00417 {
00418         int             i;
00419         Pt2D    Punto_Pantalla;
00420 
00421 //      MbufClear(MilOverlayBuffer, TransparentColor);
00422         MgraColor (M_DEFAULT,M_RGB888(17,140,255));
00423         for (i=0;i<LISTA_MAX;i++)
00424         {
00425                 if (ListaFIFO[i].x!=0 && ListaFIFO[i].y!=0)
00426                 {
00427                         Punto_Pantalla=MultVect(Base,ListaFIFO[i]);
00428                         MgraArc (M_DEFAULT,WorkingBuffer,(long)Punto_Pantalla.x,(long)Punto_Pantalla.y,5,5,0,360);
00429                 }
00430         }               
00431         MgraColor (M_DEFAULT,0);
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         FUNCIONES DE BAJO NIVEL
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 //      Calculos necesarios para la obtencion de la matriz de cambio de base
00596         IniTraslacion (b,DesplazamientoH,AlturaImagen-DesplazamientoV);
00597         IniGiroRespectoPunto (a,AnguloGiro,-EjeGiroX,-EjeGiroY);
00598         IniSimetriaX (c);
00599         IniEscalado (ESCALA_PixPorMM,ESCALA_PixPorMM,MatrizEscalado);
00600 //      IniEscaladoRespectoPunto (ESCALA_PixPorMM,ESCALA_PixPorMM,
00601 //                                                      EjeGiroX,EjeGiroY,MatrizEscalado);
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 //      Calculamos las matrices necesarias para poscicionar y orientar el robot
00641 //  en la imagen, y almacenamos la matriz resultante en "MatrizRobot"
00642         IniTraslacion((*trasl),(*PosicionActual).x,(*PosicionActual).y);
00643         IniGiro((*giro),AnguloActual);
00644         mulmat ((*giro),(*trasl),e);
00645 //      Calculamos las matrices necesarias para poscicionar y orientar las cámaras
00646 //  en la imagen, y almacenamos la matriz resultante en "matrizcamaras"
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         // Inicializamon las matrices de giro y traslacion
00674         // de las camaras
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; //Coordenada X del centro de la cámara
00681         TraslCamara[2][1]=69.86;  //Coordenada Y del centro de la cámara
00682 
00683         // Comenzamos a multiplicar las matrices con el fin
00684         // de obtener la matriz de transformacion final
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 //      MgraDot(M_DEFAULT,RefreshBuffer,Coordenadas.centro.x,Coordenadas.centro.y);
00726 //      MgraDot(M_DEFAULT,MilUnderlayBuffer,Coordenadas.centro.x,Coordenadas.centro.y);
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;  //estos cambio son devidos a las
00903                 Punto.y = SonarBuffer[i].x;       //diferentes referencias
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);        //Obtenemos el numero de objetos del mundo
00969 
00970         for (i=0;i<NumeroObjetos;i++)
00971         {
00972                 for (m=0;objetos[m]!=NULL;m++); // buscamos el primer objeto de la lista
00973                                                                                 // que no este definido y por tanto sea NULL
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);  //Obtenemos el nunero de puntos que 
00992                                                                                 // tiene el contorno
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)  //si el objeto es movil tenemos que coger
01004                 {                                               //su posicion y horientacion
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 //      Leemos la poscion y horientación que tiene que tener el robot en el mapa
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 //      MgraArc (M_DEFAULT,WorkingBuffer,
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) {               // En caso que pintemos el mundo para estar quieto
01167                         MbufClear(WorkingBuffer,0);     // pintamos el fondo negro y
01168                         MgraColor (M_DEFAULT,255);      // el contorno de blanco
01169                 }
01170                 else { 
01171                         MbufClear(WorkingBuffer,255);   // si no pintamos el fondo blanco y
01172                         MgraColor (M_DEFAULT,0);                // el contorno negro
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                 // rellenamos el objeto de blanco porque es el contorno de la habitacion
01191                 if (modo==ESTACIONARIO){ RellenarObjeto (ObjetoMundo,WorkingBuffer,255);}
01192         }
01193         else if (strncmp(objeto->codigo,"002",3)==0)
01194         {
01195                 //      Calculamos la matriz de posicion y horientacion del objeto en cuestion
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                 // rellenamos el objeto pero de negro
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 /* COPIAMOS LOS DATOS DE LA ESTRUCTURA DE LISTA A LA 
01251         ESTRUCTURA USADA POR EL ALGORITMO POINT_IN_POLY */
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 /* MIRAMOS LOS 8 VECINOS DE CADA NODO HASTA ENCONTRAR 
01261         UN PUNTO INTERIOR AL POLIGONO */
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){ //Este SWITCH se encarga de 
01302                         switch (Tolerancia)                     //afinar la tolerancia en caso
01303                         {                                                       //de no encontrar ningun
01304                         case 10:                                        //punto interno despues de
01305                                 Tolerancia=2;                   //repasar todo los puntos 
01306                                 break;                                  //del polígono
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 /*              MgraColor(M_DEFAULT,M_RGB888(50,0,0));
01316                 MgraArc (M_DEFAULT,buffer,punto.x,punto.y,10,10,0,350);
01317                 MgraColor(M_DEFAULT,(double)color);*/
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 }

Generado el Tue Apr 24 06:55:49 2001 para Dllcontrol por doxygen1.2.6 escrito por Dimitri van Heesch, © 1997-2001