source: XIOS/dev/dev_olga/src/extern/remap/src/inside.cpp @ 1022

Last change on this file since 1022 was 1022, checked in by mhnguyen, 7 years ago
File size: 9.0 KB
Line 
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
10namespace sphereRemap {
11
12static const double SNAP = 1e-11;
13
14using namespace std;
15
16/* returns the index of the element in `v` with the maximum absolute value*/
17int 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*/
44void 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
133void 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        {
143sega:
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 */
260int 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}
Note: See TracBrowser for help on using the repository browser.