[occt.git] / src / SelectMgr / SelectMgr_Frustum.lxx

// Created on: 2015-03-16
// Created by: Varvara POSKONINA
// Copyright (c) 2005-2014 OPEN CASCADE SAS
//
// This file is part of Open CASCADE Technology software library.
//
// This library is free software; you can redistribute it and/or modify it under
// the terms of the GNU Lesser General Public License version 2.1 as published
// by the Free Software Foundation, with special exception defined in the file
// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
// distribution for complete text of the license and disclaimer of any warranty.
//
// Alternatively, this file may be used under the terms of Open CASCADE
// commercial license or contractual agreement.

#include <NCollection_Vector.hxx>
#include <Poly_Array1OfTriangle.hxx>
#include <Standard_Assert.hxx>

#define DOT(A, B) (A.x() * B.x() + A.y() * B.y() + A.z() * B.z())
#define DOTp(A, B) (A.x() * B.X() + A.y() * B.Y() + A.z() * B.Z())

// =======================================================================
// function : isSeparated
// purpose  : Checks if AABB and frustum are separated along the given axis.
// =======================================================================
template <int N>
Standard_Boolean SelectMgr_Frustum<N>::isSeparated (const SelectMgr_Vec3& theBoxMin,
                                                    const SelectMgr_Vec3& theBoxMax,
                                                    const SelectMgr_Vec3& theDirect,
                                                    Standard_Boolean*     theInside) const
{
  const Standard_Real aMinB =
    theDirect.x() * (theDirect.x() < 0.0 ? theBoxMax.x() : theBoxMin.x()) +
    theDirect.y() * (theDirect.y() < 0.0 ? theBoxMax.y() : theBoxMin.y()) +
    theDirect.z() * (theDirect.z() < 0.0 ? theBoxMax.z() : theBoxMin.z());

  const Standard_Real aMaxB =
    theDirect.x() * (theDirect.x() < 0.0 ? theBoxMin.x() : theBoxMax.x()) +
    theDirect.y() * (theDirect.y() < 0.0 ? theBoxMin.y() : theBoxMax.y()) +
    theDirect.z() * (theDirect.z() < 0.0 ? theBoxMin.z() : theBoxMax.z());

  Standard_ASSERT_RAISE (aMaxB >= aMinB, "Error! Failed to project box");

  // frustum projection
  Standard_Real aMinF =  DBL_MAX;
  Standard_Real aMaxF = -DBL_MAX;

  for (Standard_Integer aVertIdx = 0; aVertIdx < N * 2; ++aVertIdx)
  {
    const Standard_Real aProj = DOT (myVertices[aVertIdx], theDirect);

    aMinF = Min (aMinF, aProj);
    aMaxF = Max (aMaxF, aProj);

    if (aMinF <= aMaxB && aMaxF >= aMinB)
    {
      if (theInside == NULL || !(*theInside)) // only overlap test
      {
        return Standard_False;
      }
    }
  }

  if (aMinF > aMaxB || aMaxF < aMinB)
  {
    return Standard_True; // fully separated
  }
  else if (theInside != NULL) // to check for inclusion?
  {
    *theInside &= aMinB >= aMinF && aMaxB <= aMaxF;
  }

  return Standard_False;
}

// =======================================================================
// function : isSeparated
// purpose  : Checks if triangle and frustum are separated along the
//            given axis
// =======================================================================
template <int N>
Standard_Boolean SelectMgr_Frustum<N>::isSeparated (const gp_Pnt& thePnt1,
                                                    const gp_Pnt& thePnt2,
                                                    const gp_Pnt& thePnt3,
                                                    const SelectMgr_Vec3& theAxis) const
{
  // frustum projection
  Standard_Real aMinF = RealLast();
  Standard_Real aMaxF = RealFirst();

  // triangle projection
  Standard_Real aMinTr = RealLast();
  Standard_Real aMaxTr = RealFirst();

  Standard_Real aTriangleProj;

  aTriangleProj = DOTp (theAxis, thePnt1);
  aMinTr = Min (aMinTr, aTriangleProj);
  aMaxTr = Max (aMaxTr, aTriangleProj);

  aTriangleProj = DOTp (theAxis, thePnt2);
  aMinTr = Min (aMinTr, aTriangleProj);
  aMaxTr = Max (aMaxTr, aTriangleProj);

  aTriangleProj = DOTp (theAxis, thePnt3);
  aMinTr = Min (aMinTr, aTriangleProj);
  aMaxTr = Max (aMaxTr, aTriangleProj);

  for (Standard_Integer aVertIter = 0; aVertIter < N * 2; ++aVertIter)
  {
    const Standard_Real aProj = DOT (myVertices[aVertIter], theAxis);

    aMinF = Min (aMinF, aProj);
    aMaxF = Max (aMaxF, aProj);

    if (aMinF <= aMaxTr && aMaxF >= aMinTr)
    {
      return Standard_False;
    }
  }

  return aMinF > aMaxTr || aMaxF < aMinTr;
}

// =======================================================================
// function : hasOverlap
// purpose  : Returns true if selecting volume is overlapped by
//            axis-aligned bounding box with minimum corner at point
//            theMinPnt and maximum at point theMaxPnt
// =======================================================================
template <int N>
Standard_Boolean SelectMgr_Frustum<N>::hasOverlap (const SelectMgr_Vec3& theMinPnt,
                                                   const SelectMgr_Vec3& theMaxPnt,
                                                   Standard_Boolean*     theInside)
{
  for (Standard_Integer anAxis = 0; anAxis < 3; ++anAxis)
  {
    if (theMinPnt[anAxis] > myMaxOrthoVertsProjections[anAxis]
     || theMaxPnt[anAxis] < myMinOrthoVertsProjections[anAxis])
    {
      return Standard_False; // fully separated
    }
    else if (theInside != NULL) // to check for inclusion?
    {
      *theInside &= theMinPnt[anAxis] >= myMinOrthoVertsProjections[anAxis]
                 && theMaxPnt[anAxis] <= myMaxOrthoVertsProjections[anAxis];
    }
  }

  const Standard_Integer anIncFactor = (myIsOrthographic && N == 4) ? 2 : 1;

  for (Standard_Integer aPlaneIdx = 0; aPlaneIdx < N + 1; aPlaneIdx += anIncFactor)
  {
    SelectMgr_Vec3 aPlane = myPlanes[aPlaneIdx];

    const Standard_Real aBoxProjMin =
      aPlane.x() * (aPlane.x() < 0.f ? theMaxPnt.x() : theMinPnt.x()) +
      aPlane.y() * (aPlane.y() < 0.f ? theMaxPnt.y() : theMinPnt.y()) +
      aPlane.z() * (aPlane.z() < 0.f ? theMaxPnt.z() : theMinPnt.z());

    const Standard_Real aBoxProjMax =
      aPlane.x() * (aPlane.x() < 0.f ? theMinPnt.x() : theMaxPnt.x()) +
      aPlane.y() * (aPlane.y() < 0.f ? theMinPnt.y() : theMaxPnt.y()) +
      aPlane.z() * (aPlane.z() < 0.f ? theMinPnt.z() : theMaxPnt.z());

    Standard_ASSERT_RAISE (aBoxProjMax >= aBoxProjMin, "Error! Failed to project box");

    if (aBoxProjMin > myMaxVertsProjections[aPlaneIdx]
     || aBoxProjMax < myMinVertsProjections[aPlaneIdx])
    {
      return Standard_False; // fully separated
    }
    else if (theInside != NULL) // to check for inclusion?
    {
      *theInside &= aBoxProjMin >= myMinVertsProjections[aPlaneIdx]
                 && aBoxProjMax <= myMaxVertsProjections[aPlaneIdx];
    }
  }

  for (Standard_Integer aDim = 0; aDim < 3; ++aDim)
  {
    SelectMgr_Vec3 anEdge1 (aDim == 0, aDim == 1, aDim == 2);

    for (Standard_Integer aVolDir = 0, aDirectionsNb = myIsOrthographic ? 4 : 6; aVolDir < aDirectionsNb; ++aVolDir)
    {
      SelectMgr_Vec3 aDirection (anEdge1.y() * myEdgeDirs[aVolDir].z() - anEdge1.z() * myEdgeDirs[aVolDir].y(),
                                 anEdge1.z() * myEdgeDirs[aVolDir].x() - anEdge1.x() * myEdgeDirs[aVolDir].z(),
                                 anEdge1.x() * myEdgeDirs[aVolDir].y() - anEdge1.y() * myEdgeDirs[aVolDir].x());

      if (isSeparated (theMinPnt, theMaxPnt, aDirection, theInside))
      {
        return Standard_False;
      }
    }
  }

  return Standard_True;
}

// =======================================================================
// function : hasOverlap
// purpose  : SAT intersection test between defined volume and given point
// =======================================================================
template <int N>
Standard_Boolean SelectMgr_Frustum<N>::hasOverlap (const gp_Pnt& thePnt)
{
  const Standard_Integer anIncFactor = (myIsOrthographic && N == 4) ? 2 : 1;

  for (Standard_Integer aPlaneIdx = 0; aPlaneIdx < N + 1; aPlaneIdx += anIncFactor)
  {
    const Select3D_Vec3& aPlane = myPlanes[aPlaneIdx];

    const Standard_Real aPointProj = aPlane.x() * thePnt.X() +
                                     aPlane.y() * thePnt.Y() +
                                     aPlane.z() * thePnt.Z();

    if (aPointProj > myMaxVertsProjections[aPlaneIdx]
     || aPointProj < myMinVertsProjections[aPlaneIdx])
    {
      return Standard_False;
    }
  }

  return Standard_True;
}

// =======================================================================
// function : hasOverlap
// purpose  : SAT intersection test between defined volume and given segment
// =======================================================================
template <int N>
Standard_Boolean SelectMgr_Frustum<N>::hasOverlap (const gp_Pnt& theStartPnt,
                                                   const gp_Pnt& theEndPnt)
{
  const SelectMgr_Vec3& aDir = SelectMgr_Vec3 (theEndPnt.X() - theStartPnt.X(),
                                               theEndPnt.Y() - theStartPnt.Y(),
                                               theEndPnt.Z() - theStartPnt.Z());
  if (std::sqrt (aDir.x() * aDir.x() + aDir.y() * aDir.y() + aDir.z () * aDir.z()) < Precision::Confusion())
    return Standard_True;

  const Standard_Integer anIncFactor = (myIsOrthographic && N == 4) ? 2 : 1;
  for (Standard_Integer aPlaneIdx = 0; aPlaneIdx < N + 1; aPlaneIdx += anIncFactor)
  {
    Standard_Real aMinSegm = RealLast(), aMaxSegm = RealFirst();
    Standard_Real aMinF    = RealLast(), aMaxF    = RealFirst();
    SelectMgr_Vec3 aPlane = myPlanes[aPlaneIdx];

    Standard_Real aProj1 = DOTp (aPlane, theStartPnt);
    Standard_Real aProj2 = DOTp (aPlane, theEndPnt);
    aMinSegm = Min (aProj1, aProj2);
    aMaxSegm = Max (aProj1, aProj2);

    aMaxF = myMaxVertsProjections[aPlaneIdx];
    aMinF = myMinVertsProjections[aPlaneIdx];

    if (aMinSegm > aMaxF
      || aMaxSegm < aMinF)
    {
      return Standard_False;
    }
  }

  Standard_Real aMin1 = DBL_MAX, aMax1 = -DBL_MAX;
  Standard_Real aMin2 = DBL_MAX, aMax2 = -DBL_MAX;
  for (Standard_Integer aVertIdx = 0; aVertIdx < N * 2; ++aVertIdx)
  {
    Standard_Real aProjection = DOT (aDir, myVertices[aVertIdx]);
    aMax2 = Max (aMax2, aProjection);
    aMin2 = Min (aMin2, aProjection);
  }
  Standard_Real aProj1 = DOTp (aDir, theStartPnt);
  Standard_Real aProj2 = DOTp (aDir, theEndPnt);
  aMin1 = Min (aProj1, aProj2);
  aMax1 = Max (aProj1, aProj2);
  if (aMin1 > aMax2
    || aMax1 < aMin2)
  {
    return Standard_False;
  }

  Standard_Integer aDirectionsNb = myIsOrthographic ? 4 : 6;
  for (Standard_Integer aEdgeDirIdx = 0; aEdgeDirIdx < aDirectionsNb; ++aEdgeDirIdx)
  {
    Standard_Real aMinSegm = DBL_MAX, aMaxSegm = -DBL_MAX;
    Standard_Real aMinF    = DBL_MAX, aMaxF    = -DBL_MAX;

    SelectMgr_Vec3 aTestDir = SelectMgr_Vec3 (aDir.y() * myEdgeDirs[aEdgeDirIdx].z() - aDir.z() * myEdgeDirs[aEdgeDirIdx].y(),
      aDir.z() * myEdgeDirs[aEdgeDirIdx].x() - aDir.x() * myEdgeDirs[aEdgeDirIdx].z(),
      aDir.x() * myEdgeDirs[aEdgeDirIdx].y() - aDir.y() * myEdgeDirs[aEdgeDirIdx].x());

    Standard_Real Proj1 = DOTp (aTestDir, theStartPnt);
    Standard_Real Proj2 = DOTp (aTestDir, theEndPnt);
    aMinSegm = Min (Proj1, Proj2);
    aMaxSegm = Max (Proj1, Proj2);

    for (Standard_Integer aVertIdx = 0; aVertIdx < N * 2; ++aVertIdx)
    {
      Standard_Real aProjection = DOT (aTestDir, myVertices[aVertIdx]);
      aMaxF = Max (aMaxF, aProjection);
      aMinF = Min (aMinF, aProjection);
    }

    if (aMinSegm > aMaxF
      || aMaxSegm < aMinF)
    {
      return Standard_False;
    }
  }

  return Standard_True;
}

// =======================================================================
// function : hasOverlap
// purpose  : SAT intersection test between frustum given and planar convex
//            polygon represented as ordered point set
// =======================================================================
template <int N>
Standard_Boolean SelectMgr_Frustum<N>::hasOverlap (const Handle(TColgp_HArray1OfPnt)& theArrayOfPnts,
                                                   SelectMgr_Vec3& theNormal)
{
  Standard_Integer aStartIdx = theArrayOfPnts->Lower();
  Standard_Integer anEndIdx = theArrayOfPnts->Upper();

  const gp_Pnt& aPnt1 = theArrayOfPnts->Value (aStartIdx);
  const gp_Pnt& aPnt2 = theArrayOfPnts->Value (aStartIdx + 1);
  const gp_Pnt& aPnt3 = theArrayOfPnts->Value (aStartIdx + 2);
  const gp_XYZ aVec1 = aPnt1.XYZ() - aPnt2.XYZ();
  const gp_XYZ aVec2 = aPnt3.XYZ() - aPnt2.XYZ();
  theNormal = SelectMgr_Vec3 (aVec2.Y() * aVec1.Z() - aVec2.Z() * aVec1.Y(),
                              aVec2.Z() * aVec1.X() - aVec2.X() * aVec1.Z(),
                              aVec2.X() * aVec1.Y() - aVec2.Y() * aVec1.X());
  Standard_Real aPolygProjection = DOTp (theNormal, aPnt1);

  Standard_Real aMax = RealFirst();
  Standard_Real aMin = RealLast();
  for (Standard_Integer aVertIdx = 0; aVertIdx < N * 2; ++aVertIdx)
  {
    Standard_Real aProjection = DOT (theNormal, myVertices[aVertIdx]);
    aMax = Max (aMax, aProjection);
    aMin = Min (aMin, aProjection);
  }
  if (aPolygProjection > aMax
    || aPolygProjection < aMin)
  {
    return Standard_False;
  }

  Standard_Integer aPlanesNb = N == 4 ? N + 2 : N + 1;
  for (Standard_Integer aPlaneIdx = 0; aPlaneIdx < aPlanesNb; ++aPlaneIdx)
  {
    Standard_Real aMaxF = RealFirst();
    Standard_Real aMinF = RealLast();
    Standard_Real aMaxPolyg = RealFirst();
    Standard_Real aMinPolyg = RealLast();
    SelectMgr_Vec3 aPlane = myPlanes[aPlaneIdx];
    for (Standard_Integer aPntIter = aStartIdx; aPntIter <= anEndIdx; ++aPntIter)
    {
      Standard_Real aProjection = DOTp (aPlane, theArrayOfPnts->Value (aPntIter));
      aMaxPolyg = Max (aMaxPolyg, aProjection);
      aMinPolyg = Min (aMinPolyg, aProjection);
    }
    aMaxF = myMaxVertsProjections[aPlaneIdx];
    aMinF = myMinVertsProjections[aPlaneIdx];
    if (aMinPolyg > aMaxF
      || aMaxPolyg < aMinF)
    {
      return Standard_False;
    }
  }

  Standard_Integer aDirectionsNb = myIsOrthographic ? 4 : 6;
  for (Standard_Integer aPntsIter = aStartIdx; aPntsIter <= anEndIdx; ++aPntsIter)
  {
    const gp_XYZ aSegmDir = aPntsIter == anEndIdx ? theArrayOfPnts->Value (aStartIdx).XYZ() - theArrayOfPnts->Value (anEndIdx).XYZ()
                                             : theArrayOfPnts->Value (aPntsIter + 1).XYZ() - theArrayOfPnts->Value (aPntsIter).XYZ();
    for (Standard_Integer aVolDir = 0; aVolDir < aDirectionsNb; ++aVolDir)
    {
      Standard_Real aMaxPolyg = RealFirst();
      Standard_Real aMinPolyg = RealLast();
      Standard_Real aMaxF = RealFirst();
      Standard_Real aMinF = RealLast();
      SelectMgr_Vec3 aTestDir = SelectMgr_Vec3 (aSegmDir.Y() * myEdgeDirs[aVolDir].z() - aSegmDir.Z() * myEdgeDirs[aVolDir].y(),
                                                aSegmDir.Z() * myEdgeDirs[aVolDir].x() - aSegmDir.X() * myEdgeDirs[aVolDir].z(),
                                                aSegmDir.X() * myEdgeDirs[aVolDir].y() - aSegmDir.Y() * myEdgeDirs[aVolDir].x());

      for (Standard_Integer aPntIter = aStartIdx; aPntIter <= anEndIdx; ++aPntIter)
      {
        Standard_Real aProjection = DOTp (aTestDir, theArrayOfPnts->Value (aPntIter));
        aMaxPolyg = Max (aMaxPolyg, aProjection);
        aMinPolyg = Min (aMinPolyg, aProjection);
      }

      for (Standard_Integer aVertIdx = 0; aVertIdx < N * 2; ++aVertIdx)
      {
        Standard_Real aProjection = DOT (aTestDir, myVertices[aVertIdx]);
        aMaxF = Max (aMaxF, aProjection);
        aMinF = Min (aMinF, aProjection);
      }

      if (aMinPolyg > aMaxF
        || aMaxPolyg < aMinF)
      {
        return Standard_False;
      }
    }
  }

  return Standard_True;
}

// =======================================================================
// function : hasOverlap
// purpose  : SAT intersection test between defined volume and given triangle
// =======================================================================
template <int N>
Standard_Boolean SelectMgr_Frustum<N>::hasOverlap (const gp_Pnt& thePnt1,
                                                   const gp_Pnt& thePnt2,
                                                   const gp_Pnt& thePnt3,
                                                   SelectMgr_Vec3& theNormal)
{

  SelectMgr_Vec3 aPnt1 (thePnt1.X(), thePnt1.Y(), thePnt1.Z());
  SelectMgr_Vec3 aPnt2 (thePnt2.X(), thePnt2.Y(), thePnt2.Z());
  SelectMgr_Vec3 aPnt3 (thePnt3.X(), thePnt3.Y(), thePnt3.Z());
  SelectMgr_Vec3 aTrEdges[3] = { aPnt2 - aPnt1,
                                 aPnt3 - aPnt2,
                                 aPnt1 - aPnt3 };

  const Standard_Integer anIncFactor = (myIsOrthographic && N == 4) ? 2 : 1;
  for (Standard_Integer aPlaneIdx = 0; aPlaneIdx < N + 1; aPlaneIdx += anIncFactor)
  {
    SelectMgr_Vec3 aPlane = myPlanes[aPlaneIdx];
    Standard_Real aTriangleProj;

    aTriangleProj = DOT (aPlane, aPnt1);
    Standard_Real aTriangleProjMin = aTriangleProj;
    Standard_Real aTriangleProjMax = aTriangleProj;

    aTriangleProj = DOT (aPlane, aPnt2);
    aTriangleProjMin = Min (aTriangleProjMin, aTriangleProj);
    aTriangleProjMax = Max (aTriangleProjMax, aTriangleProj);

    aTriangleProj = DOT (aPlane, aPnt3);
    aTriangleProjMin = Min (aTriangleProjMin, aTriangleProj);
    aTriangleProjMax = Max (aTriangleProjMax, aTriangleProj);

    Standard_Real aFrustumProjMax = myMaxVertsProjections[aPlaneIdx];
    Standard_Real aFrustumProjMin = myMinVertsProjections[aPlaneIdx];
    if (aTriangleProjMin > aFrustumProjMax
      || aTriangleProjMax < aFrustumProjMin)
    {
      return Standard_False;
    }
  }

  theNormal = SelectMgr_Vec3 (aTrEdges[2].y() * aTrEdges[0].z() - aTrEdges[2].z() * aTrEdges[0].y(),
                              aTrEdges[2].z() * aTrEdges[0].x() - aTrEdges[2].x() * aTrEdges[0].z(),
                              aTrEdges[2].x() * aTrEdges[0].y() - aTrEdges[2].y() * aTrEdges[0].x());
  if (isSeparated (thePnt1, thePnt2, thePnt3, theNormal))
  {
    return Standard_False;
  }

  Standard_Integer aDirectionsNb = myIsOrthographic ? 4 : 6;
  for (Standard_Integer aTriangleEdgeIdx = 0; aTriangleEdgeIdx < 3; ++aTriangleEdgeIdx)
  {
    for (Standard_Integer aVolDir = 0; aVolDir < aDirectionsNb; ++aVolDir)
    {
      SelectMgr_Vec3 anEdge1 = myEdgeDirs[aVolDir];
      SelectMgr_Vec3 anEdge2 = aTrEdges[aTriangleEdgeIdx];
      SelectMgr_Vec3 aTestDirection = SelectMgr_Vec3 (
        anEdge1.y() * anEdge2.z() - anEdge1.z() * anEdge2.y(),
        anEdge1.z() * anEdge2.x() - anEdge1.x() * anEdge2.z(),
        anEdge1.x() * anEdge2.y() - anEdge1.y() * anEdge2.x());

      if (isSeparated (thePnt1, thePnt2, thePnt3, aTestDirection))
      {
        return Standard_False;
      }
    }
  }

  return Standard_True;
}

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