356 lines
12 KiB
C++
356 lines
12 KiB
C++
// Created by: Eugeny MALTCHIKOV
|
|
// Created on: 2019-04-17
|
|
// Copyright (c) 2019 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.
|
|
|
|
#ifndef _BVH_Tools_Header
|
|
#define _BVH_Tools_Header
|
|
|
|
#include "BVH_Box.hxx"
|
|
#include "BVH_Ray.hxx"
|
|
#include "BVH_Types.hxx"
|
|
|
|
//! Defines a set of static methods operating with points and bounding boxes.
|
|
//! \tparam T Numeric data type
|
|
//! \tparam N Vector dimension
|
|
template <class T, int N>
|
|
class BVH_Tools
|
|
{
|
|
public: //! @name public types
|
|
typedef typename BVH::VectorType<T, N>::Type BVH_VecNt;
|
|
|
|
public:
|
|
enum BVH_PrjStateInTriangle
|
|
{
|
|
BVH_PrjStateInTriangle_VERTEX,
|
|
BVH_PrjStateInTriangle_EDGE,
|
|
BVH_PrjStateInTriangle_INNER
|
|
};
|
|
|
|
public: //! @name Box-Box Square distance
|
|
//! Computes Square distance between Axis aligned bounding boxes
|
|
static T BoxBoxSquareDistance(const BVH_Box<T, N>& theBox1, const BVH_Box<T, N>& theBox2)
|
|
{
|
|
if (!theBox1.IsValid() || !theBox2.IsValid())
|
|
{
|
|
return static_cast<T>(0);
|
|
}
|
|
return BoxBoxSquareDistance(theBox1.CornerMin(),
|
|
theBox1.CornerMax(),
|
|
theBox2.CornerMin(),
|
|
theBox2.CornerMax());
|
|
}
|
|
|
|
//! Computes Square distance between Axis aligned bounding boxes
|
|
static T BoxBoxSquareDistance(const BVH_VecNt& theCMin1,
|
|
const BVH_VecNt& theCMax1,
|
|
const BVH_VecNt& theCMin2,
|
|
const BVH_VecNt& theCMax2)
|
|
{
|
|
T aDist = 0;
|
|
for (int i = 0; i < N; ++i)
|
|
{
|
|
if (theCMin1[i] > theCMax2[i])
|
|
{
|
|
T d = theCMin1[i] - theCMax2[i];
|
|
d *= d;
|
|
aDist += d;
|
|
}
|
|
else if (theCMax1[i] < theCMin2[i])
|
|
{
|
|
T d = theCMin2[i] - theCMax1[i];
|
|
d *= d;
|
|
aDist += d;
|
|
}
|
|
}
|
|
return aDist;
|
|
}
|
|
|
|
public: //! @name Point-Box Square distance
|
|
//! Computes square distance between point and bounding box
|
|
static T PointBoxSquareDistance(const BVH_VecNt& thePoint, const BVH_Box<T, N>& theBox)
|
|
{
|
|
if (!theBox.IsValid())
|
|
{
|
|
return static_cast<T>(0);
|
|
}
|
|
return PointBoxSquareDistance(thePoint, theBox.CornerMin(), theBox.CornerMax());
|
|
}
|
|
|
|
//! Computes square distance between point and bounding box
|
|
static T PointBoxSquareDistance(const BVH_VecNt& thePoint,
|
|
const BVH_VecNt& theCMin,
|
|
const BVH_VecNt& theCMax)
|
|
{
|
|
T aDist = 0;
|
|
for (int i = 0; i < N; ++i)
|
|
{
|
|
if (thePoint[i] < theCMin[i])
|
|
{
|
|
T d = theCMin[i] - thePoint[i];
|
|
d *= d;
|
|
aDist += d;
|
|
}
|
|
else if (thePoint[i] > theCMax[i])
|
|
{
|
|
T d = thePoint[i] - theCMax[i];
|
|
d *= d;
|
|
aDist += d;
|
|
}
|
|
}
|
|
return aDist;
|
|
}
|
|
|
|
public: //! @name Point-Box projection
|
|
//! Computes projection of point on bounding box
|
|
static BVH_VecNt PointBoxProjection(const BVH_VecNt& thePoint, const BVH_Box<T, N>& theBox)
|
|
{
|
|
if (!theBox.IsValid())
|
|
{
|
|
return thePoint;
|
|
}
|
|
return PointBoxProjection(thePoint, theBox.CornerMin(), theBox.CornerMax());
|
|
}
|
|
|
|
//! Computes projection of point on bounding box
|
|
static BVH_VecNt PointBoxProjection(const BVH_VecNt& thePoint,
|
|
const BVH_VecNt& theCMin,
|
|
const BVH_VecNt& theCMax)
|
|
{
|
|
return thePoint.cwiseMax(theCMin).cwiseMin(theCMax);
|
|
}
|
|
|
|
public: //! @name Point-Triangle Square distance
|
|
//! Find nearest point on a triangle for the given point
|
|
static BVH_VecNt PointTriangleProjection(const BVH_VecNt& thePoint,
|
|
const BVH_VecNt& theNode0,
|
|
const BVH_VecNt& theNode1,
|
|
const BVH_VecNt& theNode2,
|
|
BVH_PrjStateInTriangle* thePrjState = nullptr,
|
|
Standard_Integer* theNumberOfFirstNode = nullptr,
|
|
Standard_Integer* theNumberOfLastNode = nullptr)
|
|
{
|
|
const BVH_VecNt aAB = theNode1 - theNode0;
|
|
const BVH_VecNt aAC = theNode2 - theNode0;
|
|
const BVH_VecNt aAP = thePoint - theNode0;
|
|
|
|
T aABdotAP = aAB.Dot(aAP);
|
|
T aACdotAP = aAC.Dot(aAP);
|
|
|
|
if (aABdotAP <= 0. && aACdotAP <= 0.)
|
|
{
|
|
if (thePrjState != nullptr)
|
|
{
|
|
*thePrjState = BVH_PrjStateInTriangle_VERTEX;
|
|
*theNumberOfFirstNode = 0;
|
|
*theNumberOfLastNode = 0;
|
|
}
|
|
return theNode0;
|
|
}
|
|
|
|
const BVH_VecNt aBC = theNode2 - theNode1;
|
|
const BVH_VecNt aBP = thePoint - theNode1;
|
|
|
|
T aBAdotBP = -(aAB.Dot(aBP));
|
|
T aBCdotBP = (aBC.Dot(aBP));
|
|
|
|
if (aBAdotBP <= 0. && aBCdotBP <= 0.)
|
|
{
|
|
if (thePrjState != nullptr)
|
|
{
|
|
*thePrjState = BVH_PrjStateInTriangle_VERTEX;
|
|
*theNumberOfFirstNode = 1;
|
|
*theNumberOfLastNode = 1;
|
|
}
|
|
return theNode1;
|
|
}
|
|
|
|
const BVH_VecNt aCP = thePoint - theNode2;
|
|
|
|
T aCBdotCP = -(aBC.Dot(aCP));
|
|
T aCAdotCP = -(aAC.Dot(aCP));
|
|
|
|
if (aCAdotCP <= 0. && aCBdotCP <= 0.)
|
|
{
|
|
if (thePrjState != nullptr)
|
|
{
|
|
*thePrjState = BVH_PrjStateInTriangle_VERTEX;
|
|
*theNumberOfFirstNode = 2;
|
|
*theNumberOfLastNode = 2;
|
|
}
|
|
return theNode2;
|
|
}
|
|
|
|
T aACdotBP = (aAC.Dot(aBP));
|
|
|
|
T aVC = aABdotAP * aACdotBP + aBAdotBP * aACdotAP;
|
|
|
|
if (aVC <= 0. && aABdotAP > 0. && aBAdotBP > 0.)
|
|
{
|
|
if (thePrjState != nullptr)
|
|
{
|
|
*thePrjState = BVH_PrjStateInTriangle_EDGE;
|
|
*theNumberOfFirstNode = 0;
|
|
*theNumberOfLastNode = 1;
|
|
}
|
|
return theNode0 + aAB * (aABdotAP / (aABdotAP + aBAdotBP));
|
|
}
|
|
|
|
T aABdotCP = (aAB.Dot(aCP));
|
|
|
|
T aVA = aBAdotBP * aCAdotCP - aABdotCP * aACdotBP;
|
|
|
|
if (aVA <= 0. && aBCdotBP > 0. && aCBdotCP > 0.)
|
|
{
|
|
if (thePrjState != nullptr)
|
|
{
|
|
*thePrjState = BVH_PrjStateInTriangle_EDGE;
|
|
*theNumberOfFirstNode = 1;
|
|
*theNumberOfLastNode = 2;
|
|
}
|
|
return theNode1 + aBC * (aBCdotBP / (aBCdotBP + aCBdotCP));
|
|
}
|
|
|
|
T aVB = aABdotCP * aACdotAP + aABdotAP * aCAdotCP;
|
|
|
|
if (aVB <= 0. && aACdotAP > 0. && aCAdotCP > 0.)
|
|
{
|
|
if (thePrjState != nullptr)
|
|
{
|
|
*thePrjState = BVH_PrjStateInTriangle_EDGE;
|
|
*theNumberOfFirstNode = 2;
|
|
*theNumberOfLastNode = 0;
|
|
}
|
|
return theNode0 + aAC * (aACdotAP / (aACdotAP + aCAdotCP));
|
|
}
|
|
|
|
T aNorm = aVA + aVB + aVC;
|
|
|
|
if (thePrjState != nullptr)
|
|
{
|
|
*thePrjState = BVH_PrjStateInTriangle_INNER;
|
|
}
|
|
|
|
return (theNode0 * aVA + theNode1 * aVB + theNode2 * aVC) / aNorm;
|
|
}
|
|
|
|
//! Computes square distance between point and triangle
|
|
static T PointTriangleSquareDistance(const BVH_VecNt& thePoint,
|
|
const BVH_VecNt& theNode0,
|
|
const BVH_VecNt& theNode1,
|
|
const BVH_VecNt& theNode2)
|
|
{
|
|
const BVH_VecNt aProj = PointTriangleProjection(thePoint, theNode0, theNode1, theNode2);
|
|
const BVH_VecNt aPP = aProj - thePoint;
|
|
return aPP.Dot(aPP);
|
|
}
|
|
|
|
public: //! @name Ray-Box Intersection
|
|
//! Computes hit time of ray-box intersection
|
|
static Standard_Boolean RayBoxIntersection(const BVH_Ray<T, N>& theRay,
|
|
const BVH_Box<T, N>& theBox,
|
|
T& theTimeEnter,
|
|
T& theTimeLeave)
|
|
{
|
|
if (!theBox.IsValid())
|
|
{
|
|
return Standard_False;
|
|
}
|
|
return RayBoxIntersection(theRay,
|
|
theBox.CornerMin(),
|
|
theBox.CornerMax(),
|
|
theTimeEnter,
|
|
theTimeLeave);
|
|
}
|
|
|
|
//! Computes hit time of ray-box intersection
|
|
static Standard_Boolean RayBoxIntersection(const BVH_Ray<T, N>& theRay,
|
|
const BVH_VecNt& theBoxCMin,
|
|
const BVH_VecNt& theBoxCMax,
|
|
T& theTimeEnter,
|
|
T& theTimeLeave)
|
|
{
|
|
return RayBoxIntersection(theRay.Origin,
|
|
theRay.Direct,
|
|
theBoxCMin,
|
|
theBoxCMax,
|
|
theTimeEnter,
|
|
theTimeLeave);
|
|
}
|
|
|
|
//! Computes hit time of ray-box intersection
|
|
static Standard_Boolean RayBoxIntersection(const BVH_VecNt& theRayOrigin,
|
|
const BVH_VecNt& theRayDirection,
|
|
const BVH_Box<T, N>& theBox,
|
|
T& theTimeEnter,
|
|
T& theTimeLeave)
|
|
{
|
|
if (!theBox.IsValid())
|
|
{
|
|
return Standard_False;
|
|
}
|
|
return RayBoxIntersection(theRayOrigin,
|
|
theRayDirection,
|
|
theBox.CornerMin(),
|
|
theBox.CornerMax(),
|
|
theTimeEnter,
|
|
theTimeLeave);
|
|
}
|
|
|
|
//! Computes hit time of ray-box intersection
|
|
static Standard_Boolean RayBoxIntersection(const BVH_VecNt& theRayOrigin,
|
|
const BVH_VecNt& theRayDirection,
|
|
const BVH_VecNt& theBoxCMin,
|
|
const BVH_VecNt& theBoxCMax,
|
|
T& theTimeEnter,
|
|
T& theTimeLeave)
|
|
{
|
|
BVH_VecNt aNodeMin, aNodeMax;
|
|
for (int i = 0; i < N; ++i)
|
|
{
|
|
if (theRayDirection[i] == 0)
|
|
{
|
|
aNodeMin[i] = (theBoxCMin[i] - theRayOrigin[i]) <= 0 ? (std::numeric_limits<T>::min)()
|
|
: (std::numeric_limits<T>::max)();
|
|
aNodeMax[i] = (theBoxCMax[i] - theRayOrigin[i]) < 0 ? (std::numeric_limits<T>::min)()
|
|
: (std::numeric_limits<T>::max)();
|
|
}
|
|
else
|
|
{
|
|
aNodeMin[i] = (theBoxCMin[i] - theRayOrigin[i]) / theRayDirection[i];
|
|
aNodeMax[i] = (theBoxCMax[i] - theRayOrigin[i]) / theRayDirection[i];
|
|
}
|
|
}
|
|
|
|
BVH_VecNt aTimeMin, aTimeMax;
|
|
for (int i = 0; i < N; ++i)
|
|
{
|
|
aTimeMin[i] = Min(aNodeMin[i], aNodeMax[i]);
|
|
aTimeMax[i] = Max(aNodeMin[i], aNodeMax[i]);
|
|
}
|
|
|
|
T aTimeEnter = Max(aTimeMin[0], Max(aTimeMin[1], aTimeMin[2]));
|
|
T aTimeLeave = Min(aTimeMax[0], Min(aTimeMax[1], aTimeMax[2]));
|
|
|
|
Standard_Boolean hasIntersection = aTimeEnter <= aTimeLeave && aTimeLeave >= 0;
|
|
if (hasIntersection)
|
|
{
|
|
theTimeEnter = aTimeEnter;
|
|
theTimeLeave = aTimeLeave;
|
|
}
|
|
|
|
return hasIntersection;
|
|
}
|
|
};
|
|
|
|
#endif |