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