Skip to content
GitLab
Commit 75fccd00 authored by Jia Pan's avatar Jia Pan
Browse files

Some refactor for OBBRSS continuous collision

parent ff1b38a3
Hide whitespace changes
Inline Side-by-side
include/fcl/traversal/traversal_node_bvhs.h
View file @ 75fccd00
......@@ -662,7 +662,8 @@ public:
struct ConservativeAdvancementStackData
{
ConservativeAdvancementStackData(const Vec3f& P1_, const Vec3f& P2_, int c1_, int c2_, FCL_REAL d_) : P1(P1_), P2(P2_), c1(c1_), c2(c2_), d(d_) {}
ConservativeAdvancementStackData(const Vec3f& P1_, const Vec3f& P2_, int c1_, int c2_, FCL_REAL d_)
: P1(P1_), P2(P2_), c1(c1_), c2(c2_), d(d_) {}
Vec3f P1;
Vec3f P2;
......@@ -846,6 +847,9 @@ bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(FCL_REAL c) const;
template<>
bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(FCL_REAL c) const;
template<>
bool MeshConservativeAdvancementTraversalNode<OBBRSS>::canStop(FCL_REAL c) const;
class MeshConservativeAdvancementTraversalNodeRSS : public MeshConservativeAdvancementTraversalNode<RSS>
{
......@@ -862,6 +866,20 @@ public:
Vec3f T;
};
class MeshConservativeAdvancementTraversalNodeOBBRSS : public MeshConservativeAdvancementTraversalNode<OBBRSS>
{
public:
MeshConservativeAdvancementTraversalNodeOBBRSS(FCL_REAL w_ = 1);
FCL_REAL BVTesting(int b1, int b2) const;
void leafTesting(int b1, int b2) const;
bool canStop(FCL_REAL c) const;
Matrix3f R;
Vec3f T;
};
}
......
include/fcl/traversal/traversal_node_setup.h
View file @ 75fccd00
......@@ -1048,9 +1048,9 @@ bool initialize(MeshContinuousCollisionTraversalNode<BV>& node,
/// @brief Initialize traversal node for conservative advancement computation between two meshes, given the current transforms
template<typename BV>
bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node,
BVHModel<BV>& model1,
BVHModel<BV>& model2,
const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2, FCL_REAL w = 1,
BVHModel<BV>& model1, const Transform3f& tf1,
BVHModel<BV>& model2, const Transform3f& tf2,
FCL_REAL w = 1,
bool use_refit = false, bool refit_bottomup = false)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
......@@ -1060,7 +1060,7 @@ bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node,
for(int i = 0; i < model1.num_vertices; ++i)
{
Vec3f& p = model1.vertices[i];
Vec3f new_v = R1 * p + T1;
Vec3f new_v = tf1.transform(p);
vertices_transformed1[i] = new_v;
}
......@@ -1069,7 +1069,7 @@ bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node,
for(int i = 0; i < model2.num_vertices; ++i)
{
Vec3f& p = model2.vertices[i];
Vec3f new_v = R2 * p + T2;
Vec3f new_v = tf2.transform(p);
vertices_transformed2[i] = new_v;
}
......@@ -1097,27 +1097,15 @@ bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node,
/// @brief Initialize traversal node for conservative advancement computation between two meshes, given the current transforms, specialized for RSS
inline bool initialize(MeshConservativeAdvancementTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2, FCL_REAL w = 1)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
node.model1 = &model1;
node.model2 = &model2;
node.vertices1 = model1.vertices;
node.vertices2 = model2.vertices;
node.tri_indices1 = model1.tri_indices;
node.tri_indices2 = model2.tri_indices;
node.w = w;
relativeTransform(R1, T1, R2, T2, node.R, node.T);
bool initialize(MeshConservativeAdvancementTraversalNodeRSS& node,
const BVHModel<RSS>& model1, const Transform3f& tf1,
const BVHModel<RSS>& model2, const Transform3f& tf2,
FCL_REAL w = 1);
return true;
}
bool initialize(MeshConservativeAdvancementTraversalNodeOBBRSS& node,
const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
FCL_REAL w = 1);
}
......
src/ccd/conservative_advancement.cpp
View file @ 75fccd00
......@@ -106,7 +106,7 @@ int conservativeAdvancement(const CollisionGeometry* o1,
MeshConservativeAdvancementTraversalNodeRSS node;
initialize(node, *model1, *model2, tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation());
initialize(node, *model1, tf1, *model2, tf2);
node.motion1 = motion1;
node.motion2 = motion2;
......
src/traversal/traversal_node_bvhs.cpp
View file @ 75fccd00
......@@ -428,11 +428,33 @@ void MeshDistanceTraversalNodeOBBRSS::leafTesting(int b1, int b2) const
}
/// for OBB and RSS, there is local coordinate of BV, so normal need to be transformed
namespace details
{
template<typename BV>
const Vec3f& getBVAxis(const BV& bv, int i)
{
return bv.axis[i];
}
template<>
bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(FCL_REAL c) const
const Vec3f& getBVAxis<OBBRSS>(const OBBRSS& bv, int i)
{
if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
return bv.obb.axis[i];
}
template<typename BV>
bool meshConservativeAdvancementTraversalNodeCanStop(FCL_REAL c,
FCL_REAL min_distance,
FCL_REAL abs_err, FCL_REAL rel_err, FCL_REAL w,
const BVHModel<BV>* model1, const BVHModel<BV>* model2,
const MotionBase<BV>* motion1, const MotionBase<BV>* motion2,
std::vector<ConservativeAdvancementStackData>& stack,
FCL_REAL& delta_t)
{
if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance))
{
const ConservativeAdvancementStackData& data = stack.back();
FCL_REAL d = data.d;
......@@ -457,10 +479,13 @@ bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(FCL_REAL c) const
assert(c == d);
Vec3f n_transformed = model1->getBV(c1).bv.axis[0] * n[0] + model1->getBV(c1).bv.axis[1] * n[1] + model1->getBV(c1).bv.axis[2] * n[2];
Vec3f n_transformed =
getBVAxis(model1->getBV(c1).bv, 0) * n[0] +
getBVAxis(model1->getBV(c1).bv, 1) * n[1] +
getBVAxis(model1->getBV(c1).bv, 2) * n[2];
FCL_REAL bound1 = motion1->computeMotionBound(this->model1->getBV(c1).bv, n_transformed);
FCL_REAL bound2 = motion2->computeMotionBound(this->model2->getBV(c2).bv, n_transformed);
FCL_REAL bound1 = motion1->computeMotionBound(model1->getBV(c1).bv, n_transformed);
FCL_REAL bound2 = motion2->computeMotionBound(model2->getBV(c2).bv, n_transformed);
FCL_REAL bound = bound1 + bound2;
......@@ -489,10 +514,53 @@ bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(FCL_REAL c) const
}
}
}
/// for OBB, RSS and OBBRSS, there is local coordinate of BV, so normal need to be transformed
template<>
bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(FCL_REAL c) const
{
return details::meshConservativeAdvancementTraversalNodeCanStop(c, this->min_distance,
this->abs_err, this->rel_err, w,
this->model1, this->model2,
motion1, motion2,
stack, delta_t);
}
template<>
bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(FCL_REAL c) const
{
if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
return details::meshConservativeAdvancementTraversalNodeCanStop(c, this->min_distance,
this->abs_err, this->rel_err, w,
this->model1, this->model2,
motion1, motion2,
stack, delta_t);
}
template<>
bool MeshConservativeAdvancementTraversalNode<OBBRSS>::canStop(FCL_REAL c) const
{
return details::meshConservativeAdvancementTraversalNodeCanStop(c, this->min_distance,
this->abs_err, this->rel_err, w,
this->model1, this->model2,
motion1, motion2,
stack, delta_t);
}
namespace details
{
template<typename BV>
bool meshConservativeAdvancementOrientedNodeCanStop(FCL_REAL c,
FCL_REAL min_distance,
FCL_REAL abs_err, FCL_REAL rel_err, FCL_REAL w,
const BVHModel<BV>* model1, const BVHModel<BV>* model2,
const MotionBase<BV>* motion1, const MotionBase<BV>* motion2,
std::vector<ConservativeAdvancementStackData>& stack,
FCL_REAL& delta_t)
{
if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance))
{
const ConservativeAdvancementStackData& data = stack.back();
FCL_REAL d = data.d;
......@@ -517,10 +585,18 @@ bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(FCL_REAL c) const
assert(c == d);
Vec3f n_transformed = model1->getBV(c1).bv.axis[0] * n[0] + model1->getBV(c1).bv.axis[1] * n[1] + model1->getBV(c1).bv.axis[2] * n[2];
// n is in local frame of c1, so we need to turn n into the global frame
Vec3f n_transformed =
getBVAxis(model1->getBV(c1).bv, 0) * n[0] +
getBVAxis(model1->getBV(c1).bv, 1) * n[2] +
getBVAxis(model1->getBV(c1).bv, 2) * n[2];
Matrix3f R0;
motion1->getCurrentRotation(R0);
n_transformed = R0 * n_transformed;
n_transformed.normalize();
FCL_REAL bound1 = motion1->computeMotionBound(this->model1->getBV(c1).bv, n_transformed);
FCL_REAL bound2 = motion2->computeMotionBound(this->model2->getBV(c2).bv, n_transformed);
FCL_REAL bound1 = motion1->computeMotionBound(model1->getBV(c1).bv, n_transformed);
FCL_REAL bound2 = motion2->computeMotionBound(model2->getBV(c2).bv, -n_transformed);
FCL_REAL bound = bound1 + bound2;
......@@ -549,31 +625,24 @@ bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(FCL_REAL c) const
}
}
MeshConservativeAdvancementTraversalNodeRSS::MeshConservativeAdvancementTraversalNodeRSS(FCL_REAL w_) : MeshConservativeAdvancementTraversalNode<RSS>(w_)
{
R.setIdentity();
// default T is 0
}
FCL_REAL MeshConservativeAdvancementTraversalNodeRSS::BVTesting(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
Vec3f P1, P2;
FCL_REAL d = distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv, &P1, &P2);
stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
return d;
}
void MeshConservativeAdvancementTraversalNodeRSS::leafTesting(int b1, int b2) const
template<typename BV>
void meshConservativeAdvancementOrientedNodeLeafTesting(int b1, int b2,
const BVHModel<BV>* model1, const BVHModel<BV>* model2,
const Triangle* tri_indices1, const Triangle* tri_indices2,
const Vec3f* vertices1, const Vec3f* vertices2,
const Matrix3f& R, const Vec3f& T,
const MotionBase<BV>* motion1, const MotionBase<BV>* motion2,
bool enable_statistics,
FCL_REAL& min_distance,
Vec3f& p1, Vec3f& p2,
int& last_tri_id1, int& last_tri_id2,
FCL_REAL& delta_t,
int& num_leaf_tests)
{
if(enable_statistics) num_leaf_tests++;
const BVNode<RSS>& node1 = model1->getBV(b1);
const BVNode<RSS>& node2 = model2->getBV(b2);
const BVNode<BV>& node1 = model1->getBV(b1);
const BVNode<BV>& node2 = model2->getBV(b2);
int primitive_id1 = node1.primitiveId();
int primitive_id2 = node2.primitiveId();
......@@ -628,68 +697,100 @@ void MeshConservativeAdvancementTraversalNodeRSS::leafTesting(int b1, int b2) co
delta_t = cur_delta_t;
}
bool MeshConservativeAdvancementTraversalNodeRSS::canStop(FCL_REAL c) const
}
MeshConservativeAdvancementTraversalNodeRSS::MeshConservativeAdvancementTraversalNodeRSS(FCL_REAL w_)
: MeshConservativeAdvancementTraversalNode<RSS>(w_)
{
if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance))
{
const ConservativeAdvancementStackData& data = stack.back();
FCL_REAL d = data.d;
Vec3f n;
int c1, c2;
R.setIdentity();
}
if(d > c)
{
const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2];
d = data2.d;
n = data2.P2 - data2.P1;
c1 = data2.c1;
c2 = data2.c2;
stack[stack.size() - 2] = stack[stack.size() - 1];
}
else
{
n = data.P2 - data.P1;
c1 = data.c1;
c2 = data.c2;
}
FCL_REAL MeshConservativeAdvancementTraversalNodeRSS::BVTesting(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
Vec3f P1, P2;
FCL_REAL d = distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv, &P1, &P2);
assert(c == d);
stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
// n is in local frame of RSS c1, so we need to turn n into the global frame
Vec3f n_transformed = model1->getBV(c1).bv.axis[0] * n[0] + model1->getBV(c1).bv.axis[1] * n[2] + model1->getBV(c1).bv.axis[2] * n[2];
Matrix3f R0;
motion1->getCurrentRotation(R0);
n_transformed = R0 * n_transformed;
n_transformed.normalize();
return d;
}
FCL_REAL bound1 = motion1->computeMotionBound(model1->getBV(c1).bv, n_transformed);
FCL_REAL bound2 = motion2->computeMotionBound(model2->getBV(c2).bv, -n_transformed);
FCL_REAL bound = bound1 + bound2;
void MeshConservativeAdvancementTraversalNodeRSS::leafTesting(int b1, int b2) const
{
details::meshConservativeAdvancementOrientedNodeLeafTesting(b1, b2,
model1, model2,
tri_indices1, tri_indices2,
vertices1, vertices2,
R, T,
motion1, motion2,
enable_statistics,
min_distance,
p1, p2,
last_tri_id1, last_tri_id2,
delta_t,
num_leaf_tests);
}
FCL_REAL cur_delta_t;
if(bound <= c) cur_delta_t = 1;
else cur_delta_t = c / bound;
bool MeshConservativeAdvancementTraversalNodeRSS::canStop(FCL_REAL c) const
{
return details::meshConservativeAdvancementOrientedNodeCanStop(c,
min_distance,
abs_err, rel_err, w,
model1, model2,
motion1, motion2,
stack,
delta_t);
}
if(cur_delta_t < delta_t)
delta_t = cur_delta_t;
stack.pop_back();
return true;
}
else
{
const ConservativeAdvancementStackData& data = stack.back();
FCL_REAL d = data.d;
if(d > c)
stack[stack.size() - 2] = stack[stack.size() - 1];
MeshConservativeAdvancementTraversalNodeOBBRSS::MeshConservativeAdvancementTraversalNodeOBBRSS(FCL_REAL w_)
: MeshConservativeAdvancementTraversalNode<OBBRSS>(w_)
{
R.setIdentity();
}
stack.pop_back();
FCL_REAL MeshConservativeAdvancementTraversalNodeOBBRSS::BVTesting(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
Vec3f P1, P2;
FCL_REAL d = distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv, &P1, &P2);
return false;
}
stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
return d;
}
void MeshConservativeAdvancementTraversalNodeOBBRSS::leafTesting(int b1, int b2) const
{
details::meshConservativeAdvancementOrientedNodeLeafTesting(b1, b2,
model1, model2,
tri_indices1, tri_indices2,
vertices1, vertices2,
R, T,
motion1, motion2,
enable_statistics,
min_distance,
p1, p2,
last_tri_id1, last_tri_id2,
delta_t,
num_leaf_tests);
}
bool MeshConservativeAdvancementTraversalNodeOBBRSS::canStop(FCL_REAL c) const
{
return details::meshConservativeAdvancementOrientedNodeCanStop(c,
min_distance,
abs_err, rel_err, w,
model1, model2,
motion1, motion2,
stack,
delta_t);
}
......
src/traversal/traversal_node_setup.cpp
View file @ 75fccd00
......@@ -178,5 +178,56 @@ bool initialize(MeshDistanceTraversalNodeOBBRSS& node,
return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, request, result);
}
namespace details
{
template<typename BV, typename OrientedDistanceNode>
static inline bool setupMeshConservativeAdvancementOrientedDistanceNode(OrientedDistanceNode& node,
const BVHModel<BV>& model1, const Transform3f& tf1,
const BVHModel<BV>& model2, const Transform3f& tf2,
FCL_REAL w)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
node.model1 = &model1;
node.model2 = &model2;
node.vertices1 = model1.vertices;
node.vertices2 = model2.vertices;
node.tri_indices1 = model1.tri_indices;
node.tri_indices2 = model2.tri_indices;
node.w = w;
relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
return true;
}
}
bool initialize(MeshConservativeAdvancementTraversalNodeRSS& node,
const BVHModel<RSS>& model1, const Transform3f& tf1,
const BVHModel<RSS>& model2, const Transform3f& tf2,
FCL_REAL w)
{
return details::setupMeshConservativeAdvancementOrientedDistanceNode(node, model1, tf1, model2, tf2, w);
}
bool initialize(MeshConservativeAdvancementTraversalNodeOBBRSS& node,
const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
FCL_REAL w)
{
return details::setupMeshConservativeAdvancementOrientedDistanceNode(node, model1, tf1, model2, tf2, w);
}
}
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