calcul.c
Go to the documentation of this file.
00001 /***************************************************/
00002 /* Last Revised: 
00003 $Id: calcul.c 4129 2007-08-21 23:16:24Z gerkey $
00004 */
00005 /***************************************************/
00006 
00007 
00008 #include "calcul.h"
00009 
00010 
00011 
00012 void transfor_directa_p(float x, float y, 
00013                         Tsc *sistema, Tpf *sol){
00014 
00015   /* Esta funcion transforma el punto x,y en el sistema de coordenadas mas global*/
00016   /* Es decir las coordenadas x y son vistas desde el sistema de coordenadas sistema*/
00017   /* Y se las quiere transformar en el sistema de ref desde el que se sistema*/
00018   /* Es la transformacion directa */
00019 
00020   float SinT,CosT;
00021 
00022   SinT=(float)sin(sistema->tita);
00023   CosT=(float)cos(sistema->tita);
00024  
00025   sol->x=x*CosT-y*SinT+sistema->x;
00026   sol->y=x*SinT+y*CosT+sistema->y;
00027 
00028   //fprintf(stderr,"input:<%f,%f> sis:<%f %f %f> sol:<%f %f>\n",x,y,sistema->x, sistema->y, sistema->tita,sol->x, sol->y);
00029 
00030 }
00031 
00032 void transfor_directa_pt0(float x, float y, 
00033                         Tsc *sistema, Tpf *sol){
00034 
00035   /* Esta funcion transforma el punto x,y en el sistema de coordenadas mas global*/
00036   /* Es decir las coordenadas x y son vistas desde el sistema de coordenadas sistema*/
00037   /* Y se las quiere transformar en el sistema de ref desde el que se sistema*/
00038   /* Es la transformacion directa */
00039 
00040   sol->x=x+sistema->x;
00041   sol->y=y+sistema->y;
00042 
00043 }
00044 
00045 
00046 void transfor_inversa_p(float x,float y,
00047                         Tsc *sistema, Tpf *sol){
00048 
00049   /* Esta funcion transforma el punto x,y en el sistema de coordenadas que entra*/
00050   /* Las coordenadas x y se ven desde el sistema de coordenadas desde el que se tienen las */
00051   /* las coordenadas de sistema */
00052   /* Es la transformacion directa */
00053  
00054   float a13, a23;
00055   float SinT,CosT;
00056 
00057   SinT=(float)sin(sistema->tita);
00058   CosT=(float)cos(sistema->tita);
00059 
00060 
00061   a13=-sistema->y*SinT-sistema->x*CosT;
00062   a23=-sistema->y*CosT+sistema->x*SinT;
00063     
00064   sol->x=x*CosT+y*SinT+a13;
00065   sol->y=-x*SinT+y*CosT+a23;
00066 }  
00067 
00068 float NormalizarPI(float ang){
00069 
00070   return (float)(ang+(2*M_PI)*floor((M_PI-ang)/(2*M_PI)));
00071 }
00072 
00073 void inversion_sis(Tsc *sisIn, Tsc *sisOut){
00074 
00075   float c,s;
00076 
00077   c=(float)cos(sisIn->tita);
00078   s=(float)sin(sisIn->tita);
00079   sisOut->x =-c*sisIn->x-s*sisIn->y;
00080   sisOut->y = s*sisIn->x-c*sisIn->y;
00081   sisOut->tita = NormalizarPI(-sisIn->tita);
00082 }
00083 
00084 void composicion_sis(Tsc *sis1,Tsc *sis2,Tsc *sisOut){
00085 
00086   Tpf sol;
00087 
00088   transfor_directa_p(sis2->x, sis2->y, 
00089                      sis1, &sol);
00090   sisOut->x=sol.x;
00091   sisOut->y=sol.y;
00092   sisOut->tita = NormalizarPI(sis1->tita+sis2->tita);
00093   
00094 }
00095 
00096 void car2pol(Tpf *in, Tpfp *out){
00097   
00098   out->r=(float)sqrt(in->x*in->x+in->y*in->y);
00099   out->t=(float)atan2(in->y,in->x);
00100 }
00101 
00102 void pol2car(Tpfp *in, Tpf *out){
00103   
00104   out->x=in->r*(float)cos(in->t);
00105   out->y=in->r*(float)sin(in->t);
00106 }
00107 
00108 
00109 
00110 
00111 int corte_segmentos(float x1,float y1,float x2,float y2,
00112                     float x3,float y3,float x4,float y4,
00113                     Tpf *sol){
00114 /* corte de segmentos */
00115 /* TE DEVUELVE EL PUNTO DE CORTE EN EL SISTEMA QUE ESTEN LOS SEGMENTOS */
00116  
00117   float a1,a2,b1,b2,c1,c2,xm,ym,denominador,max1_x,max1_y,min1_x,min1_y;
00118   float xerr,yerr;
00119   int si1;
00120   float error_redondeo;
00121 
00122   error_redondeo=(float)0.00001F;
00123 
00124   /* primera recta */
00125   a1=y2-y1;
00126   b1=x1-x2;
00127   c1=y1*(-b1)-x1*a1; 
00128 
00129   /* segunda recta */
00130   a2=y4-y3;
00131   b2=x3-x4;
00132   c2=y3*(-b2)-x3*a2;
00133 
00134   
00135   denominador=a1*b2-a2*b1;
00136   if (denominador==0)
00137     return 0;
00138   else{
00139     xm=(b1*c2-b2*c1)/denominador;
00140     ym=(c1*a2-c2*a1)/denominador;
00141 
00142     xerr=xm+error_redondeo;
00143     yerr=ym+error_redondeo;
00144 
00145     /* Comprobamos que cae entre los segmantos */
00146     if (x1>x2){
00147       max1_x=x1; min1_x=x2;
00148     }
00149     else{
00150       max1_x=x2; min1_x=x1;
00151     }
00152     if (y1>y2){
00153       max1_y=y1; min1_y=y2;
00154     }
00155     else{
00156       max1_y=y2; min1_y=y1;
00157     }
00158     si1=0;
00159     if (max1_x+error_redondeo>=xm && xerr>=min1_x &&  max1_y+error_redondeo>=ym && yerr>=min1_y)
00160       si1=1;
00161     
00162 
00163     if (si1){
00164 
00165       if (x3>x4){
00166         max1_x=x3; min1_x=x4;
00167       }
00168       else{
00169         max1_x=x4; min1_x=x3;
00170       }
00171       if (y3>y4){
00172         max1_y=y3; min1_y=y4;
00173       }
00174       else{
00175         max1_y=y4; min1_y=y3;
00176       }
00177       
00178       if (max1_x+error_redondeo>=xm && xerr>=min1_x &&  max1_y+error_redondeo>=ym && yerr>=min1_y){
00179         sol->x=xm;
00180         sol->y=ym;
00181         return 1;
00182       }
00183     }
00184     return 0;
00185   }
00186 }
00187 


csm_ros
Author(s): Gaƫl Ecorchard , Ivan Dryanovski, William Morris, Andrea Censi
autogenerated on Sat Jun 8 2019 19:52:42