/*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;
}
