static double
circ_node_min_distance(const CIRC_NODE* n1, const CIRC_NODE* n2)
{
double d = sphere_distance(&(n1->center), &(n2->center));
double r1 = n1->radius;
double r2 = n2->radius;
if ( d < r1 + r2 )
return 0.0;
return d - r1 - r2;
}
/**
* Create a new leaf node, storing pointers back to the end points for later.
*/
static CIRC_NODE*
circ_node_leaf_new(const POINTARRAY* pa, int i)
{
POINT2D *p1, *p2;
POINT3D q1, q2, c;
GEOGRAPHIC_POINT g1, g2, gc;
CIRC_NODE *node;
double diameter;
p1 = (POINT2D*)getPoint_internal(pa, i);
p2 = (POINT2D*)getPoint_internal(pa, i+1);
geographic_point_init(p1->x, p1->y, &g1);
geographic_point_init(p2->x, p2->y, &g2);
LWDEBUGF(3,"edge #%d (%g %g, %g %g)", i, p1->x, p1->y, p2->x, p2->y);
diameter = sphere_distance(&g1, &g2);
/* Zero length edge, doesn't get a node */
if ( FP_EQUALS(diameter, 0.0) )
return NULL;
/* Allocate */
node = lwalloc(sizeof(CIRC_NODE));
node->p1 = p1;
node->p2 = p2;
/* Convert ends to X/Y/Z, sum, and normalize to get mid-point */
geog2cart(&g1, &q1);
geog2cart(&g2, &q2);
vector_sum(&q1, &q2, &c);
normalize(&c);
cart2geog(&c, &gc);
node->center = gc;
node->radius = diameter / 2.0;
LWDEBUGF(3,"edge #%d CENTER(%g %g) RADIUS=%g", i, gc.lon, gc.lat, node->radius);
/* Leaf has no children */
node->num_nodes = 0;
node->nodes = NULL;
node->edge_num = i;
return node;
}
/**
* Computes the shortest distance along the surface of the spheroid
* between two points. Based on Vincenty's formula for the geodetic
* inverse problem as described in "Geocentric Datum of Australia
* Technical Manual", Chapter 4. Tested against:
* http://mascot.gdbc.gov.bc.ca/mascot/util1a.html
* and
* http://www.ga.gov.au/nmd/geodesy/datums/vincenty_inverse.jsp
*
* @param a - location of first point.
* @param b - location of second point.
* @param s - spheroid to calculate on
* @return spheroidal distance between a and b in spheroid units.
*/
double spheroid_distance(const GEOGRAPHIC_POINT *a, const GEOGRAPHIC_POINT *b, const SPHEROID *spheroid)
{
double lambda = (b->lon - a->lon);
double f = spheroid->f;
double omf = 1 - spheroid->f;
double u1, u2;
double cos_u1, cos_u2;
double sin_u1, sin_u2;
double big_a, big_b, delta_sigma;
double alpha, sin_alpha, cos_alphasq, c;
double sigma, sin_sigma, cos_sigma, cos2_sigma_m, sqrsin_sigma, last_lambda, omega;
double cos_lambda, sin_lambda;
double distance;
int i = 0;
/* Same point => zero distance */
if ( geographic_point_equals(a, b) )
{
return 0.0;
}
u1 = atan(omf * tan(a->lat));
cos_u1 = cos(u1);
sin_u1 = sin(u1);
u2 = atan(omf * tan(b->lat));
cos_u2 = cos(u2);
sin_u2 = sin(u2);
omega = lambda;
do
{
cos_lambda = cos(lambda);
sin_lambda = sin(lambda);
sqrsin_sigma = POW2(cos_u2 * sin_lambda) +
POW2((cos_u1 * sin_u2 - sin_u1 * cos_u2 * cos_lambda));
sin_sigma = sqrt(sqrsin_sigma);
cos_sigma = sin_u1 * sin_u2 + cos_u1 * cos_u2 * cos_lambda;
sigma = atan2(sin_sigma, cos_sigma);
sin_alpha = cos_u1 * cos_u2 * sin_lambda / sin(sigma);
/* Numerical stability issue, ensure asin is not NaN */
if ( sin_alpha > 1.0 )
alpha = M_PI_2;
else if ( sin_alpha < -1.0 )
alpha = -1.0 * M_PI_2;
else
alpha = asin(sin_alpha);
cos_alphasq = POW2(cos(alpha));
cos2_sigma_m = cos(sigma) - (2.0 * sin_u1 * sin_u2 / cos_alphasq);
/* Numerical stability issue, cos2 is in range */
if ( cos2_sigma_m > 1.0 )
cos2_sigma_m = 1.0;
if ( cos2_sigma_m < -1.0 )
cos2_sigma_m = -1.0;
c = (f / 16.0) * cos_alphasq * (4.0 + f * (4.0 - 3.0 * cos_alphasq));
last_lambda = lambda;
lambda = omega + (1.0 - c) * f * sin(alpha) * (sigma + c * sin(sigma) *
(cos2_sigma_m + c * cos(sigma) * (-1.0 + 2.0 * POW2(cos2_sigma_m))));
i++;
}
while ( (i < 999) && (lambda != 0.0) && (fabs((last_lambda - lambda)/lambda) > 1.0e-9) );
u2 = spheroid_mu2(alpha, spheroid);
big_a = spheroid_big_a(u2);
big_b = spheroid_big_b(u2);
delta_sigma = big_b * sin_sigma * (cos2_sigma_m + (big_b / 4.0) * (cos_sigma * (-1.0 + 2.0 * POW2(cos2_sigma_m)) -
(big_b / 6.0) * cos2_sigma_m * (-3.0 + 4.0 * sqrsin_sigma) * (-3.0 + 4.0 * POW2(cos2_sigma_m))));
distance = spheroid->b * big_a * (sigma - delta_sigma);
/* Algorithm failure, distance == NaN, fallback to sphere */
if ( distance != distance )
{
lwerror("spheroid_distance returned NaN: (%.20g %.20g) (%.20g %.20g) a = %.20g b = %.20g",a->lat, a->lon, b->lat, b->lon, spheroid->a, spheroid->b);
return spheroid->radius * sphere_distance(a, b);
}
return distance;
}
static double
circ_tree_distance_tree_internal(const CIRC_NODE* n1, const CIRC_NODE* n2, double threshold, double* min_dist, double* max_dist, GEOGRAPHIC_POINT* closest1, GEOGRAPHIC_POINT* closest2)
{
double max;
double d, d_min;
int i;
LWDEBUGF(4, "entered, min_dist %.8g max_dist %.8g", *min_dist, *max_dist);
// circ_tree_print(n1, 0);
// circ_tree_print(n2, 0);
/* Short circuit if we've already hit the minimum */
if( FP_LT(*min_dist, threshold) )
return *min_dist;
/* If your minimum is greater than anyone's maximum, you can't hold the winner */
if( circ_node_min_distance(n1, n2) > *max_dist )
{
LWDEBUGF(4, "pruning pair %p, %p", n1, n2);
return MAXFLOAT;
}
/* If your maximum is a new low, we'll use that as our new global tolerance */
max = circ_node_max_distance(n1, n2);
LWDEBUGF(5, "max %.8g", max);
if( max < *max_dist )
*max_dist = max;
/* Both leaf nodes, do a real distance calculation */
if( circ_node_is_leaf(n1) )
{
if( circ_node_is_leaf(n2) )
{
double d;
GEOGRAPHIC_POINT close1, close2;
LWDEBUGF(4, "testing leaf pair [%d], [%d]", n1->edge_num, n2->edge_num);
/* One of the nodes is a point */
if ( n1->p1 == n1->p2 || n2->p1 == n2->p2 )
{
GEOGRAPHIC_EDGE e;
GEOGRAPHIC_POINT gp1, gp2;
/* Both nodes are points! */
if ( n1->p1 == n1->p2 && n2->p1 == n2->p2 )
{
geographic_point_init(n1->p1->x, n1->p1->y, &gp1);
geographic_point_init(n2->p1->x, n2->p1->y, &gp2);
close1 = gp1; close2 = gp2;
d = sphere_distance(&gp1, &gp2);
}
/* Node 1 is a point */
else if ( n1->p1 == n1->p2 )
{
geographic_point_init(n1->p1->x, n1->p1->y, &gp1);
geographic_point_init(n2->p1->x, n2->p1->y, &(e.start));
geographic_point_init(n2->p2->x, n2->p2->y, &(e.end));
close1 = gp1;
d = edge_distance_to_point(&e, &gp1, &close2);
}
/* Node 2 is a point */
else
{
geographic_point_init(n2->p1->x, n2->p1->y, &gp1);
geographic_point_init(n1->p1->x, n1->p1->y, &(e.start));
geographic_point_init(n1->p2->x, n1->p2->y, &(e.end));
close1 = gp1;
d = edge_distance_to_point(&e, &gp1, &close2);
}
LWDEBUGF(4, " got distance %g", d);
}
/* Both nodes are edges */
else
{
GEOGRAPHIC_EDGE e1, e2;
GEOGRAPHIC_POINT g;
POINT3D A1, A2, B1, B2;
geographic_point_init(n1->p1->x, n1->p1->y, &(e1.start));
geographic_point_init(n1->p2->x, n1->p2->y, &(e1.end));
geographic_point_init(n2->p1->x, n2->p1->y, &(e2.start));
geographic_point_init(n2->p2->x, n2->p2->y, &(e2.end));
geog2cart(&(e1.start), &A1);
geog2cart(&(e1.end), &A2);
geog2cart(&(e2.start), &B1);
geog2cart(&(e2.end), &B2);
if ( edge_intersects(&A1, &A2, &B1, &B2) )
{
d = 0.0;
edge_intersection(&e1, &e2, &g);
close1 = close2 = g;
}
else
{
d = edge_distance_to_edge(&e1, &e2, &close1, &close2);
}
LWDEBUGF(4, "edge_distance_to_edge returned %g", d);
}
if ( d < *min_dist )
{
*min_dist = d;
*closest1 = close1;
*closest2 = close2;
}
return d;
}
else
{
d_min = MAXFLOAT;
for ( i = 0; i < n2->num_nodes; i++ )
{
d = circ_tree_distance_tree_internal(n1, n2->nodes[i], threshold, min_dist, max_dist, closest1, closest2);
d_min = FP_MIN(d_min, d);
}
return d_min;
}
}
else
{
d_min = MAXFLOAT;
for ( i = 0; i < n1->num_nodes; i++ )
{
d = circ_tree_distance_tree_internal(n2, n1->nodes[i], threshold, min_dist, max_dist, closest1, closest2);
d_min = FP_MIN(d_min, d);
}
return d_min;
}
}
static double
circ_node_max_distance(const CIRC_NODE *n1, const CIRC_NODE *n2)
{
return sphere_distance(&(n1->center), &(n2->center)) + n1->radius + n2->radius;
}
/**
* Create a new internal node, calculating the new measure range for the node,
* and storing pointers to the child nodes.
*/
static CIRC_NODE*
circ_node_internal_new(CIRC_NODE** c, int num_nodes)
{
CIRC_NODE *node = NULL;
GEOGRAPHIC_POINT new_center, c1;
double new_radius;
double offset1, dist, D, r1, ri;
int i;
LWDEBUGF(3, "called with %d nodes --", num_nodes);
/* Can't do anything w/ empty input */
if ( num_nodes < 1 )
return node;
/* Initialize calculation with values of the first circle */
new_center = c[0]->center;
new_radius = c[0]->radius;
/* Merge each remaining circle into the new circle */
for ( i = 1; i < num_nodes; i++ )
{
c1 = new_center;
r1 = new_radius;
dist = sphere_distance(&c1, &(c[i]->center));
ri = c[i]->radius;
LWDEBUGF(3, "distance between new (%g %g) and %i (%g %g) is %g", c1.lon, c1.lat, i, c[i]->center.lon, c[i]->center.lat, dist);
if ( FP_EQUALS(dist, 0) )
{
LWDEBUG(3, " distance between centers is zero");
new_radius = r1 + 2*dist;
new_center = c1;
}
else if ( dist < fabs(r1 - ri) )
{
/* new contains next */
if ( r1 > ri )
{
LWDEBUG(3, " c1 contains ci");
new_center = c1;
new_radius = r1;
}
/* next contains new */
else
{
LWDEBUG(3, " ci contains c1");
new_center = c[i]->center;
new_radius = ri;
}
}
else
{
LWDEBUG(3, " calculating new center");
/* New circle diameter */
D = dist + r1 + ri;
LWDEBUGF(3," D is %g", D);
/* New radius */
new_radius = D / 2.0;
/* Distance from cn1 center to the new center */
offset1 = ri + (D - (2.0*r1 + 2.0*ri)) / 2.0;
LWDEBUGF(3," offset1 is %g", offset1);
/* Sometimes the sphere_direction function fails... this causes the center calculation */
/* to fail too. In that case, we're going to fall back ot a cartesian calculation, which */
/* is less exact, so we also have to pad the radius by (hack alert) an arbitrary amount */
/* which is hopefully always big enough to contain the input edges */
if ( circ_center_spherical(&c1, &(c[i]->center), dist, offset1, &new_center) == LW_FAILURE )
{
circ_center_cartesian(&c1, &(c[i]->center), dist, offset1, &new_center);
new_radius *= 1.1;
}
}
LWDEBUGF(3, " new center is (%g %g) new radius is %g", new_center.lon, new_center.lat, new_radius);
}
node = lwalloc(sizeof(CIRC_NODE));
node->p1 = NULL;
node->p2 = NULL;
node->center = new_center;
node->radius = new_radius;
node->num_nodes = num_nodes;
node->nodes = c;
node->edge_num = -1;
return node;
}
