// - merges 's1' with 's2' with a line fitting method
// - points are uniformly distributed on the line segments
// - number and weight of the points are calculated from the weight of the line segment
void carmen_linemapping_line_fitting_uniformly_distribute(carmen_linemapping_segment_t *s1,
const carmen_linemapping_segment_t *s2)
{
// get uniformly distributed point set
carmen_linemapping_dyn_tab_point_t *points;
points = carmen_linemapping_uniformly_distribute_points_on_segment(s1, s2);
double x_av = 0.0, y_av = 0.0;
for(int i=0; i<points->numberOfElements(); i++){
carmen_point_t *p = points->getElement(i);
x_av += p->x * p->theta;
y_av += p->y * p->theta;
}
int weight_of_points = s1->weight + s2->weight;
x_av = x_av / (double)weight_of_points;
y_av = y_av / (double)weight_of_points;
double numerator = 0.0, denominator = 0.0;
for(int i=0; i<points->numberOfElements(); i++){
carmen_point_t *p = points->getElement(i);
numerator += p->theta * (y_av - p->y) * (x_av - p->x);
denominator += p->theta * ( carmen_square( (y_av - p->y) ) - carmen_square( (x_av - p->x) ) );
}
numerator *= -2.0;
double angle = atan2( numerator, denominator ) / 2.0;
double distance = ( x_av*cos(angle) ) + ( y_av*sin(angle) );
double min_x = MAXDOUBLE, max_x = -MAXDOUBLE, min_y = MAXDOUBLE, max_y = -MAXDOUBLE;
const carmen_point_t* pnts[4] = {&s1->p1, &s1->p2, &s2->p1, &s2->p2};
for(int i=0; i<=3; i++){
const carmen_point_t *p = pnts[i];
if( min_x > p->x ){ min_x = p->x; } if( max_x < p->x ){ max_x = p->x; }
if( min_y > p->y ){ min_y = p->y; } if( max_y < p->y ){ max_y = p->y; }
}
if(angle<M_PI/4.0 && angle>-M_PI/4.0){
s1->p1.x = ( distance - (min_y)*sin(angle) ) / cos(angle);
s1->p2.x = ( distance - (max_y)*sin(angle) ) / cos(angle);
s1->p1.y = min_y;
s1->p2.y = max_y;
}
else{
s1->p1.y = ( distance - (min_x)*cos(angle) ) / sin(angle);
s1->p2.y = ( distance - (max_x)*cos(angle) ) / sin(angle);
s1->p1.x = min_x;
s1->p2.x = max_x;
}
s1->weight = weight_of_points;
points->setAutoDelete(true);
delete points;
}
// fits the line segment 's' to the point set 'pnts[i_low] ... pnts[i_high]'
void carmen_linemapping_line_fitting(carmen_linemapping_segment_t *s,
const carmen_linemapping_dyn_tab_point_t *pnts,
int i_low, int i_high)
{
int num_of_points = i_high - i_low + 1;
double x_av = 0.0, y_av = 0.0;
for(int i=i_low; i<=i_high; i++){
x_av += pnts->getElement(i)->x;
y_av += pnts->getElement(i)->y;
}
x_av = x_av/(double)num_of_points;
y_av = y_av/(double)num_of_points;
double numerator = 0.0, denominator = 0.0;
double min_x = MAXDOUBLE, max_x = -MAXDOUBLE, min_y = MAXDOUBLE, max_y = -MAXDOUBLE;
for(int i=i_low; i<=i_high; i++){
numerator += (y_av - pnts->getElement(i)->y) * (x_av - pnts->getElement(i)->x);
denominator += carmen_square( (y_av - pnts->getElement(i)->y) )
- carmen_square( (x_av - pnts->getElement(i)->x) );
if( min_x > pnts->getElement(i)->x ){ min_x = pnts->getElement(i)->x; }
if( max_x < pnts->getElement(i)->x ){ max_x = pnts->getElement(i)->x; }
if( min_y > pnts->getElement(i)->y ){ min_y = pnts->getElement(i)->y; }
if( max_y < pnts->getElement(i)->y ){ max_y = pnts->getElement(i)->y; }
}
numerator *= -2.0;
double angle = atan2( numerator, denominator ) / 2.0;
double distance = ( x_av*cos(angle) ) + ( y_av*sin(angle) );
if(angle<M_PI/4.0 && angle>-M_PI/4.0){
s->p1.x = ( distance - (min_y)*sin(angle) ) / cos(angle);
s->p2.x = ( distance - (max_y)*sin(angle) ) / cos(angle);
s->p1.y = min_y;
s->p2.y = max_y;
}
else{
s->p1.y = ( distance - (min_x)*cos(angle) ) / sin(angle);
s->p2.y = ( distance - (max_x)*cos(angle) ) / sin(angle);
s->p1.x = min_x;
s->p2.x = max_x;
}
}
// calculates the max. distance 'max_dist' of a point 'pnt[max_index]' of the point set 'pnts[i_low] ... pnts[i_high]'
// to the line segment with the endpoints 'p1' and 'p2'
void carmen_linemapping_get_max_dist_max_index(const carmen_point_t *p1,
const carmen_point_t *p2,
const carmen_linemapping_dyn_tab_point_t *pnts,
int i_low, int i_high, double *max_dist, int *max_index)
{
double dx = p2->x - p1->x;
double dy = p2->y - p1->y;
double denominator = carmen_square(dx) + carmen_square(dy);
*max_dist = 0.0;
if(denominator < carmen_square(carmen_linemapping_epsilon)){ // line segment is a point
for(int i=i_low; i<=i_high; i++){
double distance = carmen_linemapping_distance_point_point(p1, pnts->getElement(i));
if(distance>*max_dist){ *max_dist = distance; *max_index = i; }
}
}
else{
for(int i=i_low; i<=i_high; i++){
double numerator = carmen_square(dx*(pnts->getElement(i)->y - p2->y) - dy*(pnts->getElement(i)->x - p2->x));
double distance = sqrt( numerator/denominator );
if(distance>*max_dist){ *max_dist = distance; *max_index = i; }
}
}
}
void art_draw_ellipse(art_buffer_p buffer, art_context_p context, int filled,
float x, float y, float x_var,
float xy_cov, float y_var, float k)
{
float poly_x[MAX_POLY_POINTS], poly_y[MAX_POLY_POINTS], l11, l21, l22;
int i;
l11 = sqrt(x_var);
l21 = xy_cov / l11;
l22 = sqrt(y_var - carmen_square(l21));
for(i = 0; i < MAX_POLY_POINTS; i++) {
double t = i / (float)MAX_POLY_POINTS * 2 * M_PI, xt, yt;
xt = cos(t); yt = sin(t);
poly_x[i] = x + l11 * xt * k;
poly_y[i] = y + (l21 * xt + l22 * yt) * k;
}
art_draw_poly(buffer, context, filled, poly_x, poly_y, MAX_POLY_POINTS, 1);
}
static double
dist2(carmen_ackerman_path_point_t v, carmen_ackerman_path_point_t w)
{
return (carmen_square(v.x - w.x) + carmen_square(v.y - w.y));
}
void art_draw_gaussian(art_buffer_p buffer, art_context_p context,
float x, float y, float x_var,
float xy_cov, float y_var, float k)
{
float poly_x[MAX_POLY_POINTS], poly_y[MAX_POLY_POINTS];
double len, discriminant, eigval1, eigval2,
eigvec1x, eigvec1y, eigvec2x, eigvec2y;
int i, ex1, ey1, ex2, ey2;
/* check for special case of axis-aligned */
if (fabs(xy_cov) < (fabs(x_var) + fabs(y_var) + 1e-4) * 1e-4) {
eigval1 = x_var;
eigval2 = y_var;
eigvec1x = 1.;
eigvec1y = 0.;
eigvec2x = 0.;
eigvec2y = 1.;
}
else {
/* compute axes and scales of ellipse */
discriminant = sqrt(4 * carmen_square(xy_cov) + carmen_square(x_var - y_var));
eigval1 = .5 * (x_var + y_var - discriminant);
eigval2 = .5 * (x_var + y_var + discriminant);
eigvec1x = (x_var - y_var - discriminant) / (2.0 * xy_cov);
eigvec1y = 1.0;
eigvec2x = (x_var - y_var + discriminant) / (2.0 * xy_cov);
eigvec2y = 1.0;
/* normalize eigenvectors */
len = sqrt(carmen_square(eigvec1x) + 1.0);
eigvec1x /= len;
eigvec1y /= len;
len = sqrt(carmen_square(eigvec2x) + 1.0);
eigvec2x /= len;
eigvec2y /= len;
}
/* take square root of eigenvalues and scale -- once this is
done, eigvecs are unit vectors along axes and eigvals are
corresponding radii */
if (eigval1 < 0 || eigval2 < 0) {
art_draw_circle(buffer, context, 0, x, y, 1);
return;
}
eigval1 = sqrt(eigval1) * k;
eigval2 = sqrt(eigval2) * k;
if(eigval1 < 1)
eigval1 = 1;
if(eigval2 < 1)
eigval2 = 1;
/* compute points around edge of ellipse */
for (i = 0; i < MAX_POLY_POINTS; i++) {
double theta = M_PI * (-1.0 + 2.0 * i / (float)MAX_POLY_POINTS);
double xi = cos(theta) * eigval1;
double yi = sin(theta) * eigval2;
poly_x[i] = xi * eigvec1x + yi * eigvec2x + x;
poly_y[i] = xi * eigvec1y + yi * eigvec2y + y;
}
/* finally we can draw it */
art_draw_poly(buffer, context, 0, poly_x, poly_y, MAX_POLY_POINTS, 1);
ex1 = x + eigval1 * eigvec1x;
ey1 = y + eigval1 * eigvec1y;
ex2 = x - eigval1 * eigvec1x;
ey2 = y - eigval1 * eigvec1y;
art_draw_line(buffer, context, ex1, ey1, ex2, ey2);
ex1 = x + eigval2 * eigvec2x;
ey1 = y + eigval2 * eigvec2y;
ex2 = x - eigval2 * eigvec2x;
ey2 = y - eigval2 * eigvec2y;
art_draw_line(buffer, context, ex1, ey1, ex2, ey2);
}
// euclidean distance of two points 'p1' and 'p2'
double carmen_linemapping_distance_point_point(const carmen_point_t *p1,
const carmen_point_t *p2)
{
return sqrt( carmen_square( p1->x - p2->x) + carmen_square( p1->y - p2->y) );
}
// - merges 'segment' with the best candidate of 'set', but not the candidate 'set[index_no_element]'
// - merged segment is stored in 'segment', weight of best canditate is set to '0'
int carmen_linemapping_merge_segment(carmen_linemapping_segment_set_t *set,
carmen_linemapping_segment_t *segment,
int index_no_element)
{
int best_merge_index = -1;
double best_similar_angle = MAXDOUBLE;;
for(int i=0; i<set->num_segs; i++){
if(i==index_no_element || set->segs[i].weight==0 || segment->weight==0){ continue; }
carmen_linemapping_segment_t *s_set = &(set->segs[i]);
double dx = segment->p2.x - segment->p1.x;
double dy = segment->p2.y - segment->p1.y;
double denominator = carmen_square(dx) + carmen_square(dy);
double numerator1 = carmen_square( dx*(s_set->p1.y - segment->p2.y) - dy*(s_set->p1.x - segment->p2.x) );
double numerator2 = carmen_square( dx*(s_set->p2.y - segment->p2.y) - dy*(s_set->p2.x - segment->p2.x) );
double dis_set_p1 = sqrt( numerator1/denominator );
double dis_set_p2 = sqrt( numerator2/denominator );
if( dis_set_p1 < carmen_linemapping_params_global.merge_max_dist &&
dis_set_p2 < carmen_linemapping_params_global.merge_max_dist ) {
// 's_set' is close enough to the line 'segment' (not line segment!!!)
int overlap_set_p1 = carmen_linemapping_overlap_point_linesegment(segment, &s_set->p1);
int overlap_set_p2 = carmen_linemapping_overlap_point_linesegment(segment, &s_set->p2);
if( overlap_set_p1 && overlap_set_p2 ){
// case 1: both endpoints of 's_set' overlaps -> merge
double a_dif = carmen_linemapping_angle_difference(segment, s_set);
if( a_dif < best_similar_angle ){
best_similar_angle = a_dif;
best_merge_index = i;
continue;
}
}
if( overlap_set_p1 || overlap_set_p2 ){
// case 2: only one endpoint of 's_set' overlaps -> test merge
double overlap_dist;
carmen_point_t point = carmen_linemapping_get_point_on_segment(segment, s_set);
if( overlap_set_p1 ){
overlap_dist = carmen_linemapping_distance_point_point(&s_set->p1, &point);
}
else {
overlap_dist = carmen_linemapping_distance_point_point(&s_set->p2, &point);
}
double line_size = carmen_linemapping_distance_point_point(&s_set->p1, &s_set->p2);
if( (overlap_dist/line_size) > carmen_linemapping_params_global.merge_min_relative_overlap ||
overlap_dist > carmen_linemapping_params_global.merge_overlap_min_length ){
double a_dif = carmen_linemapping_angle_difference(segment, s_set);
if( a_dif < best_similar_angle ){
best_similar_angle = a_dif;
best_merge_index = i;
continue;
}
}
}
}
dx = s_set->p2.x - s_set->p1.x;
dy = s_set->p2.y - s_set->p1.y;
denominator = carmen_square(dx) + carmen_square(dy);
numerator1 = carmen_square( dx*(segment->p1.y - s_set->p2.y) - dy*(segment->p1.x - s_set->p2.x) );
numerator2 = carmen_square( dx*(segment->p2.y - s_set->p2.y) - dy*(segment->p2.x - s_set->p2.x) );
double dis_segment_p1 = sqrt( numerator1/denominator );
double dis_segment_p2 = sqrt( numerator2/denominator );
if( dis_segment_p1 < carmen_linemapping_params_global.merge_max_dist &&
dis_segment_p2 < carmen_linemapping_params_global.merge_max_dist ){
// 'segment' is close enough to the line 's_set' (not line segment!!!)
int overlap_segment_p1 = carmen_linemapping_overlap_point_linesegment(s_set, &segment->p1);
int overlap_segment_p2 = carmen_linemapping_overlap_point_linesegment(s_set, &segment->p2);
if(overlap_segment_p1 && overlap_segment_p2){ // case 3: both endpoints of 's_old' overlaps -> merge
double a_dif = carmen_linemapping_angle_difference(segment, s_set);
if( a_dif < best_similar_angle ){
best_similar_angle = a_dif;
best_merge_index = i;
continue;
}
}
if(overlap_segment_p1 || overlap_segment_p2){ // case 4: only one endpoint of 'segment' overlaps -> test merge
double overlap_dist;
carmen_point_t point = carmen_linemapping_get_point_on_segment(s_set, segment);
if( overlap_segment_p1 ){
overlap_dist = carmen_linemapping_distance_point_point(&segment->p1, &point);
}
else {
overlap_dist = carmen_linemapping_distance_point_point(&segment->p2, &point);
}
double line_size = carmen_linemapping_distance_point_point(&segment->p1, &segment->p2);
if( (overlap_dist/line_size) > carmen_linemapping_params_global.merge_min_relative_overlap ||
overlap_dist > carmen_linemapping_params_global.merge_overlap_min_length ){
double a_dif = carmen_linemapping_angle_difference(segment, s_set);
if( a_dif < best_similar_angle ){
best_similar_angle = a_dif;
best_merge_index = i;
continue;
}
}
}
}
}
if (best_merge_index>=0){
// merge the best candidate with 'segment'
carmen_linemapping_line_fitting_uniformly_distribute(segment, &(set->segs[best_merge_index]));
set->segs[best_merge_index].weight = 0;
return true;
}
else
return false;
}
