11de588271ee74fad7873cf860f9ef3abdae28b5
[occt.git] / src / Bnd / Bnd_B3x.gxx
1 // Created on: 2005-09-08
2 // Created by: Alexander GRIGORIEV
3 // Copyright (c) 2005-2014 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 inline Standard_Boolean _compareDist  (const RealType aHSize[3],
17                                        const RealType aDist [3])
18 {
19   return (Abs(aDist[0]) > aHSize[0] ||
20           Abs(aDist[1]) > aHSize[1] ||
21           Abs(aDist[2]) > aHSize[2]);
22 }
23
24 inline Standard_Boolean _compareDistD (const gp_XYZ& aHSize,const gp_XYZ& aDist)
25 {
26   return (Abs(aDist.X()) > aHSize.X() ||
27           Abs(aDist.Y()) > aHSize.Y() ||
28           Abs(aDist.Z()) > aHSize.Z());
29 }
30
31 //=======================================================================
32 //function : Add
33 //purpose  : Update the box by a point
34 //=======================================================================
35
36 void Bnd_B3x::Add (const gp_XYZ& thePnt) {
37   if (IsVoid()) {
38     myCenter[0] = RealType(thePnt.X());
39     myCenter[1] = RealType(thePnt.Y());
40     myCenter[2] = RealType(thePnt.Z());
41     myHSize [0] = 0.;
42     myHSize [1] = 0.;
43     myHSize [2] = 0.;
44   } else {
45     const RealType aDiff[3] = {
46       RealType(thePnt.X()) - myCenter[0],
47       RealType(thePnt.Y()) - myCenter[1],
48       RealType(thePnt.Z()) - myCenter[2]
49     };
50     if (aDiff[0] > myHSize[0]) {
51       const RealType aShift = (aDiff[0] - myHSize[0]) / 2;
52       myCenter[0] += aShift;
53       myHSize [0] += aShift;
54     } else if (aDiff[0] < -myHSize[0]) {
55       const RealType aShift = (aDiff[0] + myHSize[0]) / 2;
56       myCenter[0] += aShift;
57       myHSize [0] -= aShift;
58     }
59     if (aDiff[1] > myHSize[1]) {
60       const RealType aShift = (aDiff[1] - myHSize[1]) / 2;
61       myCenter[1] +=aShift;
62       myHSize [1] +=aShift;
63     } else if (aDiff[1] < -myHSize[1]) {
64       const RealType aShift = (aDiff[1] + myHSize[1]) / 2;
65       myCenter[1] += aShift;
66       myHSize [1] -= aShift;
67     }
68     if (aDiff[2] > myHSize[2]) {
69       const RealType aShift = (aDiff[2] - myHSize[2]) / 2;
70       myCenter[2] +=aShift;
71       myHSize [2] +=aShift;
72     } else if (aDiff[2] < -myHSize[2]) {
73       const RealType aShift = (aDiff[2] + myHSize[2]) / 2;
74       myCenter[2] += aShift;
75       myHSize [2] -= aShift;
76     }
77   }
78 }
79
80 //=======================================================================
81 //function : Limit
82 //purpose  : limit the current box with the internals of theBox
83 //=======================================================================
84
85 Standard_Boolean Bnd_B3x::Limit (const Bnd_B3x& theBox)
86 {
87   Standard_Boolean aResult (Standard_False);
88   const RealType diffC[3] = {
89     theBox.myCenter[0] - myCenter[0],
90     theBox.myCenter[1] - myCenter[1],
91     theBox.myCenter[2] - myCenter[2]
92   };
93   const RealType sumH[3] = {
94     theBox.myHSize[0] + myHSize[0],
95     theBox.myHSize[1] + myHSize[1],
96     theBox.myHSize[2] + myHSize[2]
97   };
98   // check the condition IsOut
99   if (_compareDist (sumH, diffC) == Standard_False) {
100     const RealType diffH[3] = {
101       theBox.myHSize[0] - myHSize[0],
102       theBox.myHSize[1] - myHSize[1],
103       theBox.myHSize[2] - myHSize[2]
104     };
105     if (diffC[0] - diffH[0] > 0.) {
106       const RealType aShift = (diffC[0] - diffH[0]) / 2; // positive
107       myCenter[0] += aShift;
108       myHSize [0] -= aShift;
109     } else if (diffC[0] + diffH[0] < 0.) {
110       const RealType aShift = (diffC[0] + diffH[0]) / 2; // negative
111       myCenter[0] += aShift;
112       myHSize [0] += aShift;
113     }
114     if (diffC[1] - diffH[1] > 0.) {
115       const RealType aShift = (diffC[1] - diffH[1]) / 2; // positive
116       myCenter[1] += aShift;
117       myHSize [1] -= aShift;
118     } else if (diffC[1] + diffH[1] < 0.) {
119       const RealType aShift = (diffC[1] + diffH[1]) / 2; // negative
120       myCenter[1] += aShift;
121       myHSize [1] += aShift;
122     }
123     if (diffC[2] - diffH[2] > 0.) {
124       const RealType aShift = (diffC[2] - diffH[2]) / 2; // positive
125       myCenter[2] += aShift;
126       myHSize [2] -= aShift;
127     } else if (diffC[2] + diffH[2] < 0.) {
128       const RealType aShift = (diffC[2] + diffH[2]) / 2; // negative
129       myCenter[2] += aShift;
130       myHSize [2] += aShift;
131     }
132     aResult = Standard_True;
133   }
134   return aResult;
135 }
136
137 //=======================================================================
138 //function : Transformed
139 //purpose  : 
140 //=======================================================================
141
142 Bnd_B3x Bnd_B3x::Transformed (const gp_Trsf& theTrsf) const
143 {
144   Bnd_B3x aResult;
145   const gp_TrsfForm aForm = theTrsf.Form();
146   const Standard_Real aScale = theTrsf.ScaleFactor();
147   const Standard_Real aScaleAbs = Abs(aScale);
148   if (aForm == gp_Identity)
149     aResult = * this;
150   else if (aForm== gp_Translation || aForm== gp_PntMirror || aForm== gp_Scale)
151   {
152     aResult.myCenter[0] =
153       (RealType)(myCenter[0] * aScale + theTrsf.TranslationPart().X());
154     aResult.myCenter[1] =
155       (RealType)(myCenter[1] * aScale + theTrsf.TranslationPart().Y());
156     aResult.myCenter[2] =
157       (RealType)(myCenter[2] * aScale + theTrsf.TranslationPart().Z());
158     aResult.myHSize[0] = (RealType)(myHSize[0] * aScaleAbs);
159     aResult.myHSize[1] = (RealType)(myHSize[1] * aScaleAbs);
160     aResult.myHSize[2] = (RealType)(myHSize[2] * aScaleAbs);
161   } else {
162     gp_XYZ aCenter ((Standard_Real)myCenter[0],
163                     (Standard_Real)myCenter[1],
164                     (Standard_Real)myCenter[2]);
165     theTrsf.Transforms (aCenter);
166     aResult.myCenter[0] = (RealType)aCenter.X();
167     aResult.myCenter[1] = (RealType)aCenter.Y();
168     aResult.myCenter[2] = (RealType)aCenter.Z();
169
170     const Standard_Real * aMat = &theTrsf.HVectorialPart().Value(1,1);
171     aResult.myHSize[0] = (RealType)(aScaleAbs * (Abs(aMat[0]) * myHSize[0]+
172                                                  Abs(aMat[1]) * myHSize[1]+
173                                                  Abs(aMat[2]) * myHSize[2]));
174     aResult.myHSize[1] = (RealType)(aScaleAbs * (Abs(aMat[3]) * myHSize[0]+
175                                                  Abs(aMat[4]) * myHSize[1]+
176                                                  Abs(aMat[5]) * myHSize[2]));
177     aResult.myHSize[2] = (RealType)(aScaleAbs * (Abs(aMat[6]) * myHSize[0]+
178                                                  Abs(aMat[7]) * myHSize[1]+
179                                                  Abs(aMat[8]) * myHSize[2]));
180   }
181   return aResult;
182 }
183
184 //=======================================================================
185 //function : IsOut
186 //purpose  : Intersection Box - Sphere
187 //=======================================================================
188
189 Standard_Boolean Bnd_B3x::IsOut (const gp_XYZ&          theCenter,
190                                  const Standard_Real    theRadius,
191                                  const Standard_Boolean isSphereHollow) const
192 {
193   Standard_Boolean aResult (Standard_True);
194   if (isSphereHollow == Standard_False) {
195     // vector from the center of the sphere to the nearest box face
196     const Standard_Real aDist[3] = {
197       Abs(theCenter.X()-Standard_Real(myCenter[0])) - Standard_Real(myHSize[0]),
198       Abs(theCenter.Y()-Standard_Real(myCenter[1])) - Standard_Real(myHSize[1]),
199       Abs(theCenter.Z()-Standard_Real(myCenter[2])) - Standard_Real(myHSize[2])
200     };
201     Standard_Real aD (0.);
202     if (aDist[0] > 0.)
203       aD  = aDist[0]*aDist[0];
204     if (aDist[1] > 0.)
205       aD += aDist[1]*aDist[1];
206     if (aDist[2] > 0.)
207       aD += aDist[2]*aDist[2];
208     aResult = (aD > theRadius*theRadius);
209   } else {
210     const Standard_Real aDistC[3] = {
211       Abs(theCenter.X()-Standard_Real(myCenter[0])),
212       Abs(theCenter.Y()-Standard_Real(myCenter[1])),
213       Abs(theCenter.Z()-Standard_Real(myCenter[2]))
214     };
215     // vector from the center of the sphere to the nearest box face
216     Standard_Real aDist[3] = {
217       aDistC[0] - Standard_Real(myHSize[0]),
218       aDistC[1] - Standard_Real(myHSize[1]),
219       aDistC[2] - Standard_Real(myHSize[2])
220     };
221     Standard_Real aD (0.);
222     if (aDist[0] > 0.)
223       aD  = aDist[0]*aDist[0];
224     if (aDist[1] > 0.)
225       aD += aDist[1]*aDist[1];
226     if (aDist[2] > 0.)
227       aD += aDist[2]*aDist[2];
228     if (aD < theRadius*theRadius) {
229       // the box intersects the solid sphere; check if it is completely
230       // inside the circle (in such case return isOut==True)
231       aDist[0] = aDistC[0] + Standard_Real(myHSize[0]);
232       aDist[1] = aDistC[1] + Standard_Real(myHSize[1]);
233       aDist[2] = aDistC[2] + Standard_Real(myHSize[2]);
234       if (aDist[0]*aDist[0]+aDist[1]*aDist[1]+aDist[2]*aDist[2]
235            > theRadius*theRadius)
236         aResult = Standard_False;
237     }
238   }
239   return aResult;
240 }
241
242 //=======================================================================
243 //function : IsOut
244 //purpose  : Intersection Box - transformed Box
245 //=======================================================================
246
247 Standard_Boolean Bnd_B3x::IsOut (const Bnd_B3x& theBox,
248                                  const gp_Trsf& theTrsf) const
249 {
250   Standard_Boolean aResult (Standard_False);
251   const gp_TrsfForm aForm = theTrsf.Form();
252   const Standard_Real aScale = theTrsf.ScaleFactor();
253   const Standard_Real aScaleAbs = Abs(aScale);
254   if (aForm == gp_Translation || aForm == gp_Identity ||
255       aForm == gp_PntMirror   || aForm == gp_Scale)
256   {
257     aResult =
258       (Abs (RealType(theBox.myCenter[0]*aScale + theTrsf.TranslationPart().X())
259             - myCenter[0])
260          > RealType (theBox.myHSize[0]*aScaleAbs) + myHSize[0] ||
261        Abs (RealType(theBox.myCenter[1]*aScale + theTrsf.TranslationPart().Y())
262             - myCenter[1])
263          > RealType (theBox.myHSize[1]*aScaleAbs) + myHSize[1] ||
264        Abs (RealType(theBox.myCenter[2]*aScale + theTrsf.TranslationPart().Y())
265             - myCenter[2])
266          > RealType (theBox.myHSize[2]*aScaleAbs) + myHSize[2]);
267
268   }
269   else {
270     // theBox is transformed and we check the resulting (enlarged) box against
271     // 'this' box.
272     const Standard_Real * aMat = &theTrsf.HVectorialPart().Value(1,1);
273
274     gp_XYZ aCenter ((Standard_Real)theBox.myCenter[0],
275                     (Standard_Real)theBox.myCenter[1],
276                     (Standard_Real)theBox.myCenter[2]);
277     theTrsf.Transforms (aCenter);
278     const Standard_Real aDist[3] = {
279       aCenter.X() - (Standard_Real)myCenter[0],
280       aCenter.Y() - (Standard_Real)myCenter[1],
281       aCenter.Z() - (Standard_Real)myCenter[2]
282     };
283     const Standard_Real aMatAbs[9] = {
284       Abs(aMat[0]), Abs(aMat[1]), Abs(aMat[2]), Abs(aMat[3]), Abs(aMat[4]),
285       Abs(aMat[5]), Abs(aMat[6]), Abs(aMat[7]), Abs(aMat[8])
286     };
287     if (Abs(aDist[0]) > (aScaleAbs*(aMatAbs[0]*theBox.myHSize[0]+
288                                     aMatAbs[1]*theBox.myHSize[1]+
289                                     aMatAbs[2]*theBox.myHSize[2]) +
290                          (Standard_Real)myHSize[0])    ||
291         Abs(aDist[1]) > (aScaleAbs*(aMatAbs[3]*theBox.myHSize[0]+
292                                     aMatAbs[4]*theBox.myHSize[1]+
293                                     aMatAbs[5]*theBox.myHSize[2]) +
294                          (Standard_Real)myHSize[1])    ||
295         Abs(aDist[2]) > (aScaleAbs*(aMatAbs[6]*theBox.myHSize[0]+
296                                     aMatAbs[7]*theBox.myHSize[1]+
297                                     aMatAbs[8]*theBox.myHSize[2]) +
298                          (Standard_Real)myHSize[2]))
299       aResult = Standard_True;
300
301     else {
302     // theBox is rotated, scaled and translated. We apply the reverse
303     // translation and scaling then check against the rotated box 'this'
304     if ((Abs(aMat[0]*aDist[0]+aMat[3]*aDist[1]+aMat[6]*aDist[2])
305          > theBox.myHSize[0]*aScaleAbs + (aMatAbs[0]*myHSize[0] +
306                                           aMatAbs[3]*myHSize[1] +
307                                           aMatAbs[6]*myHSize[2])) ||
308         (Abs(aMat[1]*aDist[0]+aMat[4]*aDist[1]+aMat[7]*aDist[2])
309          > theBox.myHSize[1]*aScaleAbs + (aMatAbs[1]*myHSize[0] +
310                                           aMatAbs[4]*myHSize[1] +
311                                           aMatAbs[7]*myHSize[2])) ||
312         (Abs(aMat[2]*aDist[0]+aMat[5]*aDist[1]+aMat[8]*aDist[2])
313          > theBox.myHSize[2]*aScaleAbs + (aMatAbs[2]*myHSize[0] +
314                                           aMatAbs[5]*myHSize[1] +
315                                           aMatAbs[8]*myHSize[2])))
316         aResult = Standard_True;
317     }
318   }
319   return aResult;
320 }
321
322 //=======================================================================
323 //function : IsOut
324 //purpose  : 
325 //=======================================================================
326
327 Standard_Boolean Bnd_B3x::IsOut (const gp_Ax3& thePlane) const
328 {
329   if (IsVoid())
330     return Standard_True;
331   const gp_XYZ& anOrigin = thePlane.Location().XYZ();
332   const gp_XYZ& aDir     = thePlane.Direction().XYZ();
333   const gp_XYZ  aBoxCenter ((Standard_Real)myCenter[0],
334                             (Standard_Real)myCenter[1],
335                             (Standard_Real)myCenter[2]);
336   const Standard_Real aDist0 = (aBoxCenter - anOrigin) * aDir;
337   // Find the signed distances from two opposite corners of the box to the plane
338   // If the distances are not the same sign, then the plane crosses the box
339   const Standard_Real aDist1 =   // proj of HSize on aDir
340     Standard_Real(myHSize[0]) * Abs(aDir.X()) +
341     Standard_Real(myHSize[1]) * Abs(aDir.Y()) +
342     Standard_Real(myHSize[2]) * Abs(aDir.Z());
343   return ((aDist0 + aDist1) * (aDist0 - aDist1) > 0.);
344 }
345
346 //=======================================================================
347 //function : IsOut
348 //purpose  : 
349 //=======================================================================
350
351 Standard_Boolean Bnd_B3x::IsOut (const gp_Ax1&          theLine,
352                                  const Standard_Boolean isRay,
353                                  const Standard_Real    theOverthickness) const
354 {
355   const Standard_Real aRes = gp::Resolution() * 100.;
356   if (IsVoid())
357     return Standard_True;
358   Standard_Real
359     anInter0[2] = {-RealLast(), RealLast()},
360     anInter1[2] = {-RealLast(), RealLast()};
361   const gp_XYZ& aDir = theLine.Direction().XYZ();
362   const gp_XYZ  aDiff ((Standard_Real)myCenter[0] - theLine.Location().X(),
363                        (Standard_Real)myCenter[1] - theLine.Location().Y(),
364                        (Standard_Real)myCenter[2] - theLine.Location().Z());
365
366   // Find the parameter interval in X dimention
367   Standard_Real aHSize = (Standard_Real)myHSize[0]+theOverthickness;
368   if (aDir.X() > aRes) {
369     anInter0[0]= (aDiff.X() - aHSize) / aDir.X();
370     anInter0[1]= (aDiff.X() + aHSize) / aDir.X();
371   } else if (aDir.X() < -aRes) {
372     anInter0[0]= (aDiff.X() + aHSize) / aDir.X();
373     anInter0[1]= (aDiff.X() - aHSize) / aDir.X();
374   } else
375     // the line is orthogonal to OX axis. Test for inclusion in box limits
376     if (Abs(aDiff.X()) > aHSize)
377       return Standard_True;
378   
379   // Find the parameter interval in Y dimention
380   aHSize = (Standard_Real)myHSize[1]+theOverthickness;
381   if (aDir.Y() > aRes) {
382     anInter1[0]= (aDiff.Y() - aHSize) / aDir.Y();
383     anInter1[1]= (aDiff.Y() + aHSize) / aDir.Y();
384   } else if (aDir.Y() < -aRes) {
385     anInter1[0]= (aDiff.Y() + aHSize) / aDir.Y();
386     anInter1[1]= (aDiff.Y() - aHSize) / aDir.Y();
387   } else
388     // the line is orthogonal to OY axis. Test for inclusion in box limits
389     if (Abs(aDiff.Y()) > aHSize)
390       return Standard_True;
391
392   // Intersect Y-interval with X-interval 
393   if (anInter0[0] > (anInter1[1] + aRes) || anInter0[1] < (anInter1[0] - aRes))
394     return Standard_True;
395   if (anInter1[0] > anInter0[0])
396     anInter0[0] = anInter1[0];
397   if (anInter1[1] < anInter0[1])
398     anInter0[1] = anInter1[1];
399   if (isRay && anInter0[1] < -aRes)
400     return Standard_True;
401
402   // Find the parameter interval in Z dimention
403   aHSize = (Standard_Real)myHSize[2]+theOverthickness;
404   if (aDir.Z() > aRes) {
405     anInter1[0]= (aDiff.Z() - aHSize) / aDir.Z();
406     anInter1[1]= (aDiff.Z() + aHSize) / aDir.Z();
407   } else if (aDir.Z() < -aRes) {
408     anInter1[0]= (aDiff.Z() + aHSize) / aDir.Z();
409     anInter1[1]= (aDiff.Z() - aHSize) / aDir.Z();
410   } else
411     // the line is orthogonal to OZ axis. Test for inclusion in box limits
412     return (Abs(aDiff.Z()) > aHSize);
413   if (isRay && anInter1[1] < -aRes)
414     return Standard_True;
415
416   return (anInter0[0] > (anInter1[1]+aRes) || anInter0[1] < (anInter1[0]-aRes));
417 }
418
419 //=======================================================================
420 //function : IsIn
421 //purpose  : Test the complete inclusion of this box in transformed theOtherBox
422 //=======================================================================
423
424 Standard_Boolean Bnd_B3x::IsIn (const Bnd_B3x& theBox,
425                                 const gp_Trsf& theTrsf) const
426 {
427   Standard_Boolean aResult (Standard_False);
428   const gp_TrsfForm aForm = theTrsf.Form();
429   const Standard_Real aScale = theTrsf.ScaleFactor();
430   const Standard_Real aScaleAbs = Abs(aScale);
431   if (aForm == gp_Translation || aForm == gp_Identity ||
432       aForm == gp_PntMirror   || aForm == gp_Scale)
433   {
434     aResult =
435       (Abs (RealType(theBox.myCenter[0]*aScale + theTrsf.TranslationPart().X())
436             - myCenter[0])
437          < RealType (theBox.myHSize[0]*aScaleAbs) - myHSize[0] &&
438        Abs (RealType(theBox.myCenter[1]*aScale + theTrsf.TranslationPart().Y())
439             - myCenter[1])
440          < RealType (theBox.myHSize[1]*aScaleAbs) - myHSize[1] &&
441        Abs (RealType(theBox.myCenter[2]*aScale + theTrsf.TranslationPart().Y())
442             - myCenter[2])
443          < RealType (theBox.myHSize[2]*aScaleAbs) - myHSize[2]);
444
445   } else {
446     // theBox is rotated, scaled and translated. We apply the reverse
447     // translation and scaling then check against the rotated box 'this'
448     const Standard_Real * aMat = &theTrsf.HVectorialPart().Value(1,1);
449     gp_XYZ aCenter ((Standard_Real)theBox.myCenter[0],
450                     (Standard_Real)theBox.myCenter[1],
451                     (Standard_Real)theBox.myCenter[2]);
452     theTrsf.Transforms (aCenter);
453     const Standard_Real aDist[3] = {
454       aCenter.X() - (Standard_Real)myCenter[0],
455       aCenter.Y() - (Standard_Real)myCenter[1],
456       aCenter.Z() - (Standard_Real)myCenter[2]
457     };
458     if ((Abs(aMat[0]*aDist[0]+aMat[3]*aDist[1]+aMat[6]*aDist[2])
459          < theBox.myHSize[0]*aScaleAbs - (Abs(aMat[0])*myHSize[0] +
460                                           Abs(aMat[3])*myHSize[1] +
461                                           Abs(aMat[6])*myHSize[2])) &&
462         (Abs(aMat[1]*aDist[0]+aMat[4]*aDist[1]+aMat[7]*aDist[2])
463          < theBox.myHSize[1]*aScaleAbs - (Abs(aMat[1])*myHSize[0] +
464                                           Abs(aMat[4])*myHSize[1] +
465                                           Abs(aMat[7])*myHSize[2])) &&
466         (Abs(aMat[2]*aDist[0]+aMat[5]*aDist[1]+aMat[8]*aDist[2])
467          < theBox.myHSize[2]*aScaleAbs - (Abs(aMat[2])*myHSize[0] +
468                                           Abs(aMat[5])*myHSize[1] +
469                                           Abs(aMat[8])*myHSize[2])))
470       aResult = Standard_True;
471   }
472   return aResult;
473 }
474