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 | } |
---|