0030704: Modeling Data, Bnd_OBB - Oriented bounding box gives a wrong result if a...
[occt.git] / src / Bnd / Bnd_OBB.cxx
1 // Created by: Eugeny MALTCHIKOV
2 // Copyright (c) 2017 OPEN CASCADE SAS
3 //
4 // This file is part of Open CASCADE Technology software library.
5 //
6 // This library is free software; you can redistribute it and/or modify it under
7 // the terms of the GNU Lesser General Public License version 2.1 as published
8 // by the Free Software Foundation, with special exception defined in the file
9 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
10 // distribution for complete text of the license and disclaimer of any warranty.
11 //
12 // Alternatively, this file may be used under the terms of Open CASCADE
13 // commercial license or contractual agreement.
14
15 #include <Bnd_OBB.hxx>
16
17 #include <Bnd_Tools.hxx>
18 #include <Bnd_Range.hxx>
19
20 #include <BVH_BoxSet.hxx>
21 #include <BVH_LinearBuilder.hxx>
22
23 #include <BVH_Traverse.hxx>
24
25 #include <NCollection_Array1.hxx>
26 #include <Precision.hxx>
27 #include <TColStd_Array1OfReal.hxx>
28
29 //! Auxiliary class to select from the points stored in
30 //! BVH tree the two points giving the extreme projection
31 //! parameters on the axis
32 class OBB_ExtremePointsSelector : 
33   public BVH_Traverse <Standard_Real, 3, BVH_BoxSet <Standard_Real, 3, gp_XYZ>, Bnd_Range>
34 {
35 public:
36
37   //! Constructor
38   OBB_ExtremePointsSelector() :
39     BVH_Traverse <Standard_Real, 3, BVH_BoxSet <Standard_Real, 3, gp_XYZ>, Bnd_Range>(),
40     myPrmMin (RealLast()),
41     myPrmMax (RealFirst())
42   {}
43
44 public: //! @name Set axis for projection
45
46   //! Sets the axis
47   void SetAxis (const gp_XYZ& theAxis) { myAxis = theAxis; }
48
49 public: //! @name Clears the points from previous runs
50
51   //! Clear
52   void Clear()
53   {
54     myPrmMin = RealLast();
55     myPrmMax = RealFirst();
56   }
57
58 public: //! @name Getting the results
59
60   //! Returns the minimal projection parameter
61   Standard_Real MinPrm() const { return myPrmMin; }
62
63   //! Returns the maximal projection parameter
64   Standard_Real MaxPrm() const { return myPrmMax; }
65
66   //! Returns the minimal projection point
67   const gp_XYZ& MinPnt() const { return myPntMin; }
68
69   //! Returns the maximal projection point
70   const gp_XYZ& MaxPnt() const { return myPntMax; }
71
72 public: //! @name Definition of rejection/acceptance rules
73
74   //! Defines the rules for node rejection
75   virtual Standard_Boolean RejectNode (const BVH_Vec3d& theCMin,
76                                        const BVH_Vec3d& theCMax,
77                                        Bnd_Range& theMetric) const Standard_OVERRIDE
78   {
79     if (myPrmMin > myPrmMax)
80       // No parameters computed yet
81       return Standard_False;
82
83     Standard_Real aPrmMin = myPrmMin, aPrmMax = myPrmMax;
84     Standard_Boolean isToReject = Standard_True;
85
86     // Check if the current node is between already found parameters
87     for (Standard_Integer i = 0; i < 2; ++i)
88     {
89       Standard_Real x = !i ? theCMin.x() : theCMax.x();
90       for (Standard_Integer j = 0; j < 2; ++j)
91       {
92         Standard_Real y = !j ? theCMin.y() : theCMax.y();
93         for (Standard_Integer k = 0; k < 2; ++k)
94         {
95           Standard_Real z = !k ? theCMin.z() : theCMax.z();
96
97           Standard_Real aPrm = myAxis.Dot (gp_XYZ (x, y, z));
98           if (aPrm < aPrmMin)
99           {
100             aPrmMin = aPrm;
101             isToReject = Standard_False;
102           }
103           else if (aPrm > aPrmMax)
104           {
105             aPrmMax = aPrm;
106             isToReject = Standard_False;
107           }
108         }
109       }
110     }
111
112     theMetric = Bnd_Range (aPrmMin, aPrmMax);
113
114     return isToReject;
115   }
116
117   //! Rules for node rejection by the metric
118   virtual Standard_Boolean RejectMetric (const Bnd_Range& theMetric) const Standard_OVERRIDE
119   {
120     if (myPrmMin > myPrmMax)
121       // no parameters computed
122       return Standard_False;
123
124     Standard_Real aMin, aMax;
125     if (!theMetric.GetBounds (aMin, aMax))
126       // void metric
127       return Standard_False;
128
129     // Check if the box of the branch is inside of the already computed parameters
130     return aMin > myPrmMin && aMax < myPrmMax;
131   }
132
133   //! Defines the rules for leaf acceptance
134   virtual Standard_Boolean Accept (const Standard_Integer theIndex,
135                                    const Bnd_Range&) Standard_OVERRIDE
136   {
137     const gp_XYZ& theLeaf = myBVHSet->Element (theIndex);
138     Standard_Real aPrm = myAxis.Dot (theLeaf);
139     if (aPrm < myPrmMin)
140     {
141       myPrmMin = aPrm;
142       myPntMin = theLeaf;
143     }
144     if (aPrm > myPrmMax)
145     {
146       myPrmMax = aPrm;
147       myPntMax = theLeaf;
148     }
149     return Standard_True;
150   }
151
152 public: //! @name Choosing the best branch
153
154   //! Returns true if the metric of the left branch is better than the metric of the right
155   virtual Standard_Boolean IsMetricBetter (const Bnd_Range& theLeft,
156                                            const Bnd_Range& theRight) const Standard_OVERRIDE
157   {
158     if (myPrmMin > myPrmMax)
159       // no parameters computed
160       return Standard_True;
161
162     Standard_Real aMin[2], aMax[2];
163     if (!theLeft.GetBounds  (aMin[0], aMax[0]) ||
164         !theRight.GetBounds (aMin[1], aMax[1]))
165       // void metrics
166       return Standard_True;
167
168     // Choose branch with larger extension over computed parameters
169     Standard_Real anExt[2] = {0.0, 0.0};
170     for (int i = 0; i < 2; ++i)
171     {
172       if (aMin[i] < myPrmMin) anExt[i] += myPrmMin - aMin[i];
173       if (aMax[i] > myPrmMax) anExt[i] += aMax[i] - myPrmMax;
174     }
175     return anExt[0] > anExt[1];
176   }
177
178 protected: //! @name Fields
179
180   gp_XYZ myAxis;          //!< Axis to project the points to
181   Standard_Real myPrmMin; //!< Minimal projection parameter
182   Standard_Real myPrmMax; //!< Maximal projection parameter
183   gp_XYZ myPntMin;        //!< Minimal projection point
184   gp_XYZ myPntMax;        //!< Maximal projection point
185 };
186
187 //! Tool for OBB construction
188 class OBBTool
189 {
190 public:
191   //! Constructor. theL - list of points.
192   //! theLT is a pointer to the list of tolerances
193   //! (i-th element of this array is a tolerance
194   //! of i-th point in theL). If theLT is empty
195   //! then the tolerance of every point is equal to 0.
196   //! Attention! The objects, which theL and theLT links on,
197   //! must be available during all time of OBB creation
198   //! (i.e. while the object of OBBTool exists).
199   OBBTool(const TColgp_Array1OfPnt& theL,
200           const TColStd_Array1OfReal *theLT = 0,
201           Standard_Boolean theIsOptimal = Standard_False);
202
203   //! DiTO algorithm for OBB construction
204   //! (http://www.idt.mdh.se/~tla/publ/FastOBBs.pdf)
205   void ProcessDiTetrahedron();
206
207   //! Creates OBB with already computed parameters
208   void BuildBox(Bnd_OBB& theBox);
209
210 protected:
211
212   // Computes the extreme points on the set of Initial axes
213   void ComputeExtremePoints ();
214
215   //! Works with the triangle set by the points in myTriIdx.
216   //! If theIsBuiltTrg == TRUE, new set of triangles will be
217   //! recomputed.
218   void ProcessTriangle(const Standard_Integer theIdx1,
219                        const Standard_Integer theIdx2,
220                        const Standard_Integer theIdx3,
221                        const Standard_Boolean theIsBuiltTrg);
222
223   //! Computes myTriIdx[2]
224   void FillToTriangle3();
225
226   //! Computes myTriIdx[3] and myTriIdx[4]
227   void FillToTriangle5(const gp_XYZ& theNormal,
228                        const gp_XYZ& theBarryCenter);
229
230   //! Returns half of the Surface area of the box
231   static Standard_Real ComputeQuality(const Standard_Real* const thePrmArr)
232   {
233     const Standard_Real aDX = thePrmArr[1] - thePrmArr[0],
234                         aDY = thePrmArr[3] - thePrmArr[2],
235                         aDZ = thePrmArr[5] - thePrmArr[4];
236
237     return (aDX*aDY + aDY*aDZ + aDX*aDZ);
238   }
239
240 protected:
241   //! Assignment operator is forbidden
242   OBBTool& operator=(const OBBTool&);
243
244 private:
245   //! Params structure stores the two values meaning
246   //! min and max parameters on the axis
247   struct Params
248   {
249     Params() :
250       _ParamMin(RealLast()), _ParamMax(RealFirst())
251     {}
252
253     Params(Standard_Real theMin, Standard_Real theMax)
254       : _ParamMin(theMin), _ParamMax(theMax)
255     {}
256
257     Standard_Real _ParamMin;
258     Standard_Real _ParamMax;
259   };
260
261   //! Computes the Minimal and maximal parameters on the vector
262   //! connecting the points myLExtremalPoints[theId1] and myLExtremalPoints[theId2]
263   void ComputeParams (const Standard_Integer theId1,
264                       const Standard_Integer theId2,
265                       Standard_Real &theMin,
266                       Standard_Real &theMax)
267   {
268     theMin = myParams[theId1][theId2]._ParamMin;
269     theMax = myParams[theId1][theId2]._ParamMax;
270
271     if (theMin > theMax)
272     {
273       FindMinMax ((myLExtremalPoints[theId1] - myLExtremalPoints[theId2]).Normalized(), theMin, theMax);
274       myParams[theId1][theId2]._ParamMin = myParams[theId2][theId1]._ParamMin = theMin;
275       myParams[theId1][theId2]._ParamMax = myParams[theId2][theId1]._ParamMax = theMax;
276     }
277   }
278
279   //! Looks for the min-max parameters on the axis.
280   //! For optimal case projects all the points on the axis,
281   //! for not optimal - only the set of extreme points.
282   void FindMinMax (const gp_XYZ& theAxis,
283                    Standard_Real &theMin,
284                    Standard_Real &theMax)
285   {
286     theMin = RealLast(), theMax = RealFirst();
287
288     if (myOptimal)
289       Project (theAxis, theMin, theMax);
290     else
291     {
292       for (Standard_Integer i = 0; i < myNbExtremalPoints; ++i)
293       {
294         Standard_Real aPrm = theAxis.Dot (myLExtremalPoints[i]);
295         if (aPrm < theMin) theMin = aPrm;
296         if (aPrm > theMax) theMax = aPrm;
297       }
298     }
299   }
300
301   //! Projects the set of points on the axis
302   void Project (const gp_XYZ& theAxis,
303                 Standard_Real& theMin, Standard_Real& theMax,
304                 gp_XYZ* thePntMin = 0, gp_XYZ* thePntMax = 0)
305   {
306     theMin = RealLast(), theMax = RealFirst();
307
308     if (myOptimal)
309     {
310       // Project BVH
311       OBB_ExtremePointsSelector anExtremePointsSelector;
312       anExtremePointsSelector.SetBVHSet (myPointBoxSet.get());
313       anExtremePointsSelector.SetAxis (theAxis);
314       anExtremePointsSelector.Select();
315       theMin = anExtremePointsSelector.MinPrm();
316       theMax = anExtremePointsSelector.MaxPrm();
317       if (thePntMin) *thePntMin = anExtremePointsSelector.MinPnt();
318       if (thePntMax) *thePntMax = anExtremePointsSelector.MaxPnt();
319     }
320     else
321     {
322       // Project all points
323       for (Standard_Integer iP = myPntsList.Lower(); iP <= myPntsList.Upper(); ++iP)
324       {
325         const gp_XYZ& aPoint = myPntsList(iP).XYZ();
326         const Standard_Real aPrm = theAxis.Dot (aPoint);
327         if (aPrm < theMin)
328         {
329           theMin = aPrm;
330           if (thePntMin)
331             *thePntMin = aPoint;
332         }
333         if (aPrm > theMax)
334         {
335           theMax = aPrm;
336           if (thePntMax)
337             *thePntMax = aPoint;
338         }
339       }
340     }
341   }
342
343 private:
344
345   //! Number of the initial axes.
346   static const Standard_Integer myNbInitAxes = 7;
347
348   //! Number of extremal points
349   static const Standard_Integer myNbExtremalPoints = 2 * myNbInitAxes;
350
351   //! The source list of points
352   const TColgp_Array1OfPnt& myPntsList;
353
354   //! Pointer to the array of tolerances
355   const TColStd_Array1OfReal *myListOfTolers;
356
357   //! Points of ditetrahedron
358   //! given by their indices in myLExtremalPoints.
359   Standard_Integer myTriIdx[5];
360
361   //! List of extremal points
362   gp_XYZ myLExtremalPoints[myNbExtremalPoints];
363
364   //! The axes of the box (always normalized or
365   //! can be null-vector)
366   gp_XYZ myAxes[3];
367
368   //! The surface area of the OBB
369   Standard_Real myQualityCriterion;
370
371   //! Defines if the OBB should be computed more tight.
372   //! Takes more time, but the volume is less.
373   Standard_Boolean myOptimal;
374
375   //! Point box set organized with BVH
376   opencascade::handle<BVH_BoxSet <Standard_Real, 3, gp_XYZ>> myPointBoxSet;
377
378   //! Stored min/max parameters for the axes between extremal points
379   Params myParams[myNbExtremalPoints][myNbExtremalPoints];
380 };
381
382 //=======================================================================
383 // Function : SetMinMax
384 // purpose : 
385 //    ATTENTION!!! thePrmArr must be initialized before this method calling.
386 //=======================================================================
387 static inline void SetMinMax(Standard_Real* const thePrmArr,
388                              const Standard_Real theNewParam)
389 {
390   if(theNewParam < thePrmArr[0])
391   {
392     thePrmArr[0] = theNewParam;
393   }
394   if(theNewParam > thePrmArr[1])
395   {
396     thePrmArr[1] = theNewParam;
397   }
398 }
399
400 //=======================================================================
401 // Function : Constructor
402 // purpose : 
403 //=======================================================================
404 OBBTool::
405     OBBTool(const TColgp_Array1OfPnt& theL,
406             const TColStd_Array1OfReal *theLT,
407             const Standard_Boolean theIsOptimal) : myPntsList(theL),
408                                                    myListOfTolers(theLT),
409                                                    myQualityCriterion(RealLast()),
410                                                    myOptimal (theIsOptimal)
411 {
412   if (myOptimal)
413   {
414     // Use linear builder for BVH construction with 30 elements in the leaf
415     opencascade::handle<BVH_LinearBuilder<Standard_Real, 3> > aLBuilder =
416       new BVH_LinearBuilder<Standard_Real, 3> (30);
417     myPointBoxSet = new BVH_BoxSet <Standard_Real, 3, gp_XYZ> (aLBuilder);
418     myPointBoxSet->SetSize(myPntsList.Length());
419
420     // Add the points into Set
421     for (Standard_Integer iP = 0; iP < theL.Length(); ++iP)
422     {
423       const gp_Pnt& aP = theL (iP);
424       Standard_Real aTol = theLT ? theLT->Value(iP) : Precision::Confusion();
425       BVH_Box <Standard_Real, 3> aBox (BVH_Vec3d (aP.X() - aTol, aP.Y() - aTol, aP.Z() - aTol),
426                                        BVH_Vec3d (aP.X() + aTol, aP.Y() + aTol, aP.Z() + aTol));
427       myPointBoxSet->Add (aP.XYZ(), aBox);
428     }
429
430     myPointBoxSet->Build();
431   }
432
433   ComputeExtremePoints();
434 }
435
436 //=======================================================================
437 // Function : ComputeExtremePoints
438 // purpose : 
439 //=======================================================================
440 void OBBTool::ComputeExtremePoints()
441 {
442   // Six initial axes show great quality on the Optimal OBB, plus
443   // the performance is better (due to the less number of operations).
444   // But they show worse quality for the not optimal approach.
445   //const Standard_Real a = (sqrt(5) - 1) / 2.;
446   //const gp_XYZ anInitialAxes6[myNbInitAxes] = { gp_XYZ (0, 1, a),
447   //                                              gp_XYZ (0, 1, -a),
448   //                                              gp_XYZ (1, a, 0),
449   //                                              gp_XYZ (1, -a, 0),
450   //                                              gp_XYZ (a, 0, 1),
451   //                                              gp_XYZ (a, 0, -1) };
452   const Standard_Real aSqrt3 = Sqrt(3);
453   const gp_XYZ anInitialAxes7[myNbInitAxes] = { gp_XYZ (1.0, 0.0, 0.0),
454                                                 gp_XYZ (0.0, 1.0, 0.0),
455                                                 gp_XYZ (0.0, 0.0, 1.0),
456                                                 gp_XYZ (1.0, 1.0, 1.0) / aSqrt3,
457                                                 gp_XYZ (1.0, 1.0, -1.0) / aSqrt3,
458                                                 gp_XYZ (1.0, -1.0, 1.0) / aSqrt3,
459                                                 gp_XYZ (1.0, -1.0, -1.0) / aSqrt3 };
460
461   // Set of initial axes
462   const gp_XYZ *anInitialAxesArray = anInitialAxes7;
463
464   // Min and Max parameter
465   Standard_Real aParams[myNbExtremalPoints];
466   // Look for the extremal points (myLExtremalPoints)
467   for (Standard_Integer anAxeInd = 0, aPrmInd = -1; anAxeInd < myNbInitAxes; ++anAxeInd)
468   {
469     Standard_Integer aMinInd = ++aPrmInd, aMaxInd = ++aPrmInd;
470     aParams[aMinInd] = RealLast();
471     aParams[aMaxInd] = -RealLast();
472     Project (anInitialAxesArray[anAxeInd],
473              aParams[aMinInd], aParams[aMaxInd],
474              &myLExtremalPoints[aMinInd], &myLExtremalPoints[aMaxInd]);
475   }
476
477   // For not optimal box it is necessary to compute the max axis
478   // created by the maximally distant extreme points
479   if (!myOptimal)
480   {
481     for(Standard_Integer i = 0; i < 5; i++)
482       myTriIdx[i] = INT_MAX;
483
484     // Compute myTriIdx[0] and myTriIdx[1].
485     Standard_Real aMaxSqDist = -1.0;
486     for (Standard_Integer aPrmInd = 0; aPrmInd < myNbExtremalPoints; aPrmInd += 2)
487     {
488       const gp_Pnt &aP1 = myLExtremalPoints[aPrmInd],
489         &aP2 = myLExtremalPoints[aPrmInd + 1];
490       const Standard_Real aSqDist = aP1.SquareDistance(aP2);
491       if (aSqDist > aMaxSqDist)
492       {
493         aMaxSqDist = aSqDist;
494         myTriIdx[0] = aPrmInd;
495         myTriIdx[1] = aPrmInd + 1;
496       }
497     }
498
499     // Compute the maximal axis orthogonal to the found one
500     FillToTriangle3();
501   }
502 }
503
504 //=======================================================================
505 // Function : FillToTriangle3
506 // purpose : Two value of myTriIdx array is known. Let us find myTriIdx[2].
507 //            It must be in maximal distance from the infinite axis going
508 //            through the points with indexes myTriIdx[0] and myTriIdx[1].
509 //=======================================================================
510 void OBBTool::FillToTriangle3()
511 {
512   const gp_XYZ &aP0 = myLExtremalPoints[myTriIdx[0]];
513   const gp_XYZ anAxis = myLExtremalPoints[myTriIdx[1]] - aP0;
514   Standard_Real aMaxSqDist = -1.0;
515   for(Standard_Integer i = 0; i < myNbExtremalPoints; i++)
516   {
517     if((i == myTriIdx[0]) || (i == myTriIdx[1]))
518       continue;
519
520     const gp_XYZ &aP = myLExtremalPoints[i];
521     const Standard_Real aDistToAxe = anAxis.CrossSquareMagnitude(aP - aP0);
522     if(aDistToAxe > aMaxSqDist)
523     {
524       myTriIdx[2] = i;
525       aMaxSqDist = aDistToAxe;
526     }
527   }
528 }
529
530 //=======================================================================
531 // Function : FillToTriangle5
532 // purpose : Three value of myTriIdx array is known.
533 //            Let us find myTriIdx[3] and myTriIdx[4].
534 //           They must be in the different sides of the plane of
535 //            triangle set by points myTriIdx[0], myTriIdx[1] and
536 //            myTriIdx[2]. Moreover, the distance from these points
537 //            to the triangle plane must be maximal.
538 //=======================================================================
539 void OBBTool::FillToTriangle5(const gp_XYZ& theNormal,
540                               const gp_XYZ& theBarryCenter)
541 {
542   Standard_Real aParams[2] = {0.0, 0.0};
543   Standard_Integer id3 = -1, id4 = -1;
544
545   for(Standard_Integer aPtIdx = 0; aPtIdx < myNbExtremalPoints; aPtIdx++)
546   {
547     if((aPtIdx == myTriIdx[0]) || (aPtIdx == myTriIdx[1]) || (aPtIdx == myTriIdx[2]))
548       continue;
549
550     const gp_XYZ &aCurrPoint = myLExtremalPoints[aPtIdx];
551     const Standard_Real aParam = theNormal.Dot(aCurrPoint - theBarryCenter);
552
553     if (aParam < aParams[0])
554     {
555       id3 = aPtIdx;
556       aParams[0] = aParam;
557     }
558     else if (aParam > aParams[1])
559     {
560       id4 = aPtIdx;
561       aParams[1] = aParam;
562     }
563   }
564
565   // The points must be in the different sides of the triangle plane.
566   if (id3 >= 0 && aParams[0] < -Precision::Confusion())
567     myTriIdx[3] = id3;
568
569   if (id4 >= 0 && aParams[1] > Precision::Confusion())
570     myTriIdx[4] = id4;
571 }
572
573 //=======================================================================
574 // Function : ProcessTriangle
575 // purpose : Choose the optimal box with triple axes containing normal
576 //            to the triangle and some edge of the triangle (3rd axis is
577 //            computed from these two ones).
578 //=======================================================================
579 void OBBTool::ProcessTriangle(const Standard_Integer theIdx1,
580                               const Standard_Integer theIdx2,
581                               const Standard_Integer theIdx3,
582                               const Standard_Boolean theIsBuiltTrg)
583 {
584   const Standard_Integer aNbAxes = 3;
585
586   // All axes must be normalized in order to provide correct area computation
587   // (see ComputeQuality(...) method).
588   int ID1[3] = { theIdx2, theIdx3, theIdx1 },
589       ID2[3] = { theIdx1, theIdx2, theIdx3 };
590   gp_XYZ aYAxis[aNbAxes] = {(myLExtremalPoints[ID1[0]] - myLExtremalPoints[ID2[0]]),
591                             (myLExtremalPoints[ID1[1]] - myLExtremalPoints[ID2[1]]),
592                             (myLExtremalPoints[ID1[2]] - myLExtremalPoints[ID2[2]])};
593
594   // Normal to the triangle plane
595   gp_XYZ aZAxis = aYAxis[0].Crossed(aYAxis[1]);
596
597   Standard_Real aSqMod = aZAxis.SquareModulus();
598
599   if (aSqMod < Precision::SquareConfusion())
600     return;
601
602   aZAxis /= Sqrt(aSqMod);
603
604   gp_XYZ aXAxis[aNbAxes];
605   for (Standard_Integer i = 0; i < aNbAxes; i++)
606     aXAxis[i] = aYAxis[i].Crossed(aZAxis).Normalized();
607
608   if (theIsBuiltTrg)
609     FillToTriangle5 (aZAxis, myLExtremalPoints[theIdx1]);
610
611   // Min and Max parameter
612   const Standard_Integer aNbPoints = 2 * aNbAxes;
613
614   // Compute Min/Max params for ZAxis
615   Standard_Real aParams[aNbPoints];
616   FindMinMax (aZAxis, aParams[4], aParams[5]); // Compute params on ZAxis once
617
618   Standard_Integer aMinIdx = -1;
619   for(Standard_Integer anAxeInd = 0; anAxeInd < aNbAxes; anAxeInd++)
620   {
621     const gp_XYZ &aAX = aXAxis[anAxeInd];
622     // Compute params on XAxis
623     FindMinMax (aAX, aParams[0], aParams[1]);
624     // Compute params on YAxis checking for stored values
625     ComputeParams (ID1[anAxeInd], ID2[anAxeInd], aParams[2], aParams[3]);
626
627     const Standard_Real anArea = ComputeQuality(aParams);
628     if (anArea < myQualityCriterion)
629     {
630       myQualityCriterion = anArea;
631       aMinIdx = anAxeInd;
632     }
633   }
634
635   if (aMinIdx < 0)
636     return;
637
638   myAxes[0] = aXAxis[aMinIdx];
639   myAxes[1] = aYAxis[aMinIdx].Normalized();
640   myAxes[2] = aZAxis;
641 }
642 //=======================================================================
643 // Function : ProcessDiTetrahedron
644 // purpose : DiTo-algorithm (http://www.idt.mdh.se/~tla/publ/FastOBBs.pdf)
645 //=======================================================================
646 void OBBTool::ProcessDiTetrahedron()
647 {
648   // To compute the optimal OBB it is necessary to check all possible
649   // axes created by the extremal points. It is also necessary to project
650   // all the points on the axis, as for each different axis there will be
651   // different extremal points.
652   if (myOptimal)
653   {
654     for (Standard_Integer i = 0; i < myNbExtremalPoints - 2; i++)
655     {
656       for (Standard_Integer j = i + 1; j < myNbExtremalPoints - 1; j++)
657       {
658         for (Standard_Integer k = j + 1; k < myNbExtremalPoints; k++)
659         {
660           ProcessTriangle (i, j, k, Standard_False);
661         }
662       }
663     }
664   }
665   else
666   {
667     // Use the standard DiTo approach
668     ProcessTriangle(myTriIdx[0], myTriIdx[1], myTriIdx[2], Standard_True);
669
670     if (myTriIdx[3] <= myNbExtremalPoints)
671     {
672       ProcessTriangle(myTriIdx[0], myTriIdx[1], myTriIdx[3], Standard_False);
673       ProcessTriangle(myTriIdx[1], myTriIdx[2], myTriIdx[3], Standard_False);
674       ProcessTriangle(myTriIdx[0], myTriIdx[2], myTriIdx[3], Standard_False);
675     }
676
677     if (myTriIdx[4] <= myNbExtremalPoints)
678     {
679       ProcessTriangle(myTriIdx[0], myTriIdx[1], myTriIdx[4], Standard_False);
680       ProcessTriangle(myTriIdx[1], myTriIdx[2], myTriIdx[4], Standard_False);
681       ProcessTriangle(myTriIdx[0], myTriIdx[2], myTriIdx[4], Standard_False);
682     }
683   }
684 }
685
686 //=======================================================================
687 // Function : BuildBox
688 // purpose : 
689 //=======================================================================
690 void OBBTool::BuildBox(Bnd_OBB& theBox)
691 {
692   theBox.SetVoid();
693
694   // In fact, use Precision::SquareConfusion().
695   const Standard_Boolean isOBB = myAxes[0].SquareModulus()*
696                                  myAxes[1].SquareModulus()*
697                                  myAxes[2].SquareModulus() > 1.0e-14;
698
699   const gp_Dir aXDir = isOBB ? myAxes[0] : gp_Dir(1, 0, 0);
700   const gp_Dir aYDir = isOBB ? myAxes[1] : gp_Dir(0, 1, 0);
701   const gp_Dir aZDir = isOBB ? myAxes[2] : gp_Dir(0, 0, 1);
702
703   const Standard_Integer aNbPoints = 6;
704   Standard_Real aParams[aNbPoints];
705
706   gp_XYZ aFCurrPoint = myPntsList.First().XYZ();
707   
708   aParams[0] = aParams[1] = aFCurrPoint.Dot(aXDir.XYZ());
709   aParams[2] = aParams[3] = aFCurrPoint.Dot(aYDir.XYZ());
710   aParams[4] = aParams[5] = aFCurrPoint.Dot(aZDir.XYZ());
711
712   if(myListOfTolers != 0)
713   {
714     const Standard_Real aTol = myListOfTolers->First();
715     aParams[0] -= aTol;
716     aParams[1] += aTol;
717     aParams[2] -= aTol;
718     aParams[3] += aTol;
719     aParams[4] -= aTol;
720     aParams[5] += aTol;
721   }
722
723   for(Standard_Integer i = myPntsList.Lower() + 1; i <= myPntsList.Upper(); i++)
724   {
725     const gp_XYZ &aCurrPoint = myPntsList(i).XYZ();
726     const Standard_Real aDx = aCurrPoint.Dot(aXDir.XYZ()),
727                         aDy = aCurrPoint.Dot(aYDir.XYZ()),
728                         aDz = aCurrPoint.Dot(aZDir.XYZ());
729         
730     if(myListOfTolers == 0)
731     {
732       SetMinMax(&aParams[0], aDx);
733       SetMinMax(&aParams[2], aDy);
734       SetMinMax(&aParams[4], aDz);
735     }
736     else
737     {
738       const Standard_Real aTol = myListOfTolers->Value(i);
739       aParams[0] = Min(aParams[0], aDx - aTol);
740       aParams[1] = Max(aParams[1], aDx + aTol);
741       aParams[2] = Min(aParams[2], aDy - aTol);
742       aParams[3] = Max(aParams[3], aDy + aTol);
743       aParams[4] = Min(aParams[4], aDz - aTol);
744       aParams[5] = Max(aParams[5], aDz + aTol);
745     }
746   }
747
748   //Half-sizes
749   const Standard_Real aHX = 0.5*(aParams[1] - aParams[0]);
750   const Standard_Real aHY = 0.5*(aParams[3] - aParams[2]);
751   const Standard_Real aHZ = 0.5*(aParams[5] - aParams[4]);
752
753   const gp_XYZ aCenter = 0.5*((aParams[1] + aParams[0])*aXDir.XYZ() +
754                               (aParams[3] + aParams[2])*aYDir.XYZ() +
755                               (aParams[5] + aParams[4])*aZDir.XYZ());
756
757   theBox.SetCenter(aCenter);
758   theBox.SetXComponent(aXDir, aHX);
759   theBox.SetYComponent(aYDir, aHY);
760   theBox.SetZComponent(aZDir, aHZ);
761   theBox.SetAABox(!isOBB);
762 }
763
764 // =======================================================================
765 // function : ReBuild
766 // purpose  : http://www.idt.mdh.se/~tla/publ/
767 // =======================================================================
768 void Bnd_OBB::ReBuild(const TColgp_Array1OfPnt& theListOfPoints,
769                       const TColStd_Array1OfReal *theListOfTolerances,
770                       const Standard_Boolean theIsOptimal)
771 {
772   switch(theListOfPoints.Length())
773   {
774     case 1:
775       ProcessOnePoint(theListOfPoints.First());
776       if(theListOfTolerances)
777         Enlarge(theListOfTolerances->First());
778       return;
779     case 2:
780     {
781       const Standard_Real aTol1 = (theListOfTolerances == 0) ? 0.0 :
782                                       theListOfTolerances->First();
783
784       const Standard_Real aTol2 = (theListOfTolerances == 0) ? 0.0 :
785                                       theListOfTolerances->Last();
786       
787       const gp_XYZ &aP1 = theListOfPoints.First().XYZ(),
788                    &aP2 = theListOfPoints.Last().XYZ();
789       const gp_XYZ aDP = aP2 - aP1;
790       const Standard_Real aDPm = aDP.Modulus();
791       myIsAABox = Standard_False;
792       myHDims[1] = myHDims[2] = Max(aTol1, aTol2);
793
794       if(aDPm < Precision::Confusion())
795       {
796         ProcessOnePoint(aP1);
797         Enlarge(myHDims[1] + Precision::Confusion());
798         return;
799       }
800
801       myHDims[0] = 0.5*(aDPm+aTol1+aTol2);
802       myAxes[0] = aDP/aDPm;
803       if(Abs(myAxes[0].X()) > Abs(myAxes[0].Y()))
804       {
805         // Z-coord. is maximal or X-coord. is maximal
806         myAxes[1].SetCoord(-myAxes[0].Z(), 0.0, myAxes[0].X());
807       }
808       else
809       {
810         // Z-coord. is maximal or Y-coord. is maximal
811         myAxes[1].SetCoord(0.0, -myAxes[0].Z(), myAxes[0].Y());
812       }
813
814       myAxes[2] = myAxes[0].Crossed(myAxes[1]).Normalized();
815       myCenter = aP1 + 0.5*(aDPm - aTol1 + aTol2)*myAxes[0];
816     }
817     return;
818     default:
819       break;
820   }
821
822   OBBTool aTool(theListOfPoints, theListOfTolerances, theIsOptimal);
823   aTool.ProcessDiTetrahedron();
824   aTool.BuildBox(*this);
825 }
826
827 // =======================================================================
828 // function : IsOut
829 // purpose  : 
830 // =======================================================================
831 Standard_Boolean Bnd_OBB::IsOut(const Bnd_OBB& theOther) const
832 {
833   if (IsVoid() || theOther.IsVoid())
834     return Standard_True;
835
836   if (myIsAABox && theOther.myIsAABox)
837   {
838     return ((Abs(theOther.myCenter.X() - myCenter.X()) > theOther.myHDims[0] + myHDims[0]) ||
839             (Abs(theOther.myCenter.Y() - myCenter.Y()) > theOther.myHDims[1] + myHDims[1]) ||
840             (Abs(theOther.myCenter.Z() - myCenter.Z()) > theOther.myHDims[2] + myHDims[2]));
841   }
842
843   // According to the Separating Axis Theorem for Oriented Bounding Boxes
844   // it is necessary to check the 15 separating axes (Ls):
845   // - 6 axes of the boxes;
846   // - 9 cross products of the axes of the boxes.
847   // If any of these axes is valid, the boxes do not interfere.
848
849   // The algorithm is following:
850   // 1. Compute the "length" for j-th BndBox (j=1...2) according to the formula:
851   //    L(j)=Sum(myHDims[i]*Abs(myAxes[i].Dot(Ls)))
852   // 2. If (theCenter2 - theCenter1).Dot(Ls) > (L(1) + L(2))
853   //    then the considered OBBs are not interfered in terms of the axis Ls.
854   //
855   // If OBBs are not interfered in terms of at least one axis (of 15) then
856   // they are not interfered at all.
857
858   // Precomputed difference between centers
859   gp_XYZ D = theOther.myCenter - myCenter;
860
861   // Check the axes of the this box, i.e. L is one of myAxes
862   // Since the Dot product of two of these directions is null, it could be skipped:
863   // myXDirection.Dot(myYDirection) = 0
864
865   for(Standard_Integer i = 0; i < 3; ++i)
866   {
867     // Length of the second segment
868     Standard_Real aLSegm2 = 0;
869     for(Standard_Integer j = 0; j < 3; ++j)
870       aLSegm2 += theOther.myHDims[j] * Abs(theOther.myAxes[j].Dot(myAxes[i]));
871
872     // Distance between projected centers
873     Standard_Real aDistCC = Abs(D.Dot(myAxes[i]));
874
875     if(aDistCC > myHDims[i] + aLSegm2)
876       return Standard_True;
877   }
878
879   // Check the axes of the Other box, i.e. L is one of theOther.myAxes
880
881   for(Standard_Integer i = 0; i < 3; ++i)
882   {
883     // Length of the first segment
884     Standard_Real aLSegm1 = 0.;
885     for(Standard_Integer j = 0; j < 3; ++j)
886       aLSegm1 += myHDims[j] * Abs(myAxes[j].Dot(theOther.myAxes[i]));
887
888     // Distance between projected centers
889     Standard_Real aDistCC = Abs(D.Dot(theOther.myAxes[i]));
890
891     if(aDistCC > aLSegm1 + theOther.myHDims[i])
892       return Standard_True;
893   }
894
895   const Standard_Real aTolNull = Epsilon(1.0);
896
897   // Check the axes produced by the cross products
898   for(Standard_Integer i = 0; i < 3; ++i)
899   {
900     for(Standard_Integer j = 0; j < 3; ++j)
901     {
902       // Separating axis
903       gp_XYZ aLAxe = myAxes[i].Crossed(theOther.myAxes[j]);
904
905       const Standard_Real aNorm = aLAxe.Modulus();
906       if(aNorm < aTolNull)
907         continue;
908
909       aLAxe /= aNorm;
910
911       // Length of the first segment
912       Standard_Real aLSegm1 = 0.;
913       for(Standard_Integer k = 0; k < 3; ++k)
914         aLSegm1 += myHDims[k] * Abs(myAxes[k].Dot(aLAxe));
915
916       // Length of the second segment
917       Standard_Real aLSegm2 = 0.;
918       for(Standard_Integer k = 0; k < 3; ++k)
919         aLSegm2 += theOther.myHDims[k] * Abs(theOther.myAxes[k].Dot(aLAxe));
920
921       // Distance between projected centers
922       Standard_Real aDistCC = Abs(D.Dot(aLAxe));
923
924       if(aDistCC > aLSegm1 + aLSegm2)
925         return Standard_True;
926     }
927   }
928
929   return Standard_False;
930 }
931
932 // =======================================================================
933 // function : IsOut
934 // purpose  : 
935 // =======================================================================
936 Standard_Boolean Bnd_OBB::IsOut(const gp_Pnt& theP) const
937 {
938   // 1. Project the point to myAxes[i] (i=0...2).
939   // 2. Check, whether the absolute value of the correspond
940   //    projection parameter is greater than myHDims[i].
941   //    In this case, IsOut method will return TRUE.
942
943   const gp_XYZ aRV = theP.XYZ() - myCenter;
944
945   return ((Abs(myAxes[0].Dot(aRV)) > myHDims[0]) || 
946           (Abs(myAxes[1].Dot(aRV)) > myHDims[1]) ||
947           (Abs(myAxes[2].Dot(aRV)) > myHDims[2]));
948 }
949
950 // =======================================================================
951 // function : IsCompletelyInside
952 // purpose  : Checks if every vertex of theOther is completely inside *this
953 // =======================================================================
954 Standard_Boolean Bnd_OBB::IsCompletelyInside(const Bnd_OBB& theOther) const
955 {
956   if(IsVoid() || theOther.IsVoid())
957     return Standard_False;
958
959   gp_Pnt aVert[8];
960   theOther.GetVertex(aVert);
961   for(Standard_Integer i = 0; i < 8; i++)
962   {
963     if(IsOut(aVert[i]))
964       return Standard_False;
965   }
966
967   return Standard_True;
968 }
969
970 // =======================================================================
971 // function : Add
972 // purpose  : 
973 // =======================================================================
974 void Bnd_OBB::Add(const gp_Pnt& theP)
975 {
976   if (IsVoid())
977   {
978     myCenter = theP.XYZ();
979     myAxes[0] = gp::DX().XYZ();
980     myAxes[1] = gp::DY().XYZ();
981     myAxes[2] = gp::DZ().XYZ();
982     myHDims[0] = 0.0;
983     myHDims[1] = 0.0;
984     myHDims[2] = 0.0;
985     myIsAABox = Standard_True;
986   }
987   else
988   {
989     gp_Pnt aList[9];
990     GetVertex(aList);
991     aList[8] = theP;
992     ReBuild(TColgp_Array1OfPnt(aList[0], 0, 8));
993   }
994 }
995
996 // =======================================================================
997 // function : Add
998 // purpose  : 
999 // =======================================================================
1000 void Bnd_OBB::Add(const Bnd_OBB& theOther)
1001 {
1002   if (!theOther.IsVoid())
1003   {
1004     if (IsVoid())
1005     {
1006       myCenter = theOther.myCenter;
1007       myAxes[0] = theOther.myAxes[0];
1008       myAxes[1] = theOther.myAxes[1];
1009       myAxes[2] = theOther.myAxes[2];
1010       myHDims[0] = theOther.myHDims[0];
1011       myHDims[1] = theOther.myHDims[1];
1012       myHDims[2] = theOther.myHDims[2];
1013       myIsAABox = theOther.myIsAABox;
1014     }
1015     else
1016     {
1017       gp_Pnt aList[16];
1018       GetVertex(&aList[0]);
1019       theOther.GetVertex(&aList[8]);
1020       ReBuild(TColgp_Array1OfPnt(aList[0], 0, 15));
1021     }
1022   }
1023 }
1024