inline bool Line::detect_intersection(Line * other)
{
float a1 = line_side_test(other->p1);
float a2 = line_side_test(other->p2);
float b1 = other->line_side_test(p1);
float b2 = other->line_side_test(p2);
/* The product of two point based line side tests will be negative iff
* the points are not on strictly opposite sides of the line.
* If the product is 0, then at least one of the points is on the line not containing the points.
*/
return a1*a2 <= 0 && b1*b2 <= 0;
}
static void hmap_warp_check(carmen_point_t old_pose, carmen_point_t new_pose) {
int i;
double x1, x2, x3, x4, y1, y2, y3, y4;
static int warping = 0;
static char *warp_zone;
static carmen_point_t warp_pose;
static carmen_point_t warp_src;
static carmen_point_t warp_dst;
static carmen_point_t warp_post1;
static carmen_point_t warp_post2;
if (warping) {
if (line_side_test(warp_post1, warp_post2, warp_pose, new_pose) < 0) {
if (carmen_distance(&warp_pose, &new_pose) > 0.5) { //dbug: param?
carmen_map_change_map_zone(warp_zone);
carmen_ipc_sleep(1.0);
set_robot_pose(transform_robot_pose(warp_src, warp_dst));
warping = 0;
}
}
else
warping = 0;
}
else {
for (i = 0; i < hmap.num_links; i++) {
if (hmap.links[i].type == CARMEN_HMAP_LINK_DOOR) {
if (hmap.links[i].keys[0] == map_zone) {
if (segment_intersect(old_pose, new_pose, hmap.links[i].points[0], hmap.links[i].points[1])) {
warping = 1;
warp_zone = hmap.zone_names[hmap.links[i].keys[1]];
warp_pose = old_pose;
x1 = hmap.links[i].points[0].x;
x2 = hmap.links[i].points[1].x;
x3 = hmap.links[i].points[2].x;
x4 = hmap.links[i].points[3].x;
y1 = hmap.links[i].points[0].y;
y2 = hmap.links[i].points[1].y;
y3 = hmap.links[i].points[2].y;
y4 = hmap.links[i].points[3].y;
warp_src.x = (x1 + x2) / 2.0;
warp_src.y = (y1 + y2) / 2.0;
warp_src.theta = atan2(y2-y1, x2-x1);
warp_dst.x = (x3 + x4) / 2.0;
warp_dst.y = (y3 + y4) / 2.0;
warp_dst.theta = atan2(y4-y3, x4-x3);
warp_post1 = hmap.links[i].points[0];
warp_post2 = hmap.links[i].points[1];
}
}
else if (hmap.links[i].keys[1] == map_zone) {
if (segment_intersect(old_pose, new_pose, hmap.links[i].points[2], hmap.links[i].points[3])) {
warping = 1;
warp_zone = hmap.zone_names[hmap.links[i].keys[0]];
warp_pose = old_pose;
x1 = hmap.links[i].points[0].x;
x2 = hmap.links[i].points[1].x;
x3 = hmap.links[i].points[2].x;
x4 = hmap.links[i].points[3].x;
y1 = hmap.links[i].points[0].y;
y2 = hmap.links[i].points[1].y;
y3 = hmap.links[i].points[2].y;
y4 = hmap.links[i].points[3].y;
warp_dst.x = (x1 + x2) / 2.0;
warp_dst.y = (y1 + y2) / 2.0;
warp_dst.theta = atan2(y2-y1, x2-x1);
warp_src.x = (x3 + x4) / 2.0;
warp_src.y = (y3 + y4) / 2.0;
warp_src.theta = atan2(y4-y3, x4-x3);
warp_post1 = hmap.links[i].points[2];
warp_post2 = hmap.links[i].points[3];
}
}
}
}
}
}
static int segment_intersect(carmen_point_t p1, carmen_point_t p2, carmen_point_t p3, carmen_point_t p4) {
return (line_side_test(p1, p2, p3, p4) < 0 && line_side_test(p3, p4, p1, p2) < 0);
}
