[1022] | 1 | #include <cstdlib> |
---|
| 2 | #include <cmath> |
---|
| 3 | #include <cassert> |
---|
| 4 | #include <iostream> |
---|
| 5 | #include "elt.hpp" |
---|
| 6 | #include "polyg.hpp" |
---|
| 7 | |
---|
| 8 | #include "inside.hpp" |
---|
| 9 | |
---|
| 10 | namespace sphereRemap { |
---|
| 11 | |
---|
| 12 | static const double SNAP = 1e-11; |
---|
| 13 | |
---|
| 14 | using namespace std; |
---|
| 15 | |
---|
| 16 | /* returns the index of the element in `v` with the maximum absolute value*/ |
---|
| 17 | int argmaxAbs3(double *v) |
---|
| 18 | { |
---|
| 19 | /* ip |
---|
| 20 | 0 F F |v0| >= |v1| && |v0| >= |v2| max: v0 |
---|
| 21 | 1 T F |v0| < |v1| && |v1| >= |v2| v1 |
---|
| 22 | 2 ? T |v0| < |v2| && |v1| < |v2| v2 |
---|
| 23 | */ |
---|
| 24 | int amax = 0; |
---|
| 25 | double maxv = fabs(v[0]); |
---|
| 26 | if (fabs(v[1]) > maxv) |
---|
| 27 | { |
---|
| 28 | amax = 1; |
---|
| 29 | maxv = fabs(v[1]); |
---|
| 30 | } |
---|
| 31 | if (fabs(v[2]) > maxv) |
---|
| 32 | { |
---|
| 33 | amax = 2; |
---|
| 34 | maxv = fabs(v[2]); |
---|
| 35 | } |
---|
| 36 | return amax; |
---|
| 37 | } |
---|
| 38 | |
---|
| 39 | /** |
---|
| 40 | Find all intersections of edges of ea with edges of eb, and store all intersection points in ipt. |
---|
| 41 | `ipt` is flattened matix of `Ipt` with inner dimension the number of edges of `ea` and outer dimension the one of `eb`. |
---|
| 42 | `Ipt` contains two coordinates for two possible intersections between two edges. |
---|
| 43 | */ |
---|
| 44 | void ptsec(Elt *ea, Elt *eb, Ipt *ipt) |
---|
| 45 | { |
---|
| 46 | int na = ea->n; |
---|
| 47 | int nb = eb->n; |
---|
| 48 | |
---|
| 49 | for (int i = 0; i < ea->n; i++) |
---|
| 50 | { |
---|
| 51 | for (int j = 0; j < eb->n; j++) |
---|
| 52 | { |
---|
| 53 | int ij = i*eb->n + j; |
---|
| 54 | if (norm(crossprod(ea->edge[i], eb->edge[j])) < 1e-15) |
---|
| 55 | { |
---|
| 56 | ipt[ij].nb = 0; |
---|
| 57 | continue; |
---|
| 58 | } |
---|
| 59 | |
---|
| 60 | double A, B, C, D, Ap, Bp, Cp, Dp; |
---|
| 61 | double d3, d4, aa, bb, cc, dd; // if polygon then d3 = d4 = 0 |
---|
| 62 | double d[3]; |
---|
| 63 | |
---|
| 64 | A =ea->edge[i].x; B =ea->edge[i].y; C =ea->edge[i].z; D =-ea->d[i]; |
---|
| 65 | Ap=eb->edge[j].x; Bp=eb->edge[j].y; Cp=eb->edge[j].z; Dp=-eb->d[j]; |
---|
| 66 | |
---|
| 67 | d[0] = B * Cp - C * Bp; |
---|
| 68 | d[1] = C * Ap - A * Cp; |
---|
| 69 | d[2] = A * Bp - B * Ap; |
---|
| 70 | |
---|
| 71 | int ip = argmaxAbs3(d); |
---|
| 72 | if (ip == 0) { |
---|
| 73 | d3 = -ea->edge[i].z*eb->d[j] + ea->d[i] *eb->edge[j].z; |
---|
| 74 | d4 = -ea->d[i] *eb->edge[j].y + ea->edge[i].y*eb->d[j]; |
---|
| 75 | } |
---|
| 76 | if (ip == 1) { |
---|
| 77 | d[0] = C*Ap-A*Cp; |
---|
| 78 | d[1] = A*Bp-B*Ap; |
---|
| 79 | d[2] = B*Cp-C*Bp; |
---|
| 80 | d3 = A*Dp-D*Ap; |
---|
| 81 | d4 = D*Cp-C*Dp; |
---|
| 82 | } |
---|
| 83 | if (ip == 2) { |
---|
| 84 | d[0] = A*Bp-B*Ap; d[1] = B*Cp-C*Bp; d[2] = C*Ap-A*Cp; |
---|
| 85 | d3 = B*Dp-D*Bp; d4 = D*Ap-A*Dp; |
---|
| 86 | } |
---|
| 87 | aa = d[0]*d[0] + d[1]*d[1] + d[2]*d[2]; |
---|
| 88 | bb = d[1]*d3 + d[2]*d4; |
---|
| 89 | cc = d3*d3 + d4*d4 - d[0]*d[0]; |
---|
| 90 | dd = bb*bb - aa*cc; // if polygon dd = 0*0 - ||d||**2 * d0**2 |
---|
| 91 | |
---|
| 92 | if (dd < EPS*EPS) |
---|
| 93 | { |
---|
| 94 | ipt[ij].nb = 0; |
---|
| 95 | continue; |
---|
| 96 | } |
---|
| 97 | ipt[ij].nb = 2; |
---|
| 98 | |
---|
| 99 | double v_[3]; |
---|
| 100 | v_[ip] = (-bb + sqrt(dd))/aa; |
---|
| 101 | v_[(ip+1)%3] = (d[1]*v_[ip] + d3)/d[0]; |
---|
| 102 | v_[(ip+2)%3] = (d[2]*v_[ip] + d4)/d[0]; |
---|
| 103 | double w_[3]; |
---|
| 104 | w_[ip] = (-bb - sqrt(dd))/aa; |
---|
| 105 | w_[(ip+1)%3] = (d[1]*w_[ip] + d3)/d[0]; |
---|
| 106 | w_[(ip+2)%3] = (d[2]*w_[ip] + d4)/d[0]; |
---|
| 107 | |
---|
| 108 | Coord v(v_[0], v_[1], v_[2]); |
---|
| 109 | Coord w(w_[0], w_[1], w_[2]); |
---|
| 110 | |
---|
| 111 | int ii = (i + 1) % na; |
---|
| 112 | int jj = (j + 1) % nb; |
---|
| 113 | |
---|
| 114 | /* if v and/or w is close to a vertex make exact (align) */ |
---|
| 115 | if (squaredist(v, ea->vertex[i] ) < SNAP*SNAP) v = ea->vertex[i]; |
---|
| 116 | if (squaredist(v, ea->vertex[ii]) < SNAP*SNAP) v = ea->vertex[ii]; |
---|
| 117 | if (squaredist(v, eb->vertex[j] ) < SNAP*SNAP) v = eb->vertex[j]; |
---|
| 118 | if (squaredist(v, eb->vertex[jj]) < SNAP*SNAP) v = eb->vertex[jj]; |
---|
| 119 | if (squaredist(w, ea->vertex[i] ) < SNAP*SNAP) w = ea->vertex[i]; |
---|
| 120 | if (squaredist(w, ea->vertex[ii]) < SNAP*SNAP) w = ea->vertex[ii]; |
---|
| 121 | if (squaredist(w, eb->vertex[j] ) < SNAP*SNAP) w = eb->vertex[j]; |
---|
| 122 | if (squaredist(w, eb->vertex[jj]) < SNAP*SNAP) w = eb->vertex[jj]; |
---|
| 123 | |
---|
| 124 | assert(squaredist(v,w) > EPS*EPS); // inconsistency in line intersection |
---|
| 125 | |
---|
| 126 | /* in 99% os cases we will later discart one of the two points for lying on the the other side of the globe */ |
---|
| 127 | ipt[ij].pt[0] = v; |
---|
| 128 | ipt[ij].pt[1] = w; |
---|
| 129 | } |
---|
| 130 | } |
---|
| 131 | } |
---|
| 132 | |
---|
| 133 | void recense(Elt *ea, Elt *eb, Ipt *ipt, list<Sgm> &isedge, int pass) |
---|
| 134 | { |
---|
| 135 | list<Sgm> plan; |
---|
| 136 | list<Sgm>::iterator bit; |
---|
| 137 | Sgm sgm; |
---|
| 138 | int na = ea->n; |
---|
| 139 | int nb = eb->n; |
---|
| 140 | |
---|
| 141 | for (int i = 0; i<na; i++) |
---|
| 142 | { |
---|
| 143 | sega: |
---|
| 144 | int ii = (i+1)%na; |
---|
| 145 | sgm.n = ea->edge[i]; |
---|
| 146 | sgm.d = ea->d[i]; |
---|
| 147 | sgm.xt[0] = ea->vertex[i]; |
---|
| 148 | sgm.xt[1] = ea->vertex[ii]; |
---|
| 149 | plan.push_back(sgm); |
---|
| 150 | for (int j = 0; j<nb; j++) |
---|
| 151 | { |
---|
| 152 | int ij = i*nb+j; |
---|
| 153 | const double OUT = 1e-11; |
---|
| 154 | |
---|
| 155 | if (ipt[ij].nb < 2) |
---|
| 156 | { |
---|
| 157 | bit = plan.begin(); |
---|
| 158 | double side = scalarprod(bit->xt[0], eb->edge[j]) - eb->d[j]; |
---|
| 159 | double side1= scalarprod(bit->xt[1], eb->edge[j]) - eb->d[j]; |
---|
| 160 | if (side < -OUT || side1 < -OUT || |
---|
| 161 | (side < OUT && side1 < OUT && pass)) //dedans |
---|
| 162 | continue; // all bits unscathed, j++ |
---|
| 163 | plan.clear(); i++; |
---|
| 164 | if (i<na) goto sega; // all bits destroyed, i++ |
---|
| 165 | else return; |
---|
| 166 | } |
---|
| 167 | |
---|
| 168 | Coord& x = ipt[ij].pt[0]; |
---|
| 169 | Coord& y = ipt[ij].pt[1]; |
---|
| 170 | bit = plan.begin(); |
---|
| 171 | double r2 = 1 - bit->d*bit->d; |
---|
| 172 | while (bit!=plan.end()) { // bit++ !! |
---|
| 173 | double lg = squaredist(bit->xt[0], bit->xt[1]); |
---|
| 174 | double side = scalarprod(bit->xt[0], eb->edge[j]) - eb->d[j]; |
---|
| 175 | double ag = angle(bit->xt[0], bit->xt[1], bit->n) /r2; |
---|
| 176 | double agx = angle(bit->xt[0], x, bit->n) /r2; |
---|
| 177 | double agy = angle(bit->xt[0], y, bit->n) /r2; |
---|
| 178 | double r = 1;//sqrt(r2); |
---|
| 179 | int isd = 0; // is dans le seg |
---|
| 180 | |
---|
| 181 | if (ag > 0) |
---|
| 182 | { |
---|
| 183 | if (r*agx > OUT && r*agx < ag-OUT && squaredist(bit->xt[0], x) < lg) { isd += 1; } |
---|
| 184 | if (r*agy > OUT && r*agy < ag-OUT && squaredist(bit->xt[0], y) < lg) { isd += 2; } |
---|
| 185 | } else { |
---|
| 186 | if (r*agx< -OUT && r*agx > ag+OUT && squaredist(bit->xt[0], x) < lg) { isd += 1; } |
---|
| 187 | if (r*agy< -OUT && r*agy > ag+OUT && squaredist(bit->xt[0], y) < lg) { isd += 2; } |
---|
| 188 | } |
---|
| 189 | |
---|
| 190 | Coord m, ptout; |
---|
| 191 | double side_m, side_pt; |
---|
| 192 | int A=0; |
---|
| 193 | double agrot = M_PI_2; |
---|
| 194 | if (ag > 0) agrot *= -1; |
---|
| 195 | switch (isd) { |
---|
| 196 | case 0: |
---|
| 197 | if (bit->d) |
---|
| 198 | m = midpointSC(bit->xt[0], bit->xt[1]); |
---|
| 199 | else |
---|
| 200 | m = barycentre(bit->xt, 2); |
---|
| 201 | side = scalarprod(m, eb->edge[j]) - eb->d[j]; |
---|
| 202 | if (side < 0) |
---|
| 203 | ++bit; // unscathed |
---|
| 204 | else |
---|
| 205 | bit = plan.erase(bit); |
---|
| 206 | continue; |
---|
| 207 | case 1: |
---|
| 208 | if (squaredist(x, bit->xt[1]) > squaredist(x, bit->xt[0])) A = 1; |
---|
| 209 | m = (bit->d) ? midpointSC(x, bit->xt[A]) : midpoint(x, bit->xt[A]); |
---|
| 210 | side_m = scalarprod(m, eb->edge[j]) - eb->d[j]; |
---|
| 211 | if (side_m < 0) bit->xt[1-A] = x; |
---|
| 212 | else bit->xt[A] = x; |
---|
| 213 | ++bit; |
---|
| 214 | continue; |
---|
| 215 | case 2: |
---|
| 216 | if (squaredist(y, bit->xt[0]) > squaredist(y, bit->xt[1])) A = 1; |
---|
| 217 | m = (bit->d) ? midpointSC(y, bit->xt[A]) : midpoint(y, bit->xt[A]); |
---|
| 218 | side_m = scalarprod(m, eb->edge[j]) - eb->d[j]; |
---|
| 219 | if (side_m < 0) bit->xt[1-A] = y; |
---|
| 220 | else bit->xt[A] = y; |
---|
| 221 | ++bit; continue; |
---|
| 222 | case 3: |
---|
| 223 | ptout = bit->xt[0]; |
---|
| 224 | ptout.rot(bit->n, -ag); |
---|
| 225 | side_pt = scalarprod(ptout, eb->edge[j]) - eb->d[j]; |
---|
| 226 | if (side_pt < 0.) { // or side |
---|
| 227 | Sgm sgm2; |
---|
| 228 | sgm2.n = ea->edge[i]; |
---|
| 229 | sgm2.d = ea->d[i]; |
---|
| 230 | int A=0, B=1; |
---|
| 231 | if (squaredist(bit->xt[0], y) < squaredist(bit->xt[0], x)) |
---|
| 232 | { |
---|
| 233 | ++A; |
---|
| 234 | --B; |
---|
| 235 | } |
---|
| 236 | sgm2.xt[0] = ipt[ij].pt[B]; |
---|
| 237 | sgm2.xt[1] = bit->xt[1]; |
---|
| 238 | bit->xt[1] = ipt[ij].pt[A]; |
---|
| 239 | ++bit; |
---|
| 240 | plan.insert(bit,sgm2); |
---|
| 241 | } else { |
---|
| 242 | bit->xt[0] = ipt[ij].pt[0]; |
---|
| 243 | bit->xt[1] = ipt[ij].pt[1]; |
---|
| 244 | ++bit; |
---|
| 245 | } |
---|
| 246 | continue; |
---|
| 247 | } |
---|
| 248 | } |
---|
| 249 | } |
---|
| 250 | for (bit=plan.begin(); bit!=plan.end(); ++bit) { |
---|
| 251 | isedge.push_back(*bit); |
---|
| 252 | } |
---|
| 253 | plan.clear(); |
---|
| 254 | } |
---|
| 255 | |
---|
| 256 | } |
---|
| 257 | |
---|
| 258 | /* output: c, d, xc |
---|
| 259 | in/out: isedge get elements removed */ |
---|
| 260 | int assemble(list<Sgm>& isedge, Coord *c, double *d, Coord *xc) |
---|
| 261 | { |
---|
| 262 | double lgmax = 0.; |
---|
| 263 | int idmax = 0, id = 0; |
---|
| 264 | for (list<Sgm>::iterator it = isedge.begin(); it != isedge.end(); ++it, ++id) |
---|
| 265 | { |
---|
| 266 | double lg = squaredist(it->xt[0], it->xt[1]); |
---|
| 267 | if (lg > lgmax) |
---|
| 268 | { |
---|
| 269 | idmax = id; |
---|
| 270 | lgmax = lg; |
---|
| 271 | } |
---|
| 272 | } |
---|
| 273 | |
---|
| 274 | list<Sgm>::iterator it = isedge.begin(); |
---|
| 275 | advance(it, idmax); |
---|
| 276 | c[0] = it->n; |
---|
| 277 | d[0] = it->d; |
---|
| 278 | xc[0] = it->xt[0]; |
---|
| 279 | Coord t = it->xt[1]; |
---|
| 280 | int nc = 1; |
---|
| 281 | isedge.erase(it); //isedge.pop_front(); |
---|
| 282 | |
---|
| 283 | while (isedge.size()) |
---|
| 284 | { |
---|
| 285 | /* all distances as squares to omit sqrt */ |
---|
| 286 | double d0, d1; |
---|
| 287 | double dmin = pow(17.,2); |
---|
| 288 | int idmin = 0, id = 0, s = 0; |
---|
| 289 | list< pair <int, double> > way; |
---|
| 290 | list< pair <int, double> >::iterator wi; |
---|
| 291 | int ps1way = 0; |
---|
| 292 | for (it = isedge.begin(); it != isedge.end(); ++it, ++id) |
---|
| 293 | { |
---|
| 294 | d0 = squaredist(t, it->xt[0]); |
---|
| 295 | d1 = squaredist(t, it->xt[1]); |
---|
| 296 | if (d0 < SNAP*SNAP || d1 < SNAP*SNAP) |
---|
| 297 | { |
---|
| 298 | double lg = squaredist(it->xt[0], it->xt[1]); |
---|
| 299 | pair <int, double> p = make_pair(id, lg); |
---|
| 300 | if (d0 < 0.01*dmin || d1 < 0.01*dmin) |
---|
| 301 | { |
---|
| 302 | ps1way = 1; |
---|
| 303 | way.push_front(p); |
---|
| 304 | } |
---|
| 305 | else |
---|
| 306 | { |
---|
| 307 | ps1way = 0; |
---|
| 308 | way.push_back(p); |
---|
| 309 | } |
---|
| 310 | } |
---|
| 311 | if (d0 < dmin) |
---|
| 312 | { |
---|
| 313 | idmin = id; |
---|
| 314 | s = 0; |
---|
| 315 | dmin = d0; |
---|
| 316 | } |
---|
| 317 | if (d1 < dmin) |
---|
| 318 | { |
---|
| 319 | idmin = id; |
---|
| 320 | s = 1; |
---|
| 321 | dmin = d1; |
---|
| 322 | } |
---|
| 323 | } |
---|
| 324 | int ways = way.size(); |
---|
| 325 | double lgmaxx = 0.; |
---|
| 326 | int A = 0, B = 1; |
---|
| 327 | assert(ways < 3); |
---|
| 328 | switch (ways) |
---|
| 329 | { |
---|
| 330 | case 0: |
---|
| 331 | return nc; |
---|
| 332 | break; |
---|
| 333 | case 2: |
---|
| 334 | if (ps1way == 1) |
---|
| 335 | idmin = way.front().first; |
---|
| 336 | else |
---|
| 337 | { |
---|
| 338 | for (wi = way.begin(); wi != way.end(); ++wi) |
---|
| 339 | { |
---|
| 340 | if (wi->second > lgmaxx) |
---|
| 341 | { |
---|
| 342 | idmin = wi->first; |
---|
| 343 | lgmaxx = wi->second; |
---|
| 344 | } |
---|
| 345 | } |
---|
| 346 | } |
---|
| 347 | case 1: |
---|
| 348 | if (ways == 1) |
---|
| 349 | idmin = way.front().first; |
---|
| 350 | it = isedge.begin(); |
---|
| 351 | advance(it, idmin); |
---|
| 352 | c[nc] = it->n; |
---|
| 353 | d[nc] = it->d; |
---|
| 354 | if (s) |
---|
| 355 | { |
---|
| 356 | ++A; |
---|
| 357 | --B; |
---|
| 358 | } |
---|
| 359 | xc[nc] = it->xt[A]; |
---|
| 360 | t = it->xt[B]; |
---|
| 361 | nc++; |
---|
| 362 | isedge.erase(it); |
---|
| 363 | break; |
---|
| 364 | default: |
---|
| 365 | ; // assert(ways < 3) above -> cannot be reached |
---|
| 366 | } |
---|
| 367 | } |
---|
| 368 | |
---|
| 369 | return nc; |
---|
| 370 | } |
---|
| 371 | |
---|
| 372 | } |
---|