fabfd004e9dc6accfce216f5d153ff9266572948
[occt.git] / src / SelectMgr / SelectMgr_Frustum.lxx
1 // Created on: 2015-03-16
2 // Created by: Varvara POSKONINA
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 #include <NCollection_Vector.hxx>
17 #include <Poly_Array1OfTriangle.hxx>
18 #include <Standard_Assert.hxx>
19 #include <SelectMgr_FrustumBuilder.hxx>
20
21 // =======================================================================
22 // function : isSeparated
23 // purpose  : Checks if AABB and frustum are separated along the given axis.
24 // =======================================================================
25 template <int N>
26 Standard_Boolean SelectMgr_Frustum<N>::isSeparated (const SelectMgr_Vec3& theBoxMin,
27                                                     const SelectMgr_Vec3& theBoxMax,
28                                                     const gp_XYZ&         theDirect,
29                                                     Standard_Boolean*     theInside) const
30 {
31   const Standard_Real aMinB =
32     theDirect.X() * (theDirect.X() < 0.0 ? theBoxMax.x() : theBoxMin.x()) +
33     theDirect.Y() * (theDirect.Y() < 0.0 ? theBoxMax.y() : theBoxMin.y()) +
34     theDirect.Z() * (theDirect.Z() < 0.0 ? theBoxMax.z() : theBoxMin.z());
35
36   const Standard_Real aMaxB =
37     theDirect.X() * (theDirect.X() < 0.0 ? theBoxMin.x() : theBoxMax.x()) +
38     theDirect.Y() * (theDirect.Y() < 0.0 ? theBoxMin.y() : theBoxMax.y()) +
39     theDirect.Z() * (theDirect.Z() < 0.0 ? theBoxMin.z() : theBoxMax.z());
40
41   Standard_ASSERT_RAISE (aMaxB >= aMinB, "Error! Failed to project box");
42
43   // frustum projection
44   Standard_Real aMinF =  DBL_MAX;
45   Standard_Real aMaxF = -DBL_MAX;
46
47   for (Standard_Integer aVertIdx = 0; aVertIdx < N * 2; ++aVertIdx)
48   {
49     const Standard_Real aProj = myVertices[aVertIdx].XYZ().Dot (theDirect);
50
51     aMinF = Min (aMinF, aProj);
52     aMaxF = Max (aMaxF, aProj);
53
54     if (aMinF <= aMaxB && aMaxF >= aMinB)
55     {
56       if (theInside == NULL || !(*theInside)) // only overlap test
57       {
58         return Standard_False;
59       }
60     }
61   }
62
63   if (aMinF > aMaxB || aMaxF < aMinB)
64   {
65     return Standard_True; // fully separated
66   }
67   else if (theInside != NULL) // to check for inclusion?
68   {
69     *theInside &= aMinB >= aMinF && aMaxB <= aMaxF;
70   }
71
72   return Standard_False;
73 }
74
75 // =======================================================================
76 // function : isSeparated
77 // purpose  : Checks if triangle and frustum are separated along the
78 //            given axis
79 // =======================================================================
80 template <int N>
81 Standard_Boolean SelectMgr_Frustum<N>::isSeparated (const gp_Pnt& thePnt1,
82                                                     const gp_Pnt& thePnt2,
83                                                     const gp_Pnt& thePnt3,
84                                                     const gp_XYZ& theAxis) const
85 {
86   // frustum projection
87   Standard_Real aMinF = RealLast();
88   Standard_Real aMaxF = RealFirst();
89
90   // triangle projection
91   Standard_Real aMinTr = RealLast();
92   Standard_Real aMaxTr = RealFirst();
93
94   Standard_Real aTriangleProj;
95
96   aTriangleProj = theAxis.Dot (thePnt1.XYZ());
97   aMinTr = Min (aMinTr, aTriangleProj);
98   aMaxTr = Max (aMaxTr, aTriangleProj);
99
100   aTriangleProj = theAxis.Dot (thePnt2.XYZ());
101   aMinTr = Min (aMinTr, aTriangleProj);
102   aMaxTr = Max (aMaxTr, aTriangleProj);
103
104   aTriangleProj = theAxis.Dot (thePnt3.XYZ());
105   aMinTr = Min (aMinTr, aTriangleProj);
106   aMaxTr = Max (aMaxTr, aTriangleProj);
107
108   for (Standard_Integer aVertIter = 0; aVertIter < N * 2; ++aVertIter)
109   {
110     const Standard_Real aProj = myVertices[aVertIter].XYZ().Dot (theAxis);
111
112     aMinF = Min (aMinF, aProj);
113     aMaxF = Max (aMaxF, aProj);
114
115     if (aMinF <= aMaxTr && aMaxF >= aMinTr)
116     {
117       return Standard_False;
118     }
119   }
120
121   return aMinF > aMaxTr || aMaxF < aMinTr;
122 }
123
124 // =======================================================================
125 // function : hasOverlap
126 // purpose  : Returns true if selecting volume is overlapped by
127 //            axis-aligned bounding box with minimum corner at point
128 //            theMinPnt and maximum at point theMaxPnt
129 // =======================================================================
130 template <int N>
131 Standard_Boolean SelectMgr_Frustum<N>::hasOverlap (const SelectMgr_Vec3& theMinPnt,
132                                                    const SelectMgr_Vec3& theMaxPnt,
133                                                    Standard_Boolean*     theInside) const
134 {
135   for (Standard_Integer anAxis = 0; anAxis < 3; ++anAxis)
136   {
137     if (theMinPnt[anAxis] > myMaxOrthoVertsProjections[anAxis]
138      || theMaxPnt[anAxis] < myMinOrthoVertsProjections[anAxis])
139     {
140       return Standard_False; // fully separated
141     }
142     else if (theInside != NULL) // to check for inclusion?
143     {
144       *theInside &= theMinPnt[anAxis] >= myMinOrthoVertsProjections[anAxis]
145                  && theMaxPnt[anAxis] <= myMaxOrthoVertsProjections[anAxis];
146     }
147   }
148
149   const Standard_Integer anIncFactor = (myIsOrthographic && N == 4) ? 2 : 1;
150
151   for (Standard_Integer aPlaneIdx = 0; aPlaneIdx < N + 1; aPlaneIdx += anIncFactor)
152   {
153     const gp_XYZ& aPlane = myPlanes[aPlaneIdx].XYZ();
154
155     const Standard_Real aBoxProjMin =
156       aPlane.X() * (aPlane.X() < 0.f ? theMaxPnt.x() : theMinPnt.x()) +
157       aPlane.Y() * (aPlane.Y() < 0.f ? theMaxPnt.y() : theMinPnt.y()) +
158       aPlane.Z() * (aPlane.Z() < 0.f ? theMaxPnt.z() : theMinPnt.z());
159
160     const Standard_Real aBoxProjMax =
161       aPlane.X() * (aPlane.X() < 0.f ? theMinPnt.x() : theMaxPnt.x()) +
162       aPlane.Y() * (aPlane.Y() < 0.f ? theMinPnt.y() : theMaxPnt.y()) +
163       aPlane.Z() * (aPlane.Z() < 0.f ? theMinPnt.z() : theMaxPnt.z());
164
165     Standard_ASSERT_RAISE (aBoxProjMax >= aBoxProjMin, "Error! Failed to project box");
166
167     if (aBoxProjMin > myMaxVertsProjections[aPlaneIdx]
168      || aBoxProjMax < myMinVertsProjections[aPlaneIdx])
169     {
170       return Standard_False; // fully separated
171     }
172     else if (theInside != NULL) // to check for inclusion?
173     {
174       *theInside &= aBoxProjMin >= myMinVertsProjections[aPlaneIdx]
175                  && aBoxProjMax <= myMaxVertsProjections[aPlaneIdx];
176     }
177   }
178
179   for (Standard_Integer aDim = 0; aDim < 3; ++aDim)
180   {
181     // the following code performs a speedup of cross-product
182     // of vector with 1.0 at the position aDim and myEdgeDirs[aVolDir]
183     const Standard_Integer aNext = (aDim + 1) % 3;
184     const Standard_Integer aNextNext = (aDim + 2) % 3;
185     for (Standard_Integer aVolDir = 0, aDirectionsNb = myIsOrthographic ? 4 : 6; aVolDir < aDirectionsNb; ++aVolDir)
186     {
187       gp_XYZ aDirection (DBL_MAX, DBL_MAX, DBL_MAX);
188       aDirection.ChangeData()[aDim]      = 0;
189       aDirection.ChangeData()[aNext]     = -myEdgeDirs[aVolDir].XYZ().GetData()[aNextNext];
190       aDirection.ChangeData()[aNextNext] = myEdgeDirs[aVolDir].XYZ().GetData()[aNext];
191
192       if (isSeparated (theMinPnt, theMaxPnt, aDirection, theInside))
193       {
194         return Standard_False;
195       }
196     }
197   }
198
199   return Standard_True;
200 }
201
202 // =======================================================================
203 // function : hasOverlap
204 // purpose  : SAT intersection test between defined volume and given point
205 // =======================================================================
206 template <int N>
207 Standard_Boolean SelectMgr_Frustum<N>::hasOverlap (const gp_Pnt& thePnt) const
208 {
209   const Standard_Integer anIncFactor = (myIsOrthographic && N == 4) ? 2 : 1;
210
211   for (Standard_Integer aPlaneIdx = 0; aPlaneIdx < N + 1; aPlaneIdx += anIncFactor)
212   {
213     const Standard_Real aPointProj = myPlanes[aPlaneIdx].XYZ().Dot (thePnt.XYZ());
214
215     if (aPointProj > myMaxVertsProjections[aPlaneIdx]
216      || aPointProj < myMinVertsProjections[aPlaneIdx])
217     {
218       return Standard_False;
219     }
220   }
221
222   return Standard_True;
223 }
224
225 // =======================================================================
226 // function : hasOverlap
227 // purpose  : SAT intersection test between defined volume and given segment
228 // =======================================================================
229 template <int N>
230 Standard_Boolean SelectMgr_Frustum<N>::hasOverlap (const gp_Pnt& theStartPnt,
231                                                    const gp_Pnt& theEndPnt) const
232 {
233   const gp_XYZ& aDir = theEndPnt.XYZ() - theStartPnt.XYZ();
234   if (aDir.Modulus() < Precision::Confusion())
235     return Standard_True;
236
237   const Standard_Integer anIncFactor = (myIsOrthographic && N == 4) ? 2 : 1;
238   for (Standard_Integer aPlaneIdx = 0; aPlaneIdx < N + 1; aPlaneIdx += anIncFactor)
239   {
240     Standard_Real aMinSegm = RealLast(), aMaxSegm = RealFirst();
241     Standard_Real aMinF    = RealLast(), aMaxF    = RealFirst();
242
243     Standard_Real aProj1 = myPlanes[aPlaneIdx].XYZ().Dot (theStartPnt.XYZ());
244     Standard_Real aProj2 = myPlanes[aPlaneIdx].XYZ().Dot (theEndPnt.XYZ());
245     aMinSegm = Min (aProj1, aProj2);
246     aMaxSegm = Max (aProj1, aProj2);
247
248     aMaxF = myMaxVertsProjections[aPlaneIdx];
249     aMinF = myMinVertsProjections[aPlaneIdx];
250
251     if (aMinSegm > aMaxF
252       || aMaxSegm < aMinF)
253     {
254       return Standard_False;
255     }
256   }
257
258   Standard_Real aMin1 = DBL_MAX, aMax1 = -DBL_MAX;
259   Standard_Real aMin2 = DBL_MAX, aMax2 = -DBL_MAX;
260   for (Standard_Integer aVertIdx = 0; aVertIdx < N * 2; ++aVertIdx)
261   {
262     Standard_Real aProjection = aDir.Dot (myVertices[aVertIdx].XYZ());
263     aMax2 = Max (aMax2, aProjection);
264     aMin2 = Min (aMin2, aProjection);
265   }
266   Standard_Real aProj1 = aDir.Dot (theStartPnt.XYZ());
267   Standard_Real aProj2 = aDir.Dot (theEndPnt.XYZ());
268   aMin1 = Min (aProj1, aProj2);
269   aMax1 = Max (aProj1, aProj2);
270   if (aMin1 > aMax2
271     || aMax1 < aMin2)
272   {
273     return Standard_False;
274   }
275
276   Standard_Integer aDirectionsNb = myIsOrthographic ? 4 : 6;
277   for (Standard_Integer aEdgeDirIdx = 0; aEdgeDirIdx < aDirectionsNb; ++aEdgeDirIdx)
278   {
279     Standard_Real aMinSegm = DBL_MAX, aMaxSegm = -DBL_MAX;
280     Standard_Real aMinF    = DBL_MAX, aMaxF    = -DBL_MAX;
281
282     const gp_XYZ aTestDir = aDir.Crossed (myEdgeDirs[aEdgeDirIdx].XYZ());
283
284     Standard_Real Proj1 = aTestDir.Dot (theStartPnt.XYZ());
285     Standard_Real Proj2 = aTestDir.Dot (theEndPnt.XYZ());
286     aMinSegm = Min (Proj1, Proj2);
287     aMaxSegm = Max (Proj1, Proj2);
288
289     for (Standard_Integer aVertIdx = 0; aVertIdx < N * 2; ++aVertIdx)
290     {
291       Standard_Real aProjection = aTestDir.Dot (myVertices[aVertIdx].XYZ());
292       aMaxF = Max (aMaxF, aProjection);
293       aMinF = Min (aMinF, aProjection);
294     }
295
296     if (aMinSegm > aMaxF
297       || aMaxSegm < aMinF)
298     {
299       return Standard_False;
300     }
301   }
302
303   return Standard_True;
304 }
305
306 // =======================================================================
307 // function : hasOverlap
308 // purpose  : SAT intersection test between frustum given and planar convex
309 //            polygon represented as ordered point set
310 // =======================================================================
311 template <int N>
312 Standard_Boolean SelectMgr_Frustum<N>::hasOverlap (const TColgp_Array1OfPnt& theArrayOfPnts,
313                                                    gp_Vec& theNormal) const
314 {
315   Standard_Integer aStartIdx = theArrayOfPnts.Lower();
316   Standard_Integer anEndIdx = theArrayOfPnts.Upper();
317
318   const gp_XYZ& aPnt1 = theArrayOfPnts.Value (aStartIdx).XYZ();
319   const gp_XYZ& aPnt2 = theArrayOfPnts.Value (aStartIdx + 1).XYZ();
320   const gp_XYZ& aPnt3 = theArrayOfPnts.Value (aStartIdx + 2).XYZ();
321   const gp_XYZ aVec1 = aPnt1 - aPnt2;
322   const gp_XYZ aVec2 = aPnt3 - aPnt2;
323   theNormal = aVec2.Crossed (aVec1);
324   const gp_XYZ& aNormal = theNormal.XYZ();
325   Standard_Real aPolygProjection = aNormal.Dot (aPnt1);
326
327   Standard_Real aMax = RealFirst();
328   Standard_Real aMin = RealLast();
329   for (Standard_Integer aVertIdx = 0; aVertIdx < N * 2; ++aVertIdx)
330   {
331     Standard_Real aProjection = aNormal.Dot (myVertices[aVertIdx].XYZ());
332     aMax = Max (aMax, aProjection);
333     aMin = Min (aMin, aProjection);
334   }
335   if (aPolygProjection > aMax
336     || aPolygProjection < aMin)
337   {
338     return Standard_False;
339   }
340
341   const Standard_Integer anIncFactor = (myIsOrthographic && N == 4) ? 2 : 1;
342   for (Standard_Integer aPlaneIdx = 0; aPlaneIdx <  N + 1; aPlaneIdx += anIncFactor)
343   {
344     Standard_Real aMaxF = RealFirst();
345     Standard_Real aMinF = RealLast();
346     Standard_Real aMaxPolyg = RealFirst();
347     Standard_Real aMinPolyg = RealLast();
348     const gp_XYZ& aPlane = myPlanes[aPlaneIdx].XYZ();
349     for (Standard_Integer aPntIter = aStartIdx; aPntIter <= anEndIdx; ++aPntIter)
350     {
351       Standard_Real aProjection = aPlane.Dot (theArrayOfPnts.Value (aPntIter).XYZ());
352       aMaxPolyg = Max (aMaxPolyg, aProjection);
353       aMinPolyg = Min (aMinPolyg, aProjection);
354     }
355     aMaxF = myMaxVertsProjections[aPlaneIdx];
356     aMinF = myMinVertsProjections[aPlaneIdx];
357     if (aMinPolyg > aMaxF
358       || aMaxPolyg < aMinF)
359     {
360       return Standard_False;
361     }
362   }
363
364   Standard_Integer aDirectionsNb = myIsOrthographic ? 4 : 6;
365   for (Standard_Integer aPntsIter = 0, aLastIdx = anEndIdx - aStartIdx, aLen = theArrayOfPnts.Length(); aPntsIter <= aLastIdx; ++aPntsIter)
366   {
367     const gp_XYZ aSegmDir = theArrayOfPnts.Value ((aPntsIter + 1) % aLen + aStartIdx).XYZ()
368       - theArrayOfPnts.Value (aPntsIter + aStartIdx).XYZ();
369     for (Standard_Integer aVolDir = 0; aVolDir < aDirectionsNb; ++aVolDir)
370     {
371       Standard_Real aMaxPolyg = RealFirst();
372       Standard_Real aMinPolyg = RealLast();
373       Standard_Real aMaxF = RealFirst();
374       Standard_Real aMinF = RealLast();
375       const gp_XYZ aTestDir = aSegmDir.Crossed (myEdgeDirs[aVolDir].XYZ());
376
377       for (Standard_Integer aPntIter = aStartIdx; aPntIter <= anEndIdx; ++aPntIter)
378       {
379         Standard_Real aProjection = aTestDir.Dot (theArrayOfPnts.Value (aPntIter).XYZ());
380         aMaxPolyg = Max (aMaxPolyg, aProjection);
381         aMinPolyg = Min (aMinPolyg, aProjection);
382       }
383
384       for (Standard_Integer aVertIdx = 0; aVertIdx < N * 2; ++aVertIdx)
385       {
386         Standard_Real aProjection = aTestDir.Dot (myVertices[aVertIdx].XYZ());
387         aMaxF = Max (aMaxF, aProjection);
388         aMinF = Min (aMinF, aProjection);
389       }
390
391       if (aMinPolyg > aMaxF
392         || aMaxPolyg < aMinF)
393       {
394         return Standard_False;
395       }
396     }
397   }
398
399   return Standard_True;
400 }
401
402 // =======================================================================
403 // function : hasOverlap
404 // purpose  : SAT intersection test between defined volume and given triangle
405 // =======================================================================
406 template <int N>
407 Standard_Boolean SelectMgr_Frustum<N>::hasOverlap (const gp_Pnt& thePnt1,
408                                                    const gp_Pnt& thePnt2,
409                                                    const gp_Pnt& thePnt3,
410                                                    gp_Vec& theNormal) const
411 {
412   const gp_XYZ aTrEdges[3] = { thePnt2.XYZ() - thePnt1.XYZ(),
413                                thePnt3.XYZ() - thePnt2.XYZ(),
414                                thePnt1.XYZ() - thePnt3.XYZ() };
415
416   const Standard_Integer anIncFactor = (myIsOrthographic && N == 4) ? 2 : 1;
417   for (Standard_Integer aPlaneIdx = 0; aPlaneIdx < N + 1; aPlaneIdx += anIncFactor)
418   {
419     const gp_XYZ& aPlane = myPlanes[aPlaneIdx].XYZ();
420     Standard_Real aTriangleProj;
421
422     aTriangleProj = aPlane.Dot (thePnt1.XYZ());
423     Standard_Real aTriangleProjMin = aTriangleProj;
424     Standard_Real aTriangleProjMax = aTriangleProj;
425
426     aTriangleProj = aPlane.Dot (thePnt2.XYZ());
427     aTriangleProjMin = Min (aTriangleProjMin, aTriangleProj);
428     aTriangleProjMax = Max (aTriangleProjMax, aTriangleProj);
429
430     aTriangleProj = aPlane.Dot (thePnt3.XYZ());
431     aTriangleProjMin = Min (aTriangleProjMin, aTriangleProj);
432     aTriangleProjMax = Max (aTriangleProjMax, aTriangleProj);
433
434     Standard_Real aFrustumProjMax = myMaxVertsProjections[aPlaneIdx];
435     Standard_Real aFrustumProjMin = myMinVertsProjections[aPlaneIdx];
436     if (aTriangleProjMin > aFrustumProjMax
437       || aTriangleProjMax < aFrustumProjMin)
438     {
439       return Standard_False;
440     }
441   }
442
443   theNormal = aTrEdges[2].Crossed (aTrEdges[0]);
444   if (isSeparated (thePnt1, thePnt2, thePnt3, theNormal.XYZ()))
445   {
446     return Standard_False;
447   }
448
449   Standard_Integer aDirectionsNb = myIsOrthographic ? 4 : 6;
450   for (Standard_Integer aTriangleEdgeIdx = 0; aTriangleEdgeIdx < 3; ++aTriangleEdgeIdx)
451   {
452     for (Standard_Integer aVolDir = 0; aVolDir < aDirectionsNb; ++aVolDir)
453     {
454       const gp_XYZ& aTestDirection =  myEdgeDirs[aVolDir].XYZ().Crossed (aTrEdges[aTriangleEdgeIdx]);
455
456       if (isSeparated (thePnt1, thePnt2, thePnt3, aTestDirection))
457       {
458         return Standard_False;
459       }
460     }
461   }
462
463   return Standard_True;
464 }
465
466 //=======================================================================
467 //function : DumpJson
468 //purpose  : 
469 //=======================================================================
470 template <int N>
471 void SelectMgr_Frustum<N>::DumpJson (Standard_OStream& theOStream, Standard_Integer theDepth) const
472 {
473   OCCT_DUMP_TRANSIENT_CLASS_BEGIN (theOStream)
474
475   const Standard_Integer anIncFactor = (myIsOrthographic && N == 4) ? 2 : 1;
476   for (Standard_Integer aPlaneIdx = 0; aPlaneIdx < N + 1; aPlaneIdx += anIncFactor)
477   {
478     const gp_Vec& aPlane = myPlanes[aPlaneIdx];
479     OCCT_DUMP_FIELD_VALUES_DUMPED (theOStream, theDepth, &aPlane)
480
481     OCCT_DUMP_FIELD_VALUE_NUMERICAL (theOStream, myMaxVertsProjections[aPlaneIdx])
482     OCCT_DUMP_FIELD_VALUE_NUMERICAL (theOStream, myMinVertsProjections[aPlaneIdx])
483   }
484
485   for (Standard_Integer aVertIdx = 0; aVertIdx < N * 2; ++aVertIdx)
486   {
487     const gp_Pnt& aVertex = myVertices[aVertIdx];
488     OCCT_DUMP_FIELD_VALUES_DUMPED (theOStream, theDepth, &aVertex)
489   }
490
491   OCCT_DUMP_FIELD_VALUE_NUMERICAL (theOStream, myPixelTolerance)
492   OCCT_DUMP_FIELD_VALUE_NUMERICAL (theOStream, myIsOrthographic)
493   OCCT_DUMP_FIELD_VALUE_POINTER (theOStream, myBuilder)
494   OCCT_DUMP_FIELD_VALUE_POINTER (theOStream, myCamera)
495
496   for (Standard_Integer anIndex = 0; anIndex < 3; anIndex++)
497   {
498     Standard_Real aMaxOrthoVertsProjections = myMaxOrthoVertsProjections[anIndex];
499     Standard_Real aMinOrthoVertsProjections = myMinOrthoVertsProjections[anIndex];
500
501     OCCT_DUMP_FIELD_VALUE_NUMERICAL (theOStream, aMaxOrthoVertsProjections)
502     OCCT_DUMP_FIELD_VALUE_NUMERICAL (theOStream, aMinOrthoVertsProjections)
503   }
504
505   for (Standard_Integer anIndex = 0; anIndex < 6; anIndex++)
506   {
507     const gp_Vec& anEdgeDir = myEdgeDirs[anIndex];
508     OCCT_DUMP_FIELD_VALUES_DUMPED (theOStream, theDepth, &anEdgeDir)
509   }
510 }