int rc_clip_line(rc_line_t line, rc_rectangle_t clip)
{
rc_code_t C0, C1, C;
float x, y;
assert(line != NULL);
assert(clip != NULL);
C0 = rc_compute_code(line->x0, line->y0, clip);
C1 = rc_compute_code(line->x1, line->y1, clip);
for (;;) {
/* trivial accept: both ends in rectangle */
if ((C0 | C1) == 0) {
return 1;
}
/* trivial reject: both ends on the external side of the rectangle */
if ((C0 & C1) != 0)
return 0;
/* normal case: clip end outside rectangle */
C = C0 ? C0 : C1;
if (C & RCF_TOP) {
x = line->x0 + (line->x1 - line->x0) *
(clip->ymax - line->y0) / (line->y1 - line->y0);
y = clip->ymax;
} else if (C & RCF_BOTTOM) {
x = line->x0 + (line->x1 - line->x0) *
(clip->ymin - line->y0) / (line->y1 - line->y0);
y = clip->ymin;
} else if (C & RCF_RIGHT) {
x = clip->xmax;
y = line->y0 + (line->y1 - line->y0) *
(clip->xmax - line->x0) / (line->x1 - line->x0);
} else {
x = clip->xmin;
y = line->y0 + (line->y1 - line->y0) *
(clip->xmin - line->x0) / (line->x1 - line->x0);
}
/* set new end point and iterate */
if (C == C0) {
line->x0 = x; line->y0 = y;
C0 = rc_compute_code(line->x0, line->y0, clip);
} else {
line->x1 = x; line->y1 = y;
C1 = rc_compute_code(line->x1, line->y1, clip);
}
}
/* notreached */
assert(0);
}
pp_point_type_t pp_point_identify(struct robot_position *rpoint,
struct obstacle_config *oc)
{
/* determine the point type of rpoint */
pp_point_type_t retval;
robot_position this_cp;
assert(rpoint != NULL);
assert(oc != NULL);
if (rc_compute_code(rpoint->x, rpoint->y, oc) == 0)
retval = PPT_INTERNAL;
else
retval = PPT_DETACHED;
/* is this a corner point? Check each corner of this obstacle */
if (PP_TOL > pp_point_distance(
rpoint, rc_corner(RCF_TOP|RCF_LEFT, &this_cp, oc))) {
retval = PPT_CORNERPOINT;
}
else if (PP_TOL > pp_point_distance(
rpoint, rc_corner(RCF_TOP|RCF_RIGHT, &this_cp, oc))) {
retval = PPT_CORNERPOINT;
}
else if (PP_TOL > pp_point_distance(
rpoint, rc_corner(RCF_BOTTOM|RCF_RIGHT, &this_cp, oc))) {
retval = PPT_CORNERPOINT;
}
else if (PP_TOL > pp_point_distance(
rpoint, rc_corner(RCF_BOTTOM|RCF_LEFT, &this_cp, oc))) {
retval = PPT_CORNERPOINT;
}
/* is this point in bounds? */
if (!(pp_point_in_bounds(rpoint->x, rpoint->y))) {
retval = PPT_OUTOFBOUNDS;
if (debug > 1)
info("pp_point_identify says this point is out of bounds\n");
}
/* debugging output */
if (PPT_CORNERPOINT == retval) {
if (debug > 1)
info("pp_point_identify says this point is a corner point\n");
}
return retval;
}
/**
* Check that a point is not only in bounds, but also reachable by the robot.
* A point is not reachable if it is out of bounds or if the robot would have
* to travel through the center of the obstacle to reach it.
*
* @param pp A plan with the pp_actual_pos and pp_obstacle fields initialized.
* The pp_obstacle field should contain the obstacle that must be navigated
* around.
* @param rp The point to check.
* @return True if the point is reachable, false otherwise.
*/
static int pp_point_reachable(struct path_plan *pp, struct robot_position *rp)
{
int retval = 0;
assert(pp != NULL);
assert(rp != NULL);
if (pp_point_in_bounds(rp->x, rp->y)) {
struct obstacle_config oc;
struct rc_line rl;
rl.x0 = pp->pp_actual_pos.x;
rl.y0 = pp->pp_actual_pos.y;
rl.x1 = rp->x;
rl.y1 = rp->y;
oc = pp->pp_obstacle;
if (rc_compute_code(rl.x0, rl.y0, &oc) == 0) {
float xlen, ylen;
/*
* The point is inside the obstacle, shrink the size of the
* obstacle so the point is on the outside.
*/
xlen = min(fabs(rl.x0 - pp->pp_obstacle.xmin),
fabs(rl.x0 - pp->pp_obstacle.xmax));
ylen = min(fabs(rl.y0 - pp->pp_obstacle.ymin),
fabs(rl.y0 - pp->pp_obstacle.ymax));
if (xlen < ylen) {
if (fabs(rl.x0 - pp->pp_obstacle.xmin) <
fabs(rl.x0 - pp->pp_obstacle.xmax)) {
oc.xmin = rl.x0 + 0.01;
oc.xmax = pp->pp_obstacle.xmax;
}
else {
oc.xmin = pp->pp_obstacle.xmin;
oc.xmax = rl.x0 - 0.01;
}
}
else {
if (fabs(rl.y0 - pp->pp_obstacle.ymin) <
fabs(rl.y0 - pp->pp_obstacle.ymax)) {
oc.ymin = rl.y0 + 0.01;
oc.ymax = pp->pp_obstacle.ymax;
}
else {
oc.ymin = pp->pp_obstacle.ymin;
oc.ymax = rl.y0 - 0.01;
}
}
}
if (debug > 1) {
info("reachable %.2f %.2f - %.2f %.2f %.2f %.2f\n",
rp->x, rp->y, oc.xmin, oc.ymin, oc.xmax, oc.ymax);
}
/*
* Check if the path to the point is completely outside the obstacle
* or intersects just at one of the corners.
*/
if ((rc_clip_line(&rl, &oc) == 0) ||
((rl.x0 == rl.x1) && (rl.y0 == rl.y1))) {
retval = 1;
}
}
return retval;
}
pp_plot_code_t pp_plot_waypoint(struct path_plan *pp)
{
struct lnMinList dextra, sextra, intersections;
float distance, cross, theta;
pp_plot_code_t retval;
int in_ob = 0;
assert(pp != NULL);
if (debug > 0) {
info(" plot %.2f %.2f -> %.2f %.2f\n",
pp->pp_actual_pos.x, pp->pp_actual_pos.y,
pp->pp_goal_pos.x, pp->pp_goal_pos.y);
}
lnNewList(&dextra);
lnNewList(&sextra);
lnNewList(&intersections);
retval = PPC_NO_WAYPOINT; /* if nothing else */
/*
* We have to iterate over one or more waypoints, so we start with the
* Initial waypoint is the goal point.
*/
pp->pp_waypoint = pp->pp_goal_pos;
do {
struct obstacle_node *on, *min_on = NULL;
int goal_in_ob = 0, robot_ob = 0;
float min_distance = FLT_MAX;
/*
* The path doesn't cross enough of the obstacle box for us to worry
* about it so we dump it on the extra list so the intersect doesn't
* return it again. XXX The number is partially a guess and
* empirically derived.
*/
cross = PP_MIN_OBSTACLE_CROSS;
/*
* First we find all of the obstacles that intersect with this path
* and locate the minimum distance to an obstacle.
*/
if (debug)
info("wp %.2f %.2f\n", pp->pp_waypoint.x, pp->pp_waypoint.y);
while ((on = ob_find_intersect2(&pp->pp_actual_pos,
&pp->pp_waypoint,
&distance,
&cross)) != NULL) {
#if 0
info("intr %d %.2f %.2f\n",
on->on_natural.id,
distance,
cross);
#endif
if (on->on_type == OBT_ROBOT)
robot_ob = 1;
switch (pp_point_identify(&pp->pp_actual_pos, &on->on_expanded)) {
case PPT_INTERNAL:
case PPT_CORNERPOINT:
case PPT_OUTOFBOUNDS:
in_ob = 1;
break;
default:
break;
}
if (rc_compute_code(pp->pp_goal_pos.x,
pp->pp_goal_pos.y,
&on->on_expanded) == 0) {
goal_in_ob = 1;
}
lnRemove(&on->on_link);
/* Record the intersection and */
lnAddTail(&intersections, &on->on_link);
/* ... check if this is the closest obstacle. */
if (distance < min_distance) {
min_distance = distance;
min_on = on;
}
cross = PP_MIN_OBSTACLE_CROSS;
}
/* Check if there was an actual intersection. */
if (min_on != NULL) {
if (debug > 1)
info("closest %d\n", min_on->on_natural.id);
pp->pp_flags |= PPF_HAS_OBSTACLE;
lnRemove(&min_on->on_link); // from the intersections list
lnAddTail(min_on->on_type == OBT_STATIC ? &sextra : &dextra,
&min_on->on_link);
pp->pp_obstacle = min_on->on_expanded;
/*
* Find all obstacles that the path intersects with and overlap the
* closest one. The rest of the obstacles will be dealt with after
* we get around the closest one.
*/
while ((on = (struct obstacle_node *)
lnRemHead(&intersections)) != NULL) {
if (rc_rect_intersects(&pp->pp_obstacle, &on->on_expanded)) {
if (debug > 1)
info("merging %d\n", on->on_natural.id);
ob_merge_obstacles(&pp->pp_obstacle, &on->on_expanded);
}
lnAddTail(on->on_type == OBT_STATIC ? &sextra : &dextra,
&on->on_link);
}
if (goal_in_ob) {
struct robot_position rp;
struct rc_line rl;
float r;
mtp_polar(&pp->pp_goal_pos, &pp->pp_actual_pos, &r, &theta);
if (robot_ob)
theta += M_PI_2;
mtp_cartesian(&pp->pp_goal_pos, r + 1000.0, theta, &rp);
rl.x0 = pp->pp_goal_pos.x;
rl.y0 = pp->pp_goal_pos.y;
rl.x1 = rp.x;
rl.y1 = rp.y;
rc_clip_line(&rl, &pp->pp_obstacle);
pp->pp_waypoint.x = rl.x1;
pp->pp_waypoint.y = rl.y1;
if (pp_point_distance(&pp->pp_waypoint,
&pp->pp_actual_pos) > 0.075) {
if (pp_point_reachable(pp, &pp->pp_waypoint))
retval = PPC_WAYPOINT;
else if (pp_next_cornerpoint(pp))
retval = PPC_WAYPOINT;
else
retval = PPC_BLOCKED;
}
else {
retval = PPC_GOAL_IN_OBSTACLE;
}
}
else if (pp_next_cornerpoint(pp)) {
retval = PPC_WAYPOINT;
}
else {
retval = PPC_BLOCKED;
}
}
/* Restore the active obstacle list. XXX Shouldn't do this here. */
lnPrependList(&ob_data.od_active, &dextra); // dynamics first
lnAppendList(&ob_data.od_active, &sextra); // statics last
/* Check for obstacles on the path to our new waypoint. */
cross = PP_MIN_OBSTACLE_CROSS;
} while ((retval == PPC_WAYPOINT) &&
(ob_find_intersect2(&pp->pp_actual_pos,
&pp->pp_waypoint,
&distance,
&cross) != NULL) &&
!in_ob);
/* And make sure it is still sane. */
assert(ob_data_invariant());
/* restrict final waypoint to MAX_DISTANCE */
mtp_polar(&pp->pp_actual_pos,
(retval == PPC_WAYPOINT) ? &pp->pp_waypoint : &pp->pp_goal_pos,
&distance,
&theta);
if (distance > pp_data.ppd_max_distance) {
mtp_cartesian(&pp->pp_actual_pos,
pp_data.ppd_max_distance,
theta,
&pp->pp_waypoint);
retval = PPC_WAYPOINT;
}
switch (retval) {
case PPC_WAYPOINT:
info("set waypoint(%s) %.2f %.2f\n",
pp->pp_robot->hostname,
pp->pp_waypoint.x,
pp->pp_waypoint.y);
break;
case PPC_NO_WAYPOINT:
info("set waypoint(%s) NONE\n", pp->pp_robot->hostname);
break;
case PPC_BLOCKED:
info("set waypoint(%s) BLOCKED\n", pp->pp_robot->hostname);
break;
case PPC_GOAL_IN_OBSTACLE:
info("set waypoint(%s) GOAL_IN_OBSTACLE\n", pp->pp_robot->hostname);
break;
}
return retval;
}
