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 <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 }