0024050: Voxel_FastConverter::Convert / ConvertUsingSAT - problems in multithreaded...
[occt.git] / src / Voxel / Voxel_FastConverter.cxx
1 // Created on: 2008-05-30
2 // Created by: Vladislav ROMASHKO
3 // Copyright (c) 2008-2012 OPEN CASCADE SAS
4 //
5 // The content of this file is subject to the Open CASCADE Technology Public
6 // License Version 6.5 (the "License"). You may not use the content of this file
7 // except in compliance with the License. Please obtain a copy of the License
8 // at http://www.opencascade.org and read it completely before using this file.
9 //
10 // The Initial Developer of the Original Code is Open CASCADE S.A.S., having its
11 // main offices at: 1, place des Freres Montgolfier, 78280 Guyancourt, France.
12 //
13 // The Original Code and all software distributed under the License is
14 // distributed on an "AS IS" basis, without warranty of any kind, and the
15 // Initial Developer hereby disclaims all such warranties, including without
16 // limitation, any warranties of merchantability, fitness for a particular
17 // purpose or non-infringement. Please see the License for the specific terms
18 // and conditions governing the rights and limitations under the License.
19
20
21 #include <Voxel_FastConverter.ixx>
22
23 #include <Bnd_Box.hxx>
24 #include <BRep_Tool.hxx>
25 #include <BRepBndLib.hxx>
26 #include <BRepMesh.hxx>
27
28 #include <TopoDS.hxx>
29 #include <TopoDS_Face.hxx>
30 #include <TopExp_Explorer.hxx>
31
32 #include <gp_Lin2d.hxx>
33 #include <gce_MakePln.hxx>
34
35 #include <ElSLib.hxx>
36 #include <Poly_Triangulation.hxx>
37 #include <IntAna2d_AnaIntersection.hxx>
38
39 // Printing the progress in stdout.
40 //#define CONV_DUMP
41
42 Voxel_FastConverter::Voxel_FastConverter(const TopoDS_Shape&    shape,
43                                          Voxel_ROctBoolDS&      voxels, 
44                                          const Standard_Real    deflection,
45                                          const Standard_Integer nbx,
46                                          const Standard_Integer nby,
47                                          const Standard_Integer nbz,
48                                          const Standard_Integer nbthreads,
49                                          const Standard_Boolean useExistingTriangulation)
50 :myShape(shape),myVoxels(&voxels),
51  myDeflection(deflection),
52  myNbX(nbx),myNbY(nby),myNbZ(nbz),
53  myNbThreads(nbthreads),myIsBool(2),
54  myNbTriangles(0),
55  myUseExistingTriangulation(useExistingTriangulation)
56 {
57   Init();
58 }
59
60 Voxel_FastConverter::Voxel_FastConverter(const TopoDS_Shape&    shape,
61                                          Voxel_BoolDS&          voxels, 
62                                          const Standard_Real    deflection,
63                                          const Standard_Integer nbx,
64                                          const Standard_Integer nby,
65                                          const Standard_Integer nbz,
66                                          const Standard_Integer nbthreads,
67                                          const Standard_Boolean useExistingTriangulation)
68 :myShape(shape),myVoxels(&voxels),
69  myDeflection(deflection),
70  myNbX(nbx),myNbY(nby),myNbZ(nbz),
71  myNbThreads(nbthreads),myIsBool(1),
72  myNbTriangles(0),
73  myUseExistingTriangulation(useExistingTriangulation)
74 {
75   Init();
76 }
77
78 Voxel_FastConverter::Voxel_FastConverter(const TopoDS_Shape&    shape,
79                                          Voxel_ColorDS&         voxels, 
80                                          const Standard_Real    deflection,
81                                          const Standard_Integer nbx,
82                                          const Standard_Integer nby,
83                                          const Standard_Integer nbz,
84                                          const Standard_Integer nbthreads,
85                                          const Standard_Boolean useExistingTriangulation)
86 :myShape(shape),myVoxels(&voxels),
87  myDeflection(deflection),
88  myNbX(nbx),myNbY(nby),myNbZ(nbz),
89  myNbThreads(nbthreads),myIsBool(0),
90  myNbTriangles(0),
91  myUseExistingTriangulation(useExistingTriangulation)
92 {
93   Init();
94 }
95
96 void Voxel_FastConverter::Init()
97 {
98   if (myShape.IsNull())
99     return;
100   if (myNbThreads < 1)
101     return;
102
103   // Check number of splits.
104   Voxel_DS* voxels = (Voxel_DS*) myVoxels;
105   if (voxels->GetNbX() != myNbX || voxels->GetNbY() != myNbY || voxels->GetNbZ() != myNbZ)
106   {
107     // Compute boundary box of the shape
108     Bnd_Box box;
109     BRepBndLib::Add(myShape, box);
110
111     // Define the voxel model by means of the boundary box of shape
112     Standard_Real xmin, ymin, zmin, xmax, ymax, zmax;
113     box.Get(xmin, ymin, zmin, xmax, ymax, zmax);
114
115     // Initialize the voxels.
116     if (myIsBool == 2)
117       ((Voxel_ROctBoolDS*) voxels)->Init(xmin, ymin, zmin, xmax - xmin, ymax - ymin, zmax - zmin, myNbX, myNbY, myNbZ);
118     else if (myIsBool == 1)
119       ((Voxel_BoolDS*) voxels)->Init(xmin, ymin, zmin, xmax - xmin, ymax - ymin, zmax - zmin, myNbX, myNbY, myNbZ);
120     else if (myIsBool == 0)
121       ((Voxel_ColorDS*) voxels)->Init(xmin, ymin, zmin, xmax - xmin, ymax - ymin, zmax - zmin, myNbX, myNbY, myNbZ);
122   }
123
124   // Check presence of triangulation.
125   TopLoc_Location L;
126   Standard_Boolean triangulate = Standard_False;
127   TopExp_Explorer expl(myShape, TopAbs_FACE);
128   if(myUseExistingTriangulation == Standard_False)
129   {
130     for (; expl.More(); expl.Next())
131     {
132       const TopoDS_Face & F = TopoDS::Face(expl.Current());
133       Handle(Poly_Triangulation) T = BRep_Tool::Triangulation(F, L);
134       if (T.IsNull() || (T->Deflection() > myDeflection))
135       {
136         triangulate = Standard_True;
137         break;
138       }
139     }
140   }
141
142   // Re-create the triangulation.
143   if (triangulate)
144   {
145     BRepMesh::Mesh(myShape, myDeflection);
146   }
147
148   // Compute the number of triangles.
149   myNbTriangles = 0;
150   expl.Init(myShape, TopAbs_FACE);
151   for (; expl.More(); expl.Next())
152   {
153     const TopoDS_Face & F = TopoDS::Face(expl.Current());
154     Handle(Poly_Triangulation) T = BRep_Tool::Triangulation(F, L);
155     if (T.IsNull() == Standard_False)
156       myNbTriangles += T->NbTriangles();
157   }
158 }
159
160 // Destructor
161 void Voxel_FastConverter::Destroy()
162 {
163
164 }
165
166 Standard_Boolean Voxel_FastConverter::Convert(Standard_Integer&      progress,
167                                               const Standard_Integer ithread)
168 {
169   if (ithread == 1)
170     progress = 0;
171 #ifdef CONV_DUMP
172   if (ithread == 1)
173     printf("Progress = %d   \r", progress);
174 #endif
175
176   if (myNbX <= 0 || myNbY <= 0 || myNbZ <= 0)
177     return Standard_False;
178
179   if(myNbTriangles == 0)
180     return Standard_False;
181
182   // Half of diagonal of a voxel
183   Voxel_DS* ds = (Voxel_DS*) myVoxels;
184   Standard_Real dx = ds->GetXLen() / (Standard_Real) ds->GetNbX(),
185                 dy = ds->GetYLen() / (Standard_Real) ds->GetNbY(),
186                 dz = ds->GetZLen() / (Standard_Real) ds->GetNbZ();
187   Standard_Real hdiagonal = sqrt(dx * dx + dy * dy + dz * dz);
188   hdiagonal /= 2.0;
189
190   // Compute the scope of triangles for current thread
191   Standard_Integer start_thread_triangle = 1, end_thread_triangle = myNbTriangles, ithread_triangle = 0;
192   if(myNbTriangles < myNbThreads)
193   {
194     if(ithread != 1)
195       return Standard_False;
196     //in case we're in thread one process all triangles
197   }
198   else
199   {
200     div_t division = div(myNbTriangles, myNbThreads);
201     start_thread_triangle = (ithread - 1) * division.quot + 1;
202     end_thread_triangle   = (ithread - 0) * division.quot;
203
204     if(ithread == myNbThreads)
205       end_thread_triangle += division.rem;
206   }
207
208   // Convert
209   TopLoc_Location L;
210   Standard_Integer iprogress = 0, prev_progress = 0;
211   Standard_Integer n1, n2, n3;
212   Standard_Integer ixmin, iymin, izmin, ixmax, iymax, izmax;
213   Standard_Real xmin, ymin, zmin, xmax, ymax, zmax;
214   TopExp_Explorer expl(myShape, TopAbs_FACE);
215   for (; expl.More(); expl.Next())
216   {
217     const TopoDS_Face & F = TopoDS::Face(expl.Current());
218     Handle(Poly_Triangulation) T = BRep_Tool::Triangulation(F, L);
219     if (T.IsNull())
220       continue;
221
222     gp_Trsf trsf;
223     Standard_Boolean transform = !L.IsIdentity();
224     if (transform)
225       trsf = L.Transformation();
226
227     const TColgp_Array1OfPnt& nodes = T->Nodes();
228     const Poly_Array1OfTriangle& triangles = T->Triangles();
229     Standard_Integer itriangle = triangles.Lower(), nb_triangles = triangles.Upper();
230     for (; itriangle <= nb_triangles; itriangle++)
231     {
232       ithread_triangle++;
233       if (ithread_triangle < start_thread_triangle )
234         continue;
235       if (ithread_triangle > end_thread_triangle)
236       {
237         if (ithread == 1)
238           progress = 100;
239 #ifdef CONV_DUMP
240         if (ithread == 1)
241           printf("Progress = %d  \r", progress);
242 #endif
243         return Standard_True;
244       }
245
246       const Poly_Triangle& t = triangles.Value(itriangle);
247       t.Get(n1, n2, n3);
248       gp_Pnt p1 = nodes.Value(n1);
249       gp_Pnt p2 = nodes.Value(n2);
250       gp_Pnt p3 = nodes.Value(n3);
251       if (transform)
252       {
253         p1.Transform(trsf);
254         p2.Transform(trsf);
255         p3.Transform(trsf);
256       }
257
258       // Get boundary box of the triangle
259       GetBndBox(p1, p2, p3, xmin, ymin, zmin, xmax, ymax, zmax);
260
261       // Find the range of voxels inside the boudary box of the triangle.
262       if (!ds->GetVoxel(xmin, ymin, zmin, ixmin, iymin, izmin))
263         continue;
264       if (!ds->GetVoxel(xmax, ymax, zmax, ixmax, iymax, izmax))
265         continue;
266
267       // Refuse voxels for whom distance from their center to plane of triangle is greater than half of diagonal.
268       // Make a line from center of each voxel to the center of triangle and
269       // compute intersection of the line with sides of triangle.
270       // Refuse the voxel in case of intersection.
271       gce_MakePln mkPlane(p1, p2, p3);
272       if (!mkPlane.IsDone())
273         continue;
274       gp_Pln plane = mkPlane.Value();
275       ComputeVoxelsNearTriangle(plane, p1, p2, p3, hdiagonal, ixmin, iymin, izmin, ixmax, iymax, izmax);
276
277       // Progress
278       if (ithread == 1)
279       {
280         iprogress++;
281         progress = (Standard_Integer) ( (Standard_Real) iprogress / (Standard_Real) myNbTriangles * 100.0 );
282       }
283 #ifdef CONV_DUMP
284       if (ithread == 1 && prev_progress != progress)
285       {
286         printf("Progress = %d  \r", progress);
287         prev_progress = progress;
288       }
289 #endif
290
291     } // iteration of triangles
292   } // iteration of faces
293
294   if (ithread == 1)
295     progress = 100;
296 #ifdef CONV_DUMP
297   if (ithread == 1)
298     printf("Progress = %d  \r", progress);
299 #endif
300   return Standard_True;
301 }
302
303 Standard_Boolean Voxel_FastConverter::ConvertUsingSAT(Standard_Integer&      progress,
304                                                       const Standard_Integer ithread)
305 {
306   if (ithread == 1)
307     progress = 0;
308 #ifdef CONV_DUMP
309   if (ithread == 1)
310     printf("Progress = %d   \r", progress);
311 #endif
312
313   if (myNbX <= 0 || myNbY <= 0 || myNbZ <= 0)
314     return Standard_False;
315
316   if(myNbTriangles == 0)
317     return Standard_False;
318
319   // Half of size of a voxel (also for Voxel_ROctBoolDS)
320   Voxel_DS* ds = (Voxel_DS*) myVoxels;
321   Standard_Real dx = ds->GetXLen() / (Standard_Real) ds->GetNbX(),
322                 dy = ds->GetYLen() / (Standard_Real) ds->GetNbY(),
323                 dz = ds->GetZLen() / (Standard_Real) ds->GetNbZ();
324   gp_Pnt extents(dx/2.0, dy/2.0, dz/2.0);
325   gp_Pnt extents2(dx/4.0, dy/4.0, dz/4.0);
326   gp_Pnt extents4(dx/8.0, dy/8.0, dz/8.0);
327
328   // Compute the scope of triangles for current thread
329   Standard_Integer start_thread_triangle = 1, end_thread_triangle = myNbTriangles, ithread_triangle = 0;
330   if(myNbTriangles < myNbThreads)
331   {
332     if(ithread != 1)
333       return Standard_False;
334     //in case we're in thread one process all triangles
335   }
336   else
337   {
338     div_t division = div(myNbTriangles, myNbThreads);
339     start_thread_triangle = (ithread - 1) * division.quot + 1;
340     end_thread_triangle   = (ithread - 0) * division.quot;
341
342     if(ithread == myNbThreads)
343       end_thread_triangle += division.rem;
344   }
345
346   // Convert
347   TopLoc_Location L;
348   Standard_Integer iprogress = 0, prev_progress = 0;
349   Standard_Integer n1, n2, n3;
350   Standard_Integer ixmin, iymin, izmin, ixmax, iymax, izmax;
351   Standard_Real xmin, ymin, zmin, xmax, ymax, zmax;
352   TopExp_Explorer expl(myShape, TopAbs_FACE);
353   for (; expl.More(); expl.Next())
354   {
355     const TopoDS_Face & F = TopoDS::Face(expl.Current());
356     Handle(Poly_Triangulation) T = BRep_Tool::Triangulation(F, L);
357     if (T.IsNull())
358       continue;
359
360     gp_Trsf trsf;
361     Standard_Boolean transform = !L.IsIdentity();
362     if (transform)
363       trsf = L.Transformation();
364
365     const TColgp_Array1OfPnt& nodes = T->Nodes();
366     const Poly_Array1OfTriangle& triangles = T->Triangles();
367     Standard_Integer itriangle = triangles.Lower(), nb_triangles = triangles.Upper();
368     for (; itriangle <= nb_triangles; itriangle++)
369     {
370       ithread_triangle++;
371       if (ithread_triangle < start_thread_triangle )
372         continue;
373       if (ithread_triangle > end_thread_triangle)
374       {
375         if (ithread == 1)
376           progress = 100;
377 #ifdef CONV_DUMP
378         if (ithread == 1)
379           printf("Progress = %d  \r", progress);
380 #endif
381         return Standard_True;
382       }
383
384       const Poly_Triangle& t = triangles.Value(itriangle);
385
386       t.Get(n1, n2, n3);
387       gp_Pnt p1 = nodes.Value(n1);
388       gp_Pnt p2 = nodes.Value(n2);
389       gp_Pnt p3 = nodes.Value(n3);
390       if (transform)
391       {
392         p1.Transform(trsf);
393         p2.Transform(trsf);
394         p3.Transform(trsf);
395       }
396
397       // Get boundary box of the triangle
398       GetBndBox(p1, p2, p3, xmin, ymin, zmin, xmax, ymax, zmax);
399
400       // Find the range of voxels inside the boudary box of the triangle.
401       if (!ds->GetVoxel(xmin, ymin, zmin, ixmin, iymin, izmin))
402         continue;
403       if (!ds->GetVoxel(xmax, ymax, zmax, ixmax, iymax, izmax))
404         continue;
405
406       // Perform triangle-box intersection to find the voxels resulting from the processed triangle.;
407       // Using SAT theorem to quickly find the intersection.
408       ComputeVoxelsNearTriangle(p1, p2, p3,
409                                 extents, extents2, extents4, 
410                                 ixmin, iymin, izmin, ixmax, iymax, izmax);
411
412       // Progress
413       if (ithread == 1)
414       {
415         iprogress++;
416         progress = (Standard_Integer) ( (Standard_Real) iprogress / (Standard_Real) myNbTriangles * 100.0 );
417       }
418 #ifdef CONV_DUMP
419       if (ithread == 1 && prev_progress != progress)
420       {
421         printf("Progress = %d  \r", progress);
422         prev_progress = progress;
423       }
424 #endif
425
426     } // iteration of triangles
427   } // iteration of faces
428
429   if (ithread == 1)
430     progress = 100;
431 #ifdef CONV_DUMP
432   if (ithread == 1)
433     printf("Progress = %d  \r", progress);
434 #endif
435   return Standard_True;
436 }
437
438 Standard_Boolean Voxel_FastConverter::FillInVolume(const Standard_Byte inner,
439                                                    const Standard_Integer ithread)
440 {
441   Voxel_DS* ds = (Voxel_DS*) myVoxels;
442   Standard_Integer ix, iy, iz, nbx = ds->GetNbX(), nby = ds->GetNbY(), nbz = ds->GetNbZ();
443   Standard_Boolean prev_surface, surface, volume;
444
445   if (inner)
446   {
447     // Fill-in internal voxels by the value "inner"
448     for (ix = 0; ix < nbx; ix++)
449     {
450       for (iy = 0; iy < nby; iy++)
451       {
452         // Check existence of volume.
453         volume = Standard_False;
454         surface = Standard_False;
455         prev_surface = Standard_False;
456         for (iz = 0; iz < nbz; iz++)
457         {
458           surface = (myIsBool == 1) ? 
459             ((Voxel_BoolDS*)myVoxels)->Get(ix, iy, iz) == Standard_True : 
460               ((Voxel_ColorDS*)myVoxels)->Get(ix, iy, iz) > 0;
461           if (prev_surface && !surface)
462           {
463             volume = !volume;
464           }
465           prev_surface = surface;
466         }
467         if (volume)
468           continue;
469
470         // Fill-in the volume.
471         volume = Standard_False;
472         surface = Standard_False;
473         prev_surface = Standard_False;
474         for (iz = 0; iz < nbz; iz++)
475         {
476           surface = (myIsBool == 1) ? 
477             ((Voxel_BoolDS*)myVoxels)->Get(ix, iy, iz) == Standard_True : 
478               ((Voxel_ColorDS*)myVoxels)->Get(ix, iy, iz) > 0;
479           if (prev_surface && !surface)
480           {
481             volume = !volume;
482           }
483           if (volume && !surface)
484           {
485             (myIsBool == 1) ? ((Voxel_BoolDS*)myVoxels)->Set(ix, iy, iz, inner) : 
486               ((Voxel_ColorDS*)myVoxels)->Set(ix, iy, iz, inner);
487           }
488           prev_surface = surface;
489         }
490       }
491     }
492   }
493   else
494   {
495     // Set value of interbal voxels to 0 ("inner" = 0)
496     Standard_Boolean next_surface;
497     for (ix = 0; ix < nbx; ix++)
498     {
499       for (iy = 0; iy < nby; iy++)
500       {
501         volume = Standard_False;
502         surface = Standard_False;
503         prev_surface = Standard_False;
504         next_surface = Standard_False;
505         for (iz = 0; iz < nbz; iz++)
506         {
507           surface = (myIsBool == 1) ? 
508             ((Voxel_BoolDS*)myVoxels)->Get(ix, iy, iz) == Standard_True : 
509               ((Voxel_ColorDS*)myVoxels)->Get(ix, iy, iz) > 0;
510           if (prev_surface != surface)
511           {
512             volume = !volume;
513           }
514           if (volume && iz + 1 < nbz)
515           {
516             next_surface = (myIsBool == 1) ? 
517               ((Voxel_BoolDS*)myVoxels)->Get(ix, iy, iz + 1) == Standard_True : 
518               ((Voxel_ColorDS*)myVoxels)->Get(ix, iy, iz + 1) > 0;
519           }
520           if (volume && prev_surface == surface && next_surface)
521           {
522             (myIsBool == 1) ? ((Voxel_BoolDS*)myVoxels)->Set(ix, iy, iz, inner) : 
523               ((Voxel_ColorDS*)myVoxels)->Set(ix, iy, iz, inner);
524           }
525           prev_surface = surface;
526         }
527       }
528     }
529   }
530
531   return Standard_True;
532 }
533
534 void Voxel_FastConverter::GetBndBox(const gp_Pnt&  p1,
535                                     const gp_Pnt&  p2,
536                                     const gp_Pnt&  p3,
537                                     Standard_Real& xmin,
538                                     Standard_Real& ymin,
539                                     Standard_Real& zmin,
540                                     Standard_Real& xmax,
541                                     Standard_Real& ymax,
542                                     Standard_Real& zmax) const
543 {
544   // P1:
545   xmin = p1.X();
546   ymin = p1.Y();
547   zmin = p1.Z();
548   xmax = p1.X();
549   ymax = p1.Y();
550   zmax = p1.Z();
551   // P2:
552   if (xmin > p2.X())
553     xmin = p2.X();
554   if (ymin > p2.Y())
555     ymin = p2.Y();
556   if (zmin > p2.Z())
557     zmin = p2.Z();
558   if (xmax < p2.X())
559     xmax = p2.X();
560   if (ymax < p2.Y())
561     ymax = p2.Y();
562   if (zmax < p2.Z())
563     zmax = p2.Z();
564   // P3:
565   if (xmin > p3.X())
566     xmin = p3.X();
567   if (ymin > p3.Y())
568     ymin = p3.Y();
569   if (zmin > p3.Z())
570     zmin = p3.Z();
571   if (xmax < p3.X())
572     xmax = p3.X();
573   if (ymax < p3.Y())
574     ymax = p3.Y();
575   if (zmax < p3.Z())
576     zmax = p3.Z();
577 }
578
579 // This method is copied from Voxel_ShapeIntersector.cxx
580 static Standard_Boolean mayIntersect(const gp_Pnt2d& p11, const gp_Pnt2d& p12,
581                                      const gp_Pnt2d& p21, const gp_Pnt2d& p22)
582 {
583     if (p11.X() > p21.X() && p11.X() > p22.X() && p12.X() > p21.X() && p12.X() > p22.X())
584         return Standard_False;
585     if (p11.X() < p21.X() && p11.X() < p22.X() && p12.X() < p21.X() && p12.X() < p22.X())
586         return Standard_False;
587     if (p11.Y() > p21.Y() && p11.Y() > p22.Y() && p12.Y() > p21.Y() && p12.Y() > p22.Y())
588         return Standard_False;
589     if (p11.Y() < p21.Y() && p11.Y() < p22.Y() && p12.Y() < p21.Y() && p12.Y() < p22.Y())
590         return Standard_False;
591     return Standard_True;
592 }
593
594 void Voxel_FastConverter::ComputeVoxelsNearTriangle(const gp_Pln&          plane,
595                                                     const gp_Pnt&          p1,
596                                                     const gp_Pnt&          p2,
597                                                     const gp_Pnt&          p3,
598                                                     const Standard_Real    hdiagonal,
599                                                     const Standard_Integer ixmin,
600                                                     const Standard_Integer iymin,
601                                                     const Standard_Integer izmin,
602                                                     const Standard_Integer ixmax,
603                                                     const Standard_Integer iymax,
604                                                     const Standard_Integer izmax) const
605 {
606   gp_Pnt pc;
607   Standard_Real xc, yc, zc, uc, vc, u1, v1, u2, v2, u3, v3;
608   Standard_Integer ix, iy, iz;
609   IntAna2d_AnaIntersection intersector2d;
610
611   // Project points of triangle onto the plane
612   ElSLib::Parameters(plane, p1, u1, v1);
613   ElSLib::Parameters(plane, p2, u2, v2);
614   ElSLib::Parameters(plane, p3, u3, v3);
615
616   // Make lines of triangle
617   gp_Pnt2d p2d1(u1, v1), p2d2(u2, v2), p2d3(u3, v3), p2dt((u1+u2+u3)/3.0,(v1+v2+v3)/3.0), p2dc;
618   gp_Vec2d v2d12(p2d1, p2d2), v2d23(p2d2, p2d3), v2d31(p2d3, p2d1);
619   gp_Lin2d L1(p2d1, v2d12), L2(p2d2, v2d23), L3(p2d3, v2d31), Lv;
620   Standard_Real d1 = p2d1.Distance(p2d2) - Precision::Confusion(), 
621                 d2 = p2d2.Distance(p2d3) - Precision::Confusion(), 
622                 d3 = p2d3.Distance(p2d1) - Precision::Confusion(), dv;
623
624   Voxel_DS* ds = (Voxel_DS*) myVoxels;
625   for (ix = ixmin; ix <= ixmax; ix++)
626   {
627     for (iy = iymin; iy <= iymax; iy++)
628     {
629       for (iz = izmin; iz <= izmax; iz++)
630       {
631         ds->GetCenter(ix, iy, iz, xc, yc, zc);
632         pc.SetCoord(xc, yc, zc);
633         if (plane.Distance(pc) < hdiagonal)
634         {
635           ElSLib::Parameters(plane, pc, uc, vc);
636           p2dc.SetCoord(uc, vc);
637
638           gp_Vec2d v2dct(p2dc, p2dt);
639           dv = v2dct.Magnitude() - Precision::Confusion();
640           Lv.SetLocation(p2dc);
641           Lv.SetDirection(v2dct);
642
643           // Side 1:
644           if (mayIntersect(p2d1, p2d2, p2dc, p2dt))
645           {
646             intersector2d.Perform(Lv, L1);
647             if (intersector2d.IsDone() && !intersector2d.ParallelElements() && intersector2d.NbPoints())
648             {
649               const IntAna2d_IntPoint& i2d = intersector2d.Point(1);
650               Standard_Real param1 = i2d.ParamOnFirst();
651               Standard_Real param2 = i2d.ParamOnSecond();
652               if (param1 > Precision::Confusion() && param1 < dv && 
653                   param2 > Precision::Confusion() && param2 < d1)
654               {
655                 continue;
656               }    
657             }
658           }
659
660           // Side 2:
661           if (mayIntersect(p2d2, p2d3, p2dc, p2dt))
662           {
663             intersector2d.Perform(Lv, L2);
664             if (intersector2d.IsDone() && !intersector2d.ParallelElements() && intersector2d.NbPoints())
665             {
666               const IntAna2d_IntPoint& i2d = intersector2d.Point(1);
667               Standard_Real param1 = i2d.ParamOnFirst();
668               Standard_Real param2 = i2d.ParamOnSecond();
669               if (param1 > Precision::Confusion() && param1 < dv && 
670                   param2 > Precision::Confusion() && param2 < d2)
671               {
672                 continue;
673               }    
674             }
675           }
676
677           // Side 3:
678           if (mayIntersect(p2d3, p2d1, p2dc, p2dt))
679           {
680             intersector2d.Perform(Lv, L3);
681             if (intersector2d.IsDone() && !intersector2d.ParallelElements() && intersector2d.NbPoints())
682             {
683               const IntAna2d_IntPoint& i2d = intersector2d.Point(1);
684               Standard_Real param1 = i2d.ParamOnFirst();
685               Standard_Real param2 = i2d.ParamOnSecond();
686               if (param1 > Precision::Confusion() && param1 < dv && 
687                   param2 > Precision::Confusion() && param2 < d3)
688               {
689                 continue;
690               }    
691             }
692           }
693
694           // Set positive value to this voxel:
695           switch (myIsBool)
696           {
697             case 0:
698               ((Voxel_ColorDS*) myVoxels)->Set(ix, iy, iz, 15);
699               break;
700             case 1:
701               ((Voxel_BoolDS*) myVoxels)->Set(ix, iy, iz, Standard_True);
702               break;
703             case 2:
704             {
705               //((Voxel_ROctBoolDS*) myVoxels)->Set(ix, iy, iz, Standard_True);
706
707               // Check intersection between the triangle & sub-voxels of the voxel.
708               Standard_Real hdiagonal2 = hdiagonal / 2.0, hdiagonal4 = hdiagonal / 4.0;
709               for (Standard_Integer i = 0; i < 8; i++)
710               {
711                 ((Voxel_ROctBoolDS*) myVoxels)->GetCenter(ix, iy, iz, i, xc, yc, zc);
712                 pc.SetCoord(xc, yc, zc);
713                 if (plane.Distance(pc) < hdiagonal2)
714                 {
715                   ElSLib::Parameters(plane, pc, uc, vc);
716                   p2dc.SetCoord(uc, vc);
717
718                   gp_Vec2d v2dct(p2dc, p2dt);
719                   dv = v2dct.Magnitude() - Precision::Confusion();
720                   Lv.SetLocation(p2dc);
721                   Lv.SetDirection(v2dct);
722
723                   // Side 1:
724                   if (mayIntersect(p2d1, p2d2, p2dc, p2dt))
725                   {
726                     intersector2d.Perform(Lv, L1);
727                     if (intersector2d.IsDone() && !intersector2d.ParallelElements() && intersector2d.NbPoints())
728                     {
729                       const IntAna2d_IntPoint& i2d = intersector2d.Point(1);
730                       Standard_Real param1 = i2d.ParamOnFirst();
731                       Standard_Real param2 = i2d.ParamOnSecond();
732                       if (param1 > Precision::Confusion() && param1 < dv && 
733                           param2 > Precision::Confusion() && param2 < d1)
734                       {
735                         continue;
736                       }    
737                     }
738                   }
739
740                   // Side 2:
741                   if (mayIntersect(p2d2, p2d3, p2dc, p2dt))
742                   {
743                     intersector2d.Perform(Lv, L2);
744                     if (intersector2d.IsDone() && !intersector2d.ParallelElements() && intersector2d.NbPoints())
745                     {
746                       const IntAna2d_IntPoint& i2d = intersector2d.Point(1);
747                       Standard_Real param1 = i2d.ParamOnFirst();
748                       Standard_Real param2 = i2d.ParamOnSecond();
749                       if (param1 > Precision::Confusion() && param1 < dv && 
750                           param2 > Precision::Confusion() && param2 < d2)
751                       {
752                         continue;
753                       }    
754                     }
755                   }
756
757                   // Side 3:
758                   if (mayIntersect(p2d3, p2d1, p2dc, p2dt))
759                   {
760                     intersector2d.Perform(Lv, L3);
761                     if (intersector2d.IsDone() && !intersector2d.ParallelElements() && intersector2d.NbPoints())
762                     {
763                       const IntAna2d_IntPoint& i2d = intersector2d.Point(1);
764                       Standard_Real param1 = i2d.ParamOnFirst();
765                       Standard_Real param2 = i2d.ParamOnSecond();
766                       if (param1 > Precision::Confusion() && param1 < dv && 
767                           param2 > Precision::Confusion() && param2 < d3)
768                       {
769                         continue;
770                       }    
771                     }
772                   }
773
774                   //((Voxel_ROctBoolDS*) myVoxels)->Set(ix, iy, iz, i, Standard_True);
775
776                   // Check intersection between the triangle & sub-voxels of the sub-voxel.
777                   for (Standard_Integer j = 0; j < 8; j++)
778                   {
779                     ((Voxel_ROctBoolDS*) myVoxels)->GetCenter(ix, iy, iz, i, j, xc, yc, zc);
780                     pc.SetCoord(xc, yc, zc);
781                     if (plane.Distance(pc) < hdiagonal4)
782                     {
783                       ElSLib::Parameters(plane, pc, uc, vc);
784                       p2dc.SetCoord(uc, vc);
785
786                       gp_Vec2d v2dct(p2dc, p2dt);
787                       dv = v2dct.Magnitude() - Precision::Confusion();
788                       Lv.SetLocation(p2dc);
789                       Lv.SetDirection(v2dct);
790                       
791                       // Side 1:
792                       if (mayIntersect(p2d1, p2d2, p2dc, p2dt))
793                       {
794                         intersector2d.Perform(Lv, L1);
795                         if (intersector2d.IsDone() && !intersector2d.ParallelElements() && intersector2d.NbPoints())
796                         {
797                           const IntAna2d_IntPoint& i2d = intersector2d.Point(1);
798                           Standard_Real param1 = i2d.ParamOnFirst();
799                           Standard_Real param2 = i2d.ParamOnSecond();
800                           if (param1 > Precision::Confusion() && param1 < dv && 
801                               param2 > Precision::Confusion() && param2 < d1)
802                           {
803                             continue;
804                           }    
805                         }
806                       }
807                       
808                       // Side 2:
809                       if (mayIntersect(p2d2, p2d3, p2dc, p2dt))
810                       {
811                         intersector2d.Perform(Lv, L2);
812                         if (intersector2d.IsDone() && !intersector2d.ParallelElements() && intersector2d.NbPoints())
813                         {
814                           const IntAna2d_IntPoint& i2d = intersector2d.Point(1);
815                           Standard_Real param1 = i2d.ParamOnFirst();
816                           Standard_Real param2 = i2d.ParamOnSecond();
817                           if (param1 > Precision::Confusion() && param1 < dv && 
818                               param2 > Precision::Confusion() && param2 < d2)
819                           {
820                             continue;
821                           }    
822                         }
823                       }
824
825                       // Side 3:
826                       if (mayIntersect(p2d3, p2d1, p2dc, p2dt))
827                       {
828                         intersector2d.Perform(Lv, L3);
829                         if (intersector2d.IsDone() && !intersector2d.ParallelElements() && intersector2d.NbPoints())
830                         {
831                           const IntAna2d_IntPoint& i2d = intersector2d.Point(1);
832                           Standard_Real param1 = i2d.ParamOnFirst();
833                           Standard_Real param2 = i2d.ParamOnSecond();
834                           if (param1 > Precision::Confusion() && param1 < dv && 
835                               param2 > Precision::Confusion() && param2 < d3)
836                           {
837                             continue;
838                           }    
839                         }
840                       }
841
842                       ((Voxel_ROctBoolDS*) myVoxels)->Set(ix, iy, iz, i, j, Standard_True);
843                     }
844                   } // End of "Check level 2".
845
846                 }
847               } // End of "Check level 1".
848  
849               break;
850             }
851           }
852         }
853       }
854     }
855   }
856 }
857
858 //! This macro quickly finds the min & max values among 3 variables
859 #define FINDMINMAX(x0, x1, x2, min, max)        \
860   min = max = x0;                               \
861   if(x1<min) min=x1;                            \
862   if(x1>max) max=x1;                            \
863   if(x2<min) min=x2;                            \
864   if(x2>max) max=x2;
865
866 static bool planeBoxOverlap(const gp_Vec & normal, const double d, const gp_Pnt & maxbox)
867 {
868   gp_Vec vmin, vmax;
869   if(normal.X() > 0.0)    { vmin.SetX(-maxbox.X()); vmax.SetX(maxbox.X()); }
870   else                    { vmin.SetX(maxbox.X());  vmax.SetX(-maxbox.X()); }
871   
872   if(normal.Y() > 0.0)    { vmin.SetY(-maxbox.Y()); vmax.SetY(maxbox.Y()); }
873   else                    { vmin.SetY(maxbox.Y());  vmax.SetY(-maxbox.Y()); }
874   
875   if(normal.Z() > 0.0)    { vmin.SetZ(-maxbox.Z()); vmax.SetZ(maxbox.Z()); }
876   else                    { vmin.SetZ(maxbox.Z());  vmax.SetZ(-maxbox.Z()); }
877   
878   if((normal.Dot(vmin)) + d > 0.0) return false;
879   if((normal.Dot(vmax)) + d>= 0.0) return true;
880   
881   return false;
882 }
883
884 #define AXISTEST_X01(a, b, fa, fb)                            \
885     min = a*v0.Y() - b*v0.Z();                                    \
886     max = a*v2.Y() - b*v2.Z();                                    \
887     if(min>max) {const double tmp=max; max=min; min=tmp;    }    \
888     rad = fa * extents.Y() + fb * extents.Z();                    \
889     if(min>rad || max<-rad) return false;
890
891 #define AXISTEST_X2(a, b, fa, fb)                            \
892     min = a*v0.Y() - b*v0.Z();                                    \
893     max = a*v1.Y() - b*v1.Z();                                    \
894     if(min>max) {const double tmp=max; max=min; min=tmp;    }    \
895     rad = fa * extents.Y() + fb * extents.Z();                    \
896     if(min>rad || max<-rad) return false;
897
898 #define AXISTEST_Y02(a, b, fa, fb)                            \
899     min = b*v0.Z() - a*v0.X();                                    \
900     max = b*v2.Z() - a*v2.X();                                    \
901     if(min>max) {const double tmp=max; max=min; min=tmp;    }    \
902     rad = fa * extents.X() + fb * extents.Z();                    \
903     if(min>rad || max<-rad) return false;
904
905 #define AXISTEST_Y1(a, b, fa, fb)                            \
906     min = b*v0.Z() - a*v0.X();                                    \
907     max = b*v1.Z() - a*v1.X();                                    \
908     if(min>max) {const double tmp=max; max=min; min=tmp;    }    \
909     rad = fa * extents.X() + fb * extents.Z();                    \
910     if(min>rad || max<-rad) return false;
911
912 #define AXISTEST_Z12(a, b, fa, fb)                            \
913     min = a*v1.X() - b*v1.Y();                                    \
914     max = a*v2.X() - b*v2.Y();                                    \
915     if(min>max) {const double tmp=max; max=min; min=tmp;    }    \
916     rad = fa * extents.X() + fb * extents.Y();                    \
917     if(min>rad || max<-rad) return false;
918
919 #define AXISTEST_Z0(a, b, fa, fb)                            \
920     min = a*v0.X() - b*v0.Y();                                    \
921     max = a*v1.X() - b*v1.Y();                                    \
922     if(min>max) {const double tmp=max; max=min; min=tmp;    }    \
923     rad = fa * extents.X() + fb * extents.Y();                    \
924     if(min>rad || max<-rad) return false;
925
926 // compute triangle edges
927 // - edges lazy evaluated to take advantage of early exits
928 // - fabs precomputed (half less work, possible since extents are always >0)
929 // - customized macros to take advantage of the null component
930 // - axis vector discarded, possibly saves useless movs
931 #define IMPLEMENT_CLASS3_TESTS                        \
932   double rad;                                         \
933   double min, max;                                    \
934                                                       \
935   const double fey0 = fabs(e0.Y());                   \
936   const double fez0 = fabs(e0.Z());                   \
937   AXISTEST_X01(e0.Z(), e0.Y(), fez0, fey0);           \
938   const double fex0 = fabs(e0.X());                   \
939   AXISTEST_Y02(e0.Z(), e0.X(), fez0, fex0);           \
940   AXISTEST_Z12(e0.Y(), e0.X(), fey0, fex0);           \
941                                                       \
942   const double fey1 = fabs(e1.Y());                   \
943   const double fez1 = fabs(e1.Z());                   \
944   AXISTEST_X01(e1.Z(), e1.Y(), fez1, fey1);           \
945   const double fex1 = fabs(e1.X());                   \
946   AXISTEST_Y02(e1.Z(), e1.X(), fez1, fex1);           \
947   AXISTEST_Z0(e1.Y(), e1.X(), fey1, fex1);            \
948                                                       \
949   const gp_Vec e2 = v2 - v0;                          \
950   const double fey2 = fabs(e2.Y());                   \
951   const double fez2 = fabs(e2.Z());                   \
952   AXISTEST_X2(e2.Z(), e2.Y(), fez2, fey2);            \
953   const double fex2 = fabs(e2.X());                   \
954   AXISTEST_Y1(e2.Z(), e2.X(), fez2, fex2);            \
955   AXISTEST_Z12(e2.Y(), e2.X(), fey2, fex2);
956
957 static bool TriBoxOverlap(const gp_Pnt & p1, const gp_Pnt & p2, const gp_Pnt & p3,
958                           const gp_Pnt & center, const gp_Pnt & extents)
959 {
960   // use separating axis theorem to test overlap between triangle and box 
961   // need to test for overlap in these directions: 
962   // 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle 
963   //    we do not even need to test these) 
964   // 2) normal of the triangle 
965   // 3) crossproduct(edge from tri, {x,y,z}-directin) 
966   //    this gives 3x3=9 more tests 
967   
968   // move everything so that the boxcenter is in (0,0,0)
969   gp_Vec v0(center, p1);
970   gp_Vec v1(center, p2);
971   gp_Vec v2(center, p3);
972   
973   // First, test overlap in the {x,y,z}-directions
974   double min,max;
975   // Find min, max of the triangle in x-direction, and test for overlap in X
976   FINDMINMAX(v0.X(), v1.X(), v2.X(), min, max);
977   if(min>extents.X() || max<-extents.X()) return false;
978   
979   // Same for Y
980   FINDMINMAX(v0.Y(), v1.Y(), v2.Y(), min, max);
981   if(min>extents.Y() || max<-extents.Y()) return false;
982   
983   // Same for Z
984   FINDMINMAX(v0.Z(), v1.Z(), v2.Z(), min, max);
985   if(min>extents.Z() || max<-extents.Z()) return false;
986   
987   // 2) Test if the box intersects the plane of the triangle
988   // compute plane equation of triangle: normal*x+d=0
989   // ### could be precomputed since we use the same leaf triangle several times
990   const gp_Vec e0 = v1 - v0;
991   const gp_Vec e1 = v2 - v1;
992   const gp_Vec normal = e0.Crossed(e1);
993   const double d = -normal.Dot(v0);
994   if(!planeBoxOverlap(normal, d, extents)) return false;
995   
996   // 3) "Class III" tests
997   //if(mFullPrimBoxTest)
998   {
999     IMPLEMENT_CLASS3_TESTS
1000   }
1001   
1002   return true;
1003 }
1004
1005 void Voxel_FastConverter::ComputeVoxelsNearTriangle(const gp_Pnt& p1,
1006                                                     const gp_Pnt& p2,
1007                                                     const gp_Pnt& p3,
1008                                                     const gp_Pnt& extents,
1009                                                     const gp_Pnt& extents2,
1010                                                     const gp_Pnt& extents4,
1011                                                     const Standard_Integer ixmin,
1012                                                     const Standard_Integer iymin,
1013                                                     const Standard_Integer izmin,
1014                                                     const Standard_Integer ixmax,
1015                                                     const Standard_Integer iymax,
1016                                                     const Standard_Integer izmax) const
1017 {
1018   gp_Pnt pc;
1019   Standard_Real xc, yc, zc;
1020   Standard_Integer ix, iy, iz;
1021
1022   Voxel_DS* ds = (Voxel_DS*) myVoxels;
1023   for (ix = ixmin; ix <= ixmax; ix++)
1024   {
1025     for (iy = iymin; iy <= iymax; iy++)
1026     {
1027       for (iz = izmin; iz <= izmax; iz++)
1028       {
1029         ds->GetCenter(ix, iy, iz, xc, yc, zc);
1030         pc.SetCoord(xc, yc, zc);
1031
1032         if(TriBoxOverlap(p1, p2, p3, pc, extents))
1033         {
1034           // Set positive value to this voxel:
1035           switch (myIsBool)
1036           {
1037             case 0:
1038               ((Voxel_ColorDS*) myVoxels)->Set(ix, iy, iz, 15);
1039               break;
1040             case 1:
1041               ((Voxel_BoolDS*) myVoxels)->Set(ix, iy, iz, Standard_True);
1042               break;
1043             case 2:
1044             {
1045               //((Voxel_ROctBoolDS*) myVoxels)->Set(ix, iy, iz, Standard_True);
1046
1047               // Check intersection between the triangle & sub-voxels of the voxel.
1048               for (Standard_Integer i = 0; i < 8; i++)
1049               {
1050                 ((Voxel_ROctBoolDS*) myVoxels)->GetCenter(ix, iy, iz, i, xc, yc, zc);
1051                 pc.SetCoord(xc, yc, zc);
1052                 if(TriBoxOverlap(p1, p2, p3, pc, extents2))
1053                 {
1054                   //((Voxel_ROctBoolDS*) myVoxels)->Set(ix, iy, iz, i, Standard_True);
1055
1056                   // Check intersection between the triangle & sub-voxels of the sub-voxel.
1057                   for (Standard_Integer j = 0; j < 8; j++)
1058                   {
1059                     ((Voxel_ROctBoolDS*) myVoxels)->GetCenter(ix, iy, iz, i, j, xc, yc, zc);
1060                     pc.SetCoord(xc, yc, zc);
1061                     if(TriBoxOverlap(p1, p2, p3, pc, extents4))
1062                     {
1063                       ((Voxel_ROctBoolDS*) myVoxels)->Set(ix, iy, iz, i, j, Standard_True);
1064                     }
1065                   } // End of "Check level 2".
1066
1067                 }
1068               } // End of "Check level 1".
1069               break;
1070             }
1071           }
1072         }
1073       }
1074     }
1075   }
1076 }