Commit 3eacf877 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Clean code related to mesh-shape collision traversal.

parent 6cff73f2
......@@ -265,53 +265,6 @@ public:
const GJKSolver* nsolver;
};
/// @brief Traversal node for mesh and shape, when mesh BVH is one of the oriented node (OBB, RSS, OBBRSS, kIOS)
template<typename S>
class MeshShapeCollisionTraversalNodeOBB : public MeshShapeCollisionTraversalNode<OBB, S, 0>
{
public:
MeshShapeCollisionTraversalNodeOBB(const CollisionRequest& request) :
MeshShapeCollisionTraversalNode<OBB, S, 0>
(request)
{
}
};
template<typename S>
class MeshShapeCollisionTraversalNodeRSS : public MeshShapeCollisionTraversalNode<RSS, S, 0>
{
public:
MeshShapeCollisionTraversalNodeRSS (const CollisionRequest& request):
MeshShapeCollisionTraversalNode<RSS, S, 0>
(request)
{
}
};
template<typename S>
class MeshShapeCollisionTraversalNodekIOS : public MeshShapeCollisionTraversalNode<kIOS, S, 0>
{
public:
MeshShapeCollisionTraversalNodekIOS(const CollisionRequest& request):
MeshShapeCollisionTraversalNode<kIOS, S, 0>
(request)
{
}
};
template<typename S>
class MeshShapeCollisionTraversalNodeOBBRSS : public MeshShapeCollisionTraversalNode<OBBRSS, S, 0>
{
public:
MeshShapeCollisionTraversalNodeOBBRSS (const CollisionRequest& request) :
MeshShapeCollisionTraversalNode <OBBRSS, S, 0>
(request)
{
}
};
/// @brief Traversal node for collision between shape and mesh
template<typename S, typename BV,
int _Options = RelativeTransformationIsIdentity>
......@@ -425,46 +378,6 @@ public:
const GJKSolver* nsolver;
};
/// @brief Traversal node for shape and mesh, when mesh BVH is one of the oriented node (OBB, RSS, OBBRSS, kIOS)
template<typename S>
class ShapeMeshCollisionTraversalNodeOBB : public ShapeMeshCollisionTraversalNode<S, OBB, 0>
{
public:
ShapeMeshCollisionTraversalNodeOBB() : ShapeMeshCollisionTraversalNode<S, OBB>()
{
}
};
template<typename S>
class ShapeMeshCollisionTraversalNodeRSS : public ShapeMeshCollisionTraversalNode<S, RSS, 0>
{
public:
ShapeMeshCollisionTraversalNodeRSS() : ShapeMeshCollisionTraversalNode<S, RSS>()
{
}
};
template<typename S>
class ShapeMeshCollisionTraversalNodekIOS : public ShapeMeshCollisionTraversalNode<S, kIOS, 0>
{
public:
ShapeMeshCollisionTraversalNodekIOS() : ShapeMeshCollisionTraversalNode<S, kIOS>()
{
}
};
template<typename S>
class ShapeMeshCollisionTraversalNodeOBBRSS : public ShapeMeshCollisionTraversalNode<S, OBBRSS, 0>
{
public:
ShapeMeshCollisionTraversalNodeOBBRSS() : ShapeMeshCollisionTraversalNode<S, OBBRSS>()
{
}
};
/// @}
/// @addtogroup Traversal_For_Distance
......
......@@ -342,16 +342,13 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node,
return true;
}
/// @cond IGNORE
namespace details
{
template<typename BV, typename S, template<typename> class OrientedNode>
static inline bool setupMeshShapeCollisionOrientedNode(OrientedNode<S>& node,
const BVHModel<BV>& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
const GJKSolver* nsolver,
CollisionResult& result)
/// @brief Initialize traversal node for collision between one mesh and one shape
template<typename BV, typename S>
bool initialize(MeshShapeCollisionTraversalNode<BV, S, 0>& node,
const BVHModel<BV>& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
const GJKSolver* nsolver,
CollisionResult& result)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES)
return false;
......@@ -372,56 +369,6 @@ static inline bool setupMeshShapeCollisionOrientedNode(OrientedNode<S>& node,
return true;
}
}
/// @endcond
/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type
template<typename S>
bool initialize(MeshShapeCollisionTraversalNodeOBB<S>& node,
const BVHModel<OBB>& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
const GJKSolver* nsolver,
CollisionResult& result)
{
return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, result);
}
/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type
template<typename S>
bool initialize(MeshShapeCollisionTraversalNodeRSS<S>& node,
const BVHModel<RSS>& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
const GJKSolver* nsolver,
CollisionResult& result)
{
return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, result);
}
/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type
template<typename S>
bool initialize(MeshShapeCollisionTraversalNodekIOS<S>& node,
const BVHModel<kIOS>& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
const GJKSolver* nsolver,
CollisionResult& result)
{
return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, result);
}
/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type
template<typename S>
bool initialize(MeshShapeCollisionTraversalNodeOBBRSS<S>& node,
const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
const GJKSolver* nsolver,
CollisionResult& result)
{
return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, result);
}
/// @cond IGNORE
namespace details
{
......
......@@ -112,27 +112,26 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
namespace details
{
template<typename OrientMeshShapeCollisionTraveralNode, typename T_BVH, typename T_SH>
std::size_t orientedBVHShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
OrientMeshShapeCollisionTraveralNode node (request);
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
initialize(node, *obj1, tf1, *obj2, tf2, nsolver, result);
fcl::collide(&node, request, result);
return result.numContacts();
}
template<typename T_BVH, typename T_SH> struct bvh_shape_traits
{
enum { Options = RelativeTransformationIsIdentity };
};
#define BVH_SHAPE_DEFAULT_TO_ORIENTED(bv) \
template<typename T_SH> struct bvh_shape_traits<bv, T_SH> \
{ enum { Options = 0 }; }
BVH_SHAPE_DEFAULT_TO_ORIENTED(OBB);
BVH_SHAPE_DEFAULT_TO_ORIENTED(RSS);
BVH_SHAPE_DEFAULT_TO_ORIENTED(kIOS);
BVH_SHAPE_DEFAULT_TO_ORIENTED(OBBRSS);
#undef BVH_SHAPE_DEFAULT_TO_ORIENTED
}
template<typename T_BVH, typename T_SH>
/// \tparam _Options takes two values.
/// - RelativeTransformationIsIdentity if object 1 should be moved the
/// into the frame of object 2 before computing collisions.
/// - 0 if the query should be made with non-aligned object frames.
template<typename T_BVH, typename T_SH,
int _Options = RelativeTransformationIsIdentity>
struct BVHShapeCollider
{
static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
......@@ -141,7 +140,19 @@ struct BVHShapeCollider
{
if(request.isSatisfied(result)) return result.numContacts();
MeshShapeCollisionTraversalNode<T_BVH, T_SH> node (request);
if (_Options & RelativeTransformationIsIdentity)
return aligned(o1, tf1, o2, tf2, nsolver, request, result);
else
return oriented(o1, tf1, o2, tf2, nsolver, request, result);
}
static std::size_t aligned(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
MeshShapeCollisionTraversalNode<T_BVH, T_SH, RelativeTransformationIsIdentity> node (request);
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
Transform3f tf1_tmp = tf1;
......@@ -153,53 +164,22 @@ struct BVHShapeCollider
delete obj1_tmp;
return result.numContacts();
}
};
template<typename T_SH>
struct BVHShapeCollider<OBB, T_SH>
{
static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
static std::size_t oriented(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
return details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeOBB<T_SH>, OBB, T_SH>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
template<typename T_SH>
struct BVHShapeCollider<RSS, T_SH>
{
static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
return details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeRSS<T_SH>, RSS, T_SH>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
if(request.isSatisfied(result)) return result.numContacts();
template<typename T_SH>
struct BVHShapeCollider<kIOS, T_SH>
{
static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
return details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodekIOS<T_SH>, kIOS, T_SH>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
MeshShapeCollisionTraversalNode<T_BVH, T_SH, 0> node (request);
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
initialize(node, *obj1, tf1, *obj2, tf2, nsolver, result);
fcl::collide(&node, request, result);
return result.numContacts();
}
template<typename T_SH>
struct BVHShapeCollider<OBBRSS, T_SH>
{
static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
return details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeOBBRSS<T_SH>, OBBRSS, T_SH>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
namespace details
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment