From c73e21d15fadeb9e5c8ec275708a174ac794dc6c Mon Sep 17 00:00:00 2001 From: Paul Ramsey Date: Thu, 12 Jul 2012 03:38:03 +0000 Subject: [PATCH] Allow circtree nodes to have more than 2 children (#1910) git-svn-id: http://svn.osgeo.org/postgis/trunk@10046 b70326c6-7e19-0410-871a-916f4a2858ee --- liblwgeom/cunit/cu_geodetic.c | 60 ++++++++++++++++++++++++++++++++ liblwgeom/cunit/cu_tree.c | 14 ++++++-- liblwgeom/lwgeodetic.c | 64 +++++++++++++++++++++++++++++++++++ liblwgeom/lwgeodetic.h | 2 ++ liblwgeom/lwgeodetic_tree.c | 59 +++++++++++++++++--------------- liblwgeom/lwgeodetic_tree.h | 2 ++ 6 files changed, 172 insertions(+), 29 deletions(-) diff --git a/liblwgeom/cunit/cu_geodetic.c b/liblwgeom/cunit/cu_geodetic.c index 4cd708bef..239dbb7c1 100644 --- a/liblwgeom/cunit/cu_geodetic.c +++ b/liblwgeom/cunit/cu_geodetic.c @@ -1078,6 +1078,64 @@ static void test_gbox_utils(void) } +static void test_vector_angle(void) +{ + POINT3D p1, p2; + double angle; + + memset(&p1, 0, sizeof(POINT3D)); + memset(&p2, 0, sizeof(POINT3D)); + + p1.x = 1.0; + p2.y = 1.0; + angle = vector_angle(&p1, &p2); + CU_ASSERT_DOUBLE_EQUAL(angle, M_PI/2, 0.00001); + + p1.x = p2.y = 0.0; + p1.y = 1.0; + p2.x = 1.0; + angle = vector_angle(&p1, &p2); + CU_ASSERT_DOUBLE_EQUAL(angle, M_PI/2, 0.00001); + + p2.y = p2.x = 1.0; + normalize(&p2); + angle = vector_angle(&p1, &p2); + CU_ASSERT_DOUBLE_EQUAL(angle, M_PI/4, 0.00001); + + p2.x = p2.y = p2.z = 1.0; + normalize(&p2); + angle = vector_angle(&p1, &p2); + CU_ASSERT_DOUBLE_EQUAL(angle, 0.955317, 0.00001); + //printf ("angle = %g\n\n", angle); +} + +static void test_vector_rotate(void) +{ + POINT3D p1, p2, n; + double angle; + + memset(&p1, 0, sizeof(POINT3D)); + memset(&p2, 0, sizeof(POINT3D)); + memset(&n, 0, sizeof(POINT3D)); + + p1.x = 1.0; + p2.y = 1.0; + angle = M_PI/4; + vector_rotate(&p1, &p2, angle, &n); + //printf("%g %g %g\n\n", n.x, n.y, n.z); + CU_ASSERT_DOUBLE_EQUAL(n.x, 0.707107, 0.00001); + + angle = 2*M_PI/400000000; + vector_rotate(&p1, &p2, angle, &n); + //printf("%.21g %.21g %.21g\n\n", n.x, n.y, n.z); + CU_ASSERT_DOUBLE_EQUAL(n.x, 0.999999999999999888978, 0.0000000000000001); + CU_ASSERT_DOUBLE_EQUAL(n.y, 1.57079632679489654446e-08, 0.0000000000000001); + + angle = 0; + vector_rotate(&p1, &p2, angle, &n); + //printf("%.16g %.16g %.16g\n\n", n.x, n.y, n.z); + CU_ASSERT_DOUBLE_EQUAL(n.x, 1.0, 0.00000001); +} /* ** Used by test harness to register the tests in this file. @@ -1101,6 +1159,8 @@ CU_TestInfo geodetic_tests[] = PG_TEST(test_lwpoly_covers_point2d), PG_TEST(test_ptarray_point_in_ring), PG_TEST(test_gbox_utils), + PG_TEST(test_vector_angle), + PG_TEST(test_vector_rotate), CU_TEST_INFO_NULL }; CU_SuiteInfo geodetic_suite = {"Geodetic Suite", NULL, NULL, geodetic_tests}; diff --git a/liblwgeom/cunit/cu_tree.c b/liblwgeom/cunit/cu_tree.c index d96d00ea6..04c232feb 100644 --- a/liblwgeom/cunit/cu_tree.c +++ b/liblwgeom/cunit/cu_tree.c @@ -25,10 +25,20 @@ static void test_tree_circ_create(void) { LWLINE *g; CIRC_NODE *c; + /* Line with 4 edges */ g = lwgeom_as_lwline(lwgeom_from_wkt("LINESTRING(0 88,0 89,0 90,180 89,180 88)", LW_PARSER_CHECK_NONE)); c = circ_tree_new(g->points); -// circ_tree_print(c, 0); - CU_ASSERT(c->num_nodes == 2); + //circ_tree_print(c, 0); + + if ( CIRC_NODE_SIZE > 4 ) + { + CU_ASSERT(c->num_nodes == 4); + } + else + { + CU_ASSERT(c->num_nodes == ( 4 % CIRC_NODE_SIZE ? 1 : 0 ) + 4 / CIRC_NODE_SIZE); + } + circ_tree_free(c); lwline_free(g); } diff --git a/liblwgeom/lwgeodetic.c b/liblwgeom/lwgeodetic.c index e00103d60..41e8de1d9 100644 --- a/liblwgeom/lwgeodetic.c +++ b/liblwgeom/lwgeodetic.c @@ -417,6 +417,70 @@ static void vector_scale(POINT3D *n, double scale) return; } +static inline double vector_magnitude(const POINT3D* v) +{ + return sqrt(v->x*v->x + v->y*v->y + v->z*v->z); +} + +/** +* Angle between two unit vectors +*/ +double vector_angle(const POINT3D* v1, const POINT3D* v2) +{ + POINT3D v3, normal; + double angle, x, y; + + cross_product(v1, v2, &normal); + normalize(&normal); + cross_product(&normal, v1, &v3); + + x = dot_product(v1, v2); + y = dot_product(v2, &v3); + + angle = atan2(y, x); + return angle; +} + +void vector_rotate(const POINT3D* v1, const POINT3D* v2, double angle, POINT3D* n) +{ + POINT3D u; + double cos_a = cos(angle); + double sin_a = sin(angle); + double uxuy, uyuz, uxuz; + double ux2, uy2, uz2; + double rxx, rxy, rxz, ryx, ryy, ryz, rzx, rzy, rzz; + + /* Need a unit vector normal to rotate around */ + cross_product(v1, v2, &u); + normalize(&u); + + uxuy = u.x * u.y; + uxuz = u.x * u.z; + uyuz = u.y * u.z; + + ux2 = u.x * u.x; + uy2 = u.y * u.y; + uz2 = u.z * u.z; + + rxx = cos_a + ux2 * (1 - cos_a); + rxy = uxuy * (1 - cos_a) - u.z * sin_a; + rxz = uxuz * (1 - cos_a) + u.y * sin_a; + + ryx = uxuy * (1 - cos_a) + u.z * sin_a; + ryy = cos_a + uy2 * (1 - cos_a); + ryz = uyuz * (1 - cos_a) - u.x * sin_a; + + rzx = uxuz * (1 - cos_a) - u.y * sin_a; + rzy = uyuz * (1 - cos_a) + u.x * sin_a; + rzz = cos_a + uz2 * (1 - cos_a); + + n->x = rxx * v1->x + rxy * v1->y + rxz * v1->z; + n->y = ryx * v1->x + ryy * v1->y + ryz * v1->z; + n->z = rzx * v1->x + rzy * v1->y + rzz * v1->z; + + normalize(n); +} + /** * Normalize to a unit vector. */ diff --git a/liblwgeom/lwgeodetic.h b/liblwgeom/lwgeodetic.h index 0caea337c..b7f43e4e1 100644 --- a/liblwgeom/lwgeodetic.h +++ b/liblwgeom/lwgeodetic.h @@ -102,6 +102,8 @@ void point_shift(GEOGRAPHIC_POINT *p, double shift); double longitude_radians_normalize(double lon); double latitude_radians_normalize(double lat); void vector_sum(const POINT3D *a, const POINT3D *b, POINT3D *n); +double vector_angle(const POINT3D* v1, const POINT3D* v2); +void vector_rotate(const POINT3D* v1, const POINT3D* v2, double angle, POINT3D* n); void normalize(POINT3D *p); double sphere_direction(const GEOGRAPHIC_POINT *s, const GEOGRAPHIC_POINT *e, double d); diff --git a/liblwgeom/lwgeodetic_tree.c b/liblwgeom/lwgeodetic_tree.c index 5f236c957..5231cd700 100644 --- a/liblwgeom/lwgeodetic_tree.c +++ b/liblwgeom/lwgeodetic_tree.c @@ -333,43 +333,48 @@ circ_nodes_sort(CIRC_NODE** nodes, int num_nodes) static CIRC_NODE* circ_nodes_merge(CIRC_NODE** nodes, int num_nodes) { - int num_children, num_parents, j; - /* This assumption is actually hard coded into the algorithm below */ - /* Quite a few changes needed to increase node size */ - static int node_size = 2; - - num_children = num_nodes; - num_parents = num_children / 2; - while ( num_parents > 0 ) + CIRC_NODE **inodes = NULL; + int num_children = num_nodes; + int inode_num = 0; + int num_parents = 0; + int j; + + while( num_children > 1 ) { - j = 0; - while ( j < num_parents ) + 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 ) { - /* - * Each new parent includes pointers to the children, so even though - * we are over-writing their place in the list, we still have references - * to them via the tree. - */ - CIRC_NODE **inodes = lwalloc(sizeof(CIRC_NODE*)*node_size); - inodes[0] = nodes[node_size*j]; - inodes[1] = nodes[node_size*j + 1]; - LWDEBUGF(3,"merging nodes %d and %d", node_size*j, node_size*j + 1); - nodes[j++] = circ_node_internal_new(inodes, node_size); + /* Promote solo nodes without merging */ + nodes[num_parents++] = inodes[0]; + lwfree(inodes); } - /* Odd number of children, just copy the last node up a level */ - if ( num_children % 2 ) + else if ( inode_num < CIRC_NODE_SIZE-1 ) { - nodes[j] = nodes[num_children - 1]; - num_parents++; + /* Merge spare nodes */ + nodes[num_parents++] = circ_node_internal_new(inodes, inode_num+1); } - num_children = num_parents; - num_parents = num_children / 2; + + num_children = num_parents; + num_parents = 0; } - + /* Return a reference to the head of the tree */ return nodes[0]; } + /** * Walk the tree and count intersections between the stab line and the edges. * odd => containment, even => no containment. diff --git a/liblwgeom/lwgeodetic_tree.h b/liblwgeom/lwgeodetic_tree.h index 9987a3531..60244ecf9 100644 --- a/liblwgeom/lwgeodetic_tree.h +++ b/liblwgeom/lwgeodetic_tree.h @@ -4,6 +4,8 @@ #include "lwgeodetic.h" +#define CIRC_NODE_SIZE 8 + /** * Note that p1 and p2 are pointers into an independent POINTARRAY, do not free them. */ -- 2.40.0