lwgeodetic_tree.c

Go to the documentation of this file.

/**********************************************************************
 *
 * PostGIS - Spatial Types for PostgreSQL
 * http://postgis.net
 *
 * PostGIS is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 2 of the License, or
 * (at your option) any later version.
 *
 * PostGIS is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with PostGIS.  If not, see <http://www.gnu.org/licenses/>.
 *
 **********************************************************************
 *
 * Copyright (C) 2012-2015 Paul Ramsey <pramsey@cleverelephant.ca>
 * Copyright (C) 2012-2015 Sandro Santilli <strk@kbt.io>
 *
 **********************************************************************/
 
#include "liblwgeom_internal.h"
#include "lwgeodetic_tree.h"
#include "lwgeom_log.h"
 
 
/* Internal prototype */
static CIRC_NODE* circ_nodes_merge(CIRC_NODE** nodes, int num_nodes);
 
 
static inline int
circ_node_is_leaf(const CIRC_NODE* node)
{
        return (node->num_nodes == 0);
}
 
void
circ_tree_free(CIRC_NODE* node)
{
        uint32_t i;
        if ( ! node ) return;
 
        if (node->nodes)
        {
                for (i = 0; i < node->num_nodes; i++)
                        circ_tree_free(node->nodes[i]);
                lwfree(node->nodes);
        }
        lwfree(node);
}
 
 
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;
 
        /* Zero out metadata */
        node->pt_outside.x = 0.0;
        node->pt_outside.y = 0.0;
        node->geom_type = 0;
 
        return node;
}
 
static CIRC_NODE*
circ_node_leaf_point_new(const POINTARRAY* pa)
{
        CIRC_NODE* tree = lwalloc(sizeof(CIRC_NODE));
        tree->p1 = tree->p2 = (POINT2D*)getPoint_internal(pa, 0);
        geographic_point_init(tree->p1->x, tree->p1->y, &(tree->center));
        tree->radius = 0.0;
        tree->nodes = NULL;
        tree->num_nodes = 0;
        tree->edge_num = 0;
        tree->geom_type = POINTTYPE;
        tree->pt_outside.x = 0.0;
        tree->pt_outside.y = 0.0;
        return tree;
}
 
static int
circ_node_compare(const void* v1, const void* v2)
{
        POINT2D p1, p2;
        unsigned int u1, u2;
        CIRC_NODE *c1 = *((CIRC_NODE**)v1);
        CIRC_NODE *c2 = *((CIRC_NODE**)v2);
        p1.x = rad2deg((c1->center).lon);
        p1.y = rad2deg((c1->center).lat);
        p2.x = rad2deg((c2->center).lon);
        p2.y = rad2deg((c2->center).lat);
        u1 = geohash_point_as_int(&p1);
        u2 = geohash_point_as_int(&p2);
        if ( u1 < u2 ) return -1;
        if ( u1 > u2 ) return 1;
        return 0;
}
 
static int
circ_center_spherical(const GEOGRAPHIC_POINT* c1, const GEOGRAPHIC_POINT* c2, double distance, double offset, GEOGRAPHIC_POINT* center)
{
        /* Direction from c1 to c2 */
        double dir = sphere_direction(c1, c2, distance);
 
        LWDEBUG(4,"calculating spherical center");
 
        LWDEBUGF(4,"dir is %g", dir);
 
        /* Catch sphere_direction when it barfs */
        if ( isnan(dir) )
                return LW_FAILURE;
 
        /* Center of new circle is projection from start point, using offset distance*/
        return sphere_project(c1, offset, dir, center);
}
 
static int
circ_center_cartesian(const GEOGRAPHIC_POINT* c1, const GEOGRAPHIC_POINT* c2, double distance, double offset, GEOGRAPHIC_POINT* center)
{
        POINT3D p1, p2;
        POINT3D p1p2, pc;
        double proportion = offset/distance;
 
        LWDEBUG(4,"calculating cartesian center");
 
        geog2cart(c1, &p1);
        geog2cart(c2, &p2);
 
        /* Difference between p2 and p1 */
        p1p2.x = p2.x - p1.x;
        p1p2.y = p2.y - p1.y;
        p1p2.z = p2.z - p1.z;
 
        /* Scale difference to proportion */
        p1p2.x *= proportion;
        p1p2.y *= proportion;
        p1p2.z *= proportion;
 
        /* Add difference to p1 to get approximate center point */
        pc.x = p1.x + p1p2.x;
        pc.y = p1.y + p1p2.y;
        pc.z = p1.z + p1p2.z;
        normalize(&pc);
 
        /* Convert center point to geographics */
        cart2geog(&pc, center);
 
        return LW_SUCCESS;
}
 
 
static CIRC_NODE*
circ_node_internal_new(CIRC_NODE** c, uint32_t num_nodes)
{
        CIRC_NODE *node = NULL;
        GEOGRAPHIC_POINT new_center, c1;
        double new_radius;
        double offset1, dist, D, r1, ri;
        uint32_t i, new_geom_type;
 
        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;
        new_geom_type = c[0]->geom_type;
 
        /* 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;
 
                /* Promote geometry types up the tree, getting more and more collected */
                /* Go until we find a value */
                if ( ! new_geom_type )
                {
                        new_geom_type = c[i]->geom_type;
                }
                /* Promote singleton to a multi-type */
                else if ( ! lwtype_is_collection(new_geom_type) )
                {
                        /* Anonymous collection if types differ */
                        if ( new_geom_type != c[i]->geom_type )
                        {
                                new_geom_type = COLLECTIONTYPE;
                        }
                        else
                        {
                                new_geom_type = lwtype_get_collectiontype(new_geom_type);
                        }
                }
                /* If we can't add next feature to this collection cleanly, promote again to anonymous collection */
                else if ( new_geom_type != lwtype_get_collectiontype(c[i]->geom_type) )
                {
                        new_geom_type = COLLECTIONTYPE;
                }
 
 
                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 to 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;
        node->geom_type = new_geom_type;
        node->pt_outside.x = 0.0;
        node->pt_outside.y = 0.0;
        return node;
}
 
CIRC_NODE*
circ_tree_new(const POINTARRAY* pa)
{
        int num_edges;
        int i, j;
        CIRC_NODE **nodes;
        CIRC_NODE *node;
        CIRC_NODE *tree;
 
        /* Can't do anything with no points */
        if ( pa->npoints < 1 )
                return NULL;
 
        /* Special handling for a single point */
        if ( pa->npoints == 1 )
                return circ_node_leaf_point_new(pa);
 
        /* First create a flat list of nodes, one per edge. */
        num_edges = pa->npoints - 1;
        nodes = lwalloc(sizeof(CIRC_NODE*) * pa->npoints);
        j = 0;
        for ( i = 0; i < num_edges; i++ )
        {
                node = circ_node_leaf_new(pa, i);
                if ( node ) /* Not zero length? */
                        nodes[j++] = node;
        }
 
        /* Special case: only zero-length edges. Make a point node. */
        if ( j == 0 ) {
                lwfree(nodes);
                return circ_node_leaf_point_new(pa);
        }
 
        /* Merge the node list pairwise up into a tree */
        tree = circ_nodes_merge(nodes, j);
 
        /* Free the old list structure, leaving the tree in place */
        lwfree(nodes);
 
        return tree;
}
 
static void
circ_nodes_sort(CIRC_NODE** nodes, int num_nodes)
{
        qsort(nodes, num_nodes, sizeof(CIRC_NODE*), circ_node_compare);
}
 
 
static CIRC_NODE*
circ_nodes_merge(CIRC_NODE** nodes, int num_nodes)
{
        CIRC_NODE **inodes = NULL;
        int num_children = num_nodes;
        int inode_num = 0;
        int num_parents = 0;
        int j;
 
        /* TODO, roll geom_type *up* as tree is built, changing to collection types as simple types are merged
         * TODO, change the distance algorithm to drive down to simple types first, test pip on poly/other cases, then test edges
         */
 
        while( num_children > 1 )
        {
                for ( j = 0; j < num_children; j++ )
                {
                        inode_num = (j % CIRC_NODE_SIZE);
                        if ( inode_num == 0 )
                                inodes = lwalloc(sizeof(CIRC_NODE*)*CIRC_NODE_SIZE);
 
                        inodes[inode_num] = nodes[j];
 
                        if ( inode_num == CIRC_NODE_SIZE-1 )
                                nodes[num_parents++] = circ_node_internal_new(inodes, CIRC_NODE_SIZE);
                }
 
                /* Clean up any remaining nodes... */
                if ( inode_num == 0 )
                {
                        /* Promote solo nodes without merging */
                        nodes[num_parents++] = inodes[0];
                        lwfree(inodes);
                }
                else if ( inode_num < CIRC_NODE_SIZE-1 )
                {
                        /* Merge spare nodes */
                        nodes[num_parents++] = circ_node_internal_new(inodes, inode_num+1);
                }
 
                num_children = num_parents;
                num_parents = 0;
        }
 
        /* Return a reference to the head of the tree */
        return nodes[0];
}
 
 
int circ_tree_get_point(const CIRC_NODE* node, POINT2D* pt)
{
        if ( circ_node_is_leaf(node) )
        {
                pt->x = node->p1->x;
                pt->y = node->p1->y;
                return LW_SUCCESS;
        }
        else
        {
                return circ_tree_get_point(node->nodes[0], pt);
        }
}
 
int circ_tree_get_point_outside(const CIRC_NODE* node, POINT2D* pt)
{
        POINT3D center3d;
        GEOGRAPHIC_POINT g;
        // if (node->radius >= M_PI) return LW_FAILURE;
        geog2cart(&(node->center), &center3d);
        vector_scale(&center3d, -1.0);
        cart2geog(&center3d, &g);
        pt->x = rad2deg(g.lon);
        pt->y = rad2deg(g.lat);
        return LW_SUCCESS;
}
 
 
int circ_tree_contains_point(const CIRC_NODE* node, const POINT2D* pt, const POINT2D* pt_outside, int level, int* on_boundary)
{
        GEOGRAPHIC_POINT closest;
        GEOGRAPHIC_EDGE stab_edge, edge;
        POINT3D S1, S2, E1, E2;
        double d;
        uint32_t i, c;
 
        /* Construct a stabline edge from our "inside" to our known outside point */
        geographic_point_init(pt->x, pt->y, &(stab_edge.start));
        geographic_point_init(pt_outside->x, pt_outside->y, &(stab_edge.end));
        geog2cart(&(stab_edge.start), &S1);
        geog2cart(&(stab_edge.end), &S2);
 
        LWDEBUGF(3, "%*s entered", level, "");
 
        /*
        * If the stabline doesn't cross within the radius of a node, there's no
        * way it can cross.
        */
 
        LWDEBUGF(3, "%*s :working on node %p, edge_num %d, radius %g, center POINT(%.12g %.12g)", level, "", node, node->edge_num, node->radius, rad2deg(node->center.lon), rad2deg(node->center.lat));
        d = edge_distance_to_point(&stab_edge, &(node->center), &closest);
        LWDEBUGF(3, "%*s :edge_distance_to_point=%g, node_radius=%g", level, "", d, node->radius);
        if ( FP_LTEQ(d, node->radius) )
        {
                LWDEBUGF(3,"%*s :entering this branch (%p)", level, "", node);
 
                /* Return the crossing number of this leaf */
                if ( circ_node_is_leaf(node) )
                {
                        int inter;
                        LWDEBUGF(3, "%*s :leaf node calculation (edge %d)", level, "", node->edge_num);
                        geographic_point_init(node->p1->x, node->p1->y, &(edge.start));
                        geographic_point_init(node->p2->x, node->p2->y, &(edge.end));
                        geog2cart(&(edge.start), &E1);
                        geog2cart(&(edge.end), &E2);
 
                        inter = edge_intersects(&S1, &S2, &E1, &E2);
                        LWDEBUGF(3, "%*s :inter = %d", level, "", inter);
 
                        if ( inter & PIR_INTERSECTS )
                        {
                                LWDEBUGF(3,"%*s ::got stab line edge_intersection with this edge!", level, "");
                                /* To avoid double counting crossings-at-a-vertex, */
                                /* always ignore crossings at "lower" ends of edges*/
                                GEOGRAPHIC_POINT e1, e2;
                                cart2geog(&E1,&e1); cart2geog(&E2,&e2);
 
                                LWDEBUGF(3,"%*s LINESTRING(%.15g %.15g,%.15g %.15g)", level, "",
                                        pt->x, pt->y,
                                        pt_outside->x, pt_outside->y
                                        );
 
                                LWDEBUGF(3,"%*s LINESTRING(%.15g %.15g,%.15g %.15g)", level, "",
                                        rad2deg(e1.lon), rad2deg(e1.lat),
                                        rad2deg(e2.lon), rad2deg(e2.lat)
                                        );
 
                                if ( inter & PIR_B_TOUCH_RIGHT || inter & PIR_COLINEAR )
                                {
                                        LWDEBUGF(3,"%*s ::rejecting stab line grazing by left-side edge", level, "");
                                        return 0;
                                }
                                else
                                {
                                        LWDEBUGF(3,"%*s ::accepting stab line intersection", level, "");
                                        return 1;
                                }
                        }
                        else
                        {
                                LWDEBUGF(3,"%*s edge does not intersect", level, "");
                        }
                }
                /* Or, add up the crossing numbers of all children of this node. */
                else
                {
                        c = 0;
                        for ( i = 0; i < node->num_nodes; i++ )
                        {
                                LWDEBUGF(3,"%*s calling circ_tree_contains_point on child %d!", level, "", i);
                                c += circ_tree_contains_point(node->nodes[i], pt, pt_outside, level + 1, on_boundary);
                        }
                        return c % 2;
                }
        }
        else
        {
                LWDEBUGF(3,"%*s skipping this branch (%p)", level, "", node);
        }
        return 0;
}
 
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;
}
 
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;
}
 
double
circ_tree_distance_tree(const CIRC_NODE* n1, const CIRC_NODE* n2, const SPHEROID* spheroid, double threshold)
{
        double min_dist = FLT_MAX;
        double max_dist = FLT_MAX;
        GEOGRAPHIC_POINT closest1, closest2;
        /* Quietly decrease the threshold just a little to avoid cases where */
        /* the actual spheroid distance is larger than the sphere distance */
        /* causing the return value to be larger than the threshold value */
        double threshold_radians = 0.95 * threshold / spheroid->radius;
 
        circ_tree_distance_tree_internal(n1, n2, threshold_radians, &min_dist, &max_dist, &closest1, &closest2);
 
        /* Spherical case */
        if ( spheroid->a == spheroid->b )
        {
                return spheroid->radius * sphere_distance(&closest1, &closest2);
        }
        else
        {
                return spheroid_distance(&closest1, &closest2, spheroid);
        }
}
 
 
/***********************************************************************
* Internal node sorting routine to make distance calculations faster?
*/
 
struct sort_node {
        CIRC_NODE *node;
        double d;
};
 
static int
circ_nodes_sort_cmp(const void *a, const void *b)
{
        struct sort_node *node_a = (struct sort_node *)(a);
        struct sort_node *node_b = (struct sort_node *)(b);
        if (node_a->d < node_b->d) return -1;
        else if (node_a->d > node_b->d) return 1;
        else return 0;
}
 
static void
circ_internal_nodes_sort(CIRC_NODE **nodes, uint32_t num_nodes, const CIRC_NODE *target_node)
{
        uint32_t i;
        struct sort_node sort_nodes[CIRC_NODE_SIZE];
 
        /* Copy incoming nodes into sorting array and calculate */
        /* distance to the target node */
        for (i = 0; i < num_nodes; i++)
        {
                sort_nodes[i].node = nodes[i];
                sort_nodes[i].d = sphere_distance(&(nodes[i]->center), &(target_node->center));
        }
 
        /* Sort the nodes and copy the result back into the input array */
        qsort(sort_nodes, num_nodes, sizeof(struct sort_node), circ_nodes_sort_cmp);
        for (i = 0; i < num_nodes; i++)
        {
                nodes[i] = sort_nodes[i].node;
        }
        return;
}
 
/***********************************************************************/
 
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;
        uint32_t i;
 
        LWDEBUGF(4, "entered, min_dist=%.8g max_dist=%.8g, type1=%d, type2=%d", *min_dist, *max_dist, n1->geom_type, n2->geom_type);
 
        // printf("-==-\n");
        // circ_tree_print(n1, 0);
        // printf("--\n");
        // circ_tree_print(n2, 0);
 
        /* Short circuit if we've already hit the minimum */
        if( *min_dist < threshold || *min_dist == 0.0 )
                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 FLT_MAX;
        }
 
        /* 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;
 
        /* Polygon on one side, primitive type on the other. Check for point-in-polygon */
        /* short circuit. */
        if ( n1->geom_type == POLYGONTYPE && n2->geom_type && ! lwtype_is_collection(n2->geom_type) )
        {
                POINT2D pt;
                circ_tree_get_point(n2, &pt);
                LWDEBUGF(4, "n1 is polygon, testing if contains (%.5g,%.5g)", pt.x, pt.y);
                if ( circ_tree_contains_point(n1, &pt, &(n1->pt_outside), 0, NULL) )
                {
                        LWDEBUG(4, "it does");
                        *min_dist = 0.0;
                        geographic_point_init(pt.x, pt.y, closest1);
                        geographic_point_init(pt.x, pt.y, closest2);
                        return *min_dist;
                }
        }
        /* Polygon on one side, primitive type on the other. Check for point-in-polygon */
        /* short circuit. */
        if ( n2->geom_type == POLYGONTYPE && n1->geom_type && ! lwtype_is_collection(n1->geom_type) )
        {
                POINT2D pt;
                circ_tree_get_point(n1, &pt);
                LWDEBUGF(4, "n2 is polygon, testing if contains (%.5g,%.5g)", pt.x, pt.y);
                if ( circ_tree_contains_point(n2, &pt, &(n2->pt_outside), 0, NULL) )
                {
                        LWDEBUG(4, "it does");
                        geographic_point_init(pt.x, pt.y, closest1);
                        geographic_point_init(pt.x, pt.y, closest2);
                        *min_dist = 0.0;
                        return *min_dist;
                }
        }
 
        /* Both leaf nodes, do a real distance calculation */
        if( circ_node_is_leaf(n1) && 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, &gp2);
                                geographic_point_init(n1->p1->x, n1->p1->y, &(e.start));
                                geographic_point_init(n1->p2->x, n1->p2->y, &(e.end));
                                close2 = gp2;
                                d = edge_distance_to_point(&e, &gp2, &close1);
                        }
                        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 = FLT_MAX;
                /* Drive the recursion into the COLLECTION types first so we end up with */
                /* pairings of primitive geometries that can be forced into the point-in-polygon */
                /* tests above. */
                if ( n1->geom_type && lwtype_is_collection(n1->geom_type) )
                {
                        circ_internal_nodes_sort(n1->nodes, n1->num_nodes, n2);
                        for ( i = 0; i < n1->num_nodes; i++ )
                        {
                                d = circ_tree_distance_tree_internal(n1->nodes[i], n2, threshold, min_dist, max_dist, closest1, closest2);
                                d_min = FP_MIN(d_min, d);
                        }
                }
                else if ( n2->geom_type && lwtype_is_collection(n2->geom_type) )
                {
                        circ_internal_nodes_sort(n2->nodes, n2->num_nodes, n1);
                        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);
                        }
                }
                else if ( ! circ_node_is_leaf(n1) )
                {
                        circ_internal_nodes_sort(n1->nodes, n1->num_nodes, n2);
                        for ( i = 0; i < n1->num_nodes; i++ )
                        {
                                d = circ_tree_distance_tree_internal(n1->nodes[i], n2, threshold, min_dist, max_dist, closest1, closest2);
                                d_min = FP_MIN(d_min, d);
                        }
                }
                else if ( ! circ_node_is_leaf(n2) )
                {
                        circ_internal_nodes_sort(n2->nodes, n2->num_nodes, n1);
                        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);
                        }
                }
                else
                {
                        /* Never get here */
                }
 
                return d_min;
        }
}
 
 
 
 
 
void circ_tree_print(const CIRC_NODE* node, int depth)
{
        uint32_t i;
 
        if (circ_node_is_leaf(node))
        {
                printf("%*s[%d] C(%.8g %.8g) R(%.8g) ((%.8g %.8g),(%.8g,%.8g))",
                  3*depth + 6, "NODE", node->edge_num,
                  node->center.lon, node->center.lat,
                  node->radius,
                  node->p1->x, node->p1->y,
                  node->p2->x, node->p2->y
                );
                if ( node->geom_type )
                {
                        printf(" %s", lwtype_name(node->geom_type));
                }
                if ( node->geom_type == POLYGONTYPE )
                {
                        printf(" O(%.8g %.8g)", node->pt_outside.x, node->pt_outside.y);
                }
                printf("\n");
 
        }
        else
        {
                printf("%*s C(%.8g %.8g) R(%.8g)",
                  3*depth + 6, "NODE",
                  node->center.lon, node->center.lat,
                  node->radius
                );
                if ( node->geom_type )
                {
                        printf(" %s", lwtype_name(node->geom_type));
                }
                if ( node->geom_type == POLYGONTYPE )
                {
                        printf(" O(%.15g %.15g)", node->pt_outside.x, node->pt_outside.y);
                }
                printf("\n");
        }
        for ( i = 0; i < node->num_nodes; i++ )
        {
                circ_tree_print(node->nodes[i], depth + 1);
        }
        return;
}
 
 
static CIRC_NODE*
lwpoint_calculate_circ_tree(const LWPOINT* lwpoint)
{
        CIRC_NODE* node;
        node = circ_tree_new(lwpoint->point);
        node->geom_type = lwgeom_get_type((LWGEOM*)lwpoint);;
        return node;
}
 
static CIRC_NODE*
lwline_calculate_circ_tree(const LWLINE* lwline)
{
        CIRC_NODE* node;
        node = circ_tree_new(lwline->points);
        node->geom_type = lwgeom_get_type((LWGEOM*)lwline);
        return node;
}
 
static CIRC_NODE*
lwpoly_calculate_circ_tree(const LWPOLY* lwpoly)
{
        uint32_t i = 0, j = 0;
        CIRC_NODE** nodes;
        CIRC_NODE* node;
 
        /* One ring? Handle it like a line. */
        if ( lwpoly->nrings == 1 )
        {
                node = circ_tree_new(lwpoly->rings[0]);
        }
        else
        {
                /* Calculate a tree for each non-trivial ring of the polygon */
                nodes = lwalloc(lwpoly->nrings * sizeof(CIRC_NODE*));
                for ( i = 0; i < lwpoly->nrings; i++ )
                {
                        node = circ_tree_new(lwpoly->rings[i]);
                        if ( node )
                                nodes[j++] = node;
                }
                /* Put the trees into a spatially correlated order */
                circ_nodes_sort(nodes, j);
                /* Merge the trees pairwise up to a parent node and return */
                node = circ_nodes_merge(nodes, j);
                /* Don't need the working list any more */
                lwfree(nodes);
        }
 
        /* Metadata about polygons, we need this to apply P-i-P tests */
        /* selectively when doing distance calculations */
        node->geom_type = lwgeom_get_type((LWGEOM*)lwpoly);
        lwpoly_pt_outside(lwpoly, &(node->pt_outside));
 
        return node;
}
 
static CIRC_NODE*
lwcollection_calculate_circ_tree(const LWCOLLECTION* lwcol)
{
        uint32_t i = 0, j = 0;
        CIRC_NODE** nodes;
        CIRC_NODE* node;
 
        /* One geometry? Done! */
        if ( lwcol->ngeoms == 1 )
                return lwgeom_calculate_circ_tree(lwcol->geoms[0]);
 
        /* Calculate a tree for each sub-geometry*/
        nodes = lwalloc(lwcol->ngeoms * sizeof(CIRC_NODE*));
        for ( i = 0; i < lwcol->ngeoms; i++ )
        {
                node = lwgeom_calculate_circ_tree(lwcol->geoms[i]);
                if ( node )
                        nodes[j++] = node;
        }
        /* Put the trees into a spatially correlated order */
        circ_nodes_sort(nodes, j);
        /* Merge the trees pairwise up to a parent node and return */
        node = circ_nodes_merge(nodes, j);
        /* Don't need the working list any more */
        lwfree(nodes);
        node->geom_type = lwgeom_get_type((LWGEOM*)lwcol);
        return node;
}
 
CIRC_NODE*
lwgeom_calculate_circ_tree(const LWGEOM* lwgeom)
{
        if ( lwgeom_is_empty(lwgeom) )
                return NULL;
 
        switch ( lwgeom->type )
        {
                case POINTTYPE:
                        return lwpoint_calculate_circ_tree((LWPOINT*)lwgeom);
                case LINETYPE:
                        return lwline_calculate_circ_tree((LWLINE*)lwgeom);
                case POLYGONTYPE:
                        return lwpoly_calculate_circ_tree((LWPOLY*)lwgeom);
                case MULTIPOINTTYPE:
                case MULTILINETYPE:
                case MULTIPOLYGONTYPE:
                case COLLECTIONTYPE:
                        return lwcollection_calculate_circ_tree((LWCOLLECTION*)lwgeom);
                default:
                        lwerror("Unable to calculate spherical index tree for type %s", lwtype_name(lwgeom->type));
                        return NULL;
        }
 
}
 
 
/***********************************************************************
 * Closest point and closest line functions for geographies.
 ***********************************************************************/
 
LWGEOM *
geography_tree_closestpoint(const LWGEOM* lwgeom1, const LWGEOM* lwgeom2, double threshold)
{
        CIRC_NODE* circ_tree1 = NULL;
        CIRC_NODE* circ_tree2 = NULL;
        double min_dist = FLT_MAX;
        double max_dist = FLT_MAX;
        GEOGRAPHIC_POINT closest1, closest2;
        LWGEOM *result;
        POINT4D p;
 
        circ_tree1 = lwgeom_calculate_circ_tree(lwgeom1);
        circ_tree2 = lwgeom_calculate_circ_tree(lwgeom2);
 
        /* Quietly decrease the threshold just a little to avoid cases where */
        /* the actual spheroid distance is larger than the sphere distance */
        /* causing the return value to be larger than the threshold value */
        // double threshold_radians = 0.95 * threshold / spheroid->radius;
        double threshold_radians = threshold / WGS84_RADIUS;
 
        circ_tree_distance_tree_internal(
                circ_tree1, circ_tree2, threshold_radians,
                &min_dist, &max_dist, &closest1, &closest2);
 
        p.x = rad2deg(closest1.lon);
        p.y = rad2deg(closest1.lat);
        result = (LWGEOM *)lwpoint_make2d(lwgeom_get_srid(lwgeom1), p.x, p.y);
 
        circ_tree_free(circ_tree1);
        circ_tree_free(circ_tree2);
        return result;
}
 
 
LWGEOM *
geography_tree_shortestline(const LWGEOM* lwgeom1, const LWGEOM* lwgeom2, double threshold, const SPHEROID *spheroid)
{
  CIRC_NODE* circ_tree1 = NULL;
  CIRC_NODE* circ_tree2 = NULL;
  double min_dist = FLT_MAX;
  double max_dist = FLT_MAX;
  GEOGRAPHIC_POINT closest1, closest2;
  LWGEOM *geoms[2];
  LWGEOM *result;
  POINT4D p1, p2;
  uint32_t srid = lwgeom1->srid;
 
  circ_tree1 = lwgeom_calculate_circ_tree(lwgeom1);
  circ_tree2 = lwgeom_calculate_circ_tree(lwgeom2);
 
  /* Quietly decrease the threshold just a little to avoid cases where */
  /* the actual spheroid distance is larger than the sphere distance */
  /* causing the return value to be larger than the threshold value */
  // double threshold_radians = 0.95 * threshold / spheroid->radius;
  double threshold_radians = threshold / spheroid->radius;
 
  circ_tree_distance_tree_internal(circ_tree1, circ_tree2, threshold_radians,
      &min_dist, &max_dist, &closest1, &closest2);
 
  p1.x = rad2deg(closest1.lon);
  p1.y = rad2deg(closest1.lat);
  p2.x = rad2deg(closest2.lon);
  p2.y = rad2deg(closest2.lat);
 
  geoms[0] = (LWGEOM *)lwpoint_make2d(srid, p1.x, p1.y);
  geoms[1] = (LWGEOM *)lwpoint_make2d(srid, p2.x, p2.y);
  result = (LWGEOM *)lwline_from_lwgeom_array(srid, 2, geoms);
 
  lwgeom_free(geoms[0]);
  lwgeom_free(geoms[1]);
  circ_tree_free(circ_tree1);
  circ_tree_free(circ_tree2);
  return result;
}