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;
}
