Triangulation Algorithms for Mobile Robot Positioning: C source code library
|
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