1 // -*- Mode : c++ -*-
2 //
3 // SUMMARY :
4 // USAGE :
5 // ORG :
6 // AUTHOR : Frederic Hecht
7 // E-MAIL : hecht@ann.jussieu.fr
8 //
9
10 /*
11
12 This file is part of Freefem++
13
14 Freefem++ is free software; you can redistribute it and/or modify
15 it under the terms of the GNU Lesser General Public License as published by
16 the Free Software Foundation; either version 2.1 of the License, or
17 (at your option) any later version.
18
19 Freefem++ is distributed in the hope that it will be useful,
20 but WITHOUT ANY WARRANTY; without even the implied warranty of
21 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
22 GNU Lesser General Public License for more details.
23
24 You should have received a copy of the GNU Lesser General Public License
25 along with Freefem++; if not, write to the Free Software
26 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
27 */
28 // E-MAIL : Frederic.Hecht@Inria.fr
29 //
30 // ORIG-DATE: Dec 97
31 #include <cmath>
32 #include <cstdlib>
33 #include "error.hpp"
34 #include <iostream>
35
36 #include <limits.h>
37 #include <cstring>
38 #include <cstdlib>
39 using namespace std;
40
41 #include "RNM.hpp"
42 #include "rgraph.hpp"
43
44 #include "fem.hpp"
45 using namespace Fem2D;
46
47 #ifndef NEWQUADTREE
48
49 // new version ----------
50 // ----------------------
51 #ifdef DRAWING
PlotQuad(I2 pp,long hb)52 void FQuadTree::PlotQuad(I2 pp,long hb)
53 {
54 IMoveTo(pp.x,pp.y);
55 ILineTo(pp.x+hb,pp.y);
56 ILineTo(pp.x+hb,pp.y+hb);
57 ILineTo(pp.x ,pp.y+hb);
58 ILineTo(pp.x ,pp.y);
59 }
60
PlotX(I2 p,long hb)61 void FQuadTree::PlotX(I2 p,long hb)
62 {
63 IMoveTo(p.x, p.y);
64 ILineTo(p.x+hb/2,p.y+hb/2);
65 IMoveTo(p.x+hb/2,p.y);
66 ILineTo(p.x ,p.y+hb/2);
67 }
Draw()68 void FQuadTree::Draw()
69 {
70 QuadTreeBox * pb[ MaxDeep ];
71 int pi[ MaxDeep ];
72 I2 pp[MaxDeep];
73 int l=0; // level
74 QuadTreeBox * b;
75 IntQuad hb = MaxISize;
76 if (!root) return ;
77 pb[0]= root;
78 pi[0]= root->n>0 ?(int) root->n : 4 ;;
79 pp[0].x=pp[0].y=0;//ii[0]=jj[0]=0;
80 do{
81 b= pb[l];
82
83 while (pi[l]--)
84 {
85 if (b->n>0) // Vertex QuadTreeBox none empty
86 {
87 for (int k=0;k<b->n;k++)
88 {
89 DrawMark(*b->v[k],0.002);
90
91 }
92 break;
93 }
94 else // Pointer QuadTreeBox
95 {
96 int lll = pi[l];
97 QuadTreeBox *b0=b;
98
99 if ((b=b->b[lll]))
100 {
101 hb >>=1 ; // div by 2
102 I2 ppp(pp[l],lll,hb);
103
104 pb[++l]= b;
105 pi[l]= 4;
106 pp[l]=ppp;
107 PlotQuad(pp[l],hb);
108 }
109 else
110 {
111 I2 ppp(pp[l],lll,hb/2);
112 b=b0;
113 PlotX(ppp,hb/2);
114 }
115 }
116 }
117 hb <<= 1; // mul by 2
118 } while (l--);
119
120 }
121
122 #endif
TrueNearestVertex(long xi,long yj)123 Vertex * FQuadTree::TrueNearestVertex(long xi,long yj)
124 {
125 QuadTreeBox * pb[ MaxDeep ];
126 int pi[ MaxDeep ];
127 I2 pp[ MaxDeep ];
128 int l=0; // level
129 QuadTreeBox * b;
130 IntQuad h=MaxISize,h0;
131 IntQuad hb = MaxISize;
132 I2 p0(0,0);
133 I2 plus( xi<MaxISize?(xi<0?0:xi):MaxISize-1,yj<MaxISize?(yj<0?0:yj):MaxISize-1);
134
135 Vertex *vn=0;
136
137 // init for optimisation ---
138 b = root;
139 long n0;
140 if (!root->n)
141 return vn; // empty tree
142
143
144 // general case -----
145 pb[0]= b;
146 pi[0]=b->n>0 ?(int) b->n : 4 ;
147 pp[0]=p0;
148 h=hb;
149 do {
150 b= pb[l];
151 while (pi[l]--)
152 {
153 int k = pi[l];
154
155 if (b->n>0) // Vertex QuadTreeBox none empty
156 {
157 NbVerticesSearch++;
158 I2 i2 = R2ToI2(b->v[k]);
159 h0 = I2(i2,plus).norm();// NORM(iplus,i2.x,jplus,i2.y);
160 if (h0 <h)
161 {
162 h = h0;
163 vn = b->v[k];
164 }
165 }
166 else // Pointer QuadTreeBox
167 {
168 QuadTreeBox *b0=b;
169 NbQuadTreeBoxSearch++;
170 if ((b=b->b[k]))
171 {
172 hb >>=1 ; // div by 2
173 I2 ppp(pp[l],k,hb);
174
175 if ( ppp.interseg(plus,hb,h) )//(INTER_SEG(iii,iii+hb,iplus-h,iplus+h) && INTER_SEG(jjj,jjj+hb,jplus-h,jplus+h))
176 {
177 pb[++l]= b;
178 pi[l]= b->n>0 ?(int) b->n : 4 ;
179 pp[l]=ppp;
180 }
181 else
182 b=b0, hb <<=1 ;
183 }
184 else
185 b=b0;
186 }
187 }
188 hb <<= 1; // mul by 2
189 } while (l--);
190
191 return vn;
192 }
NearestVertex(long xi,long yj)193 Vertex * FQuadTree::NearestVertex(long xi,long yj)
194 {
195 QuadTreeBox * pb[ MaxDeep ];
196 int pi[ MaxDeep ];
197 I2 pp[ MaxDeep ];
198 int l=0; // level
199 QuadTreeBox * b;
200 IntQuad h=MaxISize,h0;
201 IntQuad hb = MaxISize;
202 I2 p0(0,0);
203 I2 plus( xi<MaxISize?(xi<0?0:xi):MaxISize-1,yj<MaxISize?(yj<0?0:yj):MaxISize-1);
204
205 Vertex *vn=0;
206
207 // init for optimisation ---
208 b = root;
209 long n0;
210 if (!root->n)
211 return vn; // empty tree
212
213 while( (n0 = b->n) < 0)
214 {
215 // search the non empty
216 // QuadTreeBox containing the point (i,j)
217 long hb2 = hb >> 1 ;
218 int k = plus.Case(hb2);//(iplus,jplus,hb2);// QuadTreeBox number of size hb2 contening i;j
219 QuadTreeBox * b0= b->b[k];
220 if ( ( b0 == 0) || (b0->n == 0) )
221 break; // null box or empty => break
222 NbQuadTreeBoxSearch++;
223 b=b0;
224 p0.Add(k,hb2);
225 hb = hb2;
226 }
227
228
229 if ( n0 > 0)
230 {
231 for( int k=0;k<n0;k++)
232 {
233 I2 i2 = R2ToI2(b->v[k]);
234 h0 = I2(i2,plus).norm();//NORM(iplus,i2.x,jplus,i2.y);
235 if (h0 <h) {
236 h = h0;
237 vn = b->v[k];}
238 NbVerticesSearch++;
239 }
240 return vn;
241 }
242 // general case -----
243 pb[0]= b;
244 pi[0]=b->n>0 ?(int) b->n : 4 ;
245 pp[0]=p0;
246 h=hb;
247 do {
248 b= pb[l];
249 while (pi[l]--)
250 {
251 int k = pi[l];
252
253 if (b->n>0) // Vertex QuadTreeBox none empty
254 {
255 NbVerticesSearch++;
256 I2 i2 = R2ToI2(b->v[k]);
257 h0 = I2(i2,plus).norm();// NORM(iplus,i2.x,jplus,i2.y);
258 if (h0 <h)
259 {
260 h = h0;
261 vn = b->v[k];
262 }
263 }
264 else // Pointer QuadTreeBox
265 {
266 QuadTreeBox *b0=b;
267 NbQuadTreeBoxSearch++;
268 if ((b=b->b[k]))
269 {
270 hb >>=1 ; // div by 2
271 I2 ppp(pp[l],k,hb);
272
273 if ( ppp.interseg(plus,hb,h) )//(INTER_SEG(iii,iii+hb,iplus-h,iplus+h) && INTER_SEG(jjj,jjj+hb,jplus-h,jplus+h))
274 {
275 pb[++l]= b;
276 pi[l]= b->n>0 ?(int) b->n : 4 ;
277 pp[l]=ppp;
278 }
279 else
280 b=b0, hb <<=1 ;
281 }
282 else
283 b=b0;
284 }
285 }
286 hb <<= 1; // mul by 2
287 } while (l--);
288
289 return vn;
290 }
291 // FH add to get the list for point close of point xi,yi at distance less the dh
292 // jan 2020
ListNearestVertex(Vertex ** lnv,int nlvnx,long dh,long xi,long yj)293 int FQuadTree::ListNearestVertex(Vertex **lnv,int nlvnx,long dh,long xi,long yj)
294 {
295 int nlnv=0;
296 QuadTreeBox * pb[ MaxDeep ];
297 int pi[ MaxDeep ];
298 I2 pp[ MaxDeep ];
299 int l=0; // level
300 QuadTreeBox * b;
301 IntQuad hunsed=MaxISize,h0;
302 IntQuad hb = MaxISize;
303 I2 p0(0,0);
304 I2 plus( xi<MaxISize?(xi<0?0:xi):MaxISize-1,yj<MaxISize?(yj<0?0:yj):MaxISize-1);
305
306 Vertex *vn=0;
307
308 // init for optimisation ---
309
310 b = root;
311 long n0;
312 if (!root->n)
313 return nlnv; // empty tree
314
315
316 // general case -----
317 pb[0]= b;
318 pi[0]=b->n>0 ?(int) b->n : 4 ;
319 pp[0]=p0;
320 //h=dh;
321 do {
322
323 while (pi[l]--)
324 {
325 int k = pi[l];
326 b= pb[l];
327 if (b->n>0) // Vertex QuadTreeBox none empty
328 {
329 NbVerticesSearch++;
330 I2 i2 = R2ToI2(b->v[k]);
331 h0 = I2(i2,plus).norm();// NORM(iplus,i2.x,jplus,i2.y);
332 if (h0 <dh)
333 {
334 vn = b->v[k];
335 if(nlnv<nlvnx)
336 lnv[nlnv++]=vn;
337 }
338 }
339 else // Pointer QuadTreeBox
340 {
341 NbQuadTreeBoxSearch++;
342 if ((b=b->b[k])) // new box
343 {
344 hb >>=1 ; // div by 2
345 I2 ppp(pp[l],k,hb);
346
347 if ( ppp.interseg(plus,hb,dh) )
348 {
349 pb[++l]= b;
350 pi[l]= b->n>0 ?(int) b->n : 4 ;
351 pp[l]=ppp;
352 }
353 else
354 hb <<=1 ;
355 }
356
357 }
358 }
359 hb <<= 1; // mul by 2
360 } while (l--);
361
362 return nlnv;
363 }
364 // end Add jan 2020 FH.
365
366
ToClose(const R2 & v,R seuil,long hx,long hy,bool nearest)367 Vertex * FQuadTree::ToClose(const R2 & v,R seuil,long hx,long hy,bool nearest)
368 {
369 I2 H(hx,hy);
370 const I2 p(XtoI(v.x),YtoJ(v.y));
371 const R2 X(v);
372 R seuil2 = seuil*seuil;
373 Vertex *pvr =0; // return vertex
374 QuadTreeBox * pb[ MaxDeep ];
375 int pi[ MaxDeep ];
376 I2 pp[ MaxDeep ];
377
378 int l=0; // level
379 QuadTreeBox * b;
380 long hb = MaxISize;
381 I2 p0(0,0);
382
383 if (!root->n)
384 return 0; // empty tree
385
386 // general case -----
387 pb[0]= root;
388 pi[0]= root->n>0 ?(int) root->n : 4 ;
389 pp[0]=p0;
390 do {
391 b= pb[l];
392 while (pi[l]--)
393 {
394 int k = pi[l];
395
396 if (b->n>0) // Vertex QuadTreeBox none empty
397 {
398 NbVerticesSearch++;
399 Vertex & V(*b->v[k]);
400 I2 i2 = R2ToI2(V);
401 if ( I2(i2,p).less(H) )
402 {
403 R2 XY(X,V);
404 R dd;
405 if( (dd= (XY,XY) ) < seuil2 )
406 {
407 if( nearest ) // modif FH
408 {
409 seuil2=dd;
410 pvr = & V;
411 }
412 else
413 return &V;
414 }
415 }
416 }
417 else // Pointer QuadTreeBox
418 {
419 QuadTreeBox *b0=b;
420 NbQuadTreeBoxSearch++;
421 if ((b=b->b[k]))
422 {
423 hb >>=1 ; // div by 2
424 I2 ppp(pp[l],k,hb);
425 if (ppp.interseg(p,hb,H))
426 {
427 pb[++l]= b;
428 pi[l]= b->n>0 ?(int) b->n : 4 ;
429 pp[l]=ppp;
430 }
431 else
432 b=b0, hb <<=1 ;
433 }
434 else
435 b=b0;
436 }
437 }
438 hb <<= 1; // mul by 2
439 } while (l--);
440
441 return pvr;
442 }
443
444
Add(Vertex & w)445 void FQuadTree::Add( Vertex & w)
446 {
447 QuadTreeBox ** pb , *b;
448 I2 p(XtoI(w.x),YtoJ(w.y));
449 long l=MaxISize;
450 pb = &root;
451 while( (b=*pb) && (b->n<0))
452 {
453 b->n--;
454 l >>= 1;
455 pb = &b->b[p.Case(l)];
456 }
457 if (b) {
458 if (b->n > 3 && b->v[3] == &w) return;
459 if (b->n > 2 && b->v[2] == &w) return;
460 if (b->n > 1 && b->v[1] == &w) return;
461 if (b->n > 0 && b->v[0] == &w) return;
462 }
463 throwassert(l);
464 while ((b= *pb) && (b->n == 4)) // the QuadTreeBox is full
465 {
466 Vertex *v4[4]; // copy of the QuadTreeBox vertices
467
468 v4[0]= b->v[0];
469 v4[1]= b->v[1];
470 v4[2]= b->v[2];
471 v4[3]= b->v[3];
472 b->n = -b->n; // mark is pointer QuadTreeBox
473 b->b[0]=b->b[1]=b->b[2]=b->b[3]=0; // set empty QuadTreeBox ptr
474 l >>= 1; // div the size by 2
475 for (int k=0;k<4;k++) // for the 4 vertices find the sub QuadTreeBox ij
476 {
477 int ij;
478 QuadTreeBox * bb = b->b[ij=R2ToI2(v4[k]).Case(l)];
479 if (!bb)
480 bb=b->b[ij]=NewQuadTreeBox(); // alloc the QuadTreeBox
481 bb->v[bb->n++] = v4[k];
482 }
483 pb = &b->b[p.Case(l)];
484 }
485 if (!(b = *pb))
486 b=*pb= NewQuadTreeBox(); // alloc the QuadTreeBox
487 b->v[b->n++]=&w; // we add the vertex
488 NbVertices++;
489 }
490
491
FQuadTree(Vertex * v,R2 Pmin,R2 Pmax,long nbv)492 FQuadTree::FQuadTree(Vertex * v,R2 Pmin,R2 Pmax,long nbv)
493 :
494 th(0),
495 lenStorageQuadTreeBox(Max(abs(nbv),1000L)),
496 NbQuadTreeBox(0),
497 NbVertices(0),
498 NbQuadTreeBoxSearch(0),
499 NbVerticesSearch(0),
500 cMin(Pmin-(Pmax-Pmin)/2),
501 cMax(Pmax+(Pmax-Pmin)/2),
502 coef( MaxISize/Norme_infty(cMax-cMin))
503
504 {
505 sb =new StorageQuadTreeBox(lenStorageQuadTreeBox);
506 root=NewQuadTreeBox();
507 for (long i=0;i<nbv;i++)
508 Add(v[i]);
509 #ifdef DRAWING1
510 Draw();
511 #endif
512 }
513
FQuadTree(const Mesh * t,R2 Pmin,R2 Pmax,long nbv)514 FQuadTree::FQuadTree(const Mesh * t,R2 Pmin,R2 Pmax,long nbv) :
515 lenStorageQuadTreeBox(t->nv/8+100),
516 th(t),
517 NbQuadTreeBoxSearch(0),
518 NbVerticesSearch(0),
519 NbQuadTreeBox(0),
520 NbVertices(0),
521 cMin(Pmin-(Pmax-Pmin)/2),
522 cMax(Pmax+(Pmax-Pmin)/2),
523 coef( MaxISize/Norme_infty(cMax-cMin))
524
525 {
526 if (nbv == -1) nbv = t->nv;
527 sb =new StorageQuadTreeBox(lenStorageQuadTreeBox);
528 root=NewQuadTreeBox();
529 if (t)
530 for (long i=0;i<nbv;i++)
531 Add(t->vertices[i]);
532 #ifdef DRAWING1
533 Draw();
534 #endif
535 }
536
FQuadTree(const Mesh * t,long tnv,R2 Pmin,R2 Pmax,long nbv)537 FQuadTree::FQuadTree(const Mesh* t,long tnv,R2 Pmin,R2 Pmax,long nbv) :
538 lenStorageQuadTreeBox(tnv/8+100),
539 th(t),
540 NbQuadTreeBoxSearch(0),
541 NbVerticesSearch(0),
542 NbQuadTreeBox(0),
543 NbVertices(0),
544 cMin(Pmin-(Pmax-Pmin)/2),
545 cMax(Pmax+(Pmax-Pmin)/2),
546 coef( MaxISize/Norme_infty(cMax-cMin))
547
548 {
549 if (nbv == -1) nbv = tnv;
550 sb =new StorageQuadTreeBox(lenStorageQuadTreeBox);
551 root=NewQuadTreeBox();
552 if (t)
553 for (long i=0;i<nbv;i++)
554 Add(t->vertices[i]);
555 #ifdef DRAWING1
556 Draw();
557 #endif
558 }
559
560
FQuadTree()561 FQuadTree::FQuadTree() :
562 lenStorageQuadTreeBox(100),
563 th(0),
564 NbQuadTreeBoxSearch(0),
565 NbVerticesSearch(0),
566 NbQuadTreeBox(0),
567 NbVertices(0),
568 cMin(0,0),cMax(0,0),coef(0)
569 {
570 sb =new StorageQuadTreeBox(lenStorageQuadTreeBox);
571 root=NewQuadTreeBox();
572 }
StorageQuadTreeBox(long ll,StorageQuadTreeBox * nn)573 FQuadTree::StorageQuadTreeBox::StorageQuadTreeBox(long ll,StorageQuadTreeBox *nn)
574 {
575 len = ll;
576 n = nn;
577 b = new QuadTreeBox[ll];
578 for (int i = 0; i <ll;i++)
579 b[i].n =0,b[i].b[0]=b[i].b[1]=b[i].b[2]=b[i].b[3]=0;
580 bc =b;
581 be = b +ll;
582 throwassert(b);
583 }
584
~StorageQuadTreeBox()585 FQuadTree::StorageQuadTreeBox::~StorageQuadTreeBox()
586 {
587 if(n) delete n;
588 delete [] b;
589 }
590
~FQuadTree()591 FQuadTree::~FQuadTree()
592 {
593 delete sb;
594 }
595
operator <<(ostream & f,const FQuadTree & qt)596 ostream& operator <<(ostream& f, const FQuadTree & qt)
597 {
598 f << " the quadtree " << endl;
599 f << " NbQuadTreeBox = " << qt.NbQuadTreeBox
600 << " Nb Vertices = " << qt.NbVertices << endl;
601 f << " NbQuadTreeBoxSearch " << qt.NbQuadTreeBoxSearch
602 << " NbVerticesSearch " << qt.NbVerticesSearch << endl;
603 f << " SizeOf QuadTree" << qt.SizeOf() << endl;
604 return f;
605 }
606
NearestVertexWithNormal(const R2 & P)607 Vertex * FQuadTree::NearestVertexWithNormal(const R2 &P)//(long xi,long yj)
608 {
609 long xi(XtoI(P.x)),yj(YtoJ(P.y));
610 QuadTreeBox * pb[ MaxDeep ];
611 int pi[ MaxDeep ];
612 I2 pp[ MaxDeep];
613 int l; // level
614 QuadTreeBox * b;
615 IntQuad h=MaxISize,h0;
616 IntQuad hb = MaxISize;
617 I2 p0(0,0);
618 I2 plus( xi<MaxISize?(xi<0?0:xi):MaxISize-1,yj<MaxISize?(yj<0?0:yj):MaxISize-1);
619
620 Vertex *vn=0;
621
622 // init for optimisation ---
623 b = root;
624 long n0;
625 if (!root->n)
626 return vn; // empty tree
627
628 while( (n0 = b->n) < 0)
629 {
630 // search the non empty
631 // QuadTreeBox containing the point (i,j)
632 long hb2 = hb >> 1 ;
633 int k = plus.Case(hb2);//(iplus,jplus,hb2);// QuadTreeBox number of size hb2 contening i;j
634 QuadTreeBox * b0= b->b[k];
635 if ( ( b0 == 0) || (b0->n == 0) )
636 break; // null box or empty => break
637 NbQuadTreeBoxSearch++;
638 b=b0;
639 p0.Add(k,hb2);
640 hb = hb2;
641 }
642
643
644 if ( n0 > 0)
645 {
646 for(int k=0;k<n0;k++)
647 {
648 Vertex * v=b->v[k];
649 if (v->ninside(P)) {
650 I2 i2 = R2ToI2(v);
651 // try if is in the right sens --
652 h0 = I2(i2,plus).norm();// h0 = NORM(iplus,i2.x,jplus,i2.y);
653 if (h0 <h) {
654 h = h0;
655 vn = v;}
656 NbVerticesSearch++;}
657 }
658 if (vn) return vn;
659 }
660 // general case -----
661 // INITIALISATION OF THE STACK
662 l =0; // level
663 pb[0]= b;
664 pi[0]= b->n>0 ?(int) b->n : 4 ;
665 pp[0]=p0;
666 h=hb;
667 L1:
668 do { // walk on the tree
669 b= pb[l];
670 while (pi[l]--) // loop on 4 element of the box
671 {
672 int k = pi[l];
673
674 if (b->n>0) // Vertex QuadTreeBox none empty
675 {
676 Vertex * v=b->v[k];
677 if (v->ninside(P) ) {
678 NbVerticesSearch++;
679 I2 i2 = R2ToI2(v);
680 // if good sens when try --
681 h0 = I2(i2,plus).norm();// NORM(iplus,i2.x,jplus,i2.y);
682 if (h0 <h)
683 {
684 h = h0;
685 vn =v;
686 }}
687 }
688 else // Pointer QuadTreeBox
689 {
690 QuadTreeBox *b0=b;
691 NbQuadTreeBoxSearch++;
692 if ((b=b->b[k]))
693 {
694 hb >>=1 ; // div by 2
695 I2 ppp(pp[l],k,hb);
696
697 if ( ppp.interseg(plus,hb,h) )//(INTER_SEG(iii,iii+hb,iplus-h,iplus+h) && INTER_SEG(jjj,jjj+hb,jplus-h,jplus+h))
698 {
699 pb[++l]= b;
700 pi[l]= b->n>0 ?(int) b->n : 4 ;
701 pp[l]=ppp;
702 }
703 else
704 b=b0, hb <<=1 ;
705 }
706 else
707 b=b0;
708 }
709 }
710 hb <<= 1; // mul by 2
711 } while (l--);
712 if (!vn && b != root )
713 {// cas particulier on repart du sommet on avais rien trouver
714 b=root;
715 hb = MaxISize;
716 p0=I2(0,0);
717 l=0;
718 pb[0]= b;
719 pi[0]= b->n>0 ?(int) b->n : 4 ;
720 pp[0]=I2(0,0);
721
722 goto L1;
723 }
724 return vn;
725 }
726
727 #else
728
729
730 // nouvelle version a tester
731
732
733
734 #define INTER_SEG(a,b,x,y) (((y) > (a)) && ((x) <(b)))
735 #define ABS(i) ((i)<0 ?-(i) :(i))
736 #define MAX1(i,j) ((i)>(j) ?(i) :(j))
737 #define NORM(i1,j1,i2,j2) MAX1(ABS((i1)-(j1)),ABS((i2)-(j2)))
738
739 #define IJ(i,j,l) ( ( j & l) ? (( i & l) ? 3 : 2 ) :( ( i & l)? 1 : 0 ))
740 #define I_IJ(k,l) (( k&1) ? l : 0)
741 #define J_IJ(k,l) (( k&2) ? l : 0)
742
743
744 #ifdef DRAWING
745 // old version ----
746
Draw()747 void FQuadTree::Draw()
748 {
749 QuadTreeBox * pb[ MaxDeep ];
750 int pi[ MaxDeep ];
751 long ii[ MaxDeep ], jj [ MaxDeep];
752 int l=0; // level
753 QuadTreeBox * b;
754 IntQuad hb = MaxISize;
755 if (!root) return ;
756 long kkk =0;
757 pb[0]= root;
758 pi[0]= root->n>0 ?(int) root->n : 4 ;;
759 ii[0]=jj[0]=0;
760 do{
761 b= pb[l];
762
763 while (pi[l]--)
764 {
765 if (b->n>0) // Vertex QuadTreeBox none empty
766 { //
767 for (int k=0;k<b->n;k++)
768 {
769 DrawMark(*b->v[k],0.002);
770
771 }
772 break;
773 }
774 else // Pointer QuadTreeBox
775 {
776 int lll = pi[l];
777 QuadTreeBox *b0=b;
778
779 if ((b=b->b[lll]))
780 {
781 hb >>=1 ; // div by 2
782 long iii = ii[l]+I_IJ(lll,hb);
783 long jjj = jj[l]+J_IJ(lll,hb);
784
785 pb[++l]= b;
786 pi[l]= 4;
787 ii[l]= iii;
788 jj[l]= jjj;
789
790 IMoveTo(ii[l],jj[l]);
791 ILineTo(ii[l]+hb,jj[l]);
792 ILineTo(ii[l]+hb,jj[l]+hb);
793 ILineTo(ii[l] ,jj[l]+hb);
794 ILineTo(ii[l] ,jj[l]);
795
796
797 }
798 else
799 {
800 long iii = ii[l]+I_IJ(lll,hb/2);
801 long jjj = jj[l]+J_IJ(lll,hb/2);
802 b=b0;
803
804 IMoveTo(iii, jjj);
805 ILineTo(iii+hb/2,jjj+hb/2);
806 IMoveTo(iii+hb/2,jjj);
807 ILineTo(iii ,jjj+hb/2);
808
809 }
810 }
811 }
812 hb <<= 1; // mul by 2
813 } while (l--);
814
815 }
816
817 #endif
818
NearestVertex(long xi,long yj)819 Vertex * FQuadTree::NearestVertex(long xi,long yj)
820 {
821 QuadTreeBox * pb[ MaxDeep ];
822 int pi[ MaxDeep ];
823 long ii[ MaxDeep ], jj [ MaxDeep];
824 int l=0; // level
825 QuadTreeBox * b;
826 IntQuad h=MaxISize,h0;
827 IntQuad hb = MaxISize;
828 long i0=0,j0=0;
829 long iplus( xi<MaxISize?(xi<0?0:xi):MaxISize-1);
830 long jplus( yj<MaxISize?(yj<0?0:yj):MaxISize-1);
831
832 Vertex *vn=0;
833
834 // init for optimisation ---
835 b = root;
836 long n0;
837 if (!root->n)
838 return vn; // empty tree
839
840 while( (n0 = b->n) < 0)
841 {
842 // search the non empty
843 // QuadTreeBox containing the point (i,j)
844 long hb2 = hb >> 1 ;
845 int k = IJ(iplus,jplus,hb2);// QuadTreeBox number of size hb2 contening i;j
846 QuadTreeBox * b0= b->b[k];
847 if ( ( b0 == 0) || (b0->n == 0) )
848 break; // null box or empty => break
849 NbQuadTreeBoxSearch++;
850 b=b0;
851 i0 += I_IJ(k,hb2); // i orign of QuadTreeBox
852 j0 += J_IJ(k,hb2); // j orign of QuadTreeBox
853 hb = hb2;
854 }
855
856
857 if ( n0 > 0)
858 {
859 for( int k=0;k<n0;k++)
860 {
861 I2 i2 = R2ToI2(b->v[k]);
862 h0 = NORM(iplus,i2.x,jplus,i2.y);
863 if (h0 <h) {
864 h = h0;
865 vn = b->v[k];}
866 NbVerticesSearch++;
867 }
868 return vn;
869 }
870 // general case -----
871 pb[0]= b;
872 pi[0]=b->n>0 ?(int) b->n : 4 ;
873 ii[0]=i0;
874 jj[0]=j0;
875 h=hb;
876 do {
877 b= pb[l];
878 while (pi[l]--)
879 {
880 int k = pi[l];
881
882 if (b->n>0) // Vertex QuadTreeBox none empty
883 {
884 NbVerticesSearch++;
885 I2 i2 = R2ToI2(b->v[k]);
886 h0 = NORM(iplus,i2.x,jplus,i2.y);
887 if (h0 <h)
888 {
889 h = h0;
890 vn = b->v[k];
891 }
892 }
893 else // Pointer QuadTreeBox
894 {
895 QuadTreeBox *b0=b;
896 NbQuadTreeBoxSearch++;
897 if ((b=b->b[k]))
898 {
899 hb >>=1 ; // div by 2
900 long iii = ii[l]+I_IJ(k,hb);
901 long jjj = jj[l]+J_IJ(k,hb);
902
903 if (INTER_SEG(iii,iii+hb,iplus-h,iplus+h) && INTER_SEG(jjj,jjj+hb,jplus-h,jplus+h))
904 {
905 pb[++l]= b;
906 pi[l]= b->n>0 ?(int) b->n : 4 ;
907 ii[l]= iii;
908 jj[l]= jjj;
909
910 }
911 else
912 b=b0, hb <<=1 ;
913 }
914 else
915 b=b0;
916 }
917 }
918 hb <<= 1; // mul by 2
919 } while (l--);
920
921 return vn;
922 }
923
924
925
ToClose(const R2 & v,R seuil,long hx,long hy)926 Vertex * FQuadTree::ToClose(const R2 & v,R seuil,long hx,long hy)
927 {
928 const long i=XtoI(v.x);
929 const long j=YtoJ(v.y);
930 const R2 X(v);
931 R seuil2 = seuil*seuil;
932
933 QuadTreeBox * pb[ MaxDeep ];
934 int pi[ MaxDeep ];
935 long ii[ MaxDeep ], jj [ MaxDeep];
936 int l=0; // level
937 QuadTreeBox * b;
938 long h=MaxISize;
939 long hb = MaxISize;
940 long i0=0,j0=0;
941
942 if (!root->n)
943 return 0; // empty tree
944
945 // general case -----
946 pb[0]= root;
947 pi[0]= root->n>0 ?(int) root->n : 4 ;
948 ii[0]=i0;
949 jj[0]=j0;
950 h=hb;
951 do {
952 b= pb[l];
953 while (pi[l]--)
954 {
955 int k = pi[l];
956
957 if (b->n>0) // Vertex QuadTreeBox none empty
958 {
959 NbVerticesSearch++;
960 Vertex & V(*b->v[k]);
961 I2 i2 = R2ToI2(V);
962 if ( ABS(i-i2.x) <hx && ABS(j-i2.y) <hy )
963 {
964 R2 XY(X,V);
965 R dd;
966 if( (dd= (XY,XY) ) < seuil )
967 {
968 return &V;
969 }
970 }
971 }
972 else // Pointer QuadTreeBox
973 {
974 QuadTreeBox *b0=b;
975 NbQuadTreeBoxSearch++;
976 if ((b=b->b[k]))
977 {
978 hb >>=1 ; // div by 2
979 long iii = ii[l]+I_IJ(k,hb);
980 long jjj = jj[l]+J_IJ(k,hb);
981
982 if (INTER_SEG(iii,iii+hb,i-hx,i+hx) && INTER_SEG(jjj,jjj+hb,j-hy,j+hy))
983 {
984 pb[++l]= b;
985 pi[l]= b->n>0 ?(int) b->n : 4 ;
986 ii[l]= iii;
987 jj[l]= jjj;
988
989 }
990 else
991 b=b0, hb <<=1 ;
992 }
993 else
994 b=b0;
995 }
996 }
997 hb <<= 1; // mul by 2
998 } while (l--);
999
1000 return 0;
1001 }
1002
1003
Add(Vertex & w)1004 void FQuadTree::Add( Vertex & w)
1005 {
1006 QuadTreeBox ** pb , *b;
1007 long i= XtoI(w.x), j=YtoJ(w.y),l=MaxISize;
1008 pb = &root;
1009 while( (b=*pb) && (b->n<0))
1010 {
1011 b->n--;
1012 l >>= 1;
1013 pb = &b->b[IJ(i,j,l)];
1014 }
1015 if (b) {
1016 if (b->n > 3 && b->v[3] == &w) return;
1017 if (b->n > 2 && b->v[2] == &w) return;
1018 if (b->n > 1 && b->v[1] == &w) return;
1019 if (b->n > 0 && b->v[0] == &w) return;
1020 }
1021 throwassert(l);
1022 while ((b= *pb) && (b->n == 4)) // the QuadTreeBox is full
1023 {
1024 Vertex *v4[4]; // copy of the QuadTreeBox vertices
1025
1026 v4[0]= b->v[0];
1027 v4[1]= b->v[1];
1028 v4[2]= b->v[2];
1029 v4[3]= b->v[3];
1030 b->n = -b->n; // mark is pointer QuadTreeBox
1031 b->b[0]=b->b[1]=b->b[2]=b->b[3]=0; // set empty QuadTreeBox ptr
1032 l >>= 1; // div the size by 2
1033 for (int k=0;k<4;k++) // for the 4 vertices find the sub QuadTreeBox ij
1034 {
1035 int ij;
1036 QuadTreeBox * bb = b->b[ij=IJ(XtoI(v4[k]->x),YtoJ(v4[k]->y),l)];
1037 if (!bb)
1038 bb=b->b[ij]=NewQuadTreeBox(); // alloc the QuadTreeBox
1039 bb->v[bb->n++] = v4[k];
1040 }
1041 pb = &b->b[IJ(i,j,l)];
1042 }
1043 if (!(b = *pb))
1044 b=*pb= NewQuadTreeBox(); // alloc the QuadTreeBox
1045 b->v[b->n++]=&w; // we add the vertex
1046 NbVertices++;
1047 }
1048
FQuadTree(Mesh * t,R2 Pmin,R2 Pmax,long nbv)1049 FQuadTree::FQuadTree(Mesh * t,R2 Pmin,R2 Pmax,long nbv) :
1050 th(t),
1051 lenStorageQuadTreeBox(t->nv/8+100),
1052 NbQuadTreeBox(0),
1053 NbVertices(0),
1054 NbQuadTreeBoxSearch(0),
1055 NbVerticesSearch(0),
1056 cMin(Pmin-(Pmax-Pmin)/2),
1057 cMax(Pmax+(Pmax-Pmin)/2),
1058 coef( MaxISize/Norme_infty(cMax-cMin))
1059
1060 {
1061 if (nbv == -1) nbv = t->nv;
1062 sb =new StorageQuadTreeBox(lenStorageQuadTreeBox);
1063 root=NewQuadTreeBox();
1064 if (t)
1065 for (long i=0;i<nbv;i++)
1066 Add(t->vertices[i]);
1067 #ifdef DRAWING1
1068 Draw();
1069 #endif
1070 }
1071
FQuadTree()1072 FQuadTree::FQuadTree() :
1073 th(0),
1074 lenStorageQuadTreeBox(100),
1075 NbQuadTreeBox(0),
1076 NbVertices(0),
1077 NbQuadTreeBoxSearch(0),
1078 NbVerticesSearch(0),
1079 coef(0),cMin(0,0),cMax(0,0)
1080 {
1081 sb =new StorageQuadTreeBox(lenStorageQuadTreeBox);
1082 root=NewQuadTreeBox();
1083 }
StorageQuadTreeBox(long ll,StorageQuadTreeBox * nn)1084 FQuadTree::StorageQuadTreeBox::StorageQuadTreeBox(long ll,StorageQuadTreeBox *nn)
1085 {
1086 len = ll;
1087 n = nn;
1088 b = new QuadTreeBox[ll];
1089 for (int i = 0; i <ll;i++)
1090 b[i].n =0,b[i].b[0]=b[i].b[1]=b[i].b[2]=b[i].b[3]=0;
1091 bc =b;
1092 be = b +ll;
1093 throwassert(b);
1094 }
1095
~FQuadTree()1096 FQuadTree::~FQuadTree()
1097 {
1098 delete sb;
1099 }
1100
operator <<(ostream & f,const FQuadTree & qt)1101 ostream& operator <<(ostream& f, const FQuadTree & qt)
1102 {
1103 f << " the quadtree " << endl;
1104 f << " NbQuadTreeBox = " << qt.NbQuadTreeBox
1105 << " Nb Vertices = " << qt.NbVertices << endl;
1106 f << " NbQuadTreeBoxSearch " << qt.NbQuadTreeBoxSearch
1107 << " NbVerticesSearch " << qt.NbVerticesSearch << endl;
1108 f << " SizeOf QuadTree" << qt.SizeOf() << endl;
1109 return f;
1110 }
1111
NearestVertexWithNormal(const R2 & P)1112 Vertex * FQuadTree::NearestVertexWithNormal(const R2 &P)//(long xi,long yj)
1113 {
1114 long xi(XtoI(P.x)),yj(YtoJ(P.y));
1115 QuadTreeBox * pb[ MaxDeep ];
1116 int pi[ MaxDeep ];
1117 long ii[ MaxDeep ], jj [ MaxDeep];
1118 int l; // level
1119 QuadTreeBox * b;
1120 IntQuad h=MaxISize,h0;
1121 IntQuad hb = MaxISize;
1122 long i0=0,j0=0;
1123 long iplus( xi<MaxISize?(xi<0?0:xi):MaxISize-1);
1124 long jplus( yj<MaxISize?(yj<0?0:yj):MaxISize-1);
1125
1126 Vertex *vn=0;
1127
1128 // init for optimisation ---
1129 b = root;
1130 long n0;
1131 if (!root->n)
1132 return vn; // empty tree
1133
1134 while( (n0 = b->n) < 0)
1135 {
1136 // search the non empty
1137 // QuadTreeBox containing the point (i,j)
1138 long hb2 = hb >> 1 ;
1139 int k = IJ(iplus,jplus,hb2);// QuadTreeBox number of size hb2 contening i;j
1140 QuadTreeBox * b0= b->b[k];
1141 if ( ( b0 == 0) || (b0->n == 0) )
1142 break; // null box or empty => break
1143 NbQuadTreeBoxSearch++;
1144 b=b0;
1145 i0 += I_IJ(k,hb2); // i orign of QuadTreeBox
1146 j0 += J_IJ(k,hb2); // j orign of QuadTreeBox
1147 hb = hb2;
1148 }
1149
1150
1151 if ( n0 > 0)
1152 {
1153 for(int k=0;k<n0;k++)
1154 {
1155 Vertex * v=b->v[k];
1156 if (v->ninside(P)) {
1157 I2 i2 = R2ToI2(v);
1158 // try if is in the right sens --
1159 h0 = NORM(iplus,i2.x,jplus,i2.y);
1160 if (h0 <h) {
1161 h = h0;
1162 vn = v;}
1163 NbVerticesSearch++;}
1164 }
1165 if (vn) return vn;
1166 }
1167 // general case -----
1168 // INITIALISATION OF THE STACK
1169 l =0; // level
1170 pb[0]= b;
1171 pi[0]= b->n>0 ?(int) b->n : 4 ;
1172 ii[0]=i0;
1173 jj[0]=j0;
1174 h=hb;
1175 L1:
1176 do { // walk on the tree
1177 b= pb[l];
1178 while (pi[l]--) // loop on 4 element of the box
1179 {
1180 int k = pi[l];
1181
1182 if (b->n>0) // Vertex QuadTreeBox none empty
1183 {
1184 Vertex * v=b->v[k];
1185 if (v->ninside(P) ) {
1186 NbVerticesSearch++;
1187 I2 i2 = R2ToI2(v);
1188 // if good sens when try --
1189 h0 = NORM(iplus,i2.x,jplus,i2.y);
1190 if (h0 <h)
1191 {
1192 h = h0;
1193 vn =v;
1194 }}
1195 }
1196 else // Pointer QuadTreeBox
1197 {
1198 QuadTreeBox *b0=b;
1199 NbQuadTreeBoxSearch++;
1200 if ((b=b->b[k]))
1201 {
1202 hb >>=1 ; // div by 2
1203 long iii = ii[l]+I_IJ(k,hb);
1204 long jjj = jj[l]+J_IJ(k,hb);
1205
1206 if (INTER_SEG(iii,iii+hb,iplus-h,iplus+h) && INTER_SEG(jjj,jjj+hb,jplus-h,jplus+h))
1207 {
1208 pb[++l]= b;
1209 pi[l]= b->n>0 ?(int) b->n : 4 ;
1210 ii[l]= iii;
1211 jj[l]= jjj;
1212
1213 }
1214 else
1215 b=b0, hb <<=1 ;
1216 }
1217 else
1218 b=b0;
1219 }
1220 }
1221 hb <<= 1; // mul by 2
1222 } while (l--);
1223 if (!vn && b != root )
1224 {// cas particulier on repart du sommet on avais rien trouver
1225 b=root;
1226 hb = MaxISize;
1227 i0=0;
1228 j0=0;
1229 l=0;
1230 pb[0]= b;
1231 pi[0]= b->n>0 ?(int) b->n : 4 ;
1232 ii[0]=i0;
1233 jj[0]=j0;
1234
1235 goto L1;
1236 }
1237 return vn;
1238 }
1239
1240 #endif
1241