/*Reumann-Witkam algorithm * Returns number of points in the output line */ int reumann_witkam(struct line_pnts *Points, double thresh, int with_z) { int seg1, seg2; int i, count; POINT x0, x1, x2, sub, diff; double subd, diffd, sp, dist; int n; n = Points->n_points; if (n < 3) return n; thresh *= thresh; seg1 = 0; seg2 = 1; count = 1; point_assign(Points, 0, with_z, &x1); point_assign(Points, 1, with_z, &x2); point_subtract(x2, x1, &sub); subd = point_dist2(sub); for (i = 2; i < n; i++) { point_assign(Points, i, with_z, &x0); point_subtract(x1, x0, &diff); diffd = point_dist2(diff); sp = point_dot(diff, sub); dist = (diffd * subd - sp * sp) / subd; /* if the point is out of the threshlod-sausage, store it and calculate * all variables which do not change for each line-point calculation */ if (dist > thresh) { point_assign(Points, i - 1, with_z, &x1); point_assign(Points, i, with_z, &x2); point_subtract(x2, x1, &sub); subd = point_dist2(sub); Points->x[count] = x0.x; Points->y[count] = x0.y; Points->z[count] = x0.z; count++; } } Points->x[count] = Points->x[n - 1]; Points->y[count] = Points->y[n - 1]; Points->z[count] = Points->z[n - 1]; Points->n_points = count + 1; return Points->n_points; }
inline double point_angle_between(POINT a, POINT b, POINT c) { point_subtract(b, a, &a); point_subtract(c, b, &b); return acos(point_dot(a, b) / sqrt(point_dist2(a) * point_dist2(b))); }
real_t convexhull_compute_weak_diameter(line_t *diameter, polygon_t *chull) { real_t low, high, dist; real_t temp, temp1, temp2, temp3, temp4; dlink_t *ax, *bx, *ax_prev, *bx_next; point_t *a, *b, p, *u, *v, *r; assert(diameter); assert(chull); r = point_new(); u = NULL; v = NULL; dist = 0; for (ax = chull->tail->next; ax->next != chull->head; ax = ax->next) { if (ax->prev == chull->tail) ax_prev = chull->head->prev; else ax_prev = ax->prev; a = (point_t *)ax->object; for (bx = ax->next; bx != chull->head; bx = bx->next) { if (bx->next == chull->head) bx_next = chull->tail->next; else bx_next = bx->next; b = (point_t *)bx->object; point_subtract(&p, b, a); low = point_dotproduct(a, &p); high = point_dotproduct(b, &p); if (low > high) { temp = low; low = high; high = temp; } temp1 = point_dotproduct((point_t *)ax->next->object, &p); temp2 = point_dotproduct((point_t *)ax_prev->object, &p); temp3 = point_dotproduct((point_t *)bx_next->object, &p); temp4 = point_dotproduct((point_t *)bx->prev->object, &p); if ((low <= temp1 && temp1 <= high) && (low <= temp2 && temp2 <= high) && (low <= temp3 && temp3 <= high) && (low <= temp4 && temp4 <= high)) { temp = get_distance_of_p2p(a, b); /* circle(p->x, p->y, 10, 255, 0, 0); circle(q->x, q->y, 10, 255, 0, 0); keyhit(); circle(p->x, p->y, 10, 0, 0, 0); circle(q->x, q->y, 10, 0, 0, 0); */ if (temp > dist) { u = a; v = b; dist = temp; } } } } line_set_endpoints(diameter, u, v); return dist; }
static int quickhull(polygon_t *chull, point_list_t *points) { real_t ymin, ymax, yval; point_t *p, *v1, *v2, *v3; dlink_t *ax, *bx, *x, *y, *next; dlist_t *right_group, *left_group; assert(chull); assert(points); // Allocate the structure element of convex hull for (x = points->tail->next; x != points->head; x = x->next) { p = (point_t *)x->object; point_inc_ref(p); y = dlink_new(); y->object = (void *)p; dlist_insert(y, chull); } // find the extreme points along y-axis ax = NULL; bx = NULL; for (x = chull->tail->next; x != chull->head; x = x->next) { yval = point_get_y((point_t *)(x->object)); if (ax == NULL || yval < ymin) { ax = x; ymin = yval; } if (bx == NULL || yval > ymax) { bx = x; ymax = yval; } } dlink_cutoff(ax); dlist_dec_count(chull); dlink_cutoff(bx); dlist_dec_count(chull); //point_dump((point_t *)ax->object); //point_dump((point_t *)bx->object); v1 = point_new(); v2 = point_new(); v3 = point_new(); //printf("for right section\n"); right_group= dlist_new(); dlist_insert(ax, right_group); point_subtract(v2, (point_t *)bx->object, (point_t *)ax->object); for (x = chull->tail->next; x != chull->head;) { //point_dump((point_t *)x->object); point_subtract(v1, (point_t *)x->object, (point_t *)ax->object); point_xproduct(v3, v1, v2); if (point_get_z(v3) > 0) { next = x->next; dlink_cutoff(x); dlist_dec_count(chull); dlist_insert(x, right_group); //printf(" "); //point_dump((point_t *)x->object); x = next; } else x = x->next; } dlist_insert(bx, right_group); quickhull_grouping(right_group); //printf("for left section\n"); ax = dlist_pop(right_group); bx = dlist_extract(right_group); //point_dump((point_t *)ax->object); //point_dump((point_t *)bx->object); left_group = dlist_new(); dlist_insert(bx, left_group); point_subtract(v2, (point_t *)ax->object, (point_t *)bx->object); for (x = chull->tail->next; x != chull->head; ) { point_subtract(v1, (point_t *)x->object, (point_t *)bx->object); point_xproduct(v3, v1, v2); if (point_get_z(v3) > 0) { next = x->next; dlink_cutoff(x); dlist_dec_count(chull); dlist_insert(x, left_group); //point_dump((point_t *)x->object); x = next; } else { next = x->next; dlink_cutoff(x); dlist_dec_count(chull); point_destroy((point_t *)x->object); dlink_destroy(x); x = next; } } dlist_insert(ax, left_group); quickhull_grouping(left_group); ax = dlist_extract(left_group); bx = dlist_pop(left_group); dlist_insert(ax, chull); while (dlist_get_count(right_group) > 0) { x = dlist_pop(right_group); dlist_insert(x, chull); } dlist_insert(bx, chull); while (dlist_get_count(left_group) > 0) { x = dlist_pop(left_group); dlist_insert(x, chull); } dlist_destroy(left_group); dlist_destroy(right_group); point_destroy(v3); point_destroy(v2); point_destroy(v1); return dlist_get_count(chull); }
/* * Quick-Hull * Here's an algorithm that deserves its name. * It's a fast way to compute the convex hull of a set of points on the plane. * It shares a few similarities with its namesake, quick-sort: * - it is recursive. * - each recursive step partitions data into several groups. * * The partitioning step does all the work. The basic idea is as follows: * 1. We are given a some points, * and line segment AB which we know is a chord of the convex hull. * 2. Among the given points, find the one which is farthest from AB. * Let's call this point C. * 3. The points inside the triangle ABC cannot be on the hull. * Put them in set s0. * 4. Put the points which lie outside edge AC in set s1, * and points outside edge BC in set s2. * * Once the partitioning is done, we recursively invoke quick-hull on sets s1 and s2. * The algorithm works fast on random sets of points * because step 3 of the partition typically discards a large fraction of the points. */ static int quickhull_grouping(dlist_t *group) { dlink_t *x, *ax, *bx, *cx, *next; dlist_t *s1, *s2; point_t *v1, *v2, *v3; real_t area; assert(group); if (dlist_get_count(group) <= 2) return dlist_get_count(group); v1 = point_new(); v2 = point_new(); v3 = point_new(); // Find the point with maximum parallelogram's area ax = dlist_pop(group); bx = dlist_extract(group); cx = NULL; point_subtract(v2, (point_t *)(bx->object), (point_t *)(ax->object)); for (x = group->tail->next; x != group->head; x = x->next) { point_subtract(v1, (point_t *)(x->object), (point_t *)(ax->object)); point_xproduct(v3, v1, v2); if (cx == NULL || point_get_z(v3) > area) { cx = x; area = point_get_z(v3); } } dlink_cutoff(cx); dlist_dec_count(group); // S1 grouping s1 = dlist_new(); dlist_insert(ax, s1); point_subtract(v2, (point_t *)(cx->object), (point_t *)(ax->object)); for (x = group->tail->next; x != group->head; ) { point_subtract(v1, (point_t *)(x->object), (point_t *)(ax->object)); point_xproduct(v3, v1, v2); if (point_get_z(v3) > 0) { next = x->next; dlink_cutoff(x); dlist_dec_count(group); dlist_insert(x, s1); x = next; } else x = x->next; } dlist_insert(cx, s1); quickhull_grouping(s1); assert(cx == s1->head->prev); // S2 grouping and pop out others cx = dlist_extract(s1); s2 = dlist_new(); dlist_insert(cx, s2); point_subtract(v2, (point_t *)bx->object, (point_t *)cx->object); for (x = group->tail->next; x != group->head;) { point_subtract(v1, (point_t *)x->object, (point_t *)cx->object); point_xproduct(v3, v1, v2); if (point_get_z(v3) > 0) { next = x->next; dlink_cutoff(x); dlist_dec_count(group); dlist_insert(x, s2); x = next; } else { next = x->next; dlink_cutoff(x); dlist_dec_count(group); point_destroy((point_t *)x->object); dlink_destroy(x); x = next; } } dlist_insert(bx, s2); quickhull_grouping(s2); assert(bx == s2->head->prev); assert(dlist_get_count(group) == 0); //assert(group->count == 0); // Merge s1 and s2 into group while (dlist_get_count(s1) > 0) { x = dlist_pop(s1); dlist_insert(x, group); } while (dlist_get_count(s2) > 0) { x = dlist_pop(s2); dlist_insert(x, group); } dlist_destroy(s2); dlist_destroy(s1); point_destroy(v3); point_destroy(v2); point_destroy(v1); return dlist_get_count(group); }
/* Graham's Scan * Given a set of points on the plane, Graham's scan computes their convex hull. * The algorithm works in three phases: * 1. Find an extreme point. * This point will be the pivot, is guaranteed to be on the hull, * and is chosen to be the point with largest y coordinate. * 2. Sort the points in order of increasing angle about the pivot. * We end up with a star-shaped polygon (one in which one special point, * in this case the pivot, can "see" the whole polygon). * 3. Build the hull, by marching around the star-shaped poly, adding edges * when we make a left turn, and back-tracking when we make a right turn. */ static int grahamscan(polygon_t *chull, point_list_t *points) { int i; real_t dx, dy, ymin; real_t *ang; point_t *p, *q; point_t *v1, *v2, *v3; dlink_t *ax, *bx, *cx, *x, *y, *tmp; assert(chull); assert(points); assert(point_list_get_count(points) >= 3); ang = (real_t *)malloc(point_list_get_count(points) * sizeof(real_t)); assert(ang); // Find an extreme point // Preparing the polygon, and // Find the point with minimum value of y-coordinate for (ax = NULL, x = points->tail->next; x != points->head; x = x->next) { p = (point_t *)x->object; y = dlink_new(); point_inc_ref(p); y->object = (void *)p; dlist_insert(y, chull); if (ax == NULL || point_get_y(p) < ymin) { ax = y; ymin = point_get_y(p); } } dlink_cutoff(ax); dlist_dec_count(chull); dlist_push(ax, chull); // Sort the points in order of increasing angle about the pivot. p = (point_t *)ax->object; //point_dump(p); for (i = 0, x = ax->next; x != chull->head; x = x->next) { q = (point_t *)x->object; dx = point_get_x(q) - point_get_x(p); dy = point_get_y(q) - point_get_y(p); ang[i] = arctan2r(dy, dx); x->spare = (void *)&(ang[i]); i++; //point_dump(q); //printf("ang: %lf\n", ang[i-1]); } for (x = ax->next; x->next != chull->head; x = x->next) { for (y = x->next; y != chull->head; y = y->next) { if (*((real_t *)y->spare) < *((real_t *)x->spare)) { dlink_exchange(x, y); tmp = x, x = y, y = tmp; } } } //point_dump((point_t *)c->object); v1 = point_new(); v2 = point_new(); v3 = point_new(); cx = chull->tail->next->next->next; while (cx != chull->head) { bx = cx->prev; ax = bx->prev; point_subtract(v1, (point_t *)ax->object, (point_t *)bx->object); point_subtract(v2, (point_t *)cx->object, (point_t *)bx->object); point_xproduct(v3, v1, v2); // Convex ? if (point_get_z(v3) < 0) { cx = cx->next; } else { dlink_cutoff(bx); dlist_dec_count(chull); point_destroy((point_t *)bx->object); dlink_destroy(bx); } } for (x = chull->tail->next; x != chull->head; x = x->next) x->spare = NULL; point_destroy(v3); point_destroy(v2); point_destroy(v1); free(ang); return dlist_get_count(chull); }
/* Distance squared */ double point_distance2(CheapPointType pt1, CheapPointType pt2) { CheapPointType diff = point_subtract(pt1, pt2); return (double)diff.X * (double)diff.X + (double)diff.Y * (double)diff.Y; }