[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>
#include <OSD_Parallel.hxx>

// =======================================================================
// 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),
  myIsParallel  (Standard_False)
{
  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 opencascade::handle<BVH_Tree<T, N> >& aBVH = aTriangulation->BVH();
    if (aBVH.IsNull())
    {
      return Standard_False;
    }

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

    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 BVH_Tree<T, N, BVH_BinaryTree>* aBVH = theGeometry.BVH().get();

    if (aBVH == NULL)
    {
      return Standard_False;
    }

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

    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

//! 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 Standard_Integer theIndex) const
  {
    myOutField->BuildSlices (*myGeometry, theIndex, theIndex + 1);
  }
};

// =======================================================================
// 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;

  OSD_Parallel::For (0, myDimensionZ, BVH_ParallelDistanceFieldBuilder<T, N> (this, &theGeometry), !IsParallel());

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