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