void RRG::_rewire_near_nodes(RRGNode* p_node_new, std::list<RRGNode*> near_nodes) {
for( std::list<RRGNode*>::iterator it=near_nodes.begin(); it!=near_nodes.end(); it++ ) {
RRGNode * p_near_node = (*it);
if(p_near_node->m_pos ==p_node_new->m_pos || p_near_node->m_pos==_p_root->m_pos || p_node_new->mp_parent->m_pos==p_near_node->m_pos) {
continue;
}
if( true == _is_obstacle_free( p_node_new->m_pos, p_near_node->m_pos ) ) {
double temp_delta_cost = _calculate_cost( p_node_new->m_pos, p_near_node->m_pos );
double temp_cost_from_new_node = p_node_new->m_cost + temp_delta_cost;
if( temp_cost_from_new_node < p_near_node->m_cost ) {
double min_delta_cost = p_near_node->m_cost - temp_cost_from_new_node;
RRGNode * p_parent_node = p_near_node->mp_parent;
bool removed = _remove_edge(p_parent_node, p_near_node);
if(removed) {
bool added = _add_edge(p_node_new, p_near_node);
if( added ) {
p_near_node->m_cost = temp_cost_from_new_node;
_update_cost_to_children(p_near_node, min_delta_cost);
}
}
else {
std::cout << " Failed in removing " << std::endl;
}
}
}
}
}
static void
_cairo_polygon_add_edge (cairo_polygon_t *polygon,
const cairo_point_t *p1,
const cairo_point_t *p2,
int dir)
{
/* drop horizontal edges */
if (p1->y == p2->y)
return;
if (p1->y > p2->y) {
const cairo_point_t *t;
t = p1, p1 = p2, p2 = t;
dir = -dir;
}
if (polygon->num_limits) {
if (p2->y <= polygon->limit.p1.y)
return;
if (p1->y >= polygon->limit.p2.y)
return;
_add_clipped_edge (polygon, p1, p2, p1->y, p2->y, dir);
} else
_add_edge (polygon, p1, p2, p1->y, p2->y, dir);
}
cairo_status_t
_cairo_polygon_add_line (cairo_polygon_t *polygon,
const cairo_line_t *line,
int top, int bottom,
int dir)
{
/* drop horizontal edges */
if (line->p1.y == line->p2.y)
return CAIRO_STATUS_SUCCESS;
if (bottom <= top)
return CAIRO_STATUS_SUCCESS;
if (polygon->num_limits) {
if (line->p2.y <= polygon->limit.p1.y)
return CAIRO_STATUS_SUCCESS;
if (line->p1.y >= polygon->limit.p2.y)
return CAIRO_STATUS_SUCCESS;
_add_clipped_edge (polygon, &line->p1, &line->p2, top, bottom, dir);
} else
_add_edge (polygon, &line->p1, &line->p2, top, bottom, dir);
return polygon->status;
}
static cairo_status_t
_reduce_line_to (void *closure,
const cairo_point_t *point)
{
struct reduce *r = closure;
_add_edge (r, &r->current_point, point);
r->current_point = *point;
return CAIRO_STATUS_SUCCESS;
}
struct edge * graph_add_edge(struct graph *g, struct vertex *from, struct vertex *to, const WEIGHT_T weight)
{
struct edge *e = malloc(sizeof *e);
if (!e) {
perror("malloc");
return NULL;
}
e->weight = weight;
e->to = to;
e->next = NULL;
_add_edge(&from->edges, e, g->weight_cmp);
return e;
}
void RRG::_attach_new_node(RRGNode* p_node_new, RRGNode* p_nearest_node, std::list<RRGNode*> near_nodes) {
double min_new_node_cost = p_nearest_node->m_cost + _calculate_cost(p_nearest_node->m_pos, p_node_new->m_pos);
RRGNode* p_min_node = p_nearest_node;
for(std::list<RRGNode*>::iterator it=near_nodes.begin();it!=near_nodes.end();it++) {
RRGNode* p_near_node = *it;
if ( true == _is_obstacle_free( p_near_node->m_pos, p_node_new->m_pos ) ) {
double delta_cost = _calculate_cost( p_near_node->m_pos, p_node_new->m_pos );
double new_cost = p_near_node->m_cost + delta_cost;
if ( new_cost < min_new_node_cost ) {
p_min_node = p_near_node;
min_new_node_cost = new_cost;
}
}
}
bool added = _add_edge( p_min_node, p_node_new );
if( added ) {
p_node_new->m_cost = min_new_node_cost;
}
}
/* polygon:
* Draws a filled polygon with an arbitrary number of corners. Pass the
* number of vertices, then an array containing a series of x, y points
* (a total of vertices*2 values).
*/
void _soft_polygon(BITMAP *bmp, int vertices, AL_CONST int *points, int color)
{
int c;
int top = INT_MAX;
int bottom = INT_MIN;
AL_CONST int *i1, *i2;
POLYGON_EDGE *edge, *next_edge;
POLYGON_EDGE *active_edges = NULL;
POLYGON_EDGE *inactive_edges = NULL;
ASSERT(bmp);
/* allocate some space and fill the edge table */
_grow_scratch_mem(sizeof(POLYGON_EDGE) * vertices);
edge = (POLYGON_EDGE *)_scratch_mem;
i1 = points;
i2 = points + (vertices-1) * 2;
for (c=0; c<vertices; c++) {
fill_edge_structure(edge, i1, i2);
if (edge->bottom >= edge->top) {
if (edge->top < top)
top = edge->top;
if (edge->bottom > bottom)
bottom = edge->bottom;
inactive_edges = _add_edge(inactive_edges, edge, FALSE);
edge++;
}
i2 = i1;
i1 += 2;
}
if (bottom >= bmp->cb)
bottom = bmp->cb-1;
acquire_bitmap(bmp);
/* for each scanline in the polygon... */
for (c=top; c<=bottom; c++) {
int hid = 0;
int b1 = 0;
int e1 = 0;
int up = 0;
int draw = 0;
int e;
/* check for newly active edges */
edge = inactive_edges;
while ((edge) && (edge->top == c)) {
next_edge = edge->next;
inactive_edges = _remove_edge(inactive_edges, edge);
active_edges = _add_edge(active_edges, edge, TRUE);
edge = next_edge;
}
/* draw horizontal line segments */
edge = active_edges;
while (edge) {
e = edge->w;
if (edge->bottom != c) {
up = 1 - up;
}
else {
e = edge->w >> 1;
}
if (edge->top == c) {
e = edge->w >> 1;
}
if ((draw < 1) && (up >= 1)) {
b1 = (edge->x + e) >> POLYGON_FIX_SHIFT;
}
else if (draw >= 1) {
static void
_add_clipped_edge (cairo_polygon_t *polygon,
const cairo_point_t *p1,
const cairo_point_t *p2,
const int top, const int bottom,
const int dir)
{
cairo_point_t bot_left, top_right;
cairo_fixed_t top_y, bot_y;
int n;
for (n = 0; n < polygon->num_limits; n++) {
const cairo_box_t *limits = &polygon->limits[n];
cairo_fixed_t pleft, pright;
if (top >= limits->p2.y)
continue;
if (bottom <= limits->p1.y)
continue;
bot_left.x = limits->p1.x;
bot_left.y = limits->p2.y;
top_right.x = limits->p2.x;
top_right.y = limits->p1.y;
/* The useful region */
top_y = MAX (top, limits->p1.y);
bot_y = MIN (bottom, limits->p2.y);
/* The projection of the edge on the horizontal axis */
pleft = MIN (p1->x, p2->x);
pright = MAX (p1->x, p2->x);
if (limits->p1.x <= pleft && pright <= limits->p2.x) {
/* Projection of the edge completely contained in the box:
* clip vertically by restricting top and bottom */
_add_edge (polygon, p1, p2, top_y, bot_y, dir);
assert_last_edge_is_valid (polygon, limits);
} else if (pright <= limits->p1.x) {
/* Projection of the edge to the left of the box:
* replace with the left side of the box (clipped top/bottom) */
_add_edge (polygon, &limits->p1, &bot_left, top_y, bot_y, dir);
assert_last_edge_is_valid (polygon, limits);
} else if (limits->p2.x <= pleft) {
/* Projection of the edge to the right of the box:
* replace with the right side of the box (clipped top/bottom) */
_add_edge (polygon, &top_right, &limits->p2, top_y, bot_y, dir);
assert_last_edge_is_valid (polygon, limits);
} else {
/* The edge and the box intersect in a generic way */
cairo_fixed_t left_y, right_y;
cairo_bool_t top_left_to_bottom_right;
/*
* The edge intersects the lines corresponding to the left
* and right sides of the limit box at left_y and right_y,
* but we need to add edges for the range from top_y to
* bot_y.
*
* For both intersections, there are three cases:
*
* 1) It is outside the vertical range of the limit
* box. In this case we can simply further clip the
* edge we will be emitting (i.e. restrict its
* top/bottom limits to those of the limit box).
*
* 2) It is inside the vertical range of the limit
* box. In this case, we need to add the vertical edge
* connecting the correct vertex to the intersection,
* in order to preserve the winding count.
*
* 3) It is exactly on the box. In this case, do nothing.
*
* These operations restrict the active range (stored in
* top_y/bot_y) so that the p1-p2 edge is completely
* inside the box if it is clipped to this vertical range.
*/
top_left_to_bottom_right = (p1->x <= p2->x) == (p1->y <= p2->y);
if (top_left_to_bottom_right) {
if (pleft >= limits->p1.x) {
left_y = top_y;
} else {
left_y = _cairo_edge_compute_intersection_y_for_x (p1, p2,
limits->p1.x);
if (_cairo_edge_compute_intersection_x_for_y (p1, p2, left_y) < limits->p1.x)
left_y++;
}
left_y = MIN (left_y, bot_y);
if (top_y < left_y) {
_add_edge (polygon, &limits->p1, &bot_left,
top_y, left_y, dir);
assert_last_edge_is_valid (polygon, limits);
top_y = left_y;
}
if (pright <= limits->p2.x) {
right_y = bot_y;
} else {
right_y = _cairo_edge_compute_intersection_y_for_x (p1, p2,
limits->p2.x);
if (_cairo_edge_compute_intersection_x_for_y (p1, p2, right_y) > limits->p2.x)
right_y--;
}
right_y = MAX (right_y, top_y);
if (bot_y > right_y) {
_add_edge (polygon, &top_right, &limits->p2,
right_y, bot_y, dir);
assert_last_edge_is_valid (polygon, limits);
bot_y = right_y;
}
} else {
if (pright <= limits->p2.x) {
right_y = top_y;
} else {
right_y = _cairo_edge_compute_intersection_y_for_x (p1, p2,
limits->p2.x);
if (_cairo_edge_compute_intersection_x_for_y (p1, p2, right_y) > limits->p2.x)
right_y++;
}
right_y = MIN (right_y, bot_y);
if (top_y < right_y) {
_add_edge (polygon, &top_right, &limits->p2,
top_y, right_y, dir);
assert_last_edge_is_valid (polygon, limits);
top_y = right_y;
}
if (pleft >= limits->p1.x) {
left_y = bot_y;
} else {
left_y = _cairo_edge_compute_intersection_y_for_x (p1, p2,
limits->p1.x);
if (_cairo_edge_compute_intersection_x_for_y (p1, p2, left_y) < limits->p1.x)
left_y--;
}
left_y = MAX (left_y, top_y);
if (bot_y > left_y) {
_add_edge (polygon, &limits->p1, &bot_left,
left_y, bot_y, dir);
assert_last_edge_is_valid (polygon, limits);
bot_y = left_y;
}
}
if (top_y != bot_y) {
_add_edge (polygon, p1, p2, top_y, bot_y, dir);
assert_last_edge_is_valid (polygon, limits);
}
}
}
}
void add_edge(int u, int v, int capacity, int cost) {
_add_edge(u, v, capacity, cost);
_add_edge(v, u, 0, -cost);
}
static void
_add_clipped_edge (cairo_polygon_t *polygon,
const cairo_point_t *p1,
const cairo_point_t *p2,
const int top, const int bottom,
const int dir)
{
cairo_point_t p[2];
int top_y, bot_y;
int n;
for (n = 0; n < polygon->num_limits; n++) {
const cairo_box_t *limits = &polygon->limits[n];
if (top >= limits->p2.y)
continue;
if (bottom <= limits->p1.y)
continue;
if (p1->x >= limits->p1.x && p2->x >= limits->p1.x &&
p1->x <= limits->p2.x && p2->x <= limits->p2.x)
{
top_y = top;
if (top_y < limits->p1.y)
top_y = limits->p1.y;
bot_y = bottom;
if (bot_y > limits->p2.y)
bot_y = limits->p2.y;
_add_edge (polygon, p1, p2, top_y, bot_y, dir);
}
else if (p1->x <= limits->p1.x && p2->x <= limits->p1.x)
{
p[0].x = limits->p1.x;
p[0].y = limits->p1.y;
top_y = top;
if (top_y < p[0].y)
top_y = p[0].y;
p[1].x = limits->p1.x;
p[1].y = limits->p2.y;
bot_y = bottom;
if (bot_y > p[1].y)
bot_y = p[1].y;
_add_edge (polygon, &p[0], &p[1], top_y, bot_y, dir);
}
else if (p1->x >= limits->p2.x && p2->x >= limits->p2.x)
{
p[0].x = limits->p2.x;
p[0].y = limits->p1.y;
top_y = top;
if (top_y < p[0].y)
top_y = p[0].y;
p[1].x = limits->p2.x;
p[1].y = limits->p2.y;
bot_y = bottom;
if (bot_y > p[1].y)
bot_y = p[1].y;
_add_edge (polygon, &p[0], &p[1], top_y, bot_y, dir);
}
else
{
int left_y, right_y;
int p1_y, p2_y;
left_y = _cairo_edge_compute_intersection_y_for_x (p1, p2,
limits->p1.x);
right_y = _cairo_edge_compute_intersection_y_for_x (p1, p2,
limits->p2.x);
if (left_y == right_y) /* horizontal within bounds */
continue;
p1_y = top;
p2_y = bottom;
if (left_y < right_y) {
if (p1->x < limits->p1.x && left_y > limits->p1.y) {
p[0].x = limits->p1.x;
p[0].y = limits->p1.y;
top_y = p1_y;
if (top_y < p[0].y)
top_y = p[0].y;
p[1].x = limits->p1.x;
p[1].y = limits->p2.y;
bot_y = left_y;
if (bot_y > p[1].y)
bot_y = p[1].y;
if (bot_y > top_y)
_add_edge (polygon, &p[0], &p[1], top_y, bot_y, dir);
p1_y = bot_y;
}
if (p2->x > limits->p2.x && right_y < limits->p2.y) {
p[0].x = limits->p2.x;
p[0].y = limits->p1.y;
top_y = right_y;
if (top_y < p[0].y)
top_y = p[0].y;
p[1].x = limits->p2.x;
p[1].y = limits->p2.y;
bot_y = p2_y;
if (bot_y > p[1].y)
bot_y = p[1].y;
if (bot_y > top_y)
_add_edge (polygon, &p[0], &p[1], top_y, bot_y, dir);
p2_y = top_y;
}
} else {
if (p1->x > limits->p2.x && right_y > limits->p1.y) {
p[0].x = limits->p2.x;
p[0].y = limits->p1.y;
top_y = p1_y;
if (top_y < p[0].y)
top_y = p[0].y;
p[1].x = limits->p2.x;
p[1].y = limits->p2.y;
bot_y = right_y;
if (bot_y > p[1].y)
bot_y = p[1].y;
if (bot_y > top_y)
_add_edge (polygon, &p[0], &p[1], top_y, bot_y, dir);
p1_y = bot_y;
}
if (p2->x < limits->p1.x && left_y < limits->p2.y) {
p[0].x = limits->p1.x;
p[0].y = limits->p1.y;
top_y = left_y;
if (top_y < p[0].y)
top_y = p[0].y;
p[1].x = limits->p1.x;
p[1].y = limits->p2.y;
bot_y = p2_y;
if (bot_y > p[1].y)
bot_y = p[1].y;
if (bot_y > top_y)
_add_edge (polygon, &p[0], &p[1], top_y, bot_y, dir);
p2_y = top_y;
}
}
if (p1_y < limits->p1.y)
p1_y = limits->p1.y;
if (p2_y > limits->p2.y)
p2_y = limits->p2.y;
if (p2_y > p1_y)
_add_edge (polygon, p1, p2, p1_y, p2_y, dir);
}
}
}
