}
+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.
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};
{
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);
}
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.
*/
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);
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.
#include "lwgeodetic.h"
+#define CIRC_NODE_SIZE 8
+
/**
* Note that p1 and p2 are pointers into an independent POINTARRAY, do not free them.
*/
projects / postgis / commitdiff