double ptod( double ax, double ay, double bx, double by ){ double A; A = square( bx - ax ) + square( by - ay ); A = sqrtd( A ); return A; }
uint8_t trilaterate( uint8_t i1, uint8_t i2, uint8_t i3 ){ double x_1, y_1; double x_2, y_2; double x_3, y_3; double r1, r2, r3; double dsq, gam; double xa, ya, xb, yb; double xt1, yt1; double xt2, yt2; double d1, d2; // get positions of anchor x_1 = anchor[i1].x; y_1 = anchor[i1].y; x_2 = anchor[i2].x; y_2 = anchor[i2].y; x_3 = anchor[i3].x; y_3 = anchor[i3].y; r1 = anchor[i1].d; r2 = anchor[i2].d; r3 = anchor[i3].d; // Find the two intersection points of the two circles dsq = square(x_2-x_1) + square(y_2-y_1); gam = (square(r2+r1) - dsq) * (dsq - square(r2-r1)); #ifdef __DEBUG_MODE__ dbg( 1, anchor[i1].id, x_1, y_1, r1 ); dbg( 2, anchor[i2].id, x_2, y_2, r2 ); dbg( 3, anchor[i3].id, x_3, y_3, r3 ); dbg_out(); // dbg( 1, 364, r1, r2, r3 ); // dbg( 2, 370, dsq, square(x_2-x_1), square(y_2-y_1) ); // dbg( 3, 371, gam, (square(r2+r1) - dsq), (dsq - square(r2-r1)) ); // dbg_out(); #endif if( gam < 0 || dsq <=0 ){ return ERROR_NONINTERSECT; } gam = sqrtd( gam ); xa = -(square(r2) - square(r1)) * (x_2-x_1)/(2.0 * dsq) + (x_1+x_2)*0.5; ya = -(square(r2) - square(r1)) * (y_2-y_1)/(2.0 * dsq) + (y_1+y_2)*0.5; xb = (y_2-y_1) * gam/(2.0*dsq); yb = (x_2-x_1) * gam/(2.0*dsq); xt1 = xa - xb; xt2 = xa + xb; yt1 = ya + yb; yt2 = ya - yb; // disambiguate between the two points using the third node d1 = sqrtd( square( xt1 - x_3 ) + square( yt1 - y_3 )); d2 = sqrtd( square( xt2 - x_3 ) + square( yt2 - y_3 )); if( dabs( d1 - r3 ) < dabs( d2 - r3 ) ){ delta_x = xt1; delta_y = yt1; } else { delta_x = xt2; delta_y = yt2; } return 0; }
DOUBLE /*{ ret - acosd(x) }*/ acosd ( DOUBLE x /*{ (i) - input value x }*/ ) { DOUBLE y, g; DOUBLE num, den, result; LONG i; /*{ y = |x| }*/ y = x; if (y < (DOUBLE)0.0) { y = -y; } /*{ if y > 0.5 }*/ if (y > (DOUBLE)0.5) { /*{ set i = 0 }*/ i = 0; /*{ if y > 0, return error }*/ if (y > (DOUBLE)1.0) { result = 0.0; return result; } /*{ g = (1 - y)/2 }*/ g = SUBD(1.0, y); g = MPYD(0.5, g); /*{ y = -2/sqrt(g) }*/ y = sqrtd(g); y = MPYD(y, -2.0); } /*{ else y <= 0.5 }*/ else { /*{ set i = 1 }*/ i = 1; /*{ if y < eps }*/ if (y < EPS_DOUBLE) { result = y; /*{ if x < 0, result = PI/2 + result }*/ /* (but more mathematically stable) */ if (x < (DOUBLE)0.0) { result = ADDD(PI_4, result); result = ADDD(result, PI_4); } /*{ else x >=0, result = PI/2 - result }*/ /* (but more mathematically stable) */ else { result = SUBD(PI_4, result); result = ADDD(result, PI_4); } /*{ return result }*/ return result; } /*{ g = y * y }*/ g = MPYD(y, y); } /*{ result = y + y*R(g) }*/ /*{!INDENT}*/ /*{ R(g) = g*P(g)/Q(g) }*/ /*{ P(g) = (((p5 * g + p4) * g + p3) * g + p2) * g + p1 }*/ num = MPYD(ASINDP_COEF5, g); num = ADDD(num, ASINDP_COEF4); num = MPYD(num, g); num = ADDD(num, ASINDP_COEF3); num = MPYD(num, g); num = ADDD(num, ASINDP_COEF2); num = MPYD(num, g); num = ADDD(num, ASINDP_COEF1); num = MPYD(num, g); /*{ Q(g) = ((((g + q4) * g + q3) * g + q2) * g + q1) * g + q0 }*/ den = ADDD(g, ASINDQ_COEF4); den = MPYD(den, g); den = ADDD(den, ASINDQ_COEF3); den = MPYD(den, g); den = ADDD(den, ASINDQ_COEF2); den = MPYD(den, g); den = ADDD(den, ASINDQ_COEF1); den = MPYD(den, g); den = ADDD(den, ASINDQ_COEF0); result = DIVD(num, den); result = MPYD(result, y); result = ADDD(result, y); /*{!OUTDENT}*/ /*{ if x < 0 }*/ if (x < (DOUBLE)0.0) { /*{ if i == 0, result = PI + result }*/ /* (but more mathematically stable) */ if (i == 0) { result = ADDD(result, PI_2); result = ADDD(result, PI_2); } /*{ if i == 1, result = PI/2 + result }*/ /* (but more mathematically stable) */ else { result = ADDD(PI_4, result); result = ADDD(result, PI_4); } } /*{ else x >= 0 }*/ else { /*{ if i == 1, result = PI/2 - result }*/ /* (but more mathematically stable) */ if (i == 1) { result = SUBD(PI_4, result); result = ADDD(result, PI_4); } /*{ if i == 0, result = -result }*/ else { result = -result; } } /*{ return result }*/ return result; }