src/BVH/BVH_DistanceField.lxx

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