From cf9b27a825f43d0e8ba56fbc26464d572bb79ded Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 9 Aug 2016 13:55:59 -0400 Subject: [PATCH] Template implementation structure rather than templating class --- include/fcl/BVH/BV_fitter.h | 709 +++++++++++++++++------------------- 1 file changed, 342 insertions(+), 367 deletions(-) diff --git a/include/fcl/BVH/BV_fitter.h b/include/fcl/BVH/BV_fitter.h index 47dcfdc37..d8b6a7ce6 100644 --- a/include/fcl/BVH/BV_fitter.h +++ b/include/fcl/BVH/BV_fitter.h @@ -103,110 +103,12 @@ class BVFitter : public BVFitterBase Vector3* prev_vertices; Triangle* tri_indices; BVHModelType type; -}; - - -/// @brief Specification of BVFitter for OBB bounding volume -template -class BVFitter> : public BVFitterBase> -{ -public: - /// @brief Prepare the geometry primitive data for fitting - void set(Vector3* vertices_, Triangle* tri_indices_, BVHModelType type_); - - /// @brief Prepare the geometry primitive data for fitting, for deformable mesh - void set(Vector3* vertices_, Vector3* prev_vertices_, Triangle* tri_indices_, BVHModelType type_); - - /// @brief Compute a bounding volume that fits a set of primitives (points or triangles). - /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data. - OBB fit(unsigned int* primitive_indices, int num_primitives); - - /// brief Clear the geometry primitive data - void clear(); - -private: - - Vector3* vertices; - Vector3* prev_vertices; - Triangle* tri_indices; - BVHModelType type; -}; - -/// @brief Specification of BVFitter for RSS bounding volume -template -class BVFitter> : public BVFitterBase> -{ -public: - /// brief Prepare the geometry primitive data for fitting - void set(Vector3* vertices_, Triangle* tri_indices_, BVHModelType type_); - - /// @brief Prepare the geometry primitive data for fitting, for deformable mesh - void set(Vector3* vertices_, Vector3* prev_vertices_, Triangle* tri_indices_, BVHModelType type_); - - /// @brief Compute a bounding volume that fits a set of primitives (points or triangles). - /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data. - RSS fit(unsigned int* primitive_indices, int num_primitives); - - /// @brief Clear the geometry primitive data - void clear(); - -private: - - Vector3* vertices; - Vector3* prev_vertices; - Triangle* tri_indices; - BVHModelType type; -}; - -/// @brief Specification of BVFitter for kIOS bounding volume -template -class BVFitter> : public BVFitterBase> -{ -public: - /// @brief Prepare the geometry primitive data for fitting - void set(Vector3* vertices_, Triangle* tri_indices_, BVHModelType type_); - - /// @brief Prepare the geometry primitive data for fitting - void set(Vector3* vertices_, Vector3* prev_vertices_, Triangle* tri_indices_, BVHModelType type_); - - /// @brief Compute a bounding volume that fits a set of primitives (points or triangles). - /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data. - kIOS fit(unsigned int* primitive_indices, int num_primitives); - - /// @brief Clear the geometry primitive data - void clear(); - -private: - Vector3* vertices; - Vector3* prev_vertices; - Triangle* tri_indices; - BVHModelType type; -}; - -/// @brief Specification of BVFitter for OBBRSS bounding volume -template -class BVFitter> : public BVFitterBase> -{ -public: - /// @brief Prepare the geometry primitive data for fitting - void set(Vector3* vertices_, Triangle* tri_indices_, BVHModelType type_); - /// @brief Prepare the geometry primitive data for fitting - void set(Vector3* vertices_, Vector3* prev_vertices_, Triangle* tri_indices_, BVHModelType type_); - - /// @brief Compute a bounding volume that fits a set of primitives (points or triangles). - /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data. - OBBRSS fit(unsigned int* primitive_indices, int num_primitives); - - /// @brief Clear the geometry primitive data - void clear(); - -private: + template + friend struct SetImpl; - Vector3* vertices; - Vector3* prev_vertices; - Triangle* tri_indices; - BVHModelType type; + template + friend struct FitImpl; }; //============================================================================// @@ -222,6 +124,10 @@ BVFitter::~BVFitter() // Do nothing } +//============================================================================== +template +struct SetImpl; + //============================================================================== template void BVFitter::set( @@ -229,10 +135,7 @@ void BVFitter::set( Triangle* tri_indices_, BVHModelType type_) { - vertices = vertices_; - prev_vertices = NULL; - tri_indices = tri_indices_; - type = type_; + SetImpl::run(*this, vertices_, tri_indices_, type_); } //============================================================================== @@ -243,49 +146,20 @@ void BVFitter::set( Triangle* tri_indices_, BVHModelType type_) { - vertices = vertices_; - prev_vertices = prev_vertices_; - tri_indices = tri_indices_; - type = type_; + SetImpl::run( + *this, vertices_, prev_vertices_, tri_indices_, type_); } +//============================================================================== +template +struct FitImpl; + //============================================================================== template BV BVFitter::fit(unsigned int* primitive_indices, int num_primitives) { - BV bv; - - if(type == BVH_MODEL_TRIANGLES) /// The primitive is triangle - { - for(int i = 0; i < num_primitives; ++i) - { - Triangle t = tri_indices[primitive_indices[i]]; - bv += vertices[t[0]]; - bv += vertices[t[1]]; - bv += vertices[t[2]]; - - if(prev_vertices) /// can fitting both current and previous frame - { - bv += prev_vertices[t[0]]; - bv += prev_vertices[t[1]]; - bv += prev_vertices[t[2]]; - } - } - } - else if(type == BVH_MODEL_POINTCLOUD) /// The primitive is point - { - for(int i = 0; i < num_primitives; ++i) - { - bv += vertices[primitive_indices[i]]; - - if(prev_vertices) /// can fitting both current and previous frame - { - bv += prev_vertices[primitive_indices[i]]; - } - } - } - - return bv; + return FitImpl::run( + *this, primitive_indices, num_primitives); } //============================================================================== @@ -299,279 +173,380 @@ void BVFitter::clear() } //============================================================================== -template -void BVFitter>::set( - Vector3* vertices_, Triangle* tri_indices_, BVHModelType type_) -{ - vertices = vertices_; - prev_vertices = NULL; - tri_indices = tri_indices_; - type = type_; -} - -//============================================================================== -template -void BVFitter>::set( - Vector3* vertices_, - Vector3* prev_vertices_, - Triangle* tri_indices_, BVHModelType type_) -{ - vertices = vertices_; - prev_vertices = prev_vertices_; - tri_indices = tri_indices_; - type = type_; -} - -//============================================================================== -template -OBB BVFitter>::fit( - unsigned int* primitive_indices, int num_primitives) +template +struct SetImpl { - OBB bv; - - Matrix3 M; // row first matrix - Matrix3 E; // row first eigen-vectors - Vector3 s; // three eigen values - getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); - eigen_old(M, s, E); - axisFromEigen(E, s, bv.frame); - - // set obb centers and extensions - getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.frame, bv.extent); - - return bv; -} + static void run( + BVFitter& fitter, + Vector3* vertices_, + Triangle* tri_indices_, + BVHModelType type_) + { + fitter.vertices = vertices_; + fitter.prev_vertices = NULL; + fitter.tri_indices = tri_indices_; + fitter.type = type_; + } -//============================================================================== -template -void BVFitter>::clear() -{ - vertices = NULL; - prev_vertices = NULL; - tri_indices = NULL; - type = BVH_MODEL_UNKNOWN; -} + static void run( + BVFitter& fitter, + Vector3* vertices_, + Vector3* prev_vertices_, + Triangle* tri_indices_, + BVHModelType type_) + { + fitter.vertices = vertices_; + fitter.prev_vertices = prev_vertices_; + fitter.tri_indices = tri_indices_; + fitter.type = type_; + } +}; //============================================================================== template -void BVFitter>::set( - Vector3* vertices_, Triangle* tri_indices_, BVHModelType type_) +struct SetImpl> { - vertices = vertices_; - prev_vertices = NULL; - tri_indices = tri_indices_; - type = type_; -} + static void run( + BVFitter>& fitter, + Vector3* vertices_, + Triangle* tri_indices_, + BVHModelType type_) + { + fitter.vertices = vertices_; + fitter.prev_vertices = NULL; + fitter.tri_indices = tri_indices_; + fitter.type = type_; + } -//============================================================================== -template -void BVFitter>::set( - Vector3* vertices_, - Vector3* prev_vertices_, - Triangle* tri_indices_, - BVHModelType type_) -{ - vertices = vertices_; - prev_vertices = prev_vertices_; - tri_indices = tri_indices_; - type = type_; -} + static void run( + BVFitter>& fitter, + Vector3* vertices_, + Vector3* prev_vertices_, + Triangle* tri_indices_, + BVHModelType type_) + { + fitter.vertices = vertices_; + fitter.prev_vertices = prev_vertices_; + fitter.tri_indices = tri_indices_; + fitter.type = type_; + } +}; //============================================================================== template -RSS BVFitter>::fit( - unsigned int* primitive_indices, int num_primitives) +struct SetImpl> { - RSS bv; - - Matrix3 M; // row first matrix - Matrix3 E; // row first eigen-vectors - Vector3 s; // three eigen values - getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); - eigen_old(M, s, E); - axisFromEigen(E, s, bv.frame); - - // set rss origin, rectangle size and radius - getRadiusAndOriginAndRectangleSize( - vertices, prev_vertices, tri_indices, - primitive_indices, num_primitives, bv.frame, bv.l, bv.r); - - return bv; -} + static void run( + BVFitter>& fitter, + Vector3* vertices_, + Triangle* tri_indices_, + BVHModelType type_) + { + fitter.vertices = vertices_; + fitter.prev_vertices = NULL; + fitter.tri_indices = tri_indices_; + fitter.type = type_; + } -//============================================================================== -template -void BVFitter>::clear() -{ - vertices = NULL; - prev_vertices = NULL; - tri_indices = NULL; - type = BVH_MODEL_UNKNOWN; -} + static void run( + BVFitter>& fitter, + Vector3* vertices_, + Vector3* prev_vertices_, + Triangle* tri_indices_, + BVHModelType type_) + { + fitter.vertices = vertices_; + fitter.prev_vertices = prev_vertices_; + fitter.tri_indices = tri_indices_; + fitter.type = type_; + } +}; //============================================================================== template -void BVFitter>::set( - Vector3* vertices_, - Vector3* prev_vertices_, - Triangle* tri_indices_, - BVHModelType type_) +struct SetImpl> { - vertices = vertices_; - prev_vertices = prev_vertices_; - tri_indices = tri_indices_; - type = type_; -} + static void run( + BVFitter>& fitter, + Vector3* vertices_, + Triangle* tri_indices_, + BVHModelType type_) + { + fitter.vertices = vertices_; + fitter.prev_vertices = NULL; + fitter.tri_indices = tri_indices_; + fitter.type = type_; + } -//============================================================================== -template -void BVFitter>::set( - Vector3* vertices_, Triangle* tri_indices_, BVHModelType type_) -{ - vertices = vertices_; - prev_vertices = NULL; - tri_indices = tri_indices_; - type = type_; -} + static void run( + BVFitter>& fitter, + Vector3* vertices_, + Vector3* prev_vertices_, + Triangle* tri_indices_, + BVHModelType type_) + { + fitter.vertices = vertices_; + fitter.prev_vertices = prev_vertices_; + fitter.tri_indices = tri_indices_; + fitter.type = type_; + } +}; //============================================================================== template -kIOS BVFitter>::fit( - unsigned int* primitive_indices, int num_primitives) +struct SetImpl> { - kIOS bv; - - Matrix3 M; // row first matrix - Matrix3 E; // row first eigen-vectors - Vector3 s; - getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); - eigen_old(M, s, E); - axisFromEigen(E, s, bv.obb.frame); - - // get centers and extensions - getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.obb.frame, bv.obb.extent); - - const Vector3& center = bv.obb.frame.translation(); - const Vector3& extent = bv.obb.extent; - Scalar r0 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, center); - - // decide k in kIOS - if(extent[0] > kIOS::ratio() * extent[2]) + static void run( + BVFitter>& fitter, + Vector3* vertices_, + Triangle* tri_indices_, + BVHModelType type_) { - if(extent[0] > kIOS::ratio() * extent[1]) bv.num_spheres = 5; - else bv.num_spheres = 3; + fitter.vertices = vertices_; + fitter.prev_vertices = NULL; + fitter.tri_indices = tri_indices_; + fitter.type = type_; } - else bv.num_spheres = 1; - - bv.spheres[0].o = center; - bv.spheres[0].r = r0; - if(bv.num_spheres >= 3) + static void run( + BVFitter>& fitter, + Vector3* vertices_, + Vector3* prev_vertices_, + Triangle* tri_indices_, + BVHModelType type_) { - Scalar r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * kIOS::invSinA(); - Vector3 delta = bv.obb.frame.linear().col(2) * (r10 * kIOS::cosA() - extent[2]); - bv.spheres[1].o = center - delta; - bv.spheres[2].o = center + delta; - - Scalar r11 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[1].o); - Scalar r12 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[2].o); - - bv.spheres[1].o += bv.obb.frame.linear().col(2) * (-r10 + r11); - bv.spheres[2].o += bv.obb.frame.linear().col(2) * (r10 - r12); - - bv.spheres[1].r = r10; - bv.spheres[2].r = r10; + fitter.vertices = vertices_; + fitter.prev_vertices = prev_vertices_; + fitter.tri_indices = tri_indices_; + fitter.type = type_; } +}; - if(bv.num_spheres >= 5) +//============================================================================== +template +struct FitImpl +{ + static BV run( + const BVFitter& fitter, + unsigned int* primitive_indices, + int num_primitives) { - Scalar r10 = bv.spheres[1].r; - Vector3 delta = bv.obb.frame.linear().col(1) * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]); - bv.spheres[3].o = bv.spheres[0].o - delta; - bv.spheres[4].o = bv.spheres[0].o + delta; + BV bv; - Scalar r21 = 0, r22 = 0; - r21 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[3].o); - r22 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[4].o); + if(fitter.type == BVH_MODEL_TRIANGLES) /// The primitive is triangle + { + for(int i = 0; i < num_primitives; ++i) + { + Triangle t = fitter.tri_indices[primitive_indices[i]]; + bv += fitter.vertices[t[0]]; + bv += fitter.vertices[t[1]]; + bv += fitter.vertices[t[2]]; + + if(fitter.prev_vertices) /// can fitting both current and previous frame + { + bv += fitter.prev_vertices[t[0]]; + bv += fitter.prev_vertices[t[1]]; + bv += fitter.prev_vertices[t[2]]; + } + } + } + else if(fitter.type == BVH_MODEL_POINTCLOUD) /// The primitive is point + { + for(int i = 0; i < num_primitives; ++i) + { + bv += fitter.vertices[primitive_indices[i]]; - bv.spheres[3].o += bv.obb.frame.linear().col(1) * (-r10 + r21); - bv.spheres[4].o += bv.obb.frame.linear().col(1) * (r10 - r22); + if(fitter.prev_vertices) /// can fitting both current and previous frame + { + bv += fitter.prev_vertices[primitive_indices[i]]; + } + } + } - bv.spheres[3].r = r10; - bv.spheres[4].r = r10; + return bv; } - - return bv; -} +}; //============================================================================== template -void BVFitter>::clear() +struct FitImpl> { - vertices = NULL; - prev_vertices = NULL; - tri_indices = NULL; - type = BVH_MODEL_UNKNOWN; -} + static OBB run( + const BVFitter>& fitter, + unsigned int* primitive_indices, + int num_primitives) + { + OBB bv; + + Matrix3 M; // row first matrix + Matrix3 E; // row first eigen-vectors + Vector3 s; // three eigen values + getCovariance( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, M); + eigen_old(M, s, E); + axisFromEigen(E, s, bv.frame); + + // set obb centers and extensions + getExtentAndCenter( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, + bv.frame, bv.extent); + + return bv; + } +}; //============================================================================== template -void BVFitter>::set( - Vector3* vertices_, Triangle* tri_indices_, BVHModelType type_) +struct FitImpl> { - vertices = vertices_; - prev_vertices = NULL; - tri_indices = tri_indices_; - type = type_; -} + static RSS run( + const BVFitter>& fitter, + unsigned int* primitive_indices, + int num_primitives) + { + RSS bv; + + Matrix3 M; // row first matrix + Matrix3 E; // row first eigen-vectors + Vector3 s; // three eigen values + getCovariance( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, M); + eigen_old(M, s, E); + axisFromEigen(E, s, bv.frame); + + // set rss origin, rectangle size and radius + getRadiusAndOriginAndRectangleSize( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, bv.frame, bv.l, bv.r); + + return bv; + } +}; //============================================================================== template -void BVFitter>::set( - Vector3* vertices_, - Vector3* prev_vertices_, - Triangle* tri_indices_, - BVHModelType type_) +struct FitImpl> { - vertices = vertices_; - prev_vertices = prev_vertices_; - tri_indices = tri_indices_; - type = type_; -} + static kIOS run( + const BVFitter>& fitter, + unsigned int* primitive_indices, + int num_primitives) + { + kIOS bv; + + Matrix3 M; // row first matrix + Matrix3 E; // row first eigen-vectors + Vector3 s; + getCovariance( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, M); + eigen_old(M, s, E); + axisFromEigen(E, s, bv.obb.frame); + + // get centers and extensions + getExtentAndCenter( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, bv.obb.frame, bv.obb.extent); + + const Vector3& center = bv.obb.frame.translation(); + const Vector3& extent = bv.obb.extent; + Scalar r0 = maximumDistance( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, center); + + // decide k in kIOS + if(extent[0] > kIOS::ratio() * extent[2]) + { + if(extent[0] > kIOS::ratio() * extent[1]) bv.num_spheres = 5; + else bv.num_spheres = 3; + } + else bv.num_spheres = 1; -//============================================================================== -template -OBBRSS BVFitter>::fit( - unsigned int* primitive_indices, int num_primitives) -{ - OBBRSS bv; - Matrix3 M; - Matrix3 E; - Vector3 s; - getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); - eigen_old(M, s, E); - axisFromEigen(E, s, bv.obb.frame); - bv.rss.frame.linear() = bv.obb.frame.linear(); - - getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.obb.frame, bv.obb.extent); - - getRadiusAndOriginAndRectangleSize( - vertices, prev_vertices, tri_indices, - primitive_indices, num_primitives, - bv.rss.frame, bv.rss.l, bv.rss.r); - - return bv; -} + bv.spheres[0].o = center; + bv.spheres[0].r = r0; + + if(bv.num_spheres >= 3) + { + Scalar r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * kIOS::invSinA(); + Vector3 delta = bv.obb.frame.linear().col(2) * (r10 * kIOS::cosA() - extent[2]); + bv.spheres[1].o = center - delta; + bv.spheres[2].o = center + delta; + + Scalar r11 = maximumDistance( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, bv.spheres[1].o); + Scalar r12 = maximumDistance( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, bv.spheres[2].o); + + bv.spheres[1].o += bv.obb.frame.linear().col(2) * (-r10 + r11); + bv.spheres[2].o += bv.obb.frame.linear().col(2) * (r10 - r12); + + bv.spheres[1].r = r10; + bv.spheres[2].r = r10; + } + + if(bv.num_spheres >= 5) + { + Scalar r10 = bv.spheres[1].r; + Vector3 delta = bv.obb.frame.linear().col(1) * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]); + bv.spheres[3].o = bv.spheres[0].o - delta; + bv.spheres[4].o = bv.spheres[0].o + delta; + + Scalar r21 = 0, r22 = 0; + r21 = maximumDistance( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, bv.spheres[3].o); + r22 = maximumDistance( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, bv.spheres[4].o); + + bv.spheres[3].o += bv.obb.frame.linear().col(1) * (-r10 + r21); + bv.spheres[4].o += bv.obb.frame.linear().col(1) * (r10 - r22); + + bv.spheres[3].r = r10; + bv.spheres[4].r = r10; + } + + return bv; + } +}; //============================================================================== template -void BVFitter>::clear() +struct FitImpl> { - vertices = NULL; - prev_vertices = NULL; - tri_indices = NULL; - type = BVH_MODEL_UNKNOWN; -} + static OBBRSS run( + const BVFitter>& fitter, + unsigned int* primitive_indices, + int num_primitives) + { + OBBRSS bv; + Matrix3 M; + Matrix3 E; + Vector3 s; + getCovariance( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, M); + eigen_old(M, s, E); + axisFromEigen(E, s, bv.obb.frame); + bv.rss.frame.linear() = bv.obb.frame.linear(); + + getExtentAndCenter( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, bv.obb.frame, bv.obb.extent); + + getRadiusAndOriginAndRectangleSize( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, + bv.rss.frame, bv.rss.l, bv.rss.r); + + return bv; + } +}; } // namespace fcl