/home/laradmin/lar/perception/lidar_egomotion/src/mbicp/calcul.c
Go to the documentation of this file.00001 /************************************************************************************************** 00002 Software License Agreement (BSD License) 00003 00004 Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt 00005 All rights reserved. 00006 00007 Redistribution and use in source and binary forms, with or without modification, are permitted 00008 provided that the following conditions are met: 00009 00010 *Redistributions of source code must retain the above copyright notice, this list of 00011 conditions and the following disclaimer. 00012 *Redistributions in binary form must reproduce the above copyright notice, this list of 00013 conditions and the following disclaimer in the documentation and/or other materials provided 00014 with the distribution. 00015 *Neither the name of the University of Aveiro nor the names of its contributors may be used to 00016 endorse or promote products derived from this software without specific prior written permission. 00017 00018 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 00019 IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 00020 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 00021 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 00022 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 00023 DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER 00024 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT 00025 OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00026 ***************************************************************************************************/ 00027 /***************************************************/ 00028 /* Last Revised: 00029 $Id: calcul.c 8465 2009-12-16 00:44:13Z gbiggs $ 00030 */ 00031 /***************************************************/ 00032 /* 00033 * This program is free software; you can redistribute it and/or modify 00034 * it under the terms of the GNU General Public License as published by 00035 * the Free Software Foundation; either version 2 of the License, or 00036 * (at your option) any later version. 00037 * 00038 * This program is distributed in the hope that it will be useful, 00039 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00040 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00041 * GNU General Public License for more details. 00042 * 00043 * You should have received a copy of the GNU General Public License 00044 * along with this program; if not, write to the Free Software 00045 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00046 * 00047 */ 00048 00054 #include "calcul.h" 00055 00056 00057 00058 void transfor_directa_p(float x, float y, 00059 Tsc *sistema, Tpf *sol){ 00060 00061 /* Esta funcion transforma el punto x,y en el sistema de coordenadas mas global*/ 00062 /* Es decir las coordenadas x y son vistas desde el sistema de coordenadas sistema*/ 00063 /* Y se las quiere transformar en el sistema de ref desde el que se sistema*/ 00064 /* Es la transformacion directa */ 00065 00066 float SinT,CosT; 00067 00068 SinT=(float)sin(sistema->tita); 00069 CosT=(float)cos(sistema->tita); 00070 00071 sol->x=x*CosT-y*SinT+sistema->x; 00072 sol->y=x*SinT+y*CosT+sistema->y; 00073 00074 //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); 00075 00076 } 00077 00078 void transfor_directa_pt0(float x, float y, 00079 Tsc *sistema, Tpf *sol){ 00080 00081 /* Esta funcion transforma el punto x,y en el sistema de coordenadas mas global*/ 00082 /* Es decir las coordenadas x y son vistas desde el sistema de coordenadas sistema*/ 00083 /* Y se las quiere transformar en el sistema de ref desde el que se sistema*/ 00084 /* Es la transformacion directa */ 00085 00086 sol->x=x+sistema->x; 00087 sol->y=y+sistema->y; 00088 00089 } 00090 00091 00092 void transfor_inversa_p(float x,float y, 00093 Tsc *sistema, Tpf *sol){ 00094 00095 /* Esta funcion transforma el punto x,y en el sistema de coordenadas que entra*/ 00096 /* Las coordenadas x y se ven desde el sistema de coordenadas desde el que se tienen las */ 00097 /* las coordenadas de sistema */ 00098 /* Es la transformacion directa */ 00099 00100 float a13, a23; 00101 float SinT,CosT; 00102 00103 SinT=(float)sin(sistema->tita); 00104 CosT=(float)cos(sistema->tita); 00105 00106 00107 a13=-sistema->y*SinT-sistema->x*CosT; 00108 a23=-sistema->y*CosT+sistema->x*SinT; 00109 00110 sol->x=x*CosT+y*SinT+a13; 00111 sol->y=-x*SinT+y*CosT+a23; 00112 } 00113 00114 float NormalizarPI(float ang){ 00115 00116 return (float)(ang+(2*M_PI)*floor((M_PI-ang)/(2*M_PI))); 00117 } 00118 00119 void inversion_sis(Tsc *sisIn, Tsc *sisOut){ 00120 00121 float c,s; 00122 00123 c=(float)cos(sisIn->tita); 00124 s=(float)sin(sisIn->tita); 00125 sisOut->x =-c*sisIn->x-s*sisIn->y; 00126 sisOut->y = s*sisIn->x-c*sisIn->y; 00127 sisOut->tita = NormalizarPI(-sisIn->tita); 00128 } 00129 00130 void composicion_sis(Tsc *sis1,Tsc *sis2,Tsc *sisOut){ 00131 00132 Tpf sol; 00133 00134 transfor_directa_p(sis2->x, sis2->y, 00135 sis1, &sol); 00136 sisOut->x=sol.x; 00137 sisOut->y=sol.y; 00138 sisOut->tita = NormalizarPI(sis1->tita+sis2->tita); 00139 00140 } 00141 00142 void car2pol(Tpf *in, Tpfp *out){ 00143 00144 out->r=(float)sqrt(in->x*in->x+in->y*in->y); 00145 out->t=(float)atan2(in->y,in->x); 00146 } 00147 00148 void pol2car(Tpfp *in, Tpf *out){ 00149 00150 out->x=in->r*(float)cos(in->t); 00151 out->y=in->r*(float)sin(in->t); 00152 } 00153 00154 00155 00156 00157 int corte_segmentos(float x1,float y1,float x2,float y2, 00158 float x3,float y3,float x4,float y4, 00159 Tpf *sol){ 00160 /* corte de segmentos */ 00161 /* TE DEVUELVE EL PUNTO DE CORTE EN EL SISTEMA QUE ESTEN LOS SEGMENTOS */ 00162 00163 float a1,a2,b1,b2,c1,c2,xm,ym,denominador,max1_x,max1_y,min1_x,min1_y; 00164 float xerr,yerr; 00165 int si1; 00166 float error_redondeo; 00167 00168 error_redondeo=(float)0.00001F; 00169 00170 /* primera recta */ 00171 a1=y2-y1; 00172 b1=x1-x2; 00173 c1=y1*(-b1)-x1*a1; 00174 00175 /* segunda recta */ 00176 a2=y4-y3; 00177 b2=x3-x4; 00178 c2=y3*(-b2)-x3*a2; 00179 00180 00181 denominador=a1*b2-a2*b1; 00182 if (denominador==0) 00183 return 0; 00184 else{ 00185 xm=(b1*c2-b2*c1)/denominador; 00186 ym=(c1*a2-c2*a1)/denominador; 00187 00188 xerr=xm+error_redondeo; 00189 yerr=ym+error_redondeo; 00190 00191 /* Comprobamos que cae entre los segmantos */ 00192 if (x1>x2){ 00193 max1_x=x1; min1_x=x2; 00194 } 00195 else{ 00196 max1_x=x2; min1_x=x1; 00197 } 00198 if (y1>y2){ 00199 max1_y=y1; min1_y=y2; 00200 } 00201 else{ 00202 max1_y=y2; min1_y=y1; 00203 } 00204 si1=0; 00205 if (max1_x+error_redondeo>=xm && xerr>=min1_x && max1_y+error_redondeo>=ym && yerr>=min1_y) 00206 si1=1; 00207 00208 00209 if (si1){ 00210 00211 if (x3>x4){ 00212 max1_x=x3; min1_x=x4; 00213 } 00214 else{ 00215 max1_x=x4; min1_x=x3; 00216 } 00217 if (y3>y4){ 00218 max1_y=y3; min1_y=y4; 00219 } 00220 else{ 00221 max1_y=y4; min1_y=y3; 00222 } 00223 00224 if (max1_x+error_redondeo>=xm && xerr>=min1_x && max1_y+error_redondeo>=ym && yerr>=min1_y){ 00225 sol->x=xm; 00226 sol->y=ym; 00227 return 1; 00228 } 00229 } 00230 return 0; 00231 } 00232 } 00233