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