| 1 |
|
|---|
| 2 | #include <stdlib.h>
|
|---|
| 3 | #include <math.h>
|
|---|
| 4 |
|
|---|
| 5 | #include "cr_mem.h"
|
|---|
| 6 | #include "cr_hull.h"
|
|---|
| 7 | #include "cr_error.h"
|
|---|
| 8 |
|
|---|
| 9 | #ifdef WINDOWS
|
|---|
| 10 | /* I know while(1) is constant */
|
|---|
| 11 | #pragma warning( disable: 4127 )
|
|---|
| 12 | #endif
|
|---|
| 13 |
|
|---|
| 14 | /*==================================================
|
|---|
| 15 | *
|
|---|
| 16 | * Do S1 & S2 intersect? if so, return t for s1
|
|---|
| 17 | */
|
|---|
| 18 | static double
|
|---|
| 19 | _segment_segment_intersection(const double *s1a, const double *s1b,
|
|---|
| 20 | const double *s2a, const double *s2b)
|
|---|
| 21 | {
|
|---|
| 22 | double r1[2], r2[2];
|
|---|
| 23 | double A, B, s, t;
|
|---|
| 24 | double epsilon = .0000000001;
|
|---|
| 25 |
|
|---|
| 26 | r1[0] = s1b[0] - s1a[0];
|
|---|
| 27 | r1[1] = s1b[1] - s1a[1];
|
|---|
| 28 | r2[0] = s2b[0] - s2a[0];
|
|---|
| 29 | r2[1] = s2b[1] - s2a[1];
|
|---|
| 30 |
|
|---|
| 31 | if (r1[0] == 0)
|
|---|
| 32 | return -1;
|
|---|
| 33 |
|
|---|
| 34 | A = s1a[1] - s2a[1] + (r1[1] / r1[0]) * (s2a[0] - s1a[0]);
|
|---|
| 35 | B = r2[1] - (r1[1] / r1[0]) * r2[0];
|
|---|
| 36 |
|
|---|
| 37 | if (B == 0)
|
|---|
| 38 | return -1;
|
|---|
| 39 | s = A / B;
|
|---|
| 40 |
|
|---|
| 41 | if ((s <= epsilon) || (s > 1. + epsilon))
|
|---|
| 42 | return -1;
|
|---|
| 43 |
|
|---|
| 44 | t = (s2a[0] - s1a[0] + s * r2[0]) / r1[0];
|
|---|
| 45 | if ((t < epsilon) || (t > 1. + epsilon))
|
|---|
| 46 | return -1;
|
|---|
| 47 |
|
|---|
| 48 | return t;
|
|---|
| 49 | }
|
|---|
| 50 |
|
|---|
| 51 | static int
|
|---|
| 52 | _segment_hull_intersect(double *sa, double *sb, const double *pnts,
|
|---|
| 53 | int *hull, int hull_len, double *hits)
|
|---|
| 54 | {
|
|---|
| 55 | int a, hitnum;
|
|---|
| 56 | double result;
|
|---|
| 57 | double d[2];
|
|---|
| 58 |
|
|---|
| 59 | hitnum = 0;
|
|---|
| 60 | for (a = 0; a < hull_len - 1; a++)
|
|---|
| 61 | {
|
|---|
| 62 | result = _segment_segment_intersection(sa, sb, pnts + 2 * hull[a],
|
|---|
| 63 | pnts + 2 * hull[a + 1]);
|
|---|
| 64 |
|
|---|
| 65 | if (result >= 0)
|
|---|
| 66 | {
|
|---|
| 67 | d[0] = sb[0] - sa[0];
|
|---|
| 68 | d[1] = sb[1] - sa[1];
|
|---|
| 69 | hits[2 * hitnum] = sa[0] + result * d[0];
|
|---|
| 70 | hits[2 * hitnum + 1] = sa[1] + result * d[1];
|
|---|
| 71 | hitnum++;
|
|---|
| 72 | }
|
|---|
| 73 | }
|
|---|
| 74 |
|
|---|
| 75 | return hitnum;
|
|---|
| 76 | }
|
|---|
| 77 |
|
|---|
| 78 | /*
|
|---|
| 79 | * given 4 points, find a rectangle that lays w/i the quad
|
|---|
| 80 | */
|
|---|
| 81 | static void
|
|---|
| 82 | _four_point_box(double *points, double *min, double *max)
|
|---|
| 83 | {
|
|---|
| 84 | int a, b, tmp, sort[4];
|
|---|
| 85 | double npnts[8], pnt[2], retval, d[2];
|
|---|
| 86 |
|
|---|
| 87 | for (a = 0; a < 4; a++)
|
|---|
| 88 | sort[a] = a;
|
|---|
| 89 |
|
|---|
| 90 | for (a = 0; a < 4; a++)
|
|---|
| 91 | for (b = a + 1; b < 4; b++)
|
|---|
| 92 | {
|
|---|
| 93 | if (points[2 * sort[a] + 1] > points[2 * sort[b] + 1])
|
|---|
| 94 | {
|
|---|
| 95 | tmp = sort[a];
|
|---|
| 96 | sort[a] = sort[b];
|
|---|
| 97 | sort[b] = tmp;
|
|---|
| 98 | }
|
|---|
| 99 | }
|
|---|
| 100 |
|
|---|
| 101 | npnts[0] = points[2 * sort[1]];
|
|---|
| 102 | npnts[1] = points[2 * sort[1] + 1];
|
|---|
| 103 | npnts[2] = points[2 * sort[2]];
|
|---|
| 104 | npnts[3] = points[2 * sort[2] + 1];
|
|---|
| 105 |
|
|---|
| 106 | /* now, intersect a horizontal ray from sort[1] with the quad */
|
|---|
| 107 | for (b = 0; b < 2; b++)
|
|---|
| 108 | {
|
|---|
| 109 | for (a = 0; a < 4; a++)
|
|---|
| 110 | {
|
|---|
| 111 | pnt[0] = points[2 * sort[b + 1]] + 10;
|
|---|
| 112 | pnt[1] = points[2 * sort[b + 1] + 1];
|
|---|
| 113 |
|
|---|
| 114 | retval = _segment_segment_intersection(points + 2 * sort[b + 1], pnt,
|
|---|
| 115 | points + 2 * a,
|
|---|
| 116 | points + 2 * ((a + 1) % 4));
|
|---|
| 117 | if (retval <= 0.001)
|
|---|
| 118 | {
|
|---|
| 119 | pnt[0] = points[2 * sort[b + 1]] - 10;
|
|---|
| 120 | retval = _segment_segment_intersection(points + 2 * sort[b + 1], pnt,
|
|---|
| 121 | points + 2 * a,
|
|---|
| 122 | points + 2 * ((a + 1) % 4));
|
|---|
| 123 | }
|
|---|
| 124 |
|
|---|
| 125 | if (retval > 0.001)
|
|---|
| 126 | {
|
|---|
| 127 | d[0] = pnt[0] - points[2 * sort[b + 1]];
|
|---|
| 128 | d[1] = pnt[1] - points[2 * sort[b + 1] + 1];
|
|---|
| 129 | npnts[2 * b + 4] = points[2 * sort[b + 1]] + retval * d[0];
|
|---|
| 130 | npnts[2 * b + 5] = points[2 * sort[b + 1] + 1] + retval * d[1];
|
|---|
| 131 | }
|
|---|
| 132 | }
|
|---|
| 133 | }
|
|---|
| 134 |
|
|---|
| 135 | min[1] = points[2 * sort[1] + 1];
|
|---|
| 136 | max[1] = points[2 * sort[2] + 1];
|
|---|
| 137 |
|
|---|
| 138 | /* finally, sort npnts by x */
|
|---|
| 139 | for (a = 0; a < 4; a++)
|
|---|
| 140 | sort[a] = a;
|
|---|
| 141 |
|
|---|
| 142 | for (a = 0; a < 4; a++)
|
|---|
| 143 | for (b = a + 1; b < 4; b++)
|
|---|
| 144 | {
|
|---|
| 145 | if (npnts[2 * sort[a]] > npnts[2 * sort[b]])
|
|---|
| 146 | {
|
|---|
| 147 | tmp = sort[a];
|
|---|
| 148 | sort[a] = sort[b];
|
|---|
| 149 | sort[b] = tmp;
|
|---|
| 150 | }
|
|---|
| 151 | }
|
|---|
| 152 |
|
|---|
| 153 | /* and grab the x coord of the box */
|
|---|
| 154 | min[0] = npnts[2 * sort[1]];
|
|---|
| 155 | max[0] = npnts[2 * sort[2]];
|
|---|
| 156 |
|
|---|
| 157 | }
|
|---|
| 158 |
|
|---|
| 159 | /*
|
|---|
| 160 | * Given the convex hull, find a rectangle w/i it.
|
|---|
| 161 | *
|
|---|
| 162 | * Finding the rect from 4 pnts isn't too bad. Find the bbox
|
|---|
| 163 | * of the CH, then intersect the diagonals of the bbox with
|
|---|
| 164 | * the CH. Use those 4 points to compute the box
|
|---|
| 165 | */
|
|---|
| 166 | static void
|
|---|
| 167 | _bound(const double *pnts, int npnts, int *hull, int hull_len, double *bbox)
|
|---|
| 168 | {
|
|---|
| 169 | double max[2], min[2], cent[2], dir[2], pnt[2];
|
|---|
| 170 | double x, y, intr_pnts[8];
|
|---|
| 171 | int a, retval;
|
|---|
| 172 |
|
|---|
| 173 | (void) npnts;
|
|---|
| 174 |
|
|---|
| 175 | max[0] = max[1] = -9999;
|
|---|
| 176 | min[0] = min[1] = 9999;
|
|---|
| 177 | for (a = 0; a < hull_len; a++)
|
|---|
| 178 | {
|
|---|
| 179 | x = pnts[2 * hull[a]];
|
|---|
| 180 | y = pnts[2 * hull[a] + 1];
|
|---|
| 181 | if (x < min[0])
|
|---|
| 182 | min[0] = x;
|
|---|
| 183 | if (x > max[0])
|
|---|
| 184 | max[0] = x;
|
|---|
| 185 | if (y < min[1])
|
|---|
| 186 | min[1] = y;
|
|---|
| 187 | if (y > max[1])
|
|---|
| 188 | max[1] = y;
|
|---|
| 189 | }
|
|---|
| 190 |
|
|---|
| 191 | /* push the bbox out just a hair to make intersection
|
|---|
| 192 | * a bit more sane */
|
|---|
| 193 | cent[0] = (min[0] + max[0]) / 2.;
|
|---|
| 194 | cent[1] = (min[1] + max[1]) / 2.;
|
|---|
| 195 | dir[0] = max[0] - cent[0];
|
|---|
| 196 | dir[1] = max[1] - cent[1];
|
|---|
| 197 | max[0] = cent[0] + 1.01 * dir[0];
|
|---|
| 198 | max[1] = cent[1] + 1.01 * dir[1];
|
|---|
| 199 | dir[0] = min[0] - cent[0];
|
|---|
| 200 | dir[1] = min[1] - cent[1];
|
|---|
| 201 | min[0] = cent[0] + 1.01 * dir[0];
|
|---|
| 202 | min[1] = cent[1] + 1.01 * dir[1];
|
|---|
| 203 |
|
|---|
| 204 | retval = _segment_hull_intersect(min, max, pnts, hull, hull_len, intr_pnts);
|
|---|
| 205 | if (retval != 2)
|
|---|
| 206 | crError("Bad hull intersection");
|
|---|
| 207 |
|
|---|
| 208 | dir[0] = min[0];
|
|---|
| 209 | dir[1] = max[1];
|
|---|
| 210 | pnt[0] = max[0];
|
|---|
| 211 | pnt[1] = min[1];
|
|---|
| 212 | retval =
|
|---|
| 213 | _segment_hull_intersect(dir, pnt, pnts, hull, hull_len, intr_pnts + 4);
|
|---|
| 214 | if (retval != 2)
|
|---|
| 215 | crError("Bad hull intersection");
|
|---|
| 216 |
|
|---|
| 217 | /* swap points to get them in some order */
|
|---|
| 218 | cent[0] = intr_pnts[2];
|
|---|
| 219 | cent[1] = intr_pnts[3];
|
|---|
| 220 | intr_pnts[2] = intr_pnts[4];
|
|---|
| 221 | intr_pnts[3] = intr_pnts[5];
|
|---|
| 222 | intr_pnts[4] = cent[0];
|
|---|
| 223 | intr_pnts[5] = cent[1];
|
|---|
| 224 |
|
|---|
| 225 | _four_point_box(intr_pnts, bbox, bbox + 2);
|
|---|
| 226 | }
|
|---|
| 227 |
|
|---|
| 228 | /*==================================================*/
|
|---|
| 229 | void
|
|---|
| 230 | crHullInteriorBox(const double *pnts, int npnts, double *bbox)
|
|---|
| 231 | {
|
|---|
| 232 | int lowest, a;
|
|---|
| 233 | int *hull, idx, low_idx = 0;
|
|---|
| 234 | double low_dot;
|
|---|
| 235 | const double *p0, *p1;
|
|---|
| 236 | double v[2], A, B, vnew[2], vlow[2];
|
|---|
| 237 |
|
|---|
| 238 | vlow[0] = vlow[1] = 0.0;
|
|---|
| 239 |
|
|---|
| 240 | hull = (int *) crAlloc((npnts + 1) * sizeof(int));
|
|---|
| 241 |
|
|---|
| 242 | /* find the lowest */
|
|---|
| 243 | lowest = 0;
|
|---|
| 244 | for (a = 0; a < 2 * npnts; a += 2)
|
|---|
| 245 | {
|
|---|
| 246 | if (pnts[a + 1] < pnts[2 * lowest + 1])
|
|---|
| 247 | lowest = a / 2;
|
|---|
| 248 | }
|
|---|
| 249 |
|
|---|
| 250 | hull[0] = lowest;
|
|---|
| 251 | p0 = pnts + 2 * lowest;
|
|---|
| 252 | idx = 1;
|
|---|
| 253 |
|
|---|
| 254 | v[0] = 1;
|
|---|
| 255 | v[1] = 0;
|
|---|
| 256 |
|
|---|
| 257 | while (1)
|
|---|
| 258 | {
|
|---|
| 259 | low_dot = -10;
|
|---|
| 260 |
|
|---|
| 261 | for (a = 0; a < npnts; a++)
|
|---|
| 262 | {
|
|---|
| 263 | if (a == hull[idx - 1])
|
|---|
| 264 | continue;
|
|---|
| 265 |
|
|---|
| 266 | p1 = pnts + 2 * a;
|
|---|
| 267 | if (v[0] == 0.0)
|
|---|
| 268 | A = 0.0;
|
|---|
| 269 | else
|
|---|
| 270 | A = p0[1] - p1[1] + (v[1] / v[0]) * (p1[0] - p0[0]);
|
|---|
| 271 | if (v[0] == 0.0)
|
|---|
| 272 | B = 0.0;
|
|---|
| 273 | else
|
|---|
| 274 | B = v[1] * v[1] / v[0] + v[0];
|
|---|
| 275 |
|
|---|
| 276 | if (B != 0.0 && A / B > 0)
|
|---|
| 277 | {
|
|---|
| 278 | continue;
|
|---|
| 279 | }
|
|---|
| 280 |
|
|---|
| 281 | vnew[0] = p1[0] - p0[0];
|
|---|
| 282 | vnew[1] = p1[1] - p0[1];
|
|---|
| 283 | B = sqrt(vnew[0] * vnew[0] + vnew[1] * vnew[1]);
|
|---|
| 284 |
|
|---|
| 285 | vnew[0] /= B;
|
|---|
| 286 | vnew[1] /= B;
|
|---|
| 287 |
|
|---|
| 288 | A = vnew[0] * v[0] + vnew[1] * v[1];
|
|---|
| 289 |
|
|---|
| 290 | if (A > low_dot)
|
|---|
| 291 | {
|
|---|
| 292 | low_dot = A;
|
|---|
| 293 | low_idx = a;
|
|---|
| 294 | vlow[0] = vnew[0];
|
|---|
| 295 | vlow[1] = vnew[1];
|
|---|
| 296 | }
|
|---|
| 297 | }
|
|---|
| 298 |
|
|---|
| 299 | p0 = pnts + 2 * low_idx;
|
|---|
| 300 | hull[idx] = low_idx;
|
|---|
| 301 | idx++;
|
|---|
| 302 |
|
|---|
| 303 | v[0] = vlow[0];
|
|---|
| 304 | v[1] = vlow[1];
|
|---|
| 305 |
|
|---|
| 306 | if (low_idx == lowest)
|
|---|
| 307 | {
|
|---|
| 308 | break;
|
|---|
| 309 | }
|
|---|
| 310 | }
|
|---|
| 311 |
|
|---|
| 312 | _bound(pnts, npnts, hull, idx, bbox);
|
|---|
| 313 |
|
|---|
| 314 | crFree(hull);
|
|---|
| 315 | }
|
|---|
