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