source: XIOS/dev/branch_yushan/extern/remap/src/inside.cpp @ 1085

Last change on this file since 1085 was 1072, checked in by yushan, 7 years ago

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