Triangulation Algorithms for Mobile Robot Positioning: C source code library
pierlot.c
Go to the documentation of this file.
00001 /**
00002  * @file pierlot.c
00003  * @brief Implementation of the ToTal algorithm   
00004  * @date 15/02/2011
00005  * @author Vincent Pierlot
00006 */
00007 #include  <math.h>
00008 #include  "const.h"
00009 #include  "pierlot.h"
00010 
00011 tfloat triangulationPierlot(tfloat *x, tfloat *y,
00012                         tfloat alpha1, tfloat alpha2, tfloat alpha3,
00013                         tfloat x1, tfloat y1, tfloat x2, tfloat y2, tfloat x3, tfloat y3)
00014 {
00015     tfloat cot_12 = Cot( alpha2 - alpha1 ) ;
00016     tfloat cot_23 = Cot( alpha3 - alpha2 ) ;
00017     tfloat cot_31 = ( 1.0 - cot_12 * cot_23 ) / ( cot_12 + cot_23 ) ;
00018     cot_12 = ajust_value_to_bounds( cot_12 , COT_MAX ) ;
00019     cot_23 = ajust_value_to_bounds( cot_23 , COT_MAX ) ;
00020     cot_31 = ajust_value_to_bounds( cot_31 , COT_MAX ) ;
00021     
00022     tfloat x1_ = x1 - x2 , y1_ = y1 - y2 , x3_ = x3 - x2 , y3_ = y3 - y2 ;
00023 
00024     tfloat c12x = x1_ + cot_12 * y1_ ;
00025     tfloat c12y = y1_ - cot_12 * x1_ ;
00026 
00027     tfloat c23x = x3_ - cot_23 * y3_ ;
00028     tfloat c23y = y3_ + cot_23 * x3_ ;
00029 
00030     tfloat c31x = (x3_ + x1_) + cot_31 * (y3_ - y1_) ;
00031     tfloat c31y = (y3_ + y1_) - cot_31 * (x3_ - x1_) ;
00032     tfloat k31 = (x3_ * x1_) + (y3_ * y1_) + cot_31 * ( (y3_ * x1_) - (x3_ * y1_) ) ;
00033   
00034   tfloat D = (c12x - c23x) * (c23y - c31y) - (c23x - c31x) * (c12y - c23y) ;
00035   tfloat invD = 1.0 / D ;
00036   tfloat K = k31 * invD ;
00037   
00038     *x = K * (c12y - c23y) + x2 ;
00039     *y = K * (c23x - c12x) + y2 ;
00040     
00041     return ( invD >= 0.0 ) ? invD : -invD ; /* return |1/D| */
00042 }
00043 
00044 tfloat triangulationPierlot2(tfloat *x, tfloat *y,
00045                         tfloat alpha1, tfloat alpha2, tfloat alpha3,
00046                         tfloat x1, tfloat y1, tfloat x2, tfloat y2, tfloat x3, tfloat y3)
00047 {
00048   tfloat alpha_12 = alpha2 - alpha1 ;
00049   tfloat alpha_23 = alpha3 - alpha2 ;
00050   tfloat alpha_31 = alpha1 - alpha3 ;
00051   
00052     tfloat x1_ , y1_ , x2_ , y2_ , x3_ , y3_ ;
00053     
00054     tfloat cot_12 , cot_23 , cot_31 , c12x , c12y , c23x , c23y , c31x , c31y , k , D , invD , K ;
00055   
00056   if ( alpha_12 == PI || alpha_12 == 0.0 )
00057   {
00058     x1_ = x1 - x3 , y1_ = y1 - y3 , x2_ = x2 - x3 , y2_ = y2 - y3 ;
00059     
00060     cot_23 = cot( alpha_23 ) ;
00061     
00062     c12x = (y1_ - y2_) ;
00063     c12y = (x2_ - x1_) ;
00064     k = (y1_ * x2_) - (x1_ * y2_) ;
00065       
00066       c23x = x2_ + cot_23 * y2_ ;
00067       c23y = y2_ - cot_23 * x2_ ;
00068 
00069       c31x = x1_ + cot_23 * y1_ ;
00070       c31y = y1_ - cot_23 * x1_ ;
00071   
00072     D = (c31x - c23x) * (c12y) + (c12x) * (c23y - c31y) ;
00073     invD = 1.0 / D ;
00074     K = k * invD ;
00075   
00076       *x = K * ( c23y - c31y ) + x3 ;
00077       *y = K * ( c31x - c23x ) + y3 ;
00078     
00079       return ( invD >= 0.0 ) ? invD : -invD ;
00080   }
00081   
00082   if ( alpha_23 == PI || alpha_23 == 0.0 )
00083   {
00084     x2_ = x2 - x1 , y2_ = y2 - y1 , x3_ = x3 - x1 , y3_ = y3 - y1 ;
00085     
00086     cot_31 = cot( alpha_31 ) ;
00087     
00088       c12x = x2_ + cot_31 * y2_ ;
00089       c12y = y2_ - cot_31 * x2_ ;
00090       
00091       c23x = (y2_ - y3_) ;
00092       c23y = (x3_ - x2_) ;
00093       k = (y2_ * x3_) - (x2_ * y3_) ;
00094 
00095       c31x = x3_ + cot_31 * y3_ ;
00096       c31y = y3_ - cot_31 * x3_ ;
00097   
00098     D = (c12x - c31x) * (c23y) + (c23x) * (c31y - c12y) ;
00099     invD = 1.0 / D ;
00100     K = k * invD ;
00101   
00102       *x = K * ( c31y - c12y ) + x1 ;
00103       *y = K * ( c12x - c31x ) + y1 ;
00104     
00105       return ( invD >= 0.0 ) ? invD : -invD ;
00106   }
00107   
00108   if ( alpha_31 == PI || alpha_31 == 0.0 )
00109   {
00110     x1_ = x1 - x2 , y1_ = y1 - y2 , x3_ = x3 - x2 , y3_ = y3 - y2 ;
00111     
00112     cot_12 = cot( alpha_12 ) ;
00113     
00114       c12x = x1_ + cot_12 * y1_ ;
00115       c12y = y1_ - cot_12 * x1_ ;
00116       
00117       c23x = x3_ + cot_12 * y3_ ;
00118       c23y = y3_ - cot_12 * x3_ ;
00119 
00120       c31x = (y3_ - y1_) ;
00121       c31y = (x1_ - x3_) ;
00122       k = (y3_ * x1_) - (x3_ * y1_) ;
00123   
00124     D = (c23x - c12x) * (c31y) + (c31x) * (c12y - c23y) ;
00125     invD = 1.0 / D ;
00126     K = k * invD ;
00127   
00128       *x = K * (c12y - c23y) + x2 ;
00129       *y = K * (c23x - c12x) + y2 ;
00130     
00131       return ( invD >= 0.0 ) ? invD : -invD ;
00132   }
00133   
00134   x1_ = x1 - x2 , y1_ = y1 - y2 , x3_ = x3 - x2 , y3_ = y3 - y2 ;
00135   
00136     cot_12 = cot( alpha_12 ) ;
00137     cot_23 = cot( alpha_23 ) ;
00138     cot_31 = ( 1.0 - cot_12*cot_23 ) / (cot_12 + cot_23) ;
00139     
00140     c12x = x1_ + cot_12 * y1_ ;
00141     c12y = y1_ - cot_12 * x1_ ;
00142 
00143     c23x = x3_ - cot_23 * y3_ ;
00144     c23y = y3_ + cot_23 * x3_ ;
00145 
00146     c31x = (x3_ + x1_) + cot_31 * (y3_ - y1_) ;
00147     c31y = (y3_ + y1_) - cot_31 * (x3_ - x1_) ;
00148     k = (x3_ * x1_) + (y3_ * y1_) + cot_31 * ( (y3_ * x1_) - (x3_ * y1_) ) ;
00149   
00150   D = (c12x - c23x) * (c23y - c31y) - (c23x - c31x) * (c12y - c23y) ;
00151   invD = 1.0 / D ;
00152   K = k * invD ;
00153   
00154     *x = K * (c12y - c23y) + x2 ;
00155     *y = K * (c23x - c12x) + y2 ;
00156     
00157     return ( invD >= 0.0 ) ? invD : -invD ; /* return |1/D| */
00158 }
00159