AprilTag中的g3d.h和g2d.c文件

g2d.h文件：

/* 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, thislist 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 documentationand/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.
*/#pragma once#ifdef __cplusplus
extern "C" {#endif#include "zarray.h"// This library tries to avoid needless proliferation of types.
//
// A point is a double[2]. (Note that when passing a double[2] as an
// argument, it is passed by pointer, not by value.)
//
// A polygon is a zarray_t of double[2]. (Note that in this case, the
// zarray contains the actual vertex data, and not merely a pointer to
// some other data. IMPORTANT: A polygon must be specified in CCW
// order.  It is implicitly closed (do not list the same point at the
// beginning at the end.
//
// Where sensible, it is assumed that objects should be allocated
// sparingly; consequently "init" style methods, rather than "create"
// methods are used.
// Linestypedef struct
{// Internal representation: a point that the line goes through (p) and// the direction of the line (u).double p[2];double u[2]; // always a unit vector
} g2d_line_t;// initialize a line object.
void g2d_line_init_from_points(g2d_line_t *line, const double p0[2], const double p1[2]);// The line defines a one-dimensional coordinate system whose origin
// is p. Where is q? (If q is not on the line, the point nearest q is
// returned.
double g2d_line_get_coordinate(const g2d_line_t *line, const double q[2]);// Intersect two lines. The intersection, if it exists, is written to
// p (if not NULL), and 1 is returned. Else, zero is returned.
int g2d_line_intersect_line(const g2d_line_t *linea, const g2d_line_t *lineb, double *p);
// Line Segments. line.p is always one endpoint; p1 is the other
// endpoint.
typedef struct
{g2d_line_t line;double p1[2];
} g2d_line_segment_t;void g2d_line_segment_init_from_points(g2d_line_segment_t *seg, const double p0[2], const double p1[2]);// Intersect two segments. The intersection, if it exists, is written
// to p (if not NULL), and 1 is returned. Else, zero is returned.
int g2d_line_segment_intersect_segment(const g2d_line_segment_t *sega, const g2d_line_segment_t *segb, double *p);void g2d_line_segment_closest_point(const g2d_line_segment_t *seg, const double *q, double *p);
double g2d_line_segment_closest_point_distance(const g2d_line_segment_t *seg, const double *q);
// Polygonszarray_t *g2d_polygon_create_data(double v[][2], int sz);zarray_t *g2d_polygon_create_zeros(int sz);zarray_t *g2d_polygon_create_empty();void g2d_polygon_add(zarray_t *poly, double v[2]);// Takes a polygon in either CW or CCW and modifies it (if necessary)
// to be CCW.
void g2d_polygon_make_ccw(zarray_t *poly);// Return 1 if point q lies within poly.
int g2d_polygon_contains_point(const zarray_t *poly, double q[2]);// Do the edges of the polygons cross? (Does not test for containment).
int g2d_polygon_intersects_polygon(const zarray_t *polya, const zarray_t *polyb);// Does polya completely contain polyb?
int g2d_polygon_contains_polygon(const zarray_t *polya, const zarray_t *polyb);// Is there some point which is in both polya and polyb?
int g2d_polygon_overlaps_polygon(const zarray_t *polya, const zarray_t *polyb);// returns the number of points written to x. see comments.
int g2d_polygon_rasterize(const zarray_t *poly, double y, double *x);#ifdef __cplusplus
}
#endif

g2d.c文件

/* 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, thislist 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 documentationand/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();
}
#endifdouble 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);elsezarray_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              2int quadrant;if (p[0] < q[0])quadrant = (p[1] < q[1]) ? 2 : 1;elsequadrant = (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;elsequad_acc += 2;break;}}}last_quadrant = quadrant;}int v = (quad_acc >= 2) || (quad_acc <= -2);if (0 && v != g2d_polygon_contains_point_ref(poly, q)) {printf("FAILURE %d %d\n", v, quad_acc);exit(-1);}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 mdouble det = m00*m11-m01*m10;// parallel lines?if (fabs(det) < 0.00000001)return 0;// inverse of mi00 = 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);elsec = 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 pointdouble 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");elseprintf(" ");}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---K4      |I--J3      |H-G2      |E-F1      |D--C0      A---B01234
*/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");elseprintf(" ");}printf("\n");for (double x = -3; x < 6; x += res) {double q[2] = {x, y};if (g2d_polygon_contains_point(poly, q))printf("X");elseprintf(" ");}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 hullif (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

AprilTag中的g3d.h和g2d.c文件相关推荐

AprilTag中TAG_16h5识别速率和容错率（VISP）
AprilTag识别速率和容错率(VISP) 今天测试识别AprilTag中tag16h5系列的码相机:1960X1200,200万像素,都在1.4M左右挂高:2400mm 码的大小:75X75m ...
关于VC中的stdafx.h
Windows和MFC的include文件都非常大,即使有一个快速的处理程序,编译程序也要花费相当长的时间来完成工作.由于每个.CPP文件都包含相同的include文件,为每个.CPP文件都重复处理这 ...
编译过程中，termcap.h 文件找不到路径 licli.a终于生成
编译过程中,termcap.h 文件找不到路径查看是linux 源码下找不到termcap.h文件安装了所有关于*cap*的源码包也不起作用今天终于解决了这个问题,搜termca ...
c语言中包含math.h的时用gcc编译要加-lm参数
c语言中包含math.h时,用gcc编译时要-lm参数: 如以下的程序sqrt.c,编译:gcc sqrt.c -o sqrt -lm /*sqrt.c*/ /*在0到十万里找出一个加上100且加上1 ...
C语言中 *.c和*.h文件的区别！
C语言中 *.c和*.h文件的区别! 这是HR面试我的一道题,没技术上含量,不过细想起来,还是C语言的最基本的知识!俗话说,目标决定动力,细节决定成败! C文件就是C语言系列的源文件,而H文 ...
Visual C++中 #include stdafx.h 头文件的用法
今天在做VC++实验时,总是出现莫名其妙的错误.比如说: unexpected end of file whilelooking for precompiled header directive 再比 ...
C51中的INTRINS.H：内部函数
C51中的INTRINS.H:内部函数 2007年05月14日星期一 17:02INTRINS.H:内部函数函数名: _crol_,_irol_,_lrol_ 原型: unsigned char ...
c51中的intrins.h库函数
c51中的intrins.h库函数 /*-------------------------------------------------------------------------- INTRI ...
c语言中的stdbool.h头文件,【C语言】中的stdbool.h头文件
C语言中的stdbool.h头文件一.相关基础知识二.具体内容 Win7下安装的VS2015中的stdbool.h的位置为: F:\Program Files (x86)\Microsoft Vi ...

AprilTag中的g3d.h和g2d.c文件

AprilTag中的g3d.h和g2d.c文件相关推荐

最新文章

热门文章