[occt.git] / src / BVH / BVH_DistanceField.lxx

// Created on: 2014-09-06
// Created by: Denis BOGOLEPOV
// Copyright (c) 2013-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 <BVH_Triangulation.hxx>

#ifdef HAVE_TBB
  // On Windows, function TryEnterCriticalSection has appeared in Windows NT
  // and is surrounded by #ifdef in MS VC++ 7.1 headers.
  // Thus to use it we need to define appropriate macro saying that we will
  // run on Windows NT 4.0 at least
  #if defined(_WIN32) && !defined(_WIN32_WINNT)
    #define _WIN32_WINNT 0x0501
  #endif

  #include <tbb/tbb.h>
#endif

// =======================================================================
// function : BVH_DistanceField
// purpose  :
// =======================================================================
template<class T, int N>
BVH_DistanceField<T, N>::BVH_DistanceField (const Standard_Integer theMaximumSize,
                                            const Standard_Boolean theComputeSign)
: myMaximumSize (theMaximumSize),
  myComputeSign (theComputeSign)
{
  Standard_STATIC_ASSERT (N == 3 || N == 4);

  myVoxelData = new T[myMaximumSize * myMaximumSize * myMaximumSize];
}

// =======================================================================
// function : ~BVH_DistanceField
// purpose  :
// =======================================================================
template<class T, int N>
BVH_DistanceField<T, N>::~BVH_DistanceField()
{
  delete [] myVoxelData;
}

#if defined (_WIN32) && defined (max)
  #undef max
#endif

#include <limits>

#define BVH_DOT3(A, B) (A.x() * B.x() + A.y() * B.y() + A.z() * B.z())

namespace BVH
{
  //=======================================================================
  //function : DistanceToBox
  //purpose  : Computes squared distance from point to box
  //=======================================================================
  template<class T, int N>
  T DistanceToBox (const typename VectorType<T, N>::Type& thePnt,
                   const typename VectorType<T, N>::Type& theMin,
                   const typename VectorType<T, N>::Type& theMax)
  {
    Standard_STATIC_ASSERT (N == 3 || N == 4);

    T aNearestX = Min (Max (thePnt.x(), theMin.x()), theMax.x());
    T aNearestY = Min (Max (thePnt.y(), theMin.y()), theMax.y());
    T aNearestZ = Min (Max (thePnt.z(), theMin.z()), theMax.z());

    if (aNearestX == thePnt.x()
     && aNearestY == thePnt.y()
     && aNearestZ == thePnt.z())
    {
      return static_cast<T> (0);
    }

    aNearestX -= thePnt.x();
    aNearestY -= thePnt.y();
    aNearestZ -= thePnt.z();

    return aNearestX * aNearestX +
           aNearestY * aNearestY +
           aNearestZ * aNearestZ;
  }

  //=======================================================================
  //function : DirectionToNearestPoint
  //purpose  : Computes squared distance from point to triangle
  // ======================================================================
  template<class T, int N>
  typename VectorType<T, N>::Type DirectionToNearestPoint (
    const typename VectorType<T, N>::Type& thePoint,
    const typename VectorType<T, N>::Type& theVertA,
    const typename VectorType<T, N>::Type& theVertB,
    const typename VectorType<T, N>::Type& theVertC)
  {
    Standard_STATIC_ASSERT (N == 3 || N == 4);

    const typename VectorType<T, N>::Type aAB = theVertB - theVertA;
    const typename VectorType<T, N>::Type aAC = theVertC - theVertA;
    const typename VectorType<T, N>::Type aAP = thePoint - theVertA;

    const T aABdotAP = BVH_DOT3 (aAB, aAP);
    const T aACdotAP = BVH_DOT3 (aAC, aAP);

    if (aABdotAP <= static_cast<T> (0) && aACdotAP <= static_cast<T> (0))
    {
      return aAP;
    }

    const typename VectorType<T, N>::Type aBC = theVertC - theVertB;
    const typename VectorType<T, N>::Type aBP = thePoint - theVertB;

    const T aBAdotBP = -BVH_DOT3 (aAB, aBP);
    const T aBCdotBP =  BVH_DOT3 (aBC, aBP);

    if (aBAdotBP <= static_cast<T> (0) && aBCdotBP <= static_cast<T> (0))
    {
      return aBP;
    }

    const typename VectorType<T, N>::Type aCP = thePoint - theVertC;

    const T aCBdotCP = -BVH_DOT3 (aBC, aCP);
    const T aCAdotCP = -BVH_DOT3 (aAC, aCP);

    if (aCAdotCP <= static_cast<T> (0) && aCBdotCP <= static_cast<T> (0))
    {
      return aCP;
    }

    const T aACdotBP = BVH_DOT3 (aAC, aBP);

    const T aVC = aABdotAP * aACdotBP + aBAdotBP * aACdotAP;

    if (aVC <= static_cast<T> (0) && aABdotAP >= static_cast<T> (0) && aBAdotBP >= static_cast<T> (0))
    {
      return aAP - aAB * (aABdotAP / (aABdotAP + aBAdotBP));
    }

    const T aABdotCP = BVH_DOT3 (aAB, aCP);

    const T aVA = aBAdotBP * aCAdotCP - aABdotCP * aACdotBP;

    if (aVA <= static_cast<T> (0) && aBCdotBP >= static_cast<T> (0) && aCBdotCP >= static_cast<T> (0))
    {
      return aBP - aBC * (aBCdotBP / (aBCdotBP + aCBdotCP));
    }

    const T aVB = aABdotCP * aACdotAP + aABdotAP * aCAdotCP;

    if (aVB <= static_cast<T> (0) && aACdotAP >= static_cast<T> (0) && aCAdotCP >= static_cast<T> (0))
    {
      return aAP - aAC * (aACdotAP / (aACdotAP + aCAdotCP));
    }

    const T aNorm = static_cast<T> (1.0) / (aVA + aVB + aVC);

    const T aU = aVA * aNorm;
    const T aV = aVB * aNorm;

    return thePoint - (theVertA * aU + theVertB * aV + theVertC * (static_cast<T> (1.0) - aU - aV));
  }

  //=======================================================================
  //function : SquareDistanceToObject
  //purpose  : Computes squared distance from point to BVH triangulation
  //=======================================================================
  template<class T, int N>
  T SquareDistanceToObject (BVH_Object<T, N>* theObject,
    const typename VectorType<T, N>::Type& thePnt, Standard_Boolean& theIsOutside)
  {
    Standard_STATIC_ASSERT (N == 3 || N == 4);

    T aMinDistance = std::numeric_limits<T>::max();

    BVH_Triangulation<T, N>* aTriangulation =
      dynamic_cast<BVH_Triangulation<T, N>*> (theObject);

    Standard_ASSERT_RETURN (aTriangulation != NULL,
      "Error: Unsupported BVH object (non triangulation)", aMinDistance);

    const NCollection_Handle<BVH_Tree<T, N> >& aBVH = aTriangulation->BVH();

    if (aBVH.IsNull())
    {
      return Standard_False;
    }

    std::pair<Standard_Integer, T> aStack[32];

    Standard_Integer aHead = -1;
    Standard_Integer aNode =  0; // root node

    for (;;)
    {
      BVH_Vec4i aData = aBVH->NodeInfoBuffer()[aNode];

      if (aData.x() == 0) // if inner node
      {
        const T aDistToLft = DistanceToBox<T, N> (thePnt,
                                                  aBVH->MinPoint (aData.y()),
                                                  aBVH->MaxPoint (aData.y()));

        const T aDistToRgh = DistanceToBox<T, N> (thePnt,
                                                  aBVH->MinPoint (aData.z()),
                                                  aBVH->MaxPoint (aData.z()));

        const Standard_Boolean aHitLft = aDistToLft <= aMinDistance;
        const Standard_Boolean aHitRgh = aDistToRgh <= aMinDistance;

        if (aHitLft & aHitRgh)
        {
          aNode = (aDistToLft < aDistToRgh) ? aData.y() : aData.z();

          aStack[++aHead] = std::pair<Standard_Integer, T> (
            aDistToLft < aDistToRgh ? aData.z() : aData.y(), Max (aDistToLft, aDistToRgh));
        }
        else
        {
          if (aHitLft | aHitRgh)
          {
            aNode = aHitLft ? aData.y() : aData.z();
          }
          else
          {
            if (aHead < 0)
              return aMinDistance;

            std::pair<Standard_Integer, T>& anInfo = aStack[aHead--];

            while (anInfo.second > aMinDistance)
            {
              if (aHead < 0)
                return aMinDistance;

              anInfo = aStack[aHead--];
            }

            aNode = anInfo.first;
          }
        }
      }
      else  // if leaf node
      {
        for (Standard_Integer aTrgIdx = aData.y(); aTrgIdx <= aData.z(); ++aTrgIdx)
        {
          const BVH_Vec4i aTriangle = aTriangulation->Elements[aTrgIdx];

          const typename VectorType<T, N>::Type aVertex0 = aTriangulation->Vertices[aTriangle.x()];
          const typename VectorType<T, N>::Type aVertex1 = aTriangulation->Vertices[aTriangle.y()];
          const typename VectorType<T, N>::Type aVertex2 = aTriangulation->Vertices[aTriangle.z()];

          const typename VectorType<T, N>::Type aDirection =
            DirectionToNearestPoint<T, N> (thePnt, aVertex0, aVertex1, aVertex2);

          const T aDistance = BVH_DOT3 (aDirection, aDirection);

          if (aDistance < aMinDistance)
          {
            aMinDistance = aDistance;

            typename VectorType<T, N>::Type aTrgEdges[] = { aVertex1 - aVertex0,
                                                            aVertex2 - aVertex0 };

            typename VectorType<T, N>::Type aTrgNormal;
            
            aTrgNormal.x() = aTrgEdges[0].y() * aTrgEdges[1].z() - aTrgEdges[0].z() * aTrgEdges[1].y();
            aTrgNormal.y() = aTrgEdges[0].z() * aTrgEdges[1].x() - aTrgEdges[0].x() * aTrgEdges[1].z();
            aTrgNormal.z() = aTrgEdges[0].x() * aTrgEdges[1].y() - aTrgEdges[0].y() * aTrgEdges[1].x();

            theIsOutside = BVH_DOT3 (aTrgNormal, aDirection) > 0;
          }
        }

        if (aHead < 0)
          return aMinDistance;

        std::pair<Standard_Integer, T>& anInfo = aStack[aHead--];

        while (anInfo.second > aMinDistance)
        {
          if (aHead < 0)
            return aMinDistance;

          anInfo = aStack[aHead--];
        }

        aNode = anInfo.first;
      }
    }
  }

  //=======================================================================
  //function : SquareDistanceToGeomerty
  //purpose  : Computes squared distance from point to BVH geometry
  //=======================================================================
  template<class T, int N>
  T SquareDistanceToGeomerty (BVH_Geometry<T, N>& theGeometry,
    const typename VectorType<T, N>::Type& thePnt, Standard_Boolean& theIsOutside)
  {
    Standard_STATIC_ASSERT (N == 3 || N == 4);

    const NCollection_Handle<BVH_Tree<T, N> >& aBVH = theGeometry.BVH();

    if (aBVH.IsNull())
    {
      return Standard_False;
    }

    std::pair<Standard_Integer, T> aStack[32];

    Standard_Integer aHead = -1;
    Standard_Integer aNode =  0; // root node

    T aMinDistance = std::numeric_limits<T>::max();

    for (;;)
    {
      BVH_Vec4i aData = aBVH->NodeInfoBuffer()[aNode];

      if (aData.x() == 0) // if inner node
      {
        const T aDistToLft = DistanceToBox<T, N> (thePnt,
                                                  aBVH->MinPoint (aData.y()),
                                                  aBVH->MaxPoint (aData.y()));

        const T aDistToRgh = DistanceToBox<T, N> (thePnt,
                                                  aBVH->MinPoint (aData.z()),
                                                  aBVH->MaxPoint (aData.z()));

        const Standard_Boolean aHitLft = aDistToLft <= aMinDistance;
        const Standard_Boolean aHitRgh = aDistToRgh <= aMinDistance;

        if (aHitLft & aHitRgh)
        {
          aNode = (aDistToLft < aDistToRgh) ? aData.y() : aData.z();

          aStack[++aHead] = std::pair<Standard_Integer, T> (
            aDistToLft < aDistToRgh ? aData.z() : aData.y(), Max (aDistToLft, aDistToRgh));
        }
        else
        {
          if (aHitLft | aHitRgh)
          {
            aNode = aHitLft ? aData.y() : aData.z();
          }
          else
          {
            if (aHead < 0)
              return aMinDistance;

            std::pair<Standard_Integer, T>& anInfo = aStack[aHead--];

            while (anInfo.second > aMinDistance)
            {
              if (aHead < 0)
                return aMinDistance;

              anInfo = aStack[aHead--];
            }

            aNode = anInfo.first;
          }
        }
      }
      else  // if leaf node
      {
        Standard_Boolean isOutside = Standard_True;

        const T aDistance = SquareDistanceToObject (
          theGeometry.Objects()(aNode).operator->(), thePnt, isOutside);

        if (aDistance < aMinDistance)
        {
          aMinDistance = aDistance;
          theIsOutside = isOutside;
        }

        if (aHead < 0)
          return aMinDistance;

        std::pair<Standard_Integer, T>& anInfo = aStack[aHead--];

        while (anInfo.second > aMinDistance)
        {
          if (aHead < 0)
            return aMinDistance;

          anInfo = aStack[aHead--];
        }

        aNode = anInfo.first;
      }
    }
  }
}

#undef BVH_DOT3

#ifdef HAVE_TBB

//! Tool object for parallel construction of distance field (uses Intel TBB).
template<class T, int N>
class BVH_ParallelDistanceFieldBuilder
{
private:

  //! Input BVH geometry.
  BVH_Geometry<T, N>* myGeometry;

  //! Output distance field.
  BVH_DistanceField<T, N>* myOutField;

public:

  BVH_ParallelDistanceFieldBuilder (BVH_DistanceField<T, N>* theOutField, BVH_Geometry<T, N>* theGeometry)
  : myGeometry (theGeometry),
    myOutField (theOutField)
  {
    //
  }

  void operator() (const tbb::blocked_range<Standard_Integer>& theRange) const
  {
    myOutField->BuildSlices (*myGeometry, theRange.begin(), theRange.end());
  }
};

#endif

// =======================================================================
// function : BuildSlices
// purpose  : Performs building of distance field for the given Z slices
// =======================================================================
template<class T, int N>
void BVH_DistanceField<T, N>::BuildSlices (BVH_Geometry<T, N>& theGeometry,
  const Standard_Integer theStartSlice, const Standard_Integer theFinalSlice)
{
  for (Standard_Integer aZ = theStartSlice; aZ < theFinalSlice; ++aZ)
  {
    for (Standard_Integer aY = 0; aY < myDimensionY; ++aY)
    {
      for (Standard_Integer aX = 0; aX < myDimensionX; ++aX)
      {
        BVH_VecNt aCenter;
        
        aCenter.x() = myCornerMin.x() + myVoxelSize.x() * (aX + static_cast<T> (0.5));
        aCenter.y() = myCornerMin.y() + myVoxelSize.y() * (aY + static_cast<T> (0.5));
        aCenter.z() = myCornerMin.z() + myVoxelSize.z() * (aZ + static_cast<T> (0.5));

        Standard_Boolean isOutside = Standard_True;

        const T aDistance = sqrt (
          BVH::SquareDistanceToGeomerty<T, N> (theGeometry, aCenter, isOutside));

        Voxel (aX, aY, aZ) = (!myComputeSign || isOutside) ? aDistance : -aDistance;
      }
    }
  }
}

// =======================================================================
// function : Build
// purpose  : Builds 3D distance field from BVH geometry
// =======================================================================
template<class T, int N>
Standard_Boolean BVH_DistanceField<T, N>::Build (BVH_Geometry<T, N>& theGeometry)
{
  if (theGeometry.Size() == 0)
  {
    return Standard_False;
  }

  const BVH_VecNt aGlobalBoxSize = theGeometry.Box().Size();

  const T aMaxBoxSide = Max (Max (aGlobalBoxSize.x(), aGlobalBoxSize.y()), aGlobalBoxSize.z());

  myDimensionX = static_cast<Standard_Integer> (myMaximumSize * aGlobalBoxSize.x() / aMaxBoxSide);
  myDimensionY = static_cast<Standard_Integer> (myMaximumSize * aGlobalBoxSize.y() / aMaxBoxSide);
  myDimensionZ = static_cast<Standard_Integer> (myMaximumSize * aGlobalBoxSize.z() / aMaxBoxSide);

  myDimensionX = Min (myMaximumSize, Max (myDimensionX, 16));
  myDimensionY = Min (myMaximumSize, Max (myDimensionY, 16));
  myDimensionZ = Min (myMaximumSize, Max (myDimensionZ, 16));

  const BVH_VecNt aGlobalBoxMin = theGeometry.Box().CornerMin();
  const BVH_VecNt aGlobalBoxMax = theGeometry.Box().CornerMax();

  const Standard_Integer aVoxelOffset = 2;

  myCornerMin.x() = aGlobalBoxMin.x() - aVoxelOffset * aGlobalBoxSize.x() / (myDimensionX - 2 * aVoxelOffset);
  myCornerMin.y() = aGlobalBoxMin.y() - aVoxelOffset * aGlobalBoxSize.y() / (myDimensionY - 2 * aVoxelOffset);
  myCornerMin.z() = aGlobalBoxMin.z() - aVoxelOffset * aGlobalBoxSize.z() / (myDimensionZ - 2 * aVoxelOffset);

  myCornerMax.x() = aGlobalBoxMax.x() + aVoxelOffset * aGlobalBoxSize.x() / (myDimensionX - 2 * aVoxelOffset);
  myCornerMax.y() = aGlobalBoxMax.y() + aVoxelOffset * aGlobalBoxSize.y() / (myDimensionY - 2 * aVoxelOffset);
  myCornerMax.z() = aGlobalBoxMax.z() + aVoxelOffset * aGlobalBoxSize.z() / (myDimensionZ - 2 * aVoxelOffset);
  
  myVoxelSize.x() = (myCornerMax.x() - myCornerMin.x()) / myDimensionX;
  myVoxelSize.y() = (myCornerMax.y() - myCornerMin.y()) / myDimensionY;
  myVoxelSize.z() = (myCornerMax.z() - myCornerMin.z()) / myDimensionZ;

#ifdef HAVE_TBB
  
  tbb::parallel_for (tbb::blocked_range<Standard_Integer> (0, myDimensionZ),
    BVH_ParallelDistanceFieldBuilder<T, N> (this, &theGeometry));

#else

  BuildSlices (theGeometry, 0, myDimensionZ);

#endif

  return Standard_True;
}
Commit	Line	Data
df932fdf	1	// Created on: 2014-09-06
	2	// Created by: Denis BOGOLEPOV
	3	// Copyright (c) 2013-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
df932fdf	16	#include <BVH_Triangulation.hxx>
	17
	18	#ifdef HAVE_TBB
	19	// On Windows, function TryEnterCriticalSection has appeared in Windows NT
	20	// and is surrounded by #ifdef in MS VC++ 7.1 headers.
	21	// Thus to use it we need to define appropriate macro saying that we will
	22	// run on Windows NT 4.0 at least
	23	#if defined(_WIN32) && !defined(_WIN32_WINNT)
	24	#define _WIN32_WINNT 0x0501
	25	#endif
	26
	27	#include <tbb/tbb.h>
	28	#endif
	29
	30	// =======================================================================
	31	// function : BVH_DistanceField
	32	// purpose :
	33	// =======================================================================
	34	template<class T, int N>
	35	BVH_DistanceField<T, N>::BVH_DistanceField (const Standard_Integer theMaximumSize,
	36	const Standard_Boolean theComputeSign)
	37	: myMaximumSize (theMaximumSize),
	38	myComputeSign (theComputeSign)
	39	{
	40	Standard_STATIC_ASSERT (N == 3 \|\| N == 4);
	41
	42	myVoxelData = new T[myMaximumSize * myMaximumSize * myMaximumSize];
	43	}
	44
	45	// =======================================================================
	46	// function : ~BVH_DistanceField
	47	// purpose :
	48	// =======================================================================
	49	template<class T, int N>
	50	BVH_DistanceField<T, N>::~BVH_DistanceField()
	51	{
	52	delete [] myVoxelData;
	53	}
	54
	55	#if defined (_WIN32) && defined (max)
	56	#undef max
	57	#endif
	58
	59	#include <limits>
	60
	61	#define BVH_DOT3(A, B) (A.x() * B.x() + A.y() * B.y() + A.z() * B.z())
	62
	63	namespace BVH
	64	{
	65	//=======================================================================
	66	//function : DistanceToBox
	67	//purpose : Computes squared distance from point to box
	68	//=======================================================================
	69	template<class T, int N>
	70	T DistanceToBox (const typename VectorType<T, N>::Type& thePnt,
	71	const typename VectorType<T, N>::Type& theMin,
	72	const typename VectorType<T, N>::Type& theMax)
	73	{
	74	Standard_STATIC_ASSERT (N == 3 \|\| N == 4);
	75
	76	T aNearestX = Min (Max (thePnt.x(), theMin.x()), theMax.x());
	77	T aNearestY = Min (Max (thePnt.y(), theMin.y()), theMax.y());
	78	T aNearestZ = Min (Max (thePnt.z(), theMin.z()), theMax.z());
	79
80	if (aNearestX == thePnt.x()
81	&& aNearestY == thePnt.y()
82	&& aNearestZ == thePnt.z())
83	{
84	return static_cast<T> (0);
85	}
86
87	aNearestX -= thePnt.x();
88	aNearestY -= thePnt.y();
89	aNearestZ -= thePnt.z();
90
91	return aNearestX * aNearestX +
92	aNearestY * aNearestY +
93	aNearestZ * aNearestZ;
94	}
95
96	//=======================================================================
97	//function : DirectionToNearestPoint
98	//purpose : Computes squared distance from point to triangle
99	// ======================================================================
100	template<class T, int N>
101	typename VectorType<T, N>::Type DirectionToNearestPoint (
102	const typename VectorType<T, N>::Type& thePoint,
103	const typename VectorType<T, N>::Type& theVertA,
104	const typename VectorType<T, N>::Type& theVertB,
105	const typename VectorType<T, N>::Type& theVertC)
106	{
107	Standard_STATIC_ASSERT (N == 3 \|\| N == 4);
108
109	const typename VectorType<T, N>::Type aAB = theVertB - theVertA;
110	const typename VectorType<T, N>::Type aAC = theVertC - theVertA;
111	const typename VectorType<T, N>::Type aAP = thePoint - theVertA;
112
113	const T aABdotAP = BVH_DOT3 (aAB, aAP);
114	const T aACdotAP = BVH_DOT3 (aAC, aAP);
115
116	if (aABdotAP <= static_cast<T> (0) && aACdotAP <= static_cast<T> (0))
117	{
118	return aAP;
119	}
120
121	const typename VectorType<T, N>::Type aBC = theVertC - theVertB;
122	const typename VectorType<T, N>::Type aBP = thePoint - theVertB;
123
124	const T aBAdotBP = -BVH_DOT3 (aAB, aBP);
125	const T aBCdotBP = BVH_DOT3 (aBC, aBP);
126
127	if (aBAdotBP <= static_cast<T> (0) && aBCdotBP <= static_cast<T> (0))
128	{
129	return aBP;
130	}
131
132	const typename VectorType<T, N>::Type aCP = thePoint - theVertC;
133
134	const T aCBdotCP = -BVH_DOT3 (aBC, aCP);
135	const T aCAdotCP = -BVH_DOT3 (aAC, aCP);
136
137	if (aCAdotCP <= static_cast<T> (0) && aCBdotCP <= static_cast<T> (0))
138	{
139	return aCP;
140	}
141
142	const T aACdotBP = BVH_DOT3 (aAC, aBP);
143
144	const T aVC = aABdotAP * aACdotBP + aBAdotBP * aACdotAP;
145
146	if (aVC <= static_cast<T> (0) && aABdotAP >= static_cast<T> (0) && aBAdotBP >= static_cast<T> (0))
147	{
148	return aAP - aAB * (aABdotAP / (aABdotAP + aBAdotBP));
149	}
150
151	const T aABdotCP = BVH_DOT3 (aAB, aCP);
152
153	const T aVA = aBAdotBP * aCAdotCP - aABdotCP * aACdotBP;
154
155	if (aVA <= static_cast<T> (0) && aBCdotBP >= static_cast<T> (0) && aCBdotCP >= static_cast<T> (0))
156	{
157	return aBP - aBC * (aBCdotBP / (aBCdotBP + aCBdotCP));
158	}
159
160	const T aVB = aABdotCP * aACdotAP + aABdotAP * aCAdotCP;
161
162	if (aVB <= static_cast<T> (0) && aACdotAP >= static_cast<T> (0) && aCAdotCP >= static_cast<T> (0))
163	{
164	return aAP - aAC * (aACdotAP / (aACdotAP + aCAdotCP));
165	}
166
167	const T aNorm = static_cast<T> (1.0) / (aVA + aVB + aVC);
168
169	const T aU = aVA * aNorm;
170	const T aV = aVB * aNorm;
171
172	return thePoint - (theVertA * aU + theVertB * aV + theVertC * (static_cast<T> (1.0) - aU - aV));
173	}
174
175	//=======================================================================
176	//function : SquareDistanceToObject
177	//purpose : Computes squared distance from point to BVH triangulation
178	//=======================================================================
179	template<class T, int N>
180	T SquareDistanceToObject (BVH_Object<T, N>* theObject,
181	const typename VectorType<T, N>::Type& thePnt, Standard_Boolean& theIsOutside)
182	{
183	Standard_STATIC_ASSERT (N == 3 \|\| N == 4);
184
185	T aMinDistance = std::numeric_limits<T>::max();
186
187	BVH_Triangulation<T, N>* aTriangulation =
188	dynamic_cast<BVH_Triangulation<T, N>*> (theObject);
189
190	Standard_ASSERT_RETURN (aTriangulation != NULL,
191	"Error: Unsupported BVH object (non triangulation)", aMinDistance);
192
193	const NCollection_Handle<BVH_Tree<T, N> >& aBVH = aTriangulation->BVH();
194
195	if (aBVH.IsNull())
196	{
197	return Standard_False;
198	}
199
200	std::pair<Standard_Integer, T> aStack[32];
201
202	Standard_Integer aHead = -1;
203	Standard_Integer aNode = 0; // root node
204
205	for (;;)
206	{
207	BVH_Vec4i aData = aBVH->NodeInfoBuffer()[aNode];
208
209	if (aData.x() == 0) // if inner node
210	{
211	const T aDistToLft = DistanceToBox<T, N> (thePnt,
212	aBVH->MinPoint (aData.y()),
213	aBVH->MaxPoint (aData.y()));
214
215	const T aDistToRgh = DistanceToBox<T, N> (thePnt,
216	aBVH->MinPoint (aData.z()),
217	aBVH->MaxPoint (aData.z()));
218
219	const Standard_Boolean aHitLft = aDistToLft <= aMinDistance;
220	const Standard_Boolean aHitRgh = aDistToRgh <= aMinDistance;
221
222	if (aHitLft & aHitRgh)
223	{
224	aNode = (aDistToLft < aDistToRgh) ? aData.y() : aData.z();
225
226	aStack[++aHead] = std::pair<Standard_Integer, T> (
227	aDistToLft < aDistToRgh ? aData.z() : aData.y(), Max (aDistToLft, aDistToRgh));
228	}
229	else
230	{
231	if (aHitLft \| aHitRgh)
232	{
233	aNode = aHitLft ? aData.y() : aData.z();
234	}
235	else
236	{
237	if (aHead < 0)
238	return aMinDistance;
239
240	std::pair<Standard_Integer, T>& anInfo = aStack[aHead--];
241
242	while (anInfo.second > aMinDistance)
243	{
244	if (aHead < 0)
245	return aMinDistance;
246
247	anInfo = aStack[aHead--];
248	}
249
250	aNode = anInfo.first;
251	}
252	}
253	}
254	else // if leaf node
255	{
256	for (Standard_Integer aTrgIdx = aData.y(); aTrgIdx <= aData.z(); ++aTrgIdx)
257	{
258	const BVH_Vec4i aTriangle = aTriangulation->Elements[aTrgIdx];
259
260	const typename VectorType<T, N>::Type aVertex0 = aTriangulation->Vertices[aTriangle.x()];
261	const typename VectorType<T, N>::Type aVertex1 = aTriangulation->Vertices[aTriangle.y()];
262	const typename VectorType<T, N>::Type aVertex2 = aTriangulation->Vertices[aTriangle.z()];
263
264	const typename VectorType<T, N>::Type aDirection =
265	DirectionToNearestPoint<T, N> (thePnt, aVertex0, aVertex1, aVertex2);
266
267	const T aDistance = BVH_DOT3 (aDirection, aDirection);
268
269	if (aDistance < aMinDistance)
270	{
271	aMinDistance = aDistance;
272
273	typename VectorType<T, N>::Type aTrgEdges[] = { aVertex1 - aVertex0,
274	aVertex2 - aVertex0 };
275
276	typename VectorType<T, N>::Type aTrgNormal;
277
278	aTrgNormal.x() = aTrgEdges[0].y() * aTrgEdges[1].z() - aTrgEdges[0].z() * aTrgEdges[1].y();
279	aTrgNormal.y() = aTrgEdges[0].z() * aTrgEdges[1].x() - aTrgEdges[0].x() * aTrgEdges[1].z();
280	aTrgNormal.z() = aTrgEdges[0].x() * aTrgEdges[1].y() - aTrgEdges[0].y() * aTrgEdges[1].x();
281
282	theIsOutside = BVH_DOT3 (aTrgNormal, aDirection) > 0;
283	}
284	}
285
286	if (aHead < 0)
287	return aMinDistance;
288
289	std::pair<Standard_Integer, T>& anInfo = aStack[aHead--];
290
291	while (anInfo.second > aMinDistance)
292	{
293	if (aHead < 0)
294	return aMinDistance;
295
296	anInfo = aStack[aHead--];
297	}
298
299	aNode = anInfo.first;
300	}
301	}
302	}
303
304	//=======================================================================
305	//function : SquareDistanceToGeomerty
306	//purpose : Computes squared distance from point to BVH geometry
307	//=======================================================================
308	template<class T, int N>
309	T SquareDistanceToGeomerty (BVH_Geometry<T, N>& theGeometry,
310	const typename VectorType<T, N>::Type& thePnt, Standard_Boolean& theIsOutside)
311	{
312	Standard_STATIC_ASSERT (N == 3 \|\| N == 4);
313
314	const NCollection_Handle<BVH_Tree<T, N> >& aBVH = theGeometry.BVH();
315
316	if (aBVH.IsNull())
317	{
318	return Standard_False;
319	}
320
321	std::pair<Standard_Integer, T> aStack[32];
322
323	Standard_Integer aHead = -1;
324	Standard_Integer aNode = 0; // root node
325
326	T aMinDistance = std::numeric_limits<T>::max();
327
328	for (;;)
329	{
330	BVH_Vec4i aData = aBVH->NodeInfoBuffer()[aNode];
331
332	if (aData.x() == 0) // if inner node
333	{
334	const T aDistToLft = DistanceToBox<T, N> (thePnt,
335	aBVH->MinPoint (aData.y()),
336	aBVH->MaxPoint (aData.y()));
337
338	const T aDistToRgh = DistanceToBox<T, N> (thePnt,
339	aBVH->MinPoint (aData.z()),
340	aBVH->MaxPoint (aData.z()));
341
342	const Standard_Boolean aHitLft = aDistToLft <= aMinDistance;
343	const Standard_Boolean aHitRgh = aDistToRgh <= aMinDistance;
344
345	if (aHitLft & aHitRgh)
346	{
347	aNode = (aDistToLft < aDistToRgh) ? aData.y() : aData.z();
348
349	aStack[++aHead] = std::pair<Standard_Integer, T> (
350	aDistToLft < aDistToRgh ? aData.z() : aData.y(), Max (aDistToLft, aDistToRgh));
351	}
352	else
353	{
354	if (aHitLft \| aHitRgh)
355	{
356	aNode = aHitLft ? aData.y() : aData.z();
357	}
358	else
359	{
360	if (aHead < 0)
361	return aMinDistance;
362
363	std::pair<Standard_Integer, T>& anInfo = aStack[aHead--];
364
365	while (anInfo.second > aMinDistance)
366	{
367	if (aHead < 0)
368	return aMinDistance;
369
370	anInfo = aStack[aHead--];
371	}
372
373	aNode = anInfo.first;
374	}
375	}
376	}
377	else // if leaf node
378	{
379	Standard_Boolean isOutside = Standard_True;
380
381	const T aDistance = SquareDistanceToObject (
382	theGeometry.Objects()(aNode).operator->(), thePnt, isOutside);
383
384	if (aDistance < aMinDistance)
385	{
386	aMinDistance = aDistance;
387	theIsOutside = isOutside;
388	}
389
390	if (aHead < 0)
391	return aMinDistance;
392
393	std::pair<Standard_Integer, T>& anInfo = aStack[aHead--];
394
395	while (anInfo.second > aMinDistance)
396	{
397	if (aHead < 0)
398	return aMinDistance;
399
400	anInfo = aStack[aHead--];
401	}
402
403	aNode = anInfo.first;
404	}
405	}
406	}
407	}
408
409	#undef BVH_DOT3
410
411	#ifdef HAVE_TBB
412
413	//! Tool object for parallel construction of distance field (uses Intel TBB).
414	template<class T, int N>
415	class BVH_ParallelDistanceFieldBuilder
416	{
417	private:
418
419	//! Input BVH geometry.
420	BVH_Geometry<T, N>* myGeometry;
421
422	//! Output distance field.
423	BVH_DistanceField<T, N>* myOutField;
424
425	public:
426
427	BVH_ParallelDistanceFieldBuilder (BVH_DistanceField<T, N>* theOutField, BVH_Geometry<T, N>* theGeometry)
428	: myGeometry (theGeometry),
429	myOutField (theOutField)
430	{
431	//
432	}
433
434	void operator() (const tbb::blocked_range<Standard_Integer>& theRange) const
435	{
436	myOutField->BuildSlices (*myGeometry, theRange.begin(), theRange.end());
437	}
438	};
439
440	#endif
441
442	// =======================================================================
443	// function : BuildSlices
444	// purpose : Performs building of distance field for the given Z slices
445	// =======================================================================
446	template<class T, int N>
447	void BVH_DistanceField<T, N>::BuildSlices (BVH_Geometry<T, N>& theGeometry,
448	const Standard_Integer theStartSlice, const Standard_Integer theFinalSlice)
449	{
450	for (Standard_Integer aZ = theStartSlice; aZ < theFinalSlice; ++aZ)
451	{
452	for (Standard_Integer aY = 0; aY < myDimensionY; ++aY)
453	{
454	for (Standard_Integer aX = 0; aX < myDimensionX; ++aX)
455	{
456	BVH_VecNt aCenter;
457
458	aCenter.x() = myCornerMin.x() + myVoxelSize.x() * (aX + static_cast<T> (0.5));
459	aCenter.y() = myCornerMin.y() + myVoxelSize.y() * (aY + static_cast<T> (0.5));
460	aCenter.z() = myCornerMin.z() + myVoxelSize.z() * (aZ + static_cast<T> (0.5));
461
462	Standard_Boolean isOutside = Standard_True;
463
464	const T aDistance = sqrt (
465	BVH::SquareDistanceToGeomerty<T, N> (theGeometry, aCenter, isOutside));
466
467	Voxel (aX, aY, aZ) = (!myComputeSign \|\| isOutside) ? aDistance : -aDistance;
468	}
469	}
470	}
471	}
472
473	// =======================================================================
474	// function : Build
475	// purpose : Builds 3D distance field from BVH geometry
476	// =======================================================================
477	template<class T, int N>
478	Standard_Boolean BVH_DistanceField<T, N>::Build (BVH_Geometry<T, N>& theGeometry)
479	{
480	if (theGeometry.Size() == 0)
481	{
482	return Standard_False;
483	}
484
485	const BVH_VecNt aGlobalBoxSize = theGeometry.Box().Size();
486
487	const T aMaxBoxSide = Max (Max (aGlobalBoxSize.x(), aGlobalBoxSize.y()), aGlobalBoxSize.z());
488
489	myDimensionX = static_cast<Standard_Integer> (myMaximumSize * aGlobalBoxSize.x() / aMaxBoxSide);
490	myDimensionY = static_cast<Standard_Integer> (myMaximumSize * aGlobalBoxSize.y() / aMaxBoxSide);
491	myDimensionZ = static_cast<Standard_Integer> (myMaximumSize * aGlobalBoxSize.z() / aMaxBoxSide);
492
493	myDimensionX = Min (myMaximumSize, Max (myDimensionX, 16));
494	myDimensionY = Min (myMaximumSize, Max (myDimensionY, 16));
495	myDimensionZ = Min (myMaximumSize, Max (myDimensionZ, 16));
496
497	const BVH_VecNt aGlobalBoxMin = theGeometry.Box().CornerMin();
498	const BVH_VecNt aGlobalBoxMax = theGeometry.Box().CornerMax();
499
500	const Standard_Integer aVoxelOffset = 2;
501
502	myCornerMin.x() = aGlobalBoxMin.x() - aVoxelOffset * aGlobalBoxSize.x() / (myDimensionX - 2 * aVoxelOffset);
503	myCornerMin.y() = aGlobalBoxMin.y() - aVoxelOffset * aGlobalBoxSize.y() / (myDimensionY - 2 * aVoxelOffset);
504	myCornerMin.z() = aGlobalBoxMin.z() - aVoxelOffset * aGlobalBoxSize.z() / (myDimensionZ - 2 * aVoxelOffset);
505
506	myCornerMax.x() = aGlobalBoxMax.x() + aVoxelOffset * aGlobalBoxSize.x() / (myDimensionX - 2 * aVoxelOffset);
507	myCornerMax.y() = aGlobalBoxMax.y() + aVoxelOffset * aGlobalBoxSize.y() / (myDimensionY - 2 * aVoxelOffset);
508	myCornerMax.z() = aGlobalBoxMax.z() + aVoxelOffset * aGlobalBoxSize.z() / (myDimensionZ - 2 * aVoxelOffset);
509
510	myVoxelSize.x() = (myCornerMax.x() - myCornerMin.x()) / myDimensionX;
511	myVoxelSize.y() = (myCornerMax.y() - myCornerMin.y()) / myDimensionY;
512	myVoxelSize.z() = (myCornerMax.z() - myCornerMin.z()) / myDimensionZ;
513
514	#ifdef HAVE_TBB
515
516	tbb::parallel_for (tbb::blocked_range<Standard_Integer> (0, myDimensionZ),
517	BVH_ParallelDistanceFieldBuilder<T, N> (this, &theGeometry));
518
519	#else
520
521	BuildSlices (theGeometry, 0, myDimensionZ);
522
523	#endif
524
525	return Standard_True;
526	}