1 // ORIG-DATE: Jan 2008
2 // -*- Mode : c++ -*-
3 //
4 // SUMMARY : Generic Tree (binairy, Quad, Oct)
5 // USAGE : LGPL
6 // ORG : LJLL Universite Pierre et Marie Curi, Paris, FRANCE
7 // AUTHOR : Frederic Hecht
8 // E-MAIL : frederic.hecht@ann.jussieu.fr
9 //
10
11 /*
12
13 This file is part of Freefem++
14
15 Freefem++ is free software; you can redistribute it and/or modify
16 it under the terms of the GNU Lesser General Public License as published by
17 the Free Software Foundation; either version 2.1 of the License, or
18 (at your option) any later version.
19
20 Freefem++ is distributed in the hope that it will be useful,
21 but WITHOUT ANY WARRANTY; without even the implied warranty of
22 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
23 GNU Lesser General Public License for more details.
24
25 You should have received a copy of the GNU Lesser General Public License
26 along with Freefem++; if not, write to the Free Software
27 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
28
29 Thank to the ARN () FF2A3 grant
30 ref:ANR-07-CIS7-002-01
31 */
32
33
34 #include <cmath>
35 #include <cstdlib>
36 #include "error.hpp"
37 #include <iostream>
38 #include <climits>
39 #include <cfloat>
40 #include <cstring>
41 #include "ufunction.hpp"
42 #include "HeapSort.hpp"
43 using namespace std;
44
45
46 #include "GenericMesh.hpp"
47 #include "Mesh1dn.hpp"
48 #include "Mesh2dn.hpp"
49 #include "Mesh3dn.hpp"
50
51 #include <set>
52
53 extern long npichon2d, npichon3d;
54 extern long npichon2d1, npichon3d1;
55
56
57
58 namespace EF23 {
59
60 // new version ----------
61 // ----------------------
62 template<class Rd>
OrthoProject(const Rd & P,const Rd & A,const Rd & B,R * l)63 void OrthoProject(const Rd &P,const Rd &A,const Rd & B,R * l)
64 {
65 Rd AB(A,B),AP(A,P),BP(B,P);
66 R pa=(AB,AP),pb=(AB,BP);
67 // (l0 PA + l1 PB , AB)=0
68 // l0 pa + l1 pb = 0
69 // l0 + l1 =1
70 // l0 (pa - pb) = - pb;
71 l[0] = - pb/(pa-pb);
72 l[1] = 1-l[0];
73 }
74
75 template<class Rd>
OrthoProject(const Rd & P,const Rd & A,const Rd & B,const Rd & C,R * l)76 void OrthoProject(const Rd &P,const Rd &A,const Rd &B,const Rd &C,R * l)
77 {
78 Rd AB(A,B),AC(A,C),AP(A,P),BP(B,P),CP(C,P);
79 R2 p0( (AB,AP) , (AC,AP) );
80 R2 p1( (AB,BP) , (AC,BP) );
81 R2 p2( (AB,CP) , (AC,CP) );
82 // sum li pi = 0
83 // l0 + l1 + l2 = 1
84 // =>
85 R2 O;
86 R d = det(p0,p1,p2);
87 l[0] = det(O ,p1,p2)/ d;
88 l[1] = det(p0,O ,p2)/ d;
89 l[2] = 1. -l[0] -l[1];
90 //
91 //
92 }
93
94 template<class Vertex>
ListNearestVertex(Vertex ** lnv,int nlvnx,int dh,Zd xyi)95 int GTree<Vertex>::ListNearestVertex(Vertex **lnv,int nlvnx,int dh,Zd xyi)
96 {
97 // warning this function return the NearestVertex in the first
98 // none empty box contening the point xyi.
99 // They do not return the true nearest point in classical norm.
100 int nlnv =0;
101 QuadTreeBox * pb[ MaxDeep ];
102 int pi[ MaxDeep ];
103 Zd pp[ MaxDeep ];
104 int l=0; // level
105 QuadTreeBox * b;
106 Int8 h2=(Int8)dh*dh,h0;
107 IntQuad h=dh,hb = MaxISize;
108 Zd p0(0,0,0);
109 Zd plus(xyi) ; plus.Bound();
110
111 // xi<MaxISize?(xi<0?0:xi):MaxISize-1,yj<MaxISize?(yj<0?0:yj):MaxISize-1);
112
113 Vertex *vn=0;
114
115 // init for optimisation ---
116 b = root;
117 long n0=0;
118 if (!root->n)
119 return 0; // empty tree
120
121 if(verbosity>2000)
122 cout << " general case : NearVertex" << endl;
123 // general case -----
124 pb[0]= b;
125 pi[0]=b->n>0 ?(int) b->n : N ;
126 pp[0]=p0;
127
128 do {
129 b= pb[l];
130 while (pi[l]--)
131 {
132 int k = pi[l];
133
134 if (b->n>0) // Vertex QuadTreeBox none empty
135 {
136 NbVerticesSearch++;
137 Zd i2 = VtoZd(b->v[k]);
138 h0 = Zd(i2,plus).norm2();// NORM(iplus,i2.x,jplus,i2.y);
139 if (h0 <h2)
140 {// on stock ..
141 if( nlnv < nlvnx)
142 lnv[nlnv++] = b->v[k];
143 }
144 }
145 else // Pointer QuadTreeBox
146 {
147 QuadTreeBox *b0=b;
148 if ((b=b->b[k]))
149 {
150 hb >>=1 ; // div by 2
151 Zd ppp(pp[l],k,hb);
152
153 if ( ppp.interseg(plus,hb,h) )//(INTER_SEG(iii,iii+hb,iplus-h,iplus+h) && INTER_SEG(jjj,jjj+hb,jplus-h,jplus+h))
154 {
155 NbQuadTreeBoxSearch++;// add box search
156 pb[++l]= b;
157 pi[l]= b->n>0 ?(int) b->n : N ;
158 pp[l]=ppp;
159 }
160 else
161 b=b0, hb <<=1 ;
162 }
163 else
164 b=b0;
165 }
166 }
167 hb <<= 1; // mul by 2
168 } while (l--);
169
170 return nlnv;
171 }
172
173
174 template<class Vertex>
NearestVertex(Zd xyi,bool trueNearest)175 Vertex * GTree<Vertex>::NearestVertex(Zd xyi,bool trueNearest)//long xi,long yj)
176 {
177 // warning this function return the NearestVertex in the first
178 // none empty box contening the point xyi.
179 // They do not return the true nearest point in classical norm.
180
181 QuadTreeBox * pb[ MaxDeep ];
182 int pi[ MaxDeep ];
183 Zd pp[ MaxDeep ];
184 int l=0; // level
185 QuadTreeBox * b;
186 Int8 h2=(Int8) MaxISize*(Int8)MaxISize*3,h0;
187 IntQuad h=MaxISize,hb = MaxISize;
188 Zd p0(0,0,0);
189 Zd plus(xyi) ; plus.Bound();
190 int c2infty = 1+trueNearest;// 2 if trueNearest
191 // xi<MaxISize?(xi<0?0:xi):MaxISize-1,yj<MaxISize?(yj<0?0:yj):MaxISize-1);
192
193 Vertex *vn=0;
194
195 // init for optimisation ---
196 b = root;
197 long n0=0;
198 if (!root->n)
199 return vn; // empty tree
200 if( ! trueNearest )
201 while( (n0 = b->n) < 0)
202 {
203 // search the non empty
204 // QuadTreeBox containing the point (i,j)
205 long hb2 = hb >> 1 ;
206 int k = plus.Case(hb2);//(iplus,jplus,hb2);// QuadTreeBox number of size hb2 contening i;j
207
208 QuadTreeBox * b0= b->b[k];
209
210 if ( ( b0 == 0) || (b0->n == 0) ){
211 break; // null box or empty box => break
212 }
213 NbQuadTreeBoxSearch++;
214 b=b0;
215 p0.Add(k,hb2);
216 hb = hb2;
217
218 }
219 // n0 number of boxes of in b ("b0")
220 if(verbosity>2000)
221 cout << " n0=" << n0 << " " << trueNearest <<endl;
222
223 if ( n0 > 0)
224 {
225 for( int k=0;k<n0;k++)
226 {
227 Zd i2 = VtoZd(b->v[k]);
228 h0 = Zd(i2,plus).norm2(); //warning .. norm sup and not norm 2 ...
229 if (h0 <h2) {
230 h2 = h0;
231 vn = b->v[k];
232 ffassert(vn);
233 }
234 NbVerticesSearch++;
235 }
236 if(verbosity>2000)
237 cout << " find " << vn << " " << h0 << " " << h2 << " " << endl;
238
239 return vn;
240 }
241
242 if(verbosity>2000)
243 cout << " general case : NearVertex" << endl;
244 // general case -----
245 pb[0]= b;
246 pi[0]=b->n>0 ?(int) b->n : N ;
247 pp[0]=p0;
248 h=hb;
249 do {
250 b= pb[l];
251 while (pi[l]--)
252 {
253 int k = pi[l];
254
255 if (b->n>0) // Vertex QuadTreeBox none empty
256 {
257 NbVerticesSearch++;
258 Zd i2 = VtoZd(b->v[k]);
259 h0 = Zd(i2,plus).norm2();// norme 2 ^2
260 if (h0 <h2)
261 {
262 h2 = h0;
263 h =Zd(i2,plus).norm()*c2infty;// norm infty -> norm 2 big enought
264 vn = b->v[k];
265 if(verbosity>2000)
266 cout << " find " << vn << " " << h0 << " " << h2 << " " << h << endl;
267
268 }
269 }
270 else // Pointer QuadTreeBox
271 {
272 QuadTreeBox *b0=b;
273 NbQuadTreeBoxSearch++;
274 if ((b=b->b[k]))
275 {
276 hb >>=1 ; // div by 2
277 Zd ppp(pp[l],k,hb);
278
279 if ( ppp.interseg(plus,hb,h) )//(INTER_SEG(iii,iii+hb,iplus-h,iplus+h) && INTER_SEG(jjj,jjj+hb,jplus-h,jplus+h))
280 {
281 pb[++l]= b;
282 pi[l]= b->n>0 ?(int) b->n : N ;
283 pp[l]=ppp;
284 }
285 else
286 b=b0, hb <<=1 ;
287 }
288 else
289 b=b0;
290 }
291 }
292 hb <<= 1; // mul by 2
293 } while (l--);
294
295 return vn;
296 }
297
298
299 template<class Vertex>
ToClose(const Rd & v,R seuil,Zd H,bool nearest)300 Vertex * GTree<Vertex>::ToClose(const Rd & v,R seuil,Zd H,bool nearest)
301 {
302 const Rd X(v);
303 const Zd p(RdtoZd(v));
304 R seuil2 = seuil*seuil;
305 // const Metric Mx(v.m);
306 Vertex * pvr=0;
307 QuadTreeBox * pb[ MaxDeep ];
308 int pi[ MaxDeep ];
309 Zd pp[ MaxDeep ];
310
311 int l=0; // level
312 QuadTreeBox * b;
313 long h=MaxISize;
314 long hb = MaxISize;
315 Zd p0( 0 );
316
317 if (!root->n)
318 return 0; // empty tree
319
320 // general case -----
321 pb[0]= root;
322 pi[0]= root->n>0 ?(int) root->n : N ;
323 pp[0]= p0;
324 h=hb;
325 do {
326 b= pb[l];
327 while (pi[l]--)
328 {
329 int k = pi[l];
330 //cout << "b" << b << ", k= " << k << endl;
331 //cout << " b->n " << b->n << endl;
332 if (b->n>0) // Vertex QuadTreeBox none empty
333 {
334 NbVerticesSearch++;
335 Vertex & V(*b->v[k]);
336 Zd i2 = VtoZd(V);
337
338 if ( Zd(i2,p).less(H) )
339 {
340
341 Rd XY(X,V);
342 R dd;
343 if( (dd= (XY,XY) ) < seuil2 ) // LengthInterpole(Mx(XY), b->v[k]->m(XY))) < seuil )
344 {
345 if( nearest ) // modif FH aug. 2019 For P. Jolivet.
346 {
347 seuil2=dd;
348 pvr = & V;
349 }
350 else
351 return &V;
352 }
353 }
354 }
355 else // Pointer QuadTreeBox
356 {
357 QuadTreeBox *b0=b;
358 NbQuadTreeBoxSearch++;
359 if( (b=b->b[k]) )
360 {
361
362 hb >>=1; // div by 2
363 Zd ppp(pp[l],k,hb);
364
365 if( ppp.interseg(p,hb,H) )
366 {
367
368 pb[++l]= b;
369 pi[l]= b->n>0 ?(int) b->n : N;
370 pp[l]=ppp;
371 }
372 else
373 b=b0, hb <<=1 ;
374
375 }
376 else
377 b=b0;
378 }
379 } // fin: while(pi[l]--)
380 hb <<= 1; // mul by 2
381 } while (l--);
382
383 return pvr;
384
385 }
386
387 template<class Vertex>
Add(Vertex & w)388 void GTree<Vertex>::Add( Vertex & w)
389 {
390 QuadTreeBox ** pb , *b;
391 Zd p(VtoZd(w));
392 long l=MaxISize;
393 pb = &root;
394
395 while( (b=*pb) && (b->n<0))
396 {
397 b->n--;
398 l >>= 1;
399 pb = &b->b[p.Case(l)];
400 }
401 if (b) {
402 for(int i=N-1;i>=0;--i)
403 if (b->n > i && b->v[i] == &w)
404 {
405 //if( abs(w.x+0.5)<1e-10 ) cout << "if (b->n > i && b->v[i] == &w)" <<endl;
406 return;
407 }
408 }
409 assert(l);
410 while ((b= *pb) && (b->n == N)) // the QuadTreeBox is full
411 {
412 //if(verbosity > 5) cout << " b = " << b << b->n << " " << l << endl;
413 Vertex *v4[N]; // copy of the QuadTreeBox vertices
414 for(int i=0;i<N;++i)
415 { v4[i]= b->v[i];
416 b->v[i]=0;}
417
418 b->n = -b->n; // mark is pointer QuadTreeBox
419
420 l >>= 1; // div the size by 2
421 ffassert(l);
422 for (int k=0;k<N;k++) // for the 4 vertices find the sub QuadTreeBox ij
423 {
424 int ij;
425 QuadTreeBox * bb = b->b[ij=VtoZd(v4[k]).Case(l)];
426 //if(verbosity > 5) cout << "ij= "<< ij << " " << VtoZd(v4[k])<< endl;
427 if (!bb)
428 bb=b->b[ij]=NewQuadTreeBox(); // alloc the QuadTreeBox
429 //if(verbosity > 5) cout << bb << " " << k << " " << ij << endl;
430 bb->v[bb->n++] = v4[k];
431 }
432 pb = &b->b[p.Case(l)];
433 }
434 if (!(b = *pb))
435 { //if(verbosity > 5) cout << "Qbox \n";
436 b=*pb= NewQuadTreeBox(); // alloc the QuadTreeBox
437 }
438 //if(verbosity > 5) cout << b << " " << b->n << endl;
439 b->v[b->n++]=&w; // we add the vertex
440 NbVertices++;
441
442 }
443
444
445 template<class Vertex>
GTree(Vertex * v,Rd Pmin,Rd Pmax,int nbv)446 GTree<Vertex>::GTree(Vertex * v,Rd Pmin,Rd Pmax,int nbv) :
447 lenStorageQuadTreeBox(nbv/8+100),
448 // th(t),
449 NbQuadTreeBoxSearch(0),
450 NbVerticesSearch(0),
451 NbQuadTreeBox(0),
452 NbVertices(0),
453 cMin(Pmin-(Pmax-Pmin)/2),
454 cMax(Pmax+(Pmax-Pmin)/2),
455 coef( MaxISize/Norme_infty(cMax-cMin))
456
457 {
458 if(verbosity>5){
459 cout << " GTree: box: "<< Pmin << " " << Pmax << " " << cMin << " "<< cMax << " nbv : " << nbv <<endl;
460 cout << "MaxISize " << MaxISize << endl;
461 //cout << " RdtoZd(cMin)" << RdtoZd(cMin) << " RdtoZd(cMax)" << RdtoZd(cMax) << endl;
462 //cout << " RdtoZd(Pmin)" << RdtoZd(Pmin) << " RdtoZd(Pmax)" << RdtoZd(Pmax) << endl;
463 }
464 sb =new StorageQuadTreeBox(lenStorageQuadTreeBox);
465 root=NewQuadTreeBox();
466 // throwassert( MaxISize > MaxICoor);
467 if (v)
468 for (long i=0;i<nbv;i++)
469 Add(v[i]);
470 }
471
472 template<class Vertex>
GTree()473 GTree<Vertex>::GTree() :
474 lenStorageQuadTreeBox(100),
475 // th(0),
476 NbQuadTreeBoxSearch(0),
477 NbVerticesSearch(0),
478 NbQuadTreeBox(0),
479 NbVertices(0),
480 cMin(),cMax(),coef(0)
481 {
482 sb =new StorageQuadTreeBox(lenStorageQuadTreeBox);
483 root=NewQuadTreeBox();
484 }
485
486 template<class Vertex>
StorageQuadTreeBox(int ll,StorageQuadTreeBox * nn)487 GTree<Vertex>::StorageQuadTreeBox::StorageQuadTreeBox(int ll,StorageQuadTreeBox *nn)
488 {
489 len = ll;
490 n = nn;
491 b = new QuadTreeBox[ll];
492 for (int i = 0; i <ll;i++)
493 {
494 b[i].n =0;
495 for(int j=0;j<N;++j)
496 b[i].b[j]=0;
497 }
498 bc =b;
499 be = b +ll;
500 assert(b);
501 }
502
~StorageQuadTreeBox()503 template<class Vertex> GTree<Vertex>::StorageQuadTreeBox::~StorageQuadTreeBox()
504 { //cout << "~StorageQuadTreeBox " << this << " n " << n << " b " << b << endl;
505 if(n) delete n;
506 delete [] b;
507 }
508
~GTree()509 template<class Vertex> GTree<Vertex>::~GTree()
510 {
511 delete sb;
512 }
513
operator <<(ostream & f,const GTree<Vertex> & qt)514 template<class Vertex> ostream& operator <<(ostream& f, const GTree<Vertex> & qt)
515 {
516 f << " the tree " << endl;
517 f << " NbTreeBox = " << qt.NbQuadTreeBox
518 << " Nb Vertices = " << qt.NbVertices << endl;
519 f << " NbTreeBoxSearch " << qt.NbQuadTreeBoxSearch
520 << " NbVerticesSearch " << qt.NbVerticesSearch << endl;
521 f << " SizeOf QuadTree" << qt.SizeOf() << endl;
522 // return dump(f,*qt.root);
523 return f;
524 }
525
NearestVertexWithNormal(const Rd & P)526 template<class Vertex> Vertex * GTree<Vertex>::NearestVertexWithNormal(const Rd &P)//(long xi,long yj)
527 {
528 QuadTreeBox * pb[ MaxDeep ];
529 int pi[ MaxDeep ];
530 Zd pp[ MaxDeep];
531 int l; // level
532 QuadTreeBox * b;
533 IntQuad h=MaxISize,h0;
534 IntQuad hb = MaxISize;
535 Zd p0;
536 Zd plus(RdtoZd(P) );//xi<MaxISize?(xi<0?0:xi):MaxISize-1,yj<MaxISize?(yj<0?0:yj):MaxISize-1);
537
538 Vertex *vn=0;
539 // init for optimisation ---
540 b = root;
541 long n0;
542 if (!root->n)
543 return vn; // empty tree
544
545 while( (n0 = b->n) < 0)
546 {
547 // search the non empty
548 // QuadTreeBox containing the point (i,j)
549 long hb2 = hb >> 1 ;
550 int k = plus.Case(hb2);//(iplus,jplus,hb2);// QuadTreeBox number of size hb2 contening i;j
551 QuadTreeBox * b0= b->b[k];
552 if ( ( b0 == 0) || (b0->n == 0) )
553 break; // null box or empty => break
554 NbQuadTreeBoxSearch++;
555 p0.Add(k,hb2);
556 hb = hb2;
557 }
558
559
560 if ( n0 > 0)
561 {
562 for(int k=0;k<n0;k++)
563 {
564 Vertex * v=b->v[k];
565 if (v->ninside(P)) {
566 Zd i2 = VtoZd(v);
567 // try if is in the right sens --
568 h0 = Zd(i2,plus).norm();// h0 = NORM(iplus,i2.x,jplus,i2.y);
569 if (h0 <h) {
570 h = h0;
571 vn = v;}
572 NbVerticesSearch++;}
573 }
574 if (vn) return vn;
575 }
576 // general case -----
577 // INITIALISATION OF THE STACK
578 l =0; // level
579 pb[0]= b;
580 pi[0]= b->n>0 ?(int) b->n : N ;
581 pp[0]=p0;
582 h=hb;
583 L1:
584 do { // walk on the tree
585 b= pb[l];
586 while (pi[l]--) // loop on 4 element of the box
587 {
588 int k = pi[l];
589
590 if (b->n>0) // Vertex QuadTreeBox none empty
591 {
592 Vertex * v=b->v[k];
593 if (v->ninside(P) ) {
594 NbVerticesSearch++;
595 Zd i2 = VtoZd(v);
596 // if good sens when try --
597 h0 = Zd(i2,plus).norm();// NORM(iplus,i2.x,jplus,i2.y);
598 if (h0 <h)
599 {
600 h = h0;
601 vn =v;
602 }}
603 }
604 else // Pointer QuadTreeBox
605 {
606 QuadTreeBox *b0=b;
607 NbQuadTreeBoxSearch++;
608 if ((b=b->b[k]))
609 {
610 hb >>=1 ; // div by 2
611 Zd ppp(pp[l],k,hb);
612
613 if ( ppp.interseg(plus,hb,h) )//(INTER_SEG(iii,iii+hb,iplus-h,iplus+h) && INTER_SEG(jjj,jjj+hb,jplus-h,jplus+h))
614 {
615 pb[++l]= b;
616 pi[l]= b->n>0 ?(int) b->n : N ;
617 pp[l]=ppp;
618 }
619 else
620 b=b0, hb <<=1 ;
621 }
622 else
623 b=b0;
624 }
625 }
626 hb <<= 1; // mul by 2
627 } while (l--);
628 if (!vn && b != root )
629 {// cas particulier on repart du sommet on avais rien trouver
630 b=root;
631 hb = MaxISize;
632 p0=Zd();
633 l=0;
634 pb[0]= b;
635 pi[0]= b->n>0 ?(int) b->n : N ;
636 pp[0]=Zd();
637
638 goto L1;
639 }
640 return vn;
641 }
642
643
644
645
646 // static int kfind=0;
647 // static int kthrough=0;
648
CoorBary(const Triangle2 & K,const R2 & P,R * l)649 inline void CoorBary(const Triangle2 & K,const R2 & P, R *l)
650 {
651 R detK = 2.*K.mesure() ;
652 l[1]=det(K[0],P,K[2])/detK;
653 l[2]=det(K[0],K[1],P)/detK;
654 l[0]=1-l[1]-l[2];
655 }
656
657
658
CoorBary(const Tet & K,const R3 & P,R * l)659 inline void CoorBary(const Tet & K,const R3 & P, R *l)
660 {
661 R detK = 6.*K.mesure() ;
662 l[1]=det(K[0],P,K[2],K[3])/detK;
663 l[2]=det(K[0],K[1],P,K[3])/detK;
664 l[3]=det(K[0],K[1],K[2],P)/detK;
665 l[0]=1-l[1]-l[2]-l[3];
666 }
667
668
CoorBaryPos(const Triangle2 & K,const R2 & P,R * l)669 inline int CoorBaryPos(const Triangle2 & K,const R2 & P, R *l)
670 {
671 CoorBary(K,P,l);
672 int n=0,m=-1;
673 int nl[Tet::nv+1];
674 R eps = -1e-10;
675 for(int i=0;i<Triangle2::nv;++i)// correction FH 2/04/20
676 if( l[i] < eps){
677 nl[n++]=i;
678 }
679 else m = i;
680 if( m>=0)
681 { // proj P on face .. m
682 int i0=Triangle2::nvadj[m][0];
683 int i1=Triangle2::nvadj[m][1];
684 R2 A(K[i0]),B(K[i1]);
685 R2 AB(A,B),AP(A,P);
686 double l2= AB.norme2();
687 // L = (AB,AP)/l2
688
689 l[i0] = max(0.,min(1.,(AB,AP)/l2));
690 l[i1] = 1-l[i0];
691 l[m]=0;
692 }
693 return n;
694
695
696
697 }
CoorBaryPos(const Tet & K,const R3 & P,R * l)698 inline int CoorBaryPos(const Tet & K,const R3 & P, R *l)
699 {
700 CoorBary(K,P,l);
701 int n=0,m=-1;
702 int nl[Tet::nv+1];
703 R eps = -1e-10;
704 for(int i=0;i<Tet::nv;++i)// correction FH 2/04/20
705 if( l[i] < eps){
706 nl[n++]=i;
707 }
708 else m = i;
709 if( m>=0)
710 { // proj P on face .. m
711 int i0=Tet::nvadj[m][0];
712 int i1=Tet::nvadj[m][1];
713 int i2=Tet::nvadj[m][2];
714 R3 A(K[i0]),B(K[i1]),C(K[i2]);
715 R3 AB(A,B), AC(A,C);
716 R3 N = AB^AC ;
717 double N2 = (N,N);
718 l[i0] = max(0.,min(1.,det(P,B,C,N)/N2));
719 l[i1] = max(0.,min(1.,det(A,P,C,N)/N2));
720 l[i2] = 1-l[i0]-l[i1];
721 l[m]=0;
722 }
723 return n;
724
725
726
727 }
728
729 // inline int nRand(int n) {
730 // return rand()%n; //avant random()%n;
731 // }
732
find5(int i,int * k,int l)733 inline int find5(int i,int *k,int l)
734 {
735 if(l<5)
736 {
737 for(int j=0;j<l;++j)
738 if(i==k[j]) return j;
739 }
740 else if(i>=k[0] && i<=k[l-1])
741 {
742 int i0=0,i1=l-1;
743 while(i0<=i1)
744 { int im=(i0+i1)/2;
745 if(i<k[im]) i1=im-1;
746 else
747 if(i>k[im]) i0=im+1;
748 else
749 if(i==k[im]){
750 return im;
751 }
752 }
753 }
754 return -1;
755 }
756
757 template<class Mesh>
Find(const Mesh & Th,GTree<typename Mesh::Vertex> * quadtree,typename Mesh::Rd P,typename Mesh::RdHat & Phat,bool & outside,const typename Mesh::Element * tstart)758 const typename Mesh::Element * Find(const Mesh & Th,
759 GTree<typename Mesh::Vertex> *quadtree,
760 typename Mesh::Rd P,
761 typename Mesh::RdHat & Phat,
762 bool & outside,
763 const typename Mesh::Element * tstart)
764 {
765 typedef typename Mesh::Element Element;
766 typedef typename Mesh::Vertex Vertex;
767 typedef typename Mesh::Rd Rd;
768 const int nkv=Element::nv;
769 const int d=Rd::d;
770 R dP=DBL_MAX, nddd=0;
771 Rd PPhat,Delta;
772 int k=0;
773 const int nReStartMax = 1 << Rd::d; // Nb vertex of the d-cube/
774 int nReStart = 0;
775 int itstart[100],itout[100],kstart=0;
776 Rd Pout[100];
777 int it,j,it00;
778 const int mxbord=1000;
779 int kbord[mxbord+1];
780 int nbord=0;
781
782 if(searchMethod>1) goto PICHON;
783 if ( tstart )
784 it00=it = Th(tstart);
785 else if(quadtree)
786 {
787 const Vertex * v=quadtree->NearestVertex(P);// Change Juil 2017 ..
788 /*
789 if (!v)
790 {
791 v=quadtree->NearestVertex(P);
792 assert(v);
793 }*/
794 if( !v) {
795 verbosity = 10000000;
796 cout << "Bug search " << P << endl;
797 v=quadtree->NearestVertex(P);
798 }
799 ffassert(v); // bug !!!!
800 it00=it=Th.Contening(v);
801 nddd= Norme2(P-*v);
802 if(verbosity>200)
803 cout << " Find: Close : "<< *v << " , " << Th(v) << " dist " << nddd << endl;
804
805 }
806 else ffassert(0);
807 RESTART:
808 ffassert(kstart<100);
809 itstart[kstart++]=it;
810 if(verbosity>199)
811 cout << " " << nReStart << " tstart= " << tstart << " , it=" << it << " P="<< P << " " << kstart << endl;
812 outside=true;
813 Mesh::kfind++;
814 while (1)
815 {
816 //if(verbosity>199) cout << "it " << it <<endl;
817 const Element & K(Th[it]);
818 Mesh::kthrough++;
819 assert(k++<1000);
820 int kk,n=0,nl[nkv];
821 R l[nkv];
822 for(int iii=0; iii<nkv; iii++)
823 l[iii]=0.;
824
825 CoorBary(K,P,l);
826
827 // CoorBary :: donner entre 0 et 1
828 // Pb si K.mesure*1.e-10 precision machine ==> bug
829
830 // avant:
831 // R eps = -K.mesure()*1e-10;
832 R eps = -1e-10;
833 for(int i=0;i<nkv;++i)
834 if( l[i] < eps){
835 nl[n++]=i;
836 }
837 if(verbosity>200){
838 cout << " tet it=" << it ;
839 cout << " K.mesure=" << K.mesure() ;
840 cout << " eps=" << eps << " : " ;
841 for(int i=0;i<nkv;++i)
842 cout<< " l["<< i <<"]=" << l[i] ;
843 cout << " n=" << n << endl;
844 }
845
846 if (n==0)
847 { // interior => return
848 outside=false;
849 Phat=Rd(l +1);
850 // cout << Phat <<endl;
851 #ifndef NDEBUG
852 Rd pp=K(Phat)-P;
853 if( pp.norme()>1e-5)
854 {
855 cout << " Bug find P " << P << " ==" << K(Phat) ;
856 cout << "Phat== " << Phat << " diff= " << pp << endl;
857 assert(0);
858 }
859
860 #endif
861 return &K;
862 }
863
864
865
866 kk=n==1 ? 0 : randwalk(n);
867 j= nl[ kk ];
868 int itt = Th.ElementAdj(it,j);
869 if(itt!=it && itt >=0)
870 {
871 dP=DBL_MAX;
872 it=itt;
873 continue;
874 }
875 int inkbord=find5(it,kbord,nbord);
876 if(inkbord<0 && nbord < mxbord)
877 {
878 kbord[nbord++]=it;
879 if(nbord>=5) HeapSort(kbord,nbord);
880 }
881 if(verbosity>1001)
882 {
883 cout << " bord "<< it<< " nbf < 0 : " <<n << " (inb) " << inkbord << " nfb" << nbord<<endl;
884 R ss=0;
885 for(int i=0;i<nkv;++i)
886 { ss += l[i];
887 cout << l[i] << " ";}
888 cout << " s=" << ss << endl;;
889
890 }
891
892 if(verbosity>2000)
893 cout << " GQuadTree::value of n " << n << endl;
894
895 if ( n!=1 ) // on est sur le bord, mais plusieurs face <0 => on test les autre
896 { // 1) existe t'il un adj interne
897 int nn=0,ii;
898 int nadj[d+1],ni[d+1];
899 for(int i=0;i<nkv;++i)
900 //avant :: if (l[i] < eps && (itt=Th.ElementAdj(it,ii=i)) != it && itt && find5(itt,kbord,nbord) < -1 )
901 if (l[i] < eps && (itt=Th.ElementAdj(it,ii=i)) != it && (itt>=0) && find5(itt,kbord,nbord) < 0 )
902 ni[nn++]=i,nadj[i]=itt;
903 if(verbosity>1000)
904 cout << " nn : "<< nn << endl;
905 if (nn>0)
906 {
907 //j=nadj[nRand(nn)];
908 j=ni[randwalk(nn)];
909 it=nadj[j];
910 dP=DBL_MAX;
911 //cout << "new it= " << it << endl;
912 continue;
913 }
914 }
915 // toutes les faces <0 sont sur le bord.
916 // pour l'instant on s'ar�te
917 // le point est externe. (mais trop cher pour faire mieux)
918 // on projet le points sur le bord via le coordonne
919 // barycentrique
920 { // a ridge on border (to hard to do the correct stuff)
921 // or a corner just do the projection on lambda
922 R s=0.;
923 for(int i=0;i<nkv;++i)
924 s += (l[i]= max(l[i],0.));
925 for(int i=0;i<nkv;++i)
926 l[i]/=s;
927 Phat=Rd(l +1);
928 if(verbosity>1000)
929 {
930 cout << " "<< P << " " << n << " l: ";
931 R ss=0;
932 for(int i=0;i<nkv;++i)
933 { ss += l[i];
934 cout << l[i] << " ";}
935 cout << " s=" << ss <<" " << s <<" exit by bord " << it << " " << Phat << endl;;
936 }
937 outside=true;
938 // New Method mars 2020
939 if(1) // searchMethod==0)
940 {
941 GenericDataFindBoundary<Mesh> * gdfb=Th.Buildgdfb();
942 if(gdfb )
943 {
944 double l[4],lK[4];
945 int na[4],nna=0,mma=0;
946 int loutside;// of border ??? 0 inside, 1 out close, 2, out fare, , -1 inside
947 int itt =gdfb->Find(P,l,loutside);
948 // warning l is projecton on boundary ????
949 CoorBary(Th[itt],P,lK);
950 const double eps = 1e-10;
951 for (int i=0; i<= Rd::d;++i )
952 if(lK[i] < -eps) na[nna++]=i;
953 for( int k=0; k<nna;++k)
954 {
955 int ii=na[k],ittt=Th.ElementAdj(itt,ii);
956 if( ittt<0 || ittt == itt) mma++;
957 }
958 it = itt;
959
960 if( nna) //
961 {
962 if( nReStart++ < 1) goto RESTART;
963 else
964 if(searchMethod) goto PICHON;
965 Phat=Rd(l+1);
966 }
967 else // in itt
968 Phat=Rd(lK+1);// correction FH 1/0>4/2020
969 outside=nna>0;
970
971 const Element &K=Th[it];
972 if( verbosity > 9)
973 cout << " - Find "<< P << " -> " << K(Phat) << " " << loutside << " k= " << itt
974 << " dist =" << (P-K(Phat)).norme() << " :: " << Phat << " nddd= " << nddd <<" " << nReStart << " " << nna << endl;
975 return Th.elements + it; // outside
976
977 }
978 }
979 // end New Methode
980 // on retest with a other stating point??????
981 // Mod. 23/02/2016 F. H
982 itout[kstart-1]= it;
983 Pout[kstart-1]=Phat;
984 while(nReStart++ < nReStartMax)
985 {// Loop on the vertex of a d-hyper-cude
986 {
987 int k= nReStart-1;
988 int i = (nReStart-2)/2;
989
990 for(int i=0; i<Rd::d; ++i)
991 Delta[i]= ((1<<i) & k) ? nddd : -nddd;
992 }
993 if( verbosity>199) {
994 Rd pp=Th[it](Phat)-P;
995 cout << " restart find P " << P << " !=" <<Th[it](Phat) << "\t" ;
996 cout << "Phat== " << Phat << " diff= " << pp << " it = " << it << endl;
997 }
998
999 Rd PP= P + Delta;
1000 Vertex* v=quadtree->NearestVertex(PP);
1001 if( nddd ==0) nddd= Norme2(P-*v);
1002 it=Th.Contening(v);
1003 bool same=false;
1004
1005 for(int j=0;j<kstart ; ++j)
1006 if( it == itstart[j]) {same=true; break;}
1007 if( verbosity>199)
1008 cout << " loop Search "<<nReStart << P << " Delta" << Delta << " it " << it << " same "<< same << endl;
1009 if(same) continue;
1010 if(Rd::d==2) npichon2d1++;
1011 if(Rd::d==3) npichon3d1++;
1012
1013 goto RESTART;
1014 }
1015
1016 if(searchMethod) goto PICHON;
1017 // Search best out ....
1018 int ko=0,k=0;
1019 double dmin=Norme2_2(P-Th[itout[k]](Pout[k]));
1020 for(int k=1; k<kstart; ++k)
1021 {
1022 double dk=Norme2_2(P-Th[itout[k]](Pout[k]));
1023 if( dk< dmin) { dmin=dk; ko=k;}
1024 }
1025 Phat=Pout[ko];
1026 it=itout[ko];
1027 if( verbosity>149)
1028 cout << " -- Find(out) " << it << " Outside " << outside << " it "<< it << " err = "<< sqrt(dmin) << "/" << Phat << " " << nddd << endl;
1029 return &Th[it] ;
1030 }
1031 }
1032
1033 PICHON:
1034 {
1035 if(Rd::d==2) npichon2d++;
1036 if(Rd::d==3) npichon3d++;
1037
1038 /*==============================PICHON=================*/
1039 // Brute force ** */
1040 R l[4], eps = -1e-6; //pichon
1041 double dist_baryC = 0.0, min_dist_baryC = 1e31;
1042 long closestTet = -1;
1043 l[0]=l[1]=l[2]=l[3]=1; // for d < 3
1044 for(int tet=0;tet<Th.nt;tet++) // nkv=4
1045 {
1046 const Element & K(Th[tet]);
1047 CoorBary(K,P,l);
1048
1049 // measure dist by sum ( |lambda_i| )
1050 dist_baryC = 0.0;
1051 for(int i=0; i<nkv; i++)
1052 dist_baryC += abs(l[i]);
1053
1054 // define closest Tetrahedron !!! TO VERIFY THE HYPOTHESE !!!
1055 if( dist_baryC < min_dist_baryC )
1056 {
1057 min_dist_baryC = dist_baryC;
1058 closestTet = tet;
1059 }
1060
1061
1062 if ( (l[0] >= eps) && (l[1] >= eps) && (l[2] >= eps) && (l[3] >= eps) )
1063 {
1064 if( verbosity>199)
1065 {
1066 cout << " #### brute force " << tet << " "<< endl;
1067 }
1068 outside=false;
1069 Phat=Rd(l+1);
1070 return &K;
1071 }
1072 }
1073
1074 const Element & K(Th[closestTet]);
1075 outside=true;
1076 CoorBaryPos(K,P,l);
1077
1078 Phat=Rd(l+1);
1079 if(verbosity>2)
1080 cout << " --vertex:" << P << " NOT in DOMAIN. use closestTet. Phat:" << Phat << endl;
1081 return &K;
1082 }
1083 /*==============================PICHON=================*/
1084 }
1085
1086
1087 // Instantiation manuel des templates
1088
1089 template class GTree<Vertex2>;
1090 template class GTree<Vertex3>;
1091 template class GTree<Vertex1>;
1092
1093 ///typedef MeshS::GMesh GMeshS;
1094 typedef Mesh3::GMesh GMesh3;
1095 typedef Mesh2::GMesh GMesh2;
1096 typedef Mesh1::GMesh GMesh1;
1097
1098
1099 /*template
1100 const GMeshS::Element * Find<GMeshS>(const GMeshS & Th,
1101 GTree< GMeshS::Vertex> *quadtree,
1102 GMeshS::Rd P,
1103 GMeshS::RdHat & Phat,
1104 bool & outside,
1105 const GMeshS::Element * tstart);
1106 */
1107
1108 template
1109 const GMesh3::Element * Find<GMesh3>(const GMesh3 & Th,
1110 GTree< GMesh3::Vertex> *quadtree,
1111 GMesh3::Rd P,
1112 GMesh3::RdHat & Phat,
1113 bool & outside,
1114 const GMesh3::Element * tstart);
1115 template
1116 const GMesh2::Element * Find<GMesh2>(const GMesh2 & Th,
1117 GTree< GMesh2::Vertex> *quadtree,
1118 GMesh2::Rd P,
1119 GMesh2::RdHat & Phat,
1120 bool & outside,
1121 const GMesh2::Element * tstart);
1122
1123 /*
1124 template
1125 const GMesh1::Element * Find<GMesh1>(const GMesh1 & Th,
1126 GTree< GMesh1::Vertex> *quadtree,
1127 GMesh1::Rd P,
1128 GMesh1::RdHat & Phat,
1129 bool & outside,
1130 const GMesh1::Element * tstart);
1131 */
1132 }
1133
1134 template<typename Mesh>
~GenericDataFindBoundary()1135 GenericDataFindBoundary<Mesh>::~GenericDataFindBoundary()
1136 {
1137 if(verbosity>1) cout << "\n -- ~GenericDataFindBoundary: Nb find " << nbfind << " nb element "
1138 << nbelement << " ratio " << (double) nbelement/std::max(nbfind,1L)
1139 << " mpirank " << mpirank << endl;
1140 delete tree;
1141 }
1142 template<typename Mesh>
gnuplot(const string & fn)1143 void GenericDataFindBoundary<Mesh>::gnuplot(const string & fn)
1144 { // for debugging ..
1145 if(dHat < 3)
1146 {
1147 ofstream gp(fn.c_str());
1148 ffassert(gp);
1149
1150 const Mesh &Th = *pTh;
1151 if( bborder )
1152 for(int be=0; be<Th.nbe; ++be)
1153 {
1154 const BorderElement &B=Th.be(be);
1155 int e,k = Th.BoundaryElement(be,e);
1156 {
1157 int ee=e, kk= Th.ElementAdj(k,ee);
1158 if ( kk >=0 || k != kk)
1159 {
1160 for(int j=0; j< BorderElement::nv;++j)
1161 gp << (Rd) B[j] << endl;
1162 gp << "\n\n";
1163 }
1164 }
1165 }
1166 else
1167 for(int k=0; k<Th.nt; ++k)
1168 {
1169 const Element &K=Th[k];
1170 for(int j=0; j<= Element::nv;++j)
1171 gp << (Rd) K[j%Element::nv] << endl;
1172 gp << "\n\n";
1173 }
1174 if( dHat==2)
1175 {
1176 ofstream gp((fn+"c").c_str());
1177 for(int i=0; i<P.N(); ++i)
1178 {
1179 int N=100;
1180 const double pi=3.14159265358979323846264338327950288 ;
1181 double dt = pi*2./N, r = delta[i];
1182 for(int j=0;j<=N; ++j)
1183 {
1184 Fem2D::R3 Q=P[i];
1185 double x = Q.x+r*cos(dt*j);
1186 double y= Q.y+r*sin(dt*j);
1187 double z= Q.z;
1188 gp << x << " " << y << " " << z << endl;
1189 }
1190 gp << "\n\n";
1191 }}
1192 }
1193 }
Plambla(int dhat,const Fem2D::R2 * Q,Fem2D::R2 & P,double * l)1194 inline void Plambla(int dhat,const Fem2D::R2 *Q,Fem2D::R2 &P,double *l)
1195 {
1196 ffassert(0); // to do
1197 }
Plambla(int dhat,const Fem2D::R1 * Q,Fem2D::R1 & P,double * l)1198 inline void Plambla(int dhat,const Fem2D::R1 *Q,Fem2D::R1 &P,double *l)
1199 {
1200 ffassert(0); // to do
1201 }
1202
Plambla(int dhat,const Fem2D::R3 * Q,Fem2D::R3 & P,double * l)1203 inline void Plambla(int dhat,const Fem2D::R3 *Q,Fem2D::R3 &P,double *l)
1204 {
1205 using Fem2D::R3 ;
1206 if(dhat==1)
1207 {
1208 R3 AB(Q[0],Q[1]), AP(Q[0],P);
1209 l[1] = (AP,AB)/(AB,AB);
1210 l[0] = 1- l[1];
1211 }
1212 else if(dhat==2)
1213 {
1214 R3 AB(Q[0],Q[1]), AC(Q[0],Q[2]);
1215 R3 N = AB^AC ;
1216 double N2 = (N,N);
1217 l[0] = det(Q[1]-P ,Q[2]-P ,N)/N2;
1218 l[1] = det(P -Q[0],Q[2]-Q[0],N)/N2;
1219 l[2] = 1-l[0]-l[1];
1220 }
1221 else if( dhat==3)
1222 {
1223 double d=det(Q[0],Q[1],Q[2],Q[3]);
1224 l[0]=det(P,Q[1],Q[2],Q[3])/d;
1225 l[1]=det(Q[0],P,Q[2],Q[3])/d;
1226 l[2]=det(Q[0],Q[1],P,Q[3])/d;
1227 l[3]= 1- l[0]-l[1]-l[2];
1228 }
1229 else assert(0);
1230 }
dist2(int dhat,const Fem2D::R2 * Q,Fem2D::R2 & P,double * l,double * dl)1231 inline double dist2(int dhat,const Fem2D::R2 *Q,Fem2D::R2 &P,double *l,double *dl)
1232 {
1233 ffassert(0);
1234 return 0;
1235 }
dist2(int dhat,const Fem2D::R1 * Q,Fem2D::R1 & P,double * l,double * dl)1236 inline double dist2(int dhat,const Fem2D::R1 *Q,Fem2D::R1 &P,double *l,double *dl)
1237 {
1238 ffassert(0);
1239 return 0;
1240
1241 }
dist2(int dhat,const Fem2D::R3 * Q,Fem2D::R3 & P,double * l,double * dl)1242 inline double dist2(int dhat,const Fem2D::R3 *Q,Fem2D::R3 &P,double *l,double *dl)
1243 {
1244 using Fem2D::R3 ;
1245 double d2=0;
1246 if(dhat==1)
1247 {
1248 dl[0]=min(1.,max(l[0],0.));
1249 dl[1]=1.-dl[0];
1250 R3 Pj = dl[0]*Q[0]+dl[1]*Q[1];
1251 d2 = R3(Pj,P).norme2();
1252
1253 }
1254 else if(dhat==2)
1255 {
1256 int i=-1;
1257 if(l[0]<0) i=0;
1258 else if(l[1]<0) i=1;
1259 else if(l[2]<0) i=2;
1260
1261 if( i>=0)
1262 {
1263 // project on edge i
1264 int i1= (i+1)%3,i2=(i+2)%3;
1265 R3 AB(Q[i1],Q[i2]), AP(Q[i1],P);
1266 dl[i2] = (AP,AB)/(AB,AB);
1267 dl[i2]=min(1.,max(dl[i2],0.));// proj on AB ..
1268 dl[i1] = 1- dl[i2];
1269 dl[i]=0;
1270 }
1271 else
1272 {
1273 dl[0]=l[0];
1274 dl[1]=l[1];
1275 dl[2]=l[2];
1276 }
1277 R3 Pj = dl[0]*Q[0]+dl[1]*Q[1]+dl[2]*Q[2];
1278 d2 = R3(Pj,P).norme2();
1279
1280 }
1281 else if( dhat==3)
1282 {
1283 ffassert(0); // to do FH... non ...
1284 }
1285 else assert(0);
1286 return d2;
1287 }
1288 template<typename Mesh>
Find(typename Mesh::Rd PP,double * l,int & outside) const1289 int GenericDataFindBoundary<Mesh>::Find(typename Mesh::Rd PP,double *l,int & outside) const
1290 { // FH: outside : 0 inside, 1 out close, 2, out fare, , -1 inside
1291 // warning l
1292 nbfind++;
1293 typedef double R;
1294 int nu=-1,ne=-1;
1295 R dnu= 1e200,deps=0;
1296 R dl[dHat+1];
1297 outside = 0;
1298 Vertex *p0= &P[0];
1299 Vertex *p =tree->TrueNearestVertex(PP);
1300 int ip = p-P;
1301 Rd Q[dHat+1];
1302 R ll[dHat+1],lpj[dHat+1];
1303 //#define DEBUGGING
1304 #ifdef DEBUGGING
1305 int err=0;
1306 double dispp=Rd(PP,*p).norme();
1307 for(int i=0; i<P.N(); ++i)
1308 {
1309 double l=Rd(PP,P[i]).norme();
1310 if( l < dispp*(1-1e-14) ) err++,cout << " bug "<< l << " " << dispp << " i "<< i << " ip" << ip << " " <<dispp-l << endl;
1311 }
1312 if(err)
1313 {// relay for debug
1314 p =tree->TrueNearestVertex(PP);
1315 ffassert(err==0);
1316 }
1317 #endif
1318 Vertex **plp = &lp[0];
1319 long lvp=tree->ListNearestVertex(plp,lp.N(), delta[ip],P[ip]);
1320 HeapSort(plp,lvp );
1321 // verif ListNearestVertex
1322
1323 #ifdef DEBUGGING
1324 {
1325 int err=0;
1326 set<int> lst;
1327 int nnnm=0,nnnp=0;
1328 double unp =(1+1e-14),unm=(1-1e-14);
1329 for(int j=0; j<lvp; ++j)
1330 lst.insert(lp[j]-p0);
1331 for(int i=0; i<P.N(); ++i)
1332 {
1333 double l=Rd(P[ip],P[i]).norme();
1334 if (l < delta[ip]*unp) nnnp++;
1335 if (l < delta[ip]*unm) nnnm++;
1336
1337 if ( (l < delta[ip]*unp) && (lst.find(i) == lst.end() ))
1338 {
1339 err++;
1340 cout << " bug "<< i << " not in set " << endl;
1341 }
1342 }
1343 ffassert(lvp<=nnnp && lvp >= nnnm);
1344 ffassert(err==0);
1345 }
1346 #endif
1347 if( verbosity>19)
1348 {
1349 cout << ip << " , " << delta[ip] << " | " << lvp << " : ";
1350 for(int j=0; j<lvp; ++j)
1351 {
1352 int i = lp[j]-p0;
1353 int k = lp[j]->lab/Element::ne;
1354 int e = lp[j]->lab%Element::ne;
1355 cout << " "<< k << " ";
1356 // ffassert(i == k || );
1357 }
1358 cout << endl;
1359 }
1360 for(int j=0; j<lvp; ++j)
1361 {
1362 nbelement++;
1363 int k = lp[j]->lab/Element::ne;
1364 int e = lp[j]->lab%Element::ne;
1365 if(debug) cout << " -- k = "<< k << " " << e << " " << j << endl;
1366
1367 const Element & K=(*pTh)[k];
1368 int I[4]={0,1,2,3};
1369 int nI =dHat+1;
1370 if( bborder)
1371 { // take just of part of Element ..
1372 nI= Element::nva;
1373 ffassert(nI==BorderElement::RdHat::d+1);
1374 for(int i=0; i< nI;++i)
1375 I[i]=Element::nvadj[e][i];
1376 }
1377 for(int i=0; i< nI;++i)
1378 Q[i]=K[I[i]];
1379
1380 Plambla(nI-1,Q,PP,ll);
1381 R d2 = dist2(nI-1,Q,PP,ll,lpj);
1382 if( dnu > d2)
1383 {
1384 deps = K.mesure()/100.;
1385 nu = k;
1386 ne= e;
1387 dnu=d2;
1388
1389
1390 if( bborder)
1391 { //
1392 for(int i=0;i<=dHat;++i)
1393 dl[i]=0;
1394 for(int i=0;i<nI;++i)
1395 dl[I[i]]=lpj[i];
1396 }
1397 else
1398 {
1399 for(int i=0;i<=dHat;++i)
1400 dl[i]=lpj[i];
1401 }
1402 }
1403 if(verbosity>99) cout << " Find " << k << " " << e << " / " << dnu << endl;
1404 }
1405 ffassert(nu>=0);
1406 outside = dnu < deps ? 0
1407 : ((dnu< delta[ip]*delta[ip])? 1: 2) ;// 0 in, 1 near, 2 fare ..BofBof ..
1408 for(int i=0; i<= dHat;++i)
1409 l[i]=dl[i];
1410
1411 if(debug) cout << " -- out nu "<< nu << " "<< ne << " , " << dnu <<" d_i " << delta[ip] << " : "
1412 << l[1] << " " << l[2] << " "<< outside<< endl;
1413 return nu;
1414 }
1415 template<typename Mesh>
TrueBorder(const Mesh & Th,typename Mesh::Vertex * P,double * delta)1416 int TrueBorder(const Mesh &Th,typename Mesh::Vertex *P,double *delta)
1417 {
1418 typedef typename Mesh::Vertex Vertex;
1419
1420 typedef typename Mesh::Element Element;
1421 typedef typename Mesh::BorderElement BorderElement;
1422 typedef typename Mesh::Rd Rd;
1423 typedef typename Mesh::BorderElement::RdHat RdHat;
1424 static const int d = Rd::d;
1425 static const int dHat = RdHat::d;
1426
1427 int nv =0;
1428 RdHat GHat(RdHat::diag(1./(dHat+1)));
1429
1430 for(int be=0; be<Th.nbe; ++be)
1431 {
1432 const BorderElement &E=Th.be(be);
1433 int e,k = Th.BoundaryElement(be,e);
1434 {
1435 int ee=e, kk= Th.ElementAdj(k,ee);
1436 if ( kk >=0 || k != kk)
1437 {
1438 E(GHat);
1439 Rd G(E(GHat));
1440 double l = 0;// 1.5 to be sure .. FH
1441 for(int i=0; i< BorderElement::nv ;++i)
1442 l = max(l, Rd(G,E[i]).norme2()) ;
1443 delta[nv]=l;
1444 P[nv].lab= Element::ne*k+e;// element and edge
1445 (Rd &) P[nv++]=G;
1446
1447
1448 }
1449 }
1450 }
1451 return nv;
1452 }
1453
1454 template<typename Mesh>
GenericDataFindBoundary(Mesh const * _pTh,int ddebug)1455 GenericDataFindBoundary<Mesh>::GenericDataFindBoundary(Mesh const * _pTh,int ddebug)
1456 : pTh(_pTh),tree(0),nbfind(0), nbelement(0), P(bborder ? pTh->nbe: pTh->nt),delta(P.N()),lp(0),debug(ddebug)
1457 {
1458 //cout << " enter in GenericDataFindBoundary"<< endl;
1459 const int nvE= Element::nv;
1460 const int nvB = BorderElement::nv;
1461 const int nvK = bborder ? nvB : nvE;
1462 const Mesh &Th = *pTh;
1463 // extract true Border if d != dHat
1464 // othesize keep all mesh
1465 RdHat GHat(RdHat::diag(1./(dHat+1)));
1466 long ncount=0, mcount=0;
1467 int nv =0;
1468 // warning in case of meshL , bord is points => code bborder stupide..
1469 if(bborder)
1470 nv = TrueBorder(Th,(Vertex *)P,delta);
1471 else
1472 { //
1473 for(int k=0; k<Th.nt; ++k)
1474 {
1475 const Element& K= Th[k];
1476 {
1477 Rd G(K(GHat));
1478 double l = 0;// 1.5 to be sur .. FH
1479 for(int i=0; i< Element::nv ;++i)
1480 l = max(l, Rd(G,K[i]).norme2()) ;
1481 delta[nv]=l;
1482 P[nv].lab= Element::ne*k;// element and edge
1483 (Rd &) P[nv++]=G;
1484
1485
1486 }
1487 }
1488 }
1489
1490 //P.resize(nv); no resize because no copy of vertices ...
1491 delta.resize(nv);
1492 lp.resize(nv);
1493 if(debug>7) gnuplot("dfb0.gp");
1494 Vertex * P0= &P[0];
1495 KN<double> d0(delta);
1496 for(int i=0; i< nv;++i)
1497 d0[i]=sqrt(d0[i]);
1498 delta= 0.;
1499 Rd Pn, Px;
1500 Th.BoundingBox(Pn, Px);
1501 double col=0;
1502 tree=new EF23::GTree<Vertex> (&P[0], Pn, Px,nv);// build quadtre
1503 int lvpx=0;
1504 // cout << " next step in GenericDataFindBoundary"<< endl;
1505 long NbQuadTreeBoxSearch=tree->NbQuadTreeBoxSearch,NbVerticesSearch=tree->NbVerticesSearch;
1506 for(int i=0;i<nv; ++i)
1507 {
1508 ncount++;
1509 if(debug>9) cout << i << " " << d0[i] << endl;
1510 long nbsg=tree->NbQuadTreeBoxSearch,nvsg=tree->NbVerticesSearch;
1511 int lvp=tree->ListNearestVertex(lp,nv, d0[i]*2.1,P[i]);
1512 if(debug>9) cout <<i << " " << tree->NbVerticesSearch-nvsg << " " << tree->NbQuadTreeBoxSearch-nbsg << " / " << lvp << " " << d0[i] << endl;
1513 lvpx=max(lvp,lvpx);
1514 for(int j=0,k; j<lvp; ++j)
1515 {
1516 mcount++;
1517 k= lp[j]-P0;
1518 // double dij = Rd(P[i],*lp[j]).norme();
1519 // warning must be Proof FH.
1520 delta[k]=max(delta[k],d0[i]+d0[k]);
1521 delta[i]=max(delta[i],d0[i]+d0[k]);
1522
1523 if(debug>9) cout << k << " "<< delta[k] << ", ";
1524 }
1525 }
1526 if(debug>9)
1527 for(int i=0;i<nv; ++i)
1528 cout << i << " " << d0[i] << " " <<delta[i] << endl;
1529 if(debug>5)
1530 gnuplot("dfb1.gp");
1531 if(verbosity>4)
1532 {
1533 cout << " ** BuildDataFindBoundary " << nv << " " << delta.max() << " " << delta.sum()/nv << " " << delta.min() << endl;
1534 cout << " count "<< mcount << " / " <<ncount<< " ratio "
1535 << (double) mcount / max(ncount,1L) << " max lvp " << lvpx
1536 << " gtree search box " << tree->NbQuadTreeBoxSearch - NbQuadTreeBoxSearch
1537 << " v " << tree->NbVerticesSearch-NbVerticesSearch << endl;
1538 }
1539 }
1540 // Bof Bof pas sur du tout.
1541 /*
1542 template<typename Mesh>
1543 void BuildDataFindBoundary<Mesh>() const
1544 {
1545 static int count =0;
1546 if( dfb ==0) {
1547 dfb=new DataFindBoundary(this);//,count++==0?9:0);
1548 dfb->debug=0;
1549 }
1550
1551 }
1552 */
1553 template class GenericDataFindBoundary<Fem2D::MeshS::GMesh>;
1554 template class GenericDataFindBoundary<Fem2D::Mesh3::GMesh>;
1555 template class GenericDataFindBoundary<Fem2D::Mesh2::GMesh>;
1556 template class GenericDataFindBoundary<Fem2D::Mesh1::GMesh>;
1557 template class GenericDataFindBoundary<Fem2D::MeshL::GMesh>;
1558