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
  20 // =======================================================================
  21 // function : isSeparated
  22 // purpose  : Checks if AABB and frustum are separated along the given axis.
  23 // =======================================================================
  24 template <int N>
  25 Standard_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 // =======================================================================
  79 template <int N>
  80 Standard_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 // =======================================================================
 129 template <int N>
 130 Standard_Boolean SelectMgr_Frustum<N>::hasOverlap (const SelectMgr_Vec3& theMinPnt,
 131                                                    const SelectMgr_Vec3& theMaxPnt,
 132                                                    Standard_Boolean*     theInside)
 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 // =======================================================================
 205 template <int N>
 206 Standard_Boolean SelectMgr_Frustum<N>::hasOverlap (const gp_Pnt& thePnt)
 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 // =======================================================================
 228 template <int N>
 229 Standard_Boolean SelectMgr_Frustum<N>::hasOverlap (const gp_Pnt& theStartPnt,
 230                                                    const gp_Pnt& theEndPnt)
 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 // =======================================================================
 310 template <int N>
 311 Standard_Boolean SelectMgr_Frustum<N>::hasOverlap (const TColgp_Array1OfPnt& theArrayOfPnts,
 312                                                    gp_Vec& theNormal)
 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 // =======================================================================
 405 template <int N>
 406 Standard_Boolean SelectMgr_Frustum<N>::hasOverlap (const gp_Pnt& thePnt1,
 407                                                    const gp_Pnt& thePnt2,
 408                                                    const gp_Pnt& thePnt3,
 409                                                    gp_Vec& theNormal)
 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 }