/* get the position of the robot from the angle of the 3 beacons */
int8_t angles_to_posxy(point_t *pos, double a01, double a12, double a20)
{
circle_t c01, c12, c20;
point_t dummy_pt, p1, p2, p3;
dprintf("a01 = %2.2f\n", a01);
dprintf("a12 = %2.2f\n", a12);
dprintf("a20 = %2.2f\n", a20);
if (angle_to_circles(&c01, NULL, &beacon0, &beacon1, a01))
return -1;
dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c01.x, c01.y, c01.r);
if (angle_to_circles(&c12, NULL, &beacon1, &beacon2, a12))
return -1;
dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c12.x, c12.y, c12.r);
if (angle_to_circles(&c20, NULL, &beacon2, &beacon0, a20))
return -1;
dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c20.x, c20.y, c20.r);
if (circle_intersect(&c01, &c12, &p1, &dummy_pt) == 0)
return -1;
if (circle_intersect(&c12, &c20, &p2, &dummy_pt) == 0)
return -1;
if (circle_intersect(&c20, &c01, &dummy_pt, &p3) == 0)
return -1;
dprintf("p1: x=%2.2f y=%2.2f\n", p1.x, p1.y);
dprintf("p2: x=%2.2f y=%2.2f\n", p2.x, p2.y);
dprintf("p3: x=%2.2f y=%2.2f\n", p3.x, p3.y);
/* if (xy_norm(p1.x, p1.y, p2.x, p2.y) > POS_ACCURACY || */
/* xy_norm(p2.x, p2.y, p3.x, p3.y) > POS_ACCURACY || */
/* xy_norm(p3.x, p3.y, p1.x, p1.y) > POS_ACCURACY) */
/* return -1; */
pos->x = (p1.x + p2.x + p3.x) / 3.0;
pos->y = (p1.y + p2.y + p3.y) / 3.0;
return 0;
}
/* intersect the circle formed by one distance info with the circle
* crossing the 2 beacons */
static int8_t
ad_to_posxy_one(point_t *pos,
const point_t *b0, const point_t *b1, /* beacon position */
const circle_t *cd, const circle_t *c01, /* circles to intersect */
double a01) /* seen angle of beacons */
{
double tmp_a01_p1, tmp_a01_p2;
point_t p1, p2;
uint8_t p1_ok=0, p2_ok=0;
if (circle_intersect(c01, cd, &p1, &p2) == 0)
return -1;
dprintf("p1: x=%2.2f y=%2.2f\n", p1.x, p1.y);
dprintf("p2: x=%2.2f y=%2.2f\n", p2.x, p2.y);
dprintf("real a01: %2.2f\n", a01);
tmp_a01_p1 = atan2(b1->y-p1.y, b1->x-p1.x) -
atan2(b0->y-p1.y, b0->x-p1.x);
if (tmp_a01_p1 > M_PI)
tmp_a01_p1 -= 2*M_PI;
if (tmp_a01_p1 < -M_PI)
tmp_a01_p1 += 2*M_PI;
dprintf("a01-1: %2.2f\n", tmp_a01_p1);
tmp_a01_p2 = atan2(b1->y-p2.y, b1->x-p2.x) -
atan2(b0->y-p2.y, b0->x-p2.x);
if (tmp_a01_p2 > M_PI)
tmp_a01_p2 -= 2*M_PI;
if (tmp_a01_p2 < -M_PI)
tmp_a01_p2 += 2*M_PI;
dprintf("a01-2: %2.2f\n", tmp_a01_p2);
/* in some conditions, we already know the position of the
* robot at this step */
p1_ok = is_pt_in_area(p1) &&
abs_dbl(tmp_a01_p1 - a01) < ANGLE_EPSILON;
p2_ok = is_pt_in_area(p2) &&
abs_dbl(tmp_a01_p2 - a01) < ANGLE_EPSILON;
if (!p1_ok && p2_ok) {
*pos = p2;
return 0;
}
if (p1_ok && !p2_ok) {
*pos = p1;
return 0;
}
return -1;
}
static PyObject *web_circle_intersect(PyObject *self, PyObject *args)
{
double x1,y1,r1,x2,y2,r2;
/* Parse the input tuple */
if (!PyArg_ParseTuple(args, "dddddd", &x1,&y1,&r1,&x2,&y2,&r2))
return NULL;
/* Call the external C function to compute the area. */
double *intersect = circle_intersect(x1,y1,r1,x2,y2,r2);
/* Build the output tuple */
PyObject *ret = Py_BuildValue("[d,d,d,d]",intersect[0],intersect[1],intersect[2],intersect[3]);
return ret;
}
/*
* return values:
* 0 dont cross
* 1 one intersection point
* 2 two intersection points
*
* p1, p2 arguments are the crossing points coordinates. Both p1 and
* p2 are dummy for 0 result. When result is 1, p1 and p2 are set to
* the same value.
*/
int circle_intersect(const circle_t *c1, const circle_t *c2,
point_t *p1, point_t *p2)
{
circle_t ca, cb;
float a, b, c, d, e;
uint8_t ret = 0;
/* We have to assume that either delta_x or delta_y is not zero to avoid
* the corner case of the two coincident circles. If we assume that, for
* example, delta_y is never zero, but in reality gets really close to zero,
* it creates numerical stability problems. To avoid it, we check which of
* delta_x or delta_y is smaller and assume the other cannot be zero.
*/
if (fabs(c1->y - c2->y) < fabs(c1->x - c2->x)) {
point_t pa, pb;
ca.x = c1->y;
ca.y = c1->x;
ca.r = c1->r;
cb.x = c2->y;
cb.y = c2->x;
cb.r = c2->r;
ret = circle_intersect(&ca, &cb, &pa, &pb);
p1->x=pa.y;
p1->y=pa.x;
p2->y=pb.x;
p2->x=pb.y;
return ret;
}
/* create circles with same radius, but centered on 0,0 : it
* will make process easier */
ca.x = 0;
ca.y = 0;
ca.r = c1->r;
cb.x = c2->x - c1->x;
cb.y = c2->y - c1->y;
cb.r = c2->r;
/* inspired from http://www.loria.fr/~roegel/notes/note0001.pdf
* which can be found in doc. */
a = 2.0f * cb.x;
b = 2.0f * cb.y;
c = sq(cb.x) + sq(cb.y) - sq(cb.r) + sq(ca.r);
d = sq(2.0f * a * c) -
(4.0f * (sq(a) + sq(b)) * (sq(c) - sq(b) * sq(ca.r)) );
/* no intersection */
if (d < 0.0f)
return 0;
if (fabsf(b) < 0.0001f) {
/* special case */
e = sq(cb.r) - sq((2.0f * c - sq(a)) / (2.0f * a));
/* no intersection */
if (e < 0.0f)
return 0;
p1->x = (2.0f * a * c - sqrtf(d)) / (2.0f * (sq(a) + sq(b)));
p1->y = sqrtf(e);
p2->x = p1->x;
p2->y = p1->y;
ret = 1;
}
else {
/* usual case */
p1->x = (2.0f * a * c - sqrtf(d)) / (2.0f * (sq(a) + sq(b)));
p1->y = (c - a * p1->x) / b;
p2->x = (2.0f * a * c + sqrtf(d)) / (2.0f * (sq(a) + sq(b)));
p2->y = (c - a * p2->x) / b;
ret = 2;
}
/* retranslate */
p1->x += c1->x;
p1->y += c1->y;
p2->x += c1->x;
p2->y += c1->y;
return ret;
}
