Squashed 'third_party/apriltag/' content from commit 3e8e974d0
git-subtree-dir: third_party/apriltag
git-subtree-split: 3e8e974d0d8d6ab318abf56d87506d15d7f2cc35
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
Change-Id: I04ba3cb2106b6813a1013d57aa8074c26f856598
diff --git a/common/g2d.c b/common/g2d.c
new file mode 100644
index 0000000..4645f20
--- /dev/null
+++ b/common/g2d.c
@@ -0,0 +1,919 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson@umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <assert.h>
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "g2d.h"
+#include "common/math_util.h"
+
+#ifdef _WIN32
+static inline long int random(void)
+{
+ return rand();
+}
+#endif
+
+double g2d_distance(const double a[2], const double b[2])
+{
+ return sqrtf(sq(a[0]-b[0]) + sq(a[1]-b[1]));
+}
+
+zarray_t *g2d_polygon_create_empty()
+{
+ return zarray_create(sizeof(double[2]));
+}
+
+void g2d_polygon_add(zarray_t *poly, double v[2])
+{
+ zarray_add(poly, v);
+}
+
+zarray_t *g2d_polygon_create_data(double v[][2], int sz)
+{
+ zarray_t *points = g2d_polygon_create_empty();
+
+ for (int i = 0; i < sz; i++)
+ g2d_polygon_add(points, v[i]);
+
+ return points;
+}
+
+zarray_t *g2d_polygon_create_zeros(int sz)
+{
+ zarray_t *points = zarray_create(sizeof(double[2]));
+
+ double z[2] = { 0, 0 };
+
+ for (int i = 0; i < sz; i++)
+ zarray_add(points, z);
+
+ return points;
+}
+
+void g2d_polygon_make_ccw(zarray_t *poly)
+{
+ // Step one: we want the points in counter-clockwise order.
+ // If the points are in clockwise order, we'll reverse them.
+ double total_theta = 0;
+ double last_theta = 0;
+
+ // Count the angle accumulated going around the polygon. If
+ // the sum is +2pi, it's CCW. Otherwise, we'll get -2pi.
+ int sz = zarray_size(poly);
+
+ for (int i = 0; i <= sz; i++) {
+ double p0[2], p1[2];
+ zarray_get(poly, i % sz, &p0);
+ zarray_get(poly, (i+1) % sz, &p1);
+
+ double this_theta = atan2(p1[1]-p0[1], p1[0]-p0[0]);
+
+ if (i > 0) {
+ double dtheta = mod2pi(this_theta-last_theta);
+ total_theta += dtheta;
+ }
+
+ last_theta = this_theta;
+ }
+
+ int ccw = (total_theta > 0);
+
+ // reverse order if necessary.
+ if (!ccw) {
+ for (int i = 0; i < sz / 2; i++) {
+ double a[2], b[2];
+
+ zarray_get(poly, i, a);
+ zarray_get(poly, sz-1-i, b);
+ zarray_set(poly, i, b, NULL);
+ zarray_set(poly, sz-1-i, a, NULL);
+ }
+ }
+}
+
+int g2d_polygon_contains_point_ref(const zarray_t *poly, double q[2])
+{
+ // use winding. If the point is inside the polygon, we'll wrap
+ // around it (accumulating 6.28 radians). If we're outside the
+ // polygon, we'll accumulate zero.
+ int psz = zarray_size(poly);
+
+ double acc_theta = 0;
+
+ double last_theta;
+
+ for (int i = 0; i <= psz; i++) {
+ double p[2];
+
+ zarray_get(poly, i % psz, &p);
+
+ double this_theta = atan2(q[1]-p[1], q[0]-p[0]);
+
+ if (i != 0)
+ acc_theta += mod2pi(this_theta - last_theta);
+
+ last_theta = this_theta;
+ }
+
+ return acc_theta > M_PI;
+}
+
+/*
+// sort by x coordinate, ascending
+static int g2d_convex_hull_sort(const void *_a, const void *_b)
+{
+ double *a = (double*) _a;
+ double *b = (double*) _b;
+
+ if (a[0] < b[0])
+ return -1;
+ if (a[0] == b[0])
+ return 0;
+ return 1;
+}
+*/
+
+/*
+zarray_t *g2d_convex_hull2(const zarray_t *points)
+{
+ zarray_t *hull = zarray_copy(points);
+
+ zarray_sort(hull, g2d_convex_hull_sort);
+
+ int hsz = zarray_size(hull);
+ int hout = 0;
+
+ for (int hin = 1; hin < hsz; hin++) {
+ double *p;
+ zarray_get_volatile(hull, i, &p);
+
+ // Everything to the right of hin is already convex. We now
+ // add one point, p, which begins "connected" by two
+ // (coincident) edges from the last right-most point to p.
+ double *last;
+ zarray_get_volatile(hull, hout, &last);
+
+ // We now remove points from the convex hull by moving
+ }
+
+ return hull;
+}
+*/
+
+// creates and returns a zarray(double[2]). The resulting polygon is
+// CCW and implicitly closed. Unnecessary colinear points are omitted.
+zarray_t *g2d_convex_hull(const zarray_t *points)
+{
+ zarray_t *hull = zarray_create(sizeof(double[2]));
+
+ // gift-wrap algorithm.
+
+ // step 1: find left most point.
+ int insz = zarray_size(points);
+
+ // must have at least 2 points. (XXX need 3?)
+ assert(insz >= 2);
+
+ double *pleft = NULL;
+ for (int i = 0; i < insz; i++) {
+ double *p;
+ zarray_get_volatile(points, i, &p);
+
+ if (pleft == NULL || p[0] < pleft[0])
+ pleft = p;
+ }
+
+ // cannot be NULL since there must be at least one point.
+ assert(pleft != NULL);
+
+ zarray_add(hull, pleft);
+
+ // step 2. gift wrap. Keep searching for points that make the
+ // smallest-angle left-hand turn. This implementation is carefully
+ // written to use only addition/subtraction/multiply. No division
+ // or sqrts. This guarantees exact results for integer-coordinate
+ // polygons (no rounding/precision problems).
+ double *p = pleft;
+
+ while (1) {
+ assert(p != NULL);
+
+ double *q = NULL;
+ double n0 = 0, n1 = 0; // the normal to the line (p, q) (not
+ // necessarily unit length).
+
+ // Search for the point q for which the line (p,q) is most "to
+ // the right of" the other points. (i.e., every time we find a
+ // point that is to the right of our current line, we change
+ // lines.)
+ for (int i = 0; i < insz; i++) {
+ double *thisq;
+ zarray_get_volatile(points, i, &thisq);
+
+ if (thisq == p)
+ continue;
+
+ // the first time we find another point, we initialize our
+ // value of q, forming the line (p,q)
+ if (q == NULL) {
+ q = thisq;
+ n0 = q[1] - p[1];
+ n1 = -q[0] + p[0];
+ } else {
+ // we already have a line (p,q). is point thisq RIGHT OF line (p, q)?
+ double e0 = thisq[0] - p[0], e1 = thisq[1] - p[1];
+ double dot = e0*n0 + e1*n1;
+
+ if (dot > 0) {
+ // it is. change our line.
+ q = thisq;
+ n0 = q[1] - p[1];
+ n1 = -q[0] + p[0];
+ }
+ }
+ }
+
+ // we must have elected *some* line, so long as there are at
+ // least 2 points in the polygon.
+ assert(q != NULL);
+
+ // loop completed?
+ if (q == pleft)
+ break;
+
+ int colinear = 0;
+
+ // is this new point colinear with the last two?
+ if (zarray_size(hull) > 1) {
+ double *o;
+ zarray_get_volatile(hull, zarray_size(hull) - 2, &o);
+
+ double e0 = o[0] - p[0];
+ double e1 = o[1] - p[1];
+
+ if (n0*e0 + n1*e1 == 0)
+ colinear = 1;
+ }
+
+ // if it is colinear, overwrite the last one.
+ if (colinear)
+ zarray_set(hull, zarray_size(hull)-1, q, NULL);
+ else
+ zarray_add(hull, q);
+
+ p = q;
+ }
+
+ return hull;
+}
+
+// Find point p on the boundary of poly that is closest to q.
+void g2d_polygon_closest_boundary_point(const zarray_t *poly, const double q[2], double *p)
+{
+ int psz = zarray_size(poly);
+ double min_dist = HUGE_VALF;
+
+ for (int i = 0; i < psz; i++) {
+ double *p0, *p1;
+
+ zarray_get_volatile(poly, i, &p0);
+ zarray_get_volatile(poly, (i+1) % psz, &p1);
+
+ g2d_line_segment_t seg;
+ g2d_line_segment_init_from_points(&seg, p0, p1);
+
+ double thisp[2];
+ g2d_line_segment_closest_point(&seg, q, thisp);
+
+ double dist = g2d_distance(q, thisp);
+ if (dist < min_dist) {
+ memcpy(p, thisp, sizeof(double[2]));
+ min_dist = dist;
+ }
+ }
+}
+
+int g2d_polygon_contains_point(const zarray_t *poly, double q[2])
+{
+ // use winding. If the point is inside the polygon, we'll wrap
+ // around it (accumulating 6.28 radians). If we're outside the
+ // polygon, we'll accumulate zero.
+ int psz = zarray_size(poly);
+ assert(psz > 0);
+
+ int last_quadrant;
+ int quad_acc = 0;
+
+ for (int i = 0; i <= psz; i++) {
+ double *p;
+
+ zarray_get_volatile(poly, i % psz, &p);
+
+ // p[0] < q[0] p[1] < q[1] quadrant
+ // 0 0 0
+ // 0 1 3
+ // 1 0 1
+ // 1 1 2
+
+ // p[1] < q[1] p[0] < q[0] quadrant
+ // 0 0 0
+ // 0 1 1
+ // 1 0 3
+ // 1 1 2
+
+ int quadrant;
+ if (p[0] < q[0])
+ quadrant = (p[1] < q[1]) ? 2 : 1;
+ else
+ quadrant = (p[1] < q[1]) ? 3 : 0;
+
+ if (i > 0) {
+ int dquadrant = quadrant - last_quadrant;
+
+ // encourage a jump table by mapping to small positive integers.
+ switch (dquadrant) {
+ case -3:
+ case 1:
+ quad_acc ++;
+ break;
+ case -1:
+ case 3:
+ quad_acc --;
+ break;
+ case 0:
+ break;
+ case -2:
+ case 2:
+ {
+ // get the previous point.
+ double *p0;
+ zarray_get_volatile(poly, i-1, &p0);
+
+ // Consider the points p0 and p (the points around the
+ //polygon that we are tracing) and the query point q.
+ //
+ // If we've moved diagonally across quadrants, we want
+ // to measure whether we have rotated +PI radians or
+ // -PI radians. We can test this by computing the dot
+ // product of vector (p0-q) with the vector
+ // perpendicular to vector (p-q)
+ double nx = p[1] - q[1];
+ double ny = -p[0] + q[0];
+
+ double dot = nx*(p0[0]-q[0]) + ny*(p0[1]-q[1]);
+ if (dot < 0)
+ quad_acc -= 2;
+ else
+ quad_acc += 2;
+
+ break;
+ }
+ }
+ }
+
+ last_quadrant = quadrant;
+ }
+
+ int v = (quad_acc >= 2) || (quad_acc <= -2);
+
+ #if 0
+ if (v != g2d_polygon_contains_point_ref(poly, q)) {
+ printf("FAILURE %d %d\n", v, quad_acc);
+ exit(-1);
+ }
+ #endif
+
+ return v;
+}
+
+void g2d_line_init_from_points(g2d_line_t *line, const double p0[2], const double p1[2])
+{
+ line->p[0] = p0[0];
+ line->p[1] = p0[1];
+ line->u[0] = p1[0]-p0[0];
+ line->u[1] = p1[1]-p0[1];
+ double mag = sqrtf(sq(line->u[0]) + sq(line->u[1]));
+
+ line->u[0] /= mag;
+ line->u[1] /= mag;
+}
+
+double g2d_line_get_coordinate(const g2d_line_t *line, const double q[2])
+{
+ return (q[0]-line->p[0])*line->u[0] + (q[1]-line->p[1])*line->u[1];
+}
+
+// Compute intersection of two line segments. If they intersect,
+// result is stored in p and 1 is returned. Otherwise, zero is
+// returned. p may be NULL.
+int g2d_line_intersect_line(const g2d_line_t *linea, const g2d_line_t *lineb, double *p)
+{
+ // this implementation is many times faster than the original,
+ // mostly due to avoiding a general-purpose LU decomposition in
+ // Matrix.inverse().
+ double m00, m01, m10, m11;
+ double i00, i01;
+ double b00, b10;
+
+ m00 = linea->u[0];
+ m01= -lineb->u[0];
+ m10 = linea->u[1];
+ m11= -lineb->u[1];
+
+ // determinant of m
+ double det = m00*m11-m01*m10;
+
+ // parallel lines?
+ if (fabs(det) < 0.00000001)
+ return 0;
+
+ // inverse of m
+ i00 = m11/det;
+ i01 = -m01/det;
+
+ b00 = lineb->p[0] - linea->p[0];
+ b10 = lineb->p[1] - linea->p[1];
+
+ double x00; //, x10;
+ x00 = i00*b00+i01*b10;
+
+ if (p != NULL) {
+ p[0] = linea->u[0]*x00 + linea->p[0];
+ p[1] = linea->u[1]*x00 + linea->p[1];
+ }
+
+ return 1;
+}
+
+
+void g2d_line_segment_init_from_points(g2d_line_segment_t *seg, const double p0[2], const double p1[2])
+{
+ g2d_line_init_from_points(&seg->line, p0, p1);
+ seg->p1[0] = p1[0];
+ seg->p1[1] = p1[1];
+}
+
+// Find the point p on segment seg that is closest to point q.
+void g2d_line_segment_closest_point(const g2d_line_segment_t *seg, const double *q, double *p)
+{
+ double a = g2d_line_get_coordinate(&seg->line, seg->line.p);
+ double b = g2d_line_get_coordinate(&seg->line, seg->p1);
+ double c = g2d_line_get_coordinate(&seg->line, q);
+
+ if (a < b)
+ c = dclamp(c, a, b);
+ else
+ c = dclamp(c, b, a);
+
+ p[0] = seg->line.p[0] + c * seg->line.u[0];
+ p[1] = seg->line.p[1] + c * seg->line.u[1];
+}
+
+// Compute intersection of two line segments. If they intersect,
+// result is stored in p and 1 is returned. Otherwise, zero is
+// returned. p may be NULL.
+int g2d_line_segment_intersect_segment(const g2d_line_segment_t *sega, const g2d_line_segment_t *segb, double *p)
+{
+ double tmp[2];
+
+ if (!g2d_line_intersect_line(&sega->line, &segb->line, tmp))
+ return 0;
+
+ double a = g2d_line_get_coordinate(&sega->line, sega->line.p);
+ double b = g2d_line_get_coordinate(&sega->line, sega->p1);
+ double c = g2d_line_get_coordinate(&sega->line, tmp);
+
+ // does intersection lie on the first line?
+ if ((c<a && c<b) || (c>a && c>b))
+ return 0;
+
+ a = g2d_line_get_coordinate(&segb->line, segb->line.p);
+ b = g2d_line_get_coordinate(&segb->line, segb->p1);
+ c = g2d_line_get_coordinate(&segb->line, tmp);
+
+ // does intersection lie on second line?
+ if ((c<a && c<b) || (c>a && c>b))
+ return 0;
+
+ if (p != NULL) {
+ p[0] = tmp[0];
+ p[1] = tmp[1];
+ }
+
+ return 1;
+}
+
+// Compute intersection of a line segment and a line. If they
+// intersect, result is stored in p and 1 is returned. Otherwise, zero
+// is returned. p may be NULL.
+int g2d_line_segment_intersect_line(const g2d_line_segment_t *seg, const g2d_line_t *line, double *p)
+{
+ double tmp[2];
+
+ if (!g2d_line_intersect_line(&seg->line, line, tmp))
+ return 0;
+
+ double a = g2d_line_get_coordinate(&seg->line, seg->line.p);
+ double b = g2d_line_get_coordinate(&seg->line, seg->p1);
+ double c = g2d_line_get_coordinate(&seg->line, tmp);
+
+ // does intersection lie on the first line?
+ if ((c<a && c<b) || (c>a && c>b))
+ return 0;
+
+ if (p != NULL) {
+ p[0] = tmp[0];
+ p[1] = tmp[1];
+ }
+
+ return 1;
+}
+
+// do the edges of polya and polyb collide? (Does NOT test for containment).
+int g2d_polygon_intersects_polygon(const zarray_t *polya, const zarray_t *polyb)
+{
+ // do any of the line segments collide? If so, the answer is no.
+
+ // dumb N^2 method.
+ for (int ia = 0; ia < zarray_size(polya); ia++) {
+ double pa0[2], pa1[2];
+ zarray_get(polya, ia, pa0);
+ zarray_get(polya, (ia+1)%zarray_size(polya), pa1);
+
+ g2d_line_segment_t sega;
+ g2d_line_segment_init_from_points(&sega, pa0, pa1);
+
+ for (int ib = 0; ib < zarray_size(polyb); ib++) {
+ double pb0[2], pb1[2];
+ zarray_get(polyb, ib, pb0);
+ zarray_get(polyb, (ib+1)%zarray_size(polyb), pb1);
+
+ g2d_line_segment_t segb;
+ g2d_line_segment_init_from_points(&segb, pb0, pb1);
+
+ if (g2d_line_segment_intersect_segment(&sega, &segb, NULL))
+ return 1;
+ }
+ }
+
+ return 0;
+}
+
+// does polya completely contain polyb?
+int g2d_polygon_contains_polygon(const zarray_t *polya, const zarray_t *polyb)
+{
+ // do any of the line segments collide? If so, the answer is no.
+ if (g2d_polygon_intersects_polygon(polya, polyb))
+ return 0;
+
+ // if none of the edges cross, then the polygon is either fully
+ // contained or fully outside.
+ double p[2];
+ zarray_get(polyb, 0, p);
+
+ return g2d_polygon_contains_point(polya, p);
+}
+
+// compute a point that is inside the polygon. (It may not be *far* inside though)
+void g2d_polygon_get_interior_point(const zarray_t *poly, double *p)
+{
+ // take the first three points, which form a triangle. Find the middle point
+ double a[2], b[2], c[2];
+
+ zarray_get(poly, 0, a);
+ zarray_get(poly, 1, b);
+ zarray_get(poly, 2, c);
+
+ p[0] = (a[0]+b[0]+c[0])/3;
+ p[1] = (a[1]+b[1]+c[1])/3;
+}
+
+int g2d_polygon_overlaps_polygon(const zarray_t *polya, const zarray_t *polyb)
+{
+ // do any of the line segments collide? If so, the answer is yes.
+ if (g2d_polygon_intersects_polygon(polya, polyb))
+ return 1;
+
+ // if none of the edges cross, then the polygon is either fully
+ // contained or fully outside.
+ double p[2];
+ g2d_polygon_get_interior_point(polyb, p);
+
+ if (g2d_polygon_contains_point(polya, p))
+ return 1;
+
+ g2d_polygon_get_interior_point(polya, p);
+
+ if (g2d_polygon_contains_point(polyb, p))
+ return 1;
+
+ return 0;
+}
+
+static int double_sort_up(const void *_a, const void *_b)
+{
+ double a = *((double*) _a);
+ double b = *((double*) _b);
+
+ if (a < b)
+ return -1;
+
+ if (a == b)
+ return 0;
+
+ return 1;
+}
+
+// Compute the crossings of the polygon along line y, storing them in
+// the array x. X must be allocated to be at least as long as
+// zarray_size(poly). X will be sorted, ready for
+// rasterization. Returns the number of intersections (and elements
+// written to x).
+/*
+ To rasterize, do something like this:
+
+ double res = 0.099;
+ for (double y = y0; y < y1; y += res) {
+ double xs[zarray_size(poly)];
+
+ int xsz = g2d_polygon_rasterize(poly, y, xs);
+ int xpos = 0;
+ int inout = 0; // start off "out"
+
+ for (double x = x0; x < x1; x += res) {
+ while (x > xs[xpos] && xpos < xsz) {
+ xpos++;
+ inout ^= 1;
+ }
+
+ if (inout)
+ printf("y");
+ else
+ printf(" ");
+ }
+ printf("\n");
+*/
+
+// returns the number of x intercepts
+int g2d_polygon_rasterize(const zarray_t *poly, double y, double *x)
+{
+ int sz = zarray_size(poly);
+
+ g2d_line_t line;
+ if (1) {
+ double p0[2] = { 0, y };
+ double p1[2] = { 1, y };
+
+ g2d_line_init_from_points(&line, p0, p1);
+ }
+
+ int xpos = 0;
+
+ for (int i = 0; i < sz; i++) {
+ g2d_line_segment_t seg;
+ double *p0, *p1;
+ zarray_get_volatile(poly, i, &p0);
+ zarray_get_volatile(poly, (i+1)%sz, &p1);
+
+ g2d_line_segment_init_from_points(&seg, p0, p1);
+
+ double q[2];
+ if (g2d_line_segment_intersect_line(&seg, &line, q))
+ x[xpos++] = q[0];
+ }
+
+ qsort(x, xpos, sizeof(double), double_sort_up);
+
+ return xpos;
+}
+
+/*
+ /---(1,5)
+ (-2,4)-/ |
+ \ |
+ \ (1,2)--(2,2)\
+ \ \
+ \ \
+ (0,0)------------------(4,0)
+*/
+#if 0
+
+#include "timeprofile.h"
+
+int main(int argc, char *argv[])
+{
+ timeprofile_t *tp = timeprofile_create();
+
+ zarray_t *polya = g2d_polygon_create_data((double[][2]) {
+ { 0, 0},
+ { 4, 0},
+ { 2, 2},
+ { 1, 2},
+ { 1, 5},
+ { -2,4} }, 6);
+
+ zarray_t *polyb = g2d_polygon_create_data((double[][2]) {
+ { .1, .1},
+ { .5, .1},
+ { .1, .5 } }, 3);
+
+ zarray_t *polyc = g2d_polygon_create_data((double[][2]) {
+ { 3, 0},
+ { 5, 0},
+ { 5, 1} }, 3);
+
+ zarray_t *polyd = g2d_polygon_create_data((double[][2]) {
+ { 5, 5},
+ { 6, 6},
+ { 5, 6} }, 3);
+
+/*
+ 5 L---K
+ 4 |I--J
+ 3 |H-G
+ 2 |E-F
+ 1 |D--C
+ 0 A---B
+ 01234
+*/
+ zarray_t *polyE = g2d_polygon_create_data((double[][2]) {
+ {0,0}, {4,0}, {4, 1}, {1,1},
+ {1,2}, {3,2}, {3,3}, {1,3},
+ {1,4}, {4,4}, {4,5}, {0,5}}, 12);
+
+ srand(0);
+
+ timeprofile_stamp(tp, "begin");
+
+ if (1) {
+ int niters = 100000;
+
+ for (int i = 0; i < niters; i++) {
+ double q[2];
+ q[0] = 10.0f * random() / RAND_MAX - 2;
+ q[1] = 10.0f * random() / RAND_MAX - 2;
+
+ g2d_polygon_contains_point(polyE, q);
+ }
+
+ timeprofile_stamp(tp, "fast");
+
+ for (int i = 0; i < niters; i++) {
+ double q[2];
+ q[0] = 10.0f * random() / RAND_MAX - 2;
+ q[1] = 10.0f * random() / RAND_MAX - 2;
+
+ g2d_polygon_contains_point_ref(polyE, q);
+ }
+
+ timeprofile_stamp(tp, "slow");
+
+ for (int i = 0; i < niters; i++) {
+ double q[2];
+ q[0] = 10.0f * random() / RAND_MAX - 2;
+ q[1] = 10.0f * random() / RAND_MAX - 2;
+
+ int v0 = g2d_polygon_contains_point(polyE, q);
+ int v1 = g2d_polygon_contains_point_ref(polyE, q);
+ assert(v0 == v1);
+ }
+
+ timeprofile_stamp(tp, "both");
+ timeprofile_display(tp);
+ }
+
+ if (1) {
+ zarray_t *poly = polyE;
+
+ double res = 0.399;
+ for (double y = 5.2; y >= -.5; y -= res) {
+ double xs[zarray_size(poly)];
+
+ int xsz = g2d_polygon_rasterize(poly, y, xs);
+ int xpos = 0;
+ int inout = 0; // start off "out"
+ for (double x = -3; x < 6; x += res) {
+ while (x > xs[xpos] && xpos < xsz) {
+ xpos++;
+ inout ^= 1;
+ }
+
+ if (inout)
+ printf("y");
+ else
+ printf(" ");
+ }
+ printf("\n");
+
+ for (double x = -3; x < 6; x += res) {
+ double q[2] = {x, y};
+ if (g2d_polygon_contains_point(poly, q))
+ printf("X");
+ else
+ printf(" ");
+ }
+ printf("\n");
+ }
+ }
+
+
+
+/*
+// CW order
+double p[][2] = { { 0, 0},
+{ -2, 4},
+{1, 5},
+{1, 2},
+{2, 2},
+{4, 0} };
+*/
+
+ double q[2] = { 10, 10 };
+ printf("0==%d\n", g2d_polygon_contains_point(polya, q));
+
+ q[0] = 1; q[1] = 1;
+ printf("1==%d\n", g2d_polygon_contains_point(polya, q));
+
+ q[0] = 3; q[1] = .5;
+ printf("1==%d\n", g2d_polygon_contains_point(polya, q));
+
+ q[0] = 1.2; q[1] = 2.1;
+ printf("0==%d\n", g2d_polygon_contains_point(polya, q));
+
+ printf("0==%d\n", g2d_polygon_contains_polygon(polya, polyb));
+
+ printf("0==%d\n", g2d_polygon_contains_polygon(polya, polyc));
+
+ printf("0==%d\n", g2d_polygon_contains_polygon(polya, polyd));
+
+ ////////////////////////////////////////////////////////
+ // Test convex hull
+ if (1) {
+ zarray_t *hull = g2d_convex_hull(polyE);
+
+ for (int k = 0; k < zarray_size(hull); k++) {
+ double *h;
+ zarray_get_volatile(hull, k, &h);
+
+ printf("%15f, %15f\n", h[0], h[1]);
+ }
+ }
+
+ for (int i = 0; i < 100000; i++) {
+ zarray_t *points = zarray_create(sizeof(double[2]));
+
+ for (int j = 0; j < 100; j++) {
+ double q[2];
+ q[0] = 10.0f * random() / RAND_MAX - 2;
+ q[1] = 10.0f * random() / RAND_MAX - 2;
+
+ zarray_add(points, q);
+ }
+
+ zarray_t *hull = g2d_convex_hull(points);
+ for (int j = 0; j < zarray_size(points); j++) {
+ double *q;
+ zarray_get_volatile(points, j, &q);
+
+ int on_edge;
+
+ double p[2];
+ g2d_polygon_closest_boundary_point(hull, q, p);
+ if (g2d_distance(q, p) < .00001)
+ on_edge = 1;
+
+ assert(on_edge || g2d_polygon_contains_point(hull, q));
+ }
+
+ zarray_destroy(hull);
+ zarray_destroy(points);
+ }
+}
+#endif