0030655: Modeling Data - Provide interfaces for selection of the elements from BVH...
[occt.git] / src / QABugs / QABugs_BVH.cxx
1 // Created by: Eugeny MALTCHIKOV
2 // Created on: 2019-04-17
3 // Copyright (c) 2019 OPEN CASCADE SAS
4 //
5 // This file is part of Open CASCADE Technology software library.
6 //
7 // This library is free software; you can redistribute it and/or modify it under
8 // the terms of the GNU Lesser General Public License version 2.1 as published
9 // by the Free Software Foundation, with special exception defined in the file
10 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
11 // distribution for complete text of the license and disclaimer of any warranty.
12 //
13 // Alternatively, this file may be used under the terms of Open CASCADE
14 // commercial license or contractual agreement.
15
16 #include <QABugs.hxx>
17
18 #include <Bnd_Box.hxx>
19 #include <Bnd_Tools.hxx>
20
21 #include <BRep_Builder.hxx>
22
23 #include <BRepBndLib.hxx>
24
25 #include <BVH_Box.hxx>
26 #include <BVH_BoxSet.hxx>
27 #include <BVH_DistanceField.hxx>
28 #include <BVH_Geometry.hxx>
29 #include <BVH_LinearBuilder.hxx>
30 #include <BVH_PairDistance.hxx>
31 #include <BVH_Traverse.hxx>
32 #include <BVH_Triangulation.hxx>
33
34 #include <DBRep.hxx>
35 #include <Draw.hxx>
36
37 #include <Precision.hxx>
38
39 #include <TopExp.hxx>
40 #include <TopExp_Explorer.hxx>
41
42 #include <TopoDS.hxx>
43 #include <TopoDS_Compound.hxx>
44 #include <TopoDS_Face.hxx>
45 #include <TopoDS_Shape.hxx>
46
47 #include <TopTools_IndexedMapOfShape.hxx>
48
49 //=======================================================================
50 //function : ShapeSelector
51 //purpose : Implement the simplest shape's selector
52 //=======================================================================
53 class ShapeSelector :
54   public BVH_Traverse <Standard_Real, 3, BVH_BoxSet <Standard_Real, 3, TopoDS_Shape>, Standard_Boolean>
55 {
56 public:
57   //! Constructor
58   ShapeSelector() {}
59
60   //! Sets the Box for selection
61   void SetBox (const Bnd_Box& theBox)
62   {
63     myBox = Bnd_Tools::Bnd2BVH (theBox);
64   }
65
66   //! Returns the selected shapes
67   const NCollection_List<TopoDS_Shape>& Shapes () const { return myShapes; }
68
69 public:
70
71   //! Defines the rules for node rejection by bounding box
72   virtual Standard_Boolean RejectNode (const BVH_Vec3d& theCornerMin,
73                                        const BVH_Vec3d& theCornerMax,
74                                        Standard_Boolean& theIsInside) const Standard_OVERRIDE
75   {
76     Standard_Boolean hasOverlap;
77     theIsInside = myBox.Contains (theCornerMin, theCornerMax, hasOverlap);
78     return !hasOverlap;
79   }
80
81   //! Defines the rules for leaf acceptance
82   virtual Standard_Boolean AcceptMetric (const Standard_Boolean& theIsInside) const Standard_OVERRIDE
83   {
84     return theIsInside;
85   }
86
87   //! Defines the rules for leaf acceptance
88   virtual Standard_Boolean Accept (const Standard_Integer theIndex,
89                                    const Standard_Boolean& theIsInside) Standard_OVERRIDE
90   {
91     if (theIsInside || !myBox.IsOut (myBVHSet->Box (theIndex)))
92     {
93       myShapes.Append (myBVHSet->Element (theIndex));
94       return Standard_True;
95     }
96     return Standard_False;
97   }
98
99 protected:
100
101   BVH_Box <Standard_Real, 3> myBox;         //!< Selection box
102   NCollection_List <TopoDS_Shape> myShapes; //!< Selected shapes
103 };
104
105 //=======================================================================
106 //function : ShapeSelector
107 //purpose : Implement the simplest shape's selector
108 //=======================================================================
109 class ShapeSelectorVoid :
110   public BVH_Traverse <Standard_Real, 3, void, Standard_Boolean>
111 {
112 public:
113   //! Constructor
114   ShapeSelectorVoid() {}
115
116   //! Sets the Box for selection
117   void SetBox (const Bnd_Box& theBox)
118   {
119     myBox = Bnd_Tools::Bnd2BVH (theBox);
120   }
121
122   //! Returns the selected shapes
123   const NCollection_List<TopoDS_Shape>& Shapes () const { return myShapes; }
124
125 public:
126
127   //! Sets the Box Set
128   void SetShapeBoxSet (const opencascade::handle<BVH_BoxSet<Standard_Real, 3, TopoDS_Shape>>& theBoxSet)
129   {
130     myBoxSet = theBoxSet;
131   }
132
133 public:
134
135   //! Defines the rules for node rejection by bounding box
136   virtual Standard_Boolean RejectNode (const BVH_Vec3d& theCornerMin,
137                                        const BVH_Vec3d& theCornerMax,
138                                        Standard_Boolean& theIsInside) const Standard_OVERRIDE
139   {
140     Standard_Boolean hasOverlap;
141     theIsInside = myBox.Contains (theCornerMin, theCornerMax, hasOverlap);
142     return !hasOverlap;
143   }
144
145   //! Defines the rules for leaf acceptance
146   virtual Standard_Boolean AcceptMetric (const Standard_Boolean& theIsInside) const Standard_OVERRIDE
147   {
148     return theIsInside;
149   }
150
151   //! Defines the rules for leaf acceptance
152   virtual Standard_Boolean Accept (const Standard_Integer theIndex,
153                                    const Standard_Boolean& theIsInside) Standard_OVERRIDE
154   {
155     if (theIsInside || !myBox.IsOut (myBoxSet->Box (theIndex)))
156     {
157       myShapes.Append (myBoxSet->Element (theIndex));
158       return Standard_True;
159     }
160     return Standard_False;
161   }
162
163 protected:
164
165   opencascade::handle<BVH_BoxSet<Standard_Real, 3, TopoDS_Shape>> myBoxSet; //!< ShapeBoxSet
166   BVH_Box <Standard_Real, 3> myBox;         //!< Selection box
167   NCollection_List <TopoDS_Shape> myShapes; //!< Selected shapes
168 };
169
170 //=======================================================================
171 //function : QABVH_ShapeSelect
172 //purpose : Test the work of BVH on the simple example of shapes selection
173 //=======================================================================
174 static Standard_Integer QABVH_ShapeSelect (Draw_Interpretor& theDI,
175                                            Standard_Integer theArgc,
176                                            const char** theArgv)
177 {
178   if (theArgc < 4)
179   {
180     theDI.PrintHelp (theArgv[0]);
181     return 1;
182   }
183
184   // Get the shape to add its sub-shapes into BVH
185   TopoDS_Shape aShape = DBRep::Get (theArgv [2]);
186   if (aShape.IsNull())
187   {
188     std::cout << theArgv[2] << " does not exist" << std::endl;
189     return 1;
190   }
191
192   // Get the shape to get the Box for selection
193   TopoDS_Shape aBShape = DBRep::Get (theArgv [3]);
194   if (aBShape.IsNull())
195   {
196     std::cout << theArgv[3] << " does not exist" << std::endl;
197     return 1;
198   }
199
200   // Which selector to use
201   Standard_Boolean useVoidSelector = Standard_False;
202   if (theArgc > 4)
203     useVoidSelector = !strcmp (theArgv[4], "-void");
204
205   // Define BVH Builder
206   opencascade::handle <BVH_LinearBuilder <Standard_Real, 3> > aLBuilder =
207       new BVH_LinearBuilder <Standard_Real, 3>();
208
209   // Create the ShapeSet
210   opencascade::handle <BVH_BoxSet <Standard_Real, 3, TopoDS_Shape> > aShapeBoxSet =
211     new BVH_BoxSet <Standard_Real, 3, TopoDS_Shape> (aLBuilder);
212
213   // Add elements into BVH
214
215   // Map the shape
216   TopTools_IndexedMapOfShape aMapShapes;
217   TopExp::MapShapes (aShape, TopAbs_VERTEX, aMapShapes);
218   TopExp::MapShapes (aShape, TopAbs_EDGE,   aMapShapes);
219   TopExp::MapShapes (aShape, TopAbs_FACE,   aMapShapes);
220
221   for (Standard_Integer iS = 1; iS <= aMapShapes.Extent(); ++iS)
222   {
223     const TopoDS_Shape& aS = aMapShapes(iS);
224
225     Bnd_Box aSBox;
226     BRepBndLib::Add (aS, aSBox);
227
228     aShapeBoxSet->Add (aS, Bnd_Tools::Bnd2BVH (aSBox));
229   }
230
231   // Build BVH
232   aShapeBoxSet->Build();
233
234   TopTools_ListOfShape aSelectedShapes;
235
236   // Prepare a Box for selection
237   Bnd_Box aSelectionBox;
238   BRepBndLib::Add (aBShape, aSelectionBox);
239
240   // Perform selection
241   if (!useVoidSelector)
242   {
243     ShapeSelector aSelector;
244     aSelector.SetBox (aSelectionBox);
245     aSelector.SetBVHSet (aShapeBoxSet.get());
246     aSelector.Select();
247     aSelectedShapes = aSelector.Shapes();
248   }
249   else
250   {
251     ShapeSelectorVoid aSelector;
252     aSelector.SetBox (aSelectionBox);
253     aSelector.SetShapeBoxSet (aShapeBoxSet);
254     aSelector.Select (aShapeBoxSet->BVH());
255     aSelectedShapes = aSelector.Shapes();
256   }
257
258   // Draw the selected shapes
259   TopoDS_Compound aResult;
260   BRep_Builder().MakeCompound (aResult);
261
262   for (TopTools_ListOfShape::Iterator it (aSelectedShapes); it.More(); it.Next())
263     BRep_Builder().Add (aResult, it.Value());
264
265   DBRep::Set (theArgv[1], aResult);
266   return 0;
267 }
268
269 //=======================================================================
270 //function : PairShapeSelector
271 //purpose : Implement the simplest shape's selector
272 //=======================================================================
273 class PairShapesSelector :
274   public BVH_PairTraverse <Standard_Real, 3, BVH_BoxSet <Standard_Real, 3, TopoDS_Shape>>
275 {
276 public:
277   //! Constructor
278   PairShapesSelector() {}
279
280   //! Returns the selected pairs of shapes
281   const NCollection_List <std::pair <TopoDS_Shape, TopoDS_Shape> >& Pairs () const { return myPairs; }
282
283 public:
284
285   //! Defines the rules for node rejection
286   virtual Standard_Boolean RejectNode (const BVH_Vec3d& theCornerMin1,
287                                        const BVH_Vec3d& theCornerMax1,
288                                        const BVH_Vec3d& theCornerMin2,
289                                        const BVH_Vec3d& theCornerMax2,
290                                        Standard_Real&) const Standard_OVERRIDE
291   {
292     return BVH_Box <Standard_Real, 3>(theCornerMin1, theCornerMax1).IsOut (
293            BVH_Box <Standard_Real, 3>(theCornerMin2, theCornerMax2));
294   }
295
296   //! Defines the rules for leaf acceptance
297   virtual Standard_Boolean Accept (const Standard_Integer theIndex1,
298                                    const Standard_Integer theIndex2) Standard_OVERRIDE
299   {
300     BVH_Box<Standard_Real, 3> aBox1 = myBVHSet1->Box (theIndex1);
301     BVH_Box<Standard_Real, 3> aBox2 = myBVHSet2->Box (theIndex2);
302     if (!aBox1.IsOut (aBox2))
303     {
304       myPairs.Append (std::pair <TopoDS_Shape, TopoDS_Shape> (myBVHSet1->Element (theIndex1),
305                                                               myBVHSet2->Element (theIndex2)));
306       return Standard_True;
307     }
308     return Standard_False;
309   }
310
311 protected:
312
313   NCollection_List <std::pair <TopoDS_Shape, TopoDS_Shape> > myPairs; //!< Selected pairs
314 };
315
316 //=======================================================================
317 //function : PairShapeSelector
318 //purpose : Implement the simplest shape's selector
319 //=======================================================================
320 class PairShapesSelectorVoid :
321   public BVH_PairTraverse <Standard_Real, 3>
322 {
323 public:
324   //! Constructor
325   PairShapesSelectorVoid() {}
326
327   //! Returns the selected pairs of shapes
328   const NCollection_List <std::pair <TopoDS_Shape, TopoDS_Shape> >& Pairs () const { return myPairs; }
329
330 public:
331
332   //! Sets the sets to access the elements
333   void SetShapeBoxSets (const opencascade::handle<BVH_BoxSet<Standard_Real, 3, TopoDS_Shape>>& theSBSet1,
334                         const opencascade::handle<BVH_BoxSet<Standard_Real, 3, TopoDS_Shape>>& theSBSet2)
335   {
336     mySBSet1 = theSBSet1;
337     mySBSet2 = theSBSet2;
338   }
339
340 public:
341
342   //! Defines the rules for node rejection
343   virtual Standard_Boolean RejectNode (const BVH_Vec3d& theCornerMin1,
344                                        const BVH_Vec3d& theCornerMax1,
345                                        const BVH_Vec3d& theCornerMin2,
346                                        const BVH_Vec3d& theCornerMax2,
347                                        Standard_Real&) const Standard_OVERRIDE
348   {
349     return BVH_Box <Standard_Real, 3>(theCornerMin1, theCornerMax1).IsOut (
350            BVH_Box <Standard_Real, 3>(theCornerMin2, theCornerMax2));
351   }
352
353   //! Defines the rules for leaf acceptance
354   virtual Standard_Boolean Accept (const Standard_Integer theIndex1,
355                                    const Standard_Integer theIndex2) Standard_OVERRIDE
356   {
357     BVH_Box<Standard_Real, 3> aBox1 = mySBSet1->Box (theIndex1);
358     BVH_Box<Standard_Real, 3> aBox2 = mySBSet2->Box (theIndex2);
359     if (!aBox1.IsOut (aBox2))
360     {
361       myPairs.Append (std::pair <TopoDS_Shape, TopoDS_Shape> (mySBSet1->Element (theIndex1),
362                                                               mySBSet2->Element (theIndex2)));
363       return Standard_True;
364     }
365     return Standard_False;
366   }
367
368 protected:
369   opencascade::handle<BVH_BoxSet<Standard_Real, 3, TopoDS_Shape>> mySBSet1; //!< First ShapeBoxSet
370   opencascade::handle<BVH_BoxSet<Standard_Real, 3, TopoDS_Shape>> mySBSet2; //!< Second ShapeBoxSet
371   NCollection_List <std::pair <TopoDS_Shape, TopoDS_Shape> > myPairs; //!< Selected pairs
372 };
373
374 //=======================================================================
375 //function : QABVH_PairSelect
376 //purpose : Test the work of BVH on the simple example of pairs of shapes selection
377 //=======================================================================
378 static Standard_Integer QABVH_PairSelect (Draw_Interpretor& theDI,
379                                           Standard_Integer theArgc,
380                                           const char** theArgv)
381 {
382   if (theArgc < 4)
383   {
384     theDI.PrintHelp (theArgv[0]);
385     return 1;
386   }
387
388   TopoDS_Shape aShape[2];
389   // Get the first shape
390   aShape[0] = DBRep::Get (theArgv [2]);
391   if (aShape[0].IsNull())
392   {
393     std::cout << theArgv[2] << " does not exist" << std::endl;
394     return 1;
395   }
396
397   // Get the second shape
398   aShape[1] = DBRep::Get (theArgv [3]);
399   if (aShape[1].IsNull())
400   {
401     std::cout << theArgv[3] << " does not exist" << std::endl;
402     return 1;
403   }
404
405   // Which selector to use
406   Standard_Boolean useVoidSelector = Standard_False;
407   if (theArgc > 4)
408     useVoidSelector = !strcmp (theArgv[4], "-void");
409
410   // Define BVH Builder
411   opencascade::handle <BVH_LinearBuilder <Standard_Real, 3> > aLBuilder =
412       new BVH_LinearBuilder <Standard_Real, 3>();
413
414   // Create the ShapeSet
415   opencascade::handle <BVH_BoxSet <Standard_Real, 3, TopoDS_Shape> > aShapeBoxSet[2];
416
417   for (Standard_Integer i = 0; i < 2; ++i)
418   {
419     aShapeBoxSet[i] = new BVH_BoxSet <Standard_Real, 3, TopoDS_Shape> (aLBuilder);
420     // Add elements into set
421     TopTools_IndexedMapOfShape aMapShapes;
422     TopExp::MapShapes (aShape[i], TopAbs_VERTEX, aMapShapes);
423     TopExp::MapShapes (aShape[i], TopAbs_EDGE,   aMapShapes);
424     TopExp::MapShapes (aShape[i], TopAbs_FACE,   aMapShapes);
425
426     for (Standard_Integer iS = 1; iS <= aMapShapes.Extent(); ++iS)
427     {
428       const TopoDS_Shape& aS = aMapShapes(iS);
429   
430       Bnd_Box aSBox;
431       BRepBndLib::Add (aS, aSBox);
432   
433       aShapeBoxSet[i]->Add (aS, Bnd_Tools::Bnd2BVH (aSBox));
434     }
435     // Build BVH
436     aShapeBoxSet[i]->Build();
437   }
438
439   NCollection_List <std::pair <TopoDS_Shape, TopoDS_Shape> > aPairs;
440   if (!useVoidSelector)
441   {
442     // Initialize selector
443     PairShapesSelector aSelector;
444     // Select the elements
445     aSelector.SetBVHSets (aShapeBoxSet[0].get(), aShapeBoxSet[1].get());
446     aSelector.Select();
447     aPairs = aSelector.Pairs();
448   }
449   else
450   {
451     // Initialize selector
452     PairShapesSelectorVoid aSelector;
453     // Select the elements
454     aSelector.SetShapeBoxSets (aShapeBoxSet[0], aShapeBoxSet[1]);
455     aSelector.Select (aShapeBoxSet[0]->BVH(), aShapeBoxSet[1]->BVH());
456     aPairs = aSelector.Pairs();
457   }
458
459   // Draw the selected shapes
460   TopoDS_Compound aResult;
461   BRep_Builder().MakeCompound (aResult);
462
463   for (NCollection_List <std::pair <TopoDS_Shape, TopoDS_Shape> >::Iterator it (aPairs); it.More(); it.Next())
464   {
465     TopoDS_Compound aPair;
466     BRep_Builder().MakeCompound (aPair);
467
468     BRep_Builder().Add (aPair, it.Value().first);
469     BRep_Builder().Add (aPair, it.Value().second);
470
471     BRep_Builder().Add (aResult, aPair);
472   }
473
474   DBRep::Set (theArgv[1], aResult);
475   return 0;
476 }
477
478
479 //=======================================================================
480 //function : Triangle
481 //purpose : Auxiliary structure to keep the nodes of the triangle
482 //=======================================================================
483 struct Triangle
484 {
485   Triangle() {}
486   Triangle(const BVH_Vec3d& theP1, 
487            const BVH_Vec3d& theP2,
488            const BVH_Vec3d& theP3)
489     : _Node1 (theP1), _Node2 (theP2), _Node3 (theP3)
490   {}
491
492   BVH_Vec3d _Node1;
493   BVH_Vec3d _Node2;
494   BVH_Vec3d _Node3;
495 };
496
497 //=======================================================================
498 //function : TriangleTriangleSqDistance
499 //purpose : Computes the Triangle-Triangle square distance
500 //=======================================================================
501 static Standard_Real TriangleTriangleSqDistance (const BVH_Vec3d& theNode11,
502                                                  const BVH_Vec3d& theNode12,
503                                                  const BVH_Vec3d& theNode13,
504                                                  const BVH_Vec3d& theNode21,
505                                                  const BVH_Vec3d& theNode22,
506                                                  const BVH_Vec3d& theNode23)
507 {
508   Standard_Real aDist, aMinDist = RealLast();
509
510   BVH_Vec3d aNodes[2][3] = { { theNode11, theNode12, theNode13 },
511                              { theNode21, theNode22, theNode23 } };
512
513   const Standard_Integer aNbSeg = 100; // number of segments on edge
514   for (Standard_Integer iT = 0; iT < 2; ++iT)
515   {
516     // projecting points of one triangle on the opposite triangle
517     for (Standard_Integer iP = 0; iP < 3; ++iP)
518     {
519       aDist = 
520         BVH_Tools<Standard_Real, 3>::PointTriangleSquareDistance (aNodes[iT][iP],
521                                                                   aNodes[(iT + 1) % 2][0],
522                                                                   aNodes[(iT + 1) % 2][1],
523                                                                   aNodes[(iT + 1) % 2][2]);
524       if (aDist < aMinDist)
525       {
526         aMinDist = aDist;
527         if (aMinDist == 0)
528           return aMinDist;
529       }
530     }
531
532     // projecting edges on the opposite triangle
533     std::pair<BVH_Vec3d, BVH_Vec3d> anEdges[3] =
534       { std::pair<BVH_Vec3d, BVH_Vec3d> (aNodes[iT][0], aNodes[iT][1]),
535         std::pair<BVH_Vec3d, BVH_Vec3d> (aNodes[iT][1], aNodes[iT][2]),
536         std::pair<BVH_Vec3d, BVH_Vec3d> (aNodes[iT][2], aNodes[iT][0]) };
537
538     for (Standard_Integer iE = 0; iE < 3; ++iE)
539     {
540       const BVH_Vec3d& aPFirst = anEdges[iE].first, aPLast = anEdges[iE].second;
541       BVH_Vec3d anEdge = (aPLast - aPFirst);
542       Standard_Real aLength = anEdge.Modulus();
543       if (aLength < Precision::Confusion())
544         continue;
545       anEdge /= aLength;
546
547       Standard_Real aDelta = aLength / aNbSeg;
548   
549       for (Standard_Integer iP = 1; iP < aNbSeg; ++iP)
550       {
551         BVH_Vec3d aPE = aPFirst + anEdge.Multiplied (iP * aDelta);
552         aDist = 
553           BVH_Tools<Standard_Real, 3>::PointTriangleSquareDistance (aPE, 
554                                                                     aNodes[(iT + 1) % 2][0],
555                                                                     aNodes[(iT + 1) % 2][1],
556                                                                     aNodes[(iT + 1) % 2][2]);
557         if (aDist < aMinDist)
558         {
559           aMinDist = aDist;
560           if (aMinDist == 0)
561             return aMinDist;
562         }
563       }
564     }
565   }
566   return aMinDist;
567 }
568
569 //=======================================================================
570 //function : MeshMeshDistance
571 //purpose : Class to compute the distance between two meshes
572 //=======================================================================
573 class MeshMeshDistance : public BVH_PairDistance<Standard_Real, 3, BVH_BoxSet<Standard_Real, 3, Triangle>>
574 {
575 public:
576   //! Constructor
577   MeshMeshDistance() {}
578
579 public:
580   //! Defines the rules for leaf acceptance
581   virtual Standard_Boolean Accept (const Standard_Integer theIndex1,
582                                    const Standard_Integer theIndex2) Standard_OVERRIDE
583   {
584     const Triangle& aTri1 = myBVHSet1->Element (theIndex1);
585     const Triangle& aTri2 = myBVHSet2->Element (theIndex2);
586
587     Standard_Real aDistance = TriangleTriangleSqDistance (aTri1._Node1, aTri1._Node2, aTri1._Node3,
588                                                           aTri2._Node1, aTri2._Node2, aTri2._Node3);
589     if (aDistance < myDistance)
590     {
591       myDistance = aDistance;
592       return Standard_True;
593     }
594     return Standard_False;
595   }
596 };
597
598 //=======================================================================
599 //function : QABVH_PairDistance
600 //purpose : Computes the distance between two meshes of the given shapes
601 //=======================================================================
602 static Standard_Integer QABVH_PairDistance (Draw_Interpretor& theDI,
603                                             Standard_Integer theArgc,
604                                             const char** theArgv)
605 {
606   if (theArgc != 3)
607   {
608     theDI.PrintHelp (theArgv[0]);
609     return 1;
610   }
611
612   TopoDS_Shape aShape[2];
613   // Get the first shape
614   aShape[0] = DBRep::Get (theArgv [1]);
615   if (aShape[0].IsNull())
616   {
617     std::cout << theArgv[1] << " does not exist" << std::endl;
618     return 1;
619   }
620
621   // Get the second shape
622   aShape[1] = DBRep::Get (theArgv [2]);
623   if (aShape[1].IsNull())
624   {
625     std::cout << theArgv[2] << " does not exist" << std::endl;
626     return 1;
627   }
628
629   // Define BVH Builder
630   opencascade::handle <BVH_LinearBuilder <Standard_Real, 3> > aLBuilder =
631       new BVH_LinearBuilder <Standard_Real, 3>();
632
633   // Create the ShapeSet
634   opencascade::handle <BVH_BoxSet <Standard_Real, 3, Triangle> > aTriangleBoxSet[2];
635
636   for (Standard_Integer i = 0; i < 2; ++i)
637   {
638     aTriangleBoxSet[i] = new BVH_BoxSet <Standard_Real, 3, Triangle> (aLBuilder);
639
640     TopTools_IndexedMapOfShape aMapShapes;
641     TopExp::MapShapes (aShape[i], TopAbs_FACE,   aMapShapes);
642
643     for (Standard_Integer iS = 1; iS <= aMapShapes.Extent(); ++iS)
644     {
645       const TopoDS_Face& aF = TopoDS::Face (aMapShapes(iS));
646       TopLoc_Location aLoc;
647       const Handle(Poly_Triangulation)& aTriangulation = BRep_Tool::Triangulation(aF, aLoc);
648
649       const TColgp_Array1OfPnt& aNodes = aTriangulation->Nodes();
650       const Poly_Array1OfTriangle& aTriangles = aTriangulation->Triangles();
651
652       const int aNbTriangles = aTriangles.Length();
653       for (int iT = 1; iT <= aNbTriangles; ++iT)
654       {
655         const Poly_Triangle& aTriangle = aTriangles (iT);
656         // Nodes indices
657         Standard_Integer id1, id2, id3;
658         aTriangle.Get (id1, id2, id3);
659
660         const gp_Pnt& aP1 = aNodes(id1).Transformed(aLoc.Transformation());
661         const gp_Pnt& aP2 = aNodes(id2).Transformed(aLoc.Transformation());
662         const gp_Pnt& aP3 = aNodes(id3).Transformed(aLoc.Transformation());
663
664         BVH_Vec3d aBVHP1 (aP1.X(), aP1.Y(), aP1.Z());
665         BVH_Vec3d aBVHP2 (aP2.X(), aP2.Y(), aP2.Z());
666         BVH_Vec3d aBVHP3 (aP3.X(), aP3.Y(), aP3.Z());
667
668         BVH_Box<Standard_Real, 3> aBox;
669         aBox.Add (aBVHP1);
670         aBox.Add (aBVHP2);
671         aBox.Add (aBVHP3);
672
673         aTriangleBoxSet[i]->Add (Triangle (aBVHP1, aBVHP2, aBVHP3), aBox);
674       }
675     }
676     // Build BVH
677     aTriangleBoxSet[i]->Build();
678   }
679
680   // Initialize selector
681   MeshMeshDistance aDistTool;
682   // Select the elements
683   aDistTool.SetBVHSets (aTriangleBoxSet[0].get(), aTriangleBoxSet[1].get());
684   Standard_Real aSqDist = aDistTool.ComputeDistance();
685   if (!aDistTool.IsDone())
686     std::cout << "Not Done" << std::endl;
687   else
688     theDI << "Distance " << sqrt (aSqDist) << "\n";
689
690   return 0;
691 }
692
693 //=======================================================================
694 //function : QABVH_TriangleSet
695 //purpose : Auxiliary class to contain triangulation of a face
696 //=======================================================================
697 class QABVH_TriangleSet : public BVH_Triangulation<Standard_Real, 3>
698 {
699 public:
700   QABVH_TriangleSet()
701     : BVH_Triangulation<Standard_Real, 3>()
702   {}
703
704 public:
705
706   //! Creates the triangulation from a face
707   void Build (const TopoDS_Face& theFace)
708   {
709     TopLoc_Location aLoc;
710     const Handle(Poly_Triangulation)& aTriangulation = BRep_Tool::Triangulation(theFace, aLoc);
711
712     const TColgp_Array1OfPnt& aNodes = aTriangulation->Nodes();
713     const Poly_Array1OfTriangle& aTriangles = aTriangulation->Triangles();
714
715     const int aNbTriangles = aTriangles.Length();
716     for (int iT = 1; iT <= aNbTriangles; ++iT)
717     {
718       const Poly_Triangle& aTriangle = aTriangles (iT);
719       // Nodes indices
720       Standard_Integer id1, id2, id3;
721       aTriangle.Get (id1, id2, id3);
722
723       const gp_Pnt& aP1 = aNodes(id1).Transformed(aLoc.Transformation());
724       const gp_Pnt& aP2 = aNodes(id2).Transformed(aLoc.Transformation());
725       const gp_Pnt& aP3 = aNodes(id3).Transformed(aLoc.Transformation());
726
727       BVH_Vec3d aBVHP1 (aP1.X(), aP1.Y(), aP1.Z());
728       BVH_Vec3d aBVHP2 (aP2.X(), aP2.Y(), aP2.Z());
729       BVH_Vec3d aBVHP3 (aP3.X(), aP3.Y(), aP3.Z());
730
731       Standard_Integer id = static_cast<Standard_Integer>(Vertices.size()) - 1;
732       Vertices.push_back (aBVHP1);
733       Vertices.push_back (aBVHP2);
734       Vertices.push_back (aBVHP3);
735
736       Elements.push_back (BVH_Vec4i (id + 1, id + 2, id + 3, iT));
737     }
738
739     MarkDirty();
740     BVH();
741   }
742 };
743
744 //=======================================================================
745 //function : QABVH_Geometry
746 //purpose : Auxiliary class to contain triangulation of a shape
747 //=======================================================================
748 class QABVH_Geometry : public BVH_Geometry<Standard_Real, 3>
749 {
750 public:
751   QABVH_Geometry()
752     : BVH_Geometry<Standard_Real, 3>()
753   {}
754
755 public:
756
757   //! Creates the triangulation from a face
758   void Build (const TopoDS_Shape& theShape)
759   {
760     TopExp_Explorer anExp (theShape, TopAbs_FACE);
761     for (; anExp.More(); anExp.Next())
762     {
763       const TopoDS_Face& aF = TopoDS::Face (anExp.Current());
764       Handle(QABVH_TriangleSet) aTriSet = new QABVH_TriangleSet();
765       aTriSet->Build (aF);
766       myObjects.Append (aTriSet);
767     }
768
769     MarkDirty();
770     BVH();
771   }
772 };
773
774 //=======================================================================
775 //function : QABVH_DistanceField
776 //purpose : Computes the distance field on a given shape
777 //=======================================================================
778 static Standard_Integer QABVH_DistanceField (Draw_Interpretor& theDI,
779                                              Standard_Integer theArgc,
780                                              const char** theArgv)
781 {
782   if (theArgc < 2)
783   {
784     theDI.PrintHelp (theArgv[0]);
785     return 1;
786   }
787
788   TopoDS_Shape aShape;
789   // Get the first shape
790   aShape = DBRep::Get (theArgv [1]);
791   if (aShape.IsNull())
792   {
793     std::cout << theArgv[1] << " does not exist" << std::endl;
794     return 1;
795   }
796
797   Standard_Integer aNbDim = 10;
798   if (theArgc > 2)
799   {
800     aNbDim = Draw::Atoi (theArgv[2]);
801   }
802
803   QABVH_Geometry aGeometry;
804   aGeometry.Build(aShape);
805
806   BVH_DistanceField<Standard_Real, 3> aDField(aNbDim, Standard_True);
807   aDField.Build (aGeometry);
808
809   for (Standard_Integer iX = 0; iX < aDField.DimensionX(); ++iX)
810   {
811     for (Standard_Integer iY = 0; iY < aDField.DimensionY(); ++iY)
812     {
813       for (Standard_Integer iZ = 0; iZ < aDField.DimensionZ(); ++iZ)
814       {
815         Standard_Real aDist = aDField.Voxel (iX, iY, iZ);
816         std::cout << "(" << iX << ", " << iY << ", " << iZ << "): " << aDist << std::endl;
817       }
818     }
819   }
820   return 0;
821 }
822
823 //=======================================================================
824 //function : Commands_BVH
825 //purpose : BVH commands
826 //=======================================================================
827 void QABugs::Commands_BVH (Draw_Interpretor& theCommands)
828 {
829   const char *group = "QABugs";
830
831   theCommands.Add ("QABVH_ShapeSelect",
832                    "Tests the work of BHV_BoxSet algorithm on the simple example of selection of shapes which boxes interfere with given box.\n"
833                    "Usage: QABVH_ShapeSelect result shape box (defined as a solid) [-void]\n"
834                    "\tResult should contain all sub-shapes of the shape interfering with given box",
835                    __FILE__, QABVH_ShapeSelect, group);
836
837   theCommands.Add ("QABVH_PairSelect",
838                    "Tests the work of BHV_BoxSet algorithm on the simple example of selection of pairs of shapes with interfering bounding boxes.\n"
839                    "Usage: QABVH_PairSelect result shape1 shape2 [-void]\n"
840                    "\tResult should contain all interfering pairs (compound of pairs)",
841                    __FILE__, QABVH_PairSelect, group);
842
843   theCommands.Add ("QABVH_PairDistance",
844                    "Computes the distance between the meshes of the given shapes.\n"
845                    "Usage: QABVH_PairDistance shape1 shape2\n"
846                    "\tThe given shapes should contain triangulation\n",
847                    __FILE__, QABVH_PairDistance, group);
848
849   theCommands.Add ("QABVH_DistanceField",
850                    "Computes the distance field for a shape with triangulation\n"
851                    "Usage: QABVH_DistanceField shape [nbSplit]\n",
852                    __FILE__, QABVH_DistanceField, group);
853
854 }