Commit 6003226c authored by jpan's avatar jpan
Browse files

fix some bugs in mesh-shape, refactoring more.


git-svn-id: https://kforge.ros.org/fcl/fcl_ros@31 253336fb-580f-4252-a368-f3cef5a2a82b
parent 035bab78
......@@ -46,5 +46,8 @@ target_link_libraries(test_core_collision_point fcl)
rosbuild_add_gtest(test_core_geometric_shapes test/test_core_geometric_shapes.cpp)
target_link_libraries(test_core_geometric_shapes fcl)
rosbuild_add_gtest(test_core_collision_shape_mesh_consistency test/test_core_collision_shape_mesh_consistency.cpp)
target_link_libraries(test_core_collision_shape_mesh_consistency fcl)
rosbuild_add_gtest(test_core_broad_phase test/test_core_broad_phase.cpp)
target_link_libraries(test_core_broad_phase fcl)
\ No newline at end of file
......@@ -114,6 +114,16 @@ public:
t.setTransform(q, T);
}
bool isIdentityTransform() const
{
return t.isIdentity();
}
void setIdentityTransform()
{
t.setIdentity();
}
protected:
/** AABB in global coordinate */
......
......@@ -66,7 +66,7 @@ void generateBVHModel(BVHModel<BV>& model, const Box& shape)
tri_indices[0] = Triangle(0, 4, 1);
tri_indices[1] = Triangle(1, 4, 5);
tri_indices[2] = Triangle(2, 5, 3);
tri_indices[2] = Triangle(2, 6, 3);
tri_indices[3] = Triangle(3, 6, 7);
tri_indices[4] = Triangle(3, 0, 2);
tri_indices[5] = Triangle(2, 0, 1);
......@@ -201,7 +201,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, unsigned int t
for(unsigned int i = 0; i < tot; ++i)
{
Triangle tmp((h_num + 1) * tot + 1, h_num * tot + i, h_num * tot + ((i == tot - 1) ? 0 : (i + 1)));
Triangle tmp((h_num + 1) * tot + 1, h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i);
tri_indices.push_back(tmp);
}
......@@ -221,6 +221,88 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, unsigned int t
}
}
for(unsigned int i = 0; i < points.size(); ++i)
{
Vec3f v = matMulVec(shape.getLocalRotation(), points[i]) + shape.getLocalTranslation();
v = matMulVec(shape.getRotation(), v) + shape.getTranslation();
points[i] = v;
}
model.beginModel();
model.addSubModel(points, tri_indices);
model.endModel();
model.computeLocalAABB();
}
/** \brief Generate BVH model from cone */
template<typename BV>
void generateBVHModel(BVHModel<BV>& model, const Cone& shape, unsigned int tot = 16)
{
std::vector<Vec3f> points;
std::vector<Triangle> tri_indices;
double r = shape.radius;
double h = shape.lz;
double phi, phid;
const double pi = boost::math::constants::pi<double>();
phid = pi * 2 / tot;
phi = 0;
double circle_edge = phid * r;
unsigned int h_num = ceil(h / circle_edge);
double hd = h / h_num;
for(unsigned int i = 0; i < h_num - 1; ++i)
{
for(unsigned int j = 0; j < tot; ++j)
{
points.push_back(Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j), h / 2 - (i + 1) * hd));
}
}
for(unsigned int i = 0; i < tot; ++i)
points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2));
points.push_back(Vec3f(0, 0, h / 2));
points.push_back(Vec3f(0, 0, -h / 2));
for(unsigned int i = 0; i < tot; ++i)
{
Triangle tmp(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1));
tri_indices.push_back(tmp);
}
for(unsigned int i = 0; i < tot; ++i)
{
Triangle tmp(h_num * tot + 1, (h_num - 1) * tot + (i == tot - 1) ? 0 : (i + 1), (h_num - 1) * tot + i);
tri_indices.push_back(tmp);
}
for(unsigned int i = 0; i < h_num - 1; ++i)
{
for(unsigned int j = 0; j < tot; ++j)
{
int a, b, c, d;
a = j;
b = (j == tot - 1) ? 0 : (j + 1);
c = j + tot;
d = (j == tot - 1) ? tot : (j + 1 + tot);
int start = i * tot;
tri_indices.push_back(Triangle(start + b, start + a, start + c));
tri_indices.push_back(Triangle(start + b, start + c, start + d));
}
}
for(unsigned int i = 0; i < points.size(); ++i)
{
Vec3f v = matMulVec(shape.getLocalRotation(), points[i]) + shape.getLocalTranslation();
v = matMulVec(shape.getRotation(), v) + shape.getTranslation();
points[i] = v;
}
model.beginModel();
model.addSubModel(points, tri_indices);
model.endModel();
......
This diff is collapsed.
......@@ -205,6 +205,21 @@ public:
return q.transform(v) + T;
}
bool isIdentity() const
{
return (R[0][0] == 1) && (R[0][1] == 0) && (R[0][2] == 0) && (R[1][0] == 0) && (R[1][1] == 1) && (R[1][2] == 1) && (R[2][0] == 0) && (R[2][1] == 0) && (R[2][2] == 1)
&& (T[0] == 0) && (T[1] == 0) && (T[2] == 0);
}
void setIdentity()
{
R[0] = Vec3f(1, 0, 0);
R[1] = Vec3f(0, 1, 0);
R[2] = Vec3f(0, 0, 1);
T = Vec3f();
q = SimpleQuaternion();
}
};
......
......@@ -207,18 +207,18 @@ public:
Vec3f normal;
Vec3f contactp;
if(enable_contact) // only interested in collision or not
if(!enable_contact) // only interested in collision or not
{
if(shapeTriangleIntersect(this->model2, p1, p2, p3))
if(shapeTriangleIntersect(*(this->model2), p1, p2, p3))
{
pairs.push_back(BVHShapeCollisionPair(primitive_id));
}
else
}
else
{
if(shapeTriangleIntersect(*(this->model2), p1, p2, p3, &contactp, &penetration, &normal))
{
if(shapeTriangleIntersect(this->model2, p1, p2, p3, &contactp, &penetration, &normal))
{
pairs.push_back(BVHShapeCollisionPair(primitive_id, normal, contactp, penetration));
}
pairs.push_back(BVHShapeCollisionPair(primitive_id, normal, contactp, penetration));
}
}
}
......@@ -274,18 +274,18 @@ public:
Vec3f normal;
Vec3f contactp;
if(this->enable_contact == 0) // only interested in collision or not
if(!this->enable_contact) // only interested in collision or not
{
if(shapeTriangleIntersect(this->model2, p1, p2, p3, R, T))
if(shapeTriangleIntersect(*(this->model2), p1, p2, p3, R, T))
{
this->pairs.push_back(BVHShapeCollisionPair(primitive_id));
}
else
}
else
{
if(shapeTriangleIntersect(*(this->model2), p1, p2, p3, R, T, &contactp, &penetration, &normal))
{
if(shapeTriangleIntersect(this->model2, p1, p2, p3, R, T, &contactp, &penetration, &normal))
{
this->pairs.push_back(BVHShapeCollisionPair(primitive_id, normal, contactp, penetration));
}
this->pairs.push_back(BVHShapeCollisionPair(primitive_id, normal, contactp, penetration));
}
}
}
......
......@@ -54,6 +54,7 @@ public:
model2 = NULL;
enable_contact = false;
is_collision = false;
}
bool BVTesting(int, int) const
......
......@@ -51,7 +51,7 @@ namespace fcl
/** \brief Hey, I know it is ugly... but it is the best way I can find now... */
#define SHAPESHAPE_COMMON_CODE() do{ \
initialize(node, *obj1, *obj2, obj1->getRotation(), obj1->getTranslation(), obj2->getRotation(), obj2->getTranslation(), enable_contact); \
initialize(node, *obj1, *obj2, enable_contact); \
collide(&node); \
if(!node.is_collision) return 0; \
contacts.resize(1); \
......@@ -62,7 +62,7 @@ namespace fcl
#define MESHSHAPE_COMMON_CODE() do{ \
initialize(node, *obj1_tmp, *obj2_tmp, obj1_tmp->getRotation(), obj1_tmp->getTranslation(), obj2_tmp->getRotation(), obj2_tmp->getTranslation(), num_max_contacts, exhaustive, enable_contact); \
initialize(node, *obj1_tmp, *obj2_tmp, num_max_contacts, exhaustive, enable_contact); \
collide(&node); \
int num_contacts = node.pairs.size(); \
if(num_contacts > 0) \
......@@ -88,7 +88,7 @@ namespace fcl
} while(0)
#define MESHSHAPEOBBRSS_COMMON_CODE() do{ \
initialize(node, *obj1, *obj2_tmp, obj1->getRotation(), obj1->getTranslation(), obj2_tmp->getRotation(), obj2_tmp->getTranslation(), num_max_contacts, exhaustive, enable_contact); \
initialize(node, *obj1, *obj2_tmp, num_max_contacts, exhaustive, enable_contact); \
collide(&node); \
int num_contacts = node.pairs.size(); \
if(num_contacts > 0) \
......@@ -114,7 +114,7 @@ namespace fcl
#define MESHMESH_COMMON_CODE() do{ \
initialize(node, *obj1_tmp, *obj2_tmp, obj1_tmp->getRotation(), obj1_tmp->getTranslation(), obj2_tmp->getRotation(), obj2_tmp->getTranslation(), num_max_contacts, exhaustive, enable_contact); \
initialize(node, *obj1_tmp, *obj2_tmp, num_max_contacts, exhaustive, enable_contact); \
collide(&node); \
int num_contacts = node.pairs.size(); \
if(num_contacts > 0) \
......@@ -141,7 +141,7 @@ namespace fcl
#define MESHMESHOBBRSS_COMMON_CODE() do{ \
initialize(node, *obj1, *obj2, obj1->getRotation(), obj1->getTranslation(), obj2->getRotation(), obj2->getTranslation(), num_max_contacts, exhaustive, enable_contact); \
initialize(node, *obj1, *obj2, num_max_contacts, exhaustive, enable_contact); \
collide(&node); \
int num_contacts = node.pairs.size(); \
if(num_contacts > 0) \
......
......@@ -1659,7 +1659,7 @@ BVH_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int s
else
{
Vec3f rotatedSigma[3];
SMST(uc2[i - nPositiveExamples].Sigma, R, rotatedSigma);
tensorTransform(uc2[i - nPositiveExamples].Sigma, R, rotatedSigma);
double sigma = matMulVec(rotatedSigma, fgrad).dot(fgrad);
BVH_REAL col_prob = gaussianCDF(f / sqrt(sigma));
if(max_collision_prob < col_prob)
......@@ -1743,7 +1743,7 @@ BVH_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc
// compute the projected uncertainty by P * S * P'
const Vec3f* S = uc1[i].Sigma;
Vec3f newS[3];
SMST(S, P, newS);
tensorTransform(S, P, newS);
// check whether the point is inside or outside the triangle
......@@ -1751,9 +1751,9 @@ BVH_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc
if(b_inside)
{
BVH_REAL prob1 = gaussianCDF((projected_p.dot(edge_n[0]) - edge_t[0]) / sqrt(vTMv(newS, edge_n[0])));
BVH_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[1]) - edge_t[1]) / sqrt(vTMv(newS, edge_n[1])));
BVH_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[2]) - edge_t[2]) / sqrt(vTMv(newS, edge_n[2])));
BVH_REAL prob1 = gaussianCDF((projected_p.dot(edge_n[0]) - edge_t[0]) / sqrt(quadraticForm(newS, edge_n[0])));
BVH_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[1]) - edge_t[1]) / sqrt(quadraticForm(newS, edge_n[1])));
BVH_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[2]) - edge_t[2]) / sqrt(quadraticForm(newS, edge_n[2])));
BVH_REAL prob = 1.0 - prob1 - prob2 - prob3;
if(prob > max_prob) max_prob = prob;
}
......@@ -1772,12 +1772,12 @@ BVH_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc
if(pos_plane.size() == 1)
{
int pos_id = pos_plane[0];
BVH_REAL prob1 = gaussianCDF(-(projected_p.dot(edge_n[pos_id]) - edge_t[pos_id]) / sqrt(vTMv(newS, edge_n[pos_id])));
BVH_REAL prob1 = gaussianCDF(-(projected_p.dot(edge_n[pos_id]) - edge_t[pos_id]) / sqrt(quadraticForm(newS, edge_n[pos_id])));
int neg_id1 = neg_plane[0];
int neg_id2 = neg_plane[1];
BVH_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[neg_id1]) - edge_t[neg_id1]) / sqrt(vTMv(newS, edge_n[neg_id2])));
BVH_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[neg_id2]) - edge_t[neg_id2]) / sqrt(vTMv(newS, edge_n[neg_id2])));
BVH_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[neg_id1]) - edge_t[neg_id1]) / sqrt(quadraticForm(newS, edge_n[neg_id2])));
BVH_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[neg_id2]) - edge_t[neg_id2]) / sqrt(quadraticForm(newS, edge_n[neg_id2])));
BVH_REAL prob = prob1 - prob2 - prob3;
if(prob > max_prob) max_prob = prob;
......@@ -1786,13 +1786,13 @@ BVH_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc
else if(pos_plane.size() == 2)
{
int neg_id = neg_plane[0];
BVH_REAL prob1 = gaussianCDF(-(projected_p.dot(edge_n[neg_id]) - edge_t[neg_id]) / sqrt(vTMv(newS, edge_n[neg_id])));
BVH_REAL prob1 = gaussianCDF(-(projected_p.dot(edge_n[neg_id]) - edge_t[neg_id]) / sqrt(quadraticForm(newS, edge_n[neg_id])));
int pos_id1 = pos_plane[0];
int pos_id2 = pos_plane[1];
BVH_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[pos_id1])) / sqrt(vTMv(newS, edge_n[pos_id1])));
BVH_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[pos_id2])) / sqrt(vTMv(newS, edge_n[pos_id2])));
BVH_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[pos_id1])) / sqrt(quadraticForm(newS, edge_n[pos_id1])));
BVH_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[pos_id2])) / sqrt(quadraticForm(newS, edge_n[pos_id2])));
BVH_REAL prob = prob1 - prob2 - prob3;
if(prob > max_prob) max_prob = prob;
......
......@@ -41,7 +41,7 @@ namespace fcl
{
bool initialize(MeshCollisionTraversalNodeOBB& node, const BVHModel<OBB>& model1, const BVHModel<OBB>& model2,
const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2, int num_max_contacts, bool exhaustive, bool enable_contact)
int num_max_contacts, bool exhaustive, bool enable_contact)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
......@@ -59,14 +59,14 @@ bool initialize(MeshCollisionTraversalNodeOBB& node, const BVHModel<OBB>& model1
node.exhaustive = exhaustive;
node.enable_contact = enable_contact;
relativeTransform(R1, T1, R2, T2, node.R, node.T);
relativeTransform(model1.getRotation(), model1.getTranslation(), model2.getRotation(), model2.getTranslation(), node.R, node.T);
return true;
}
bool initialize(MeshCollisionTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2, int num_max_contacts, bool exhaustive, bool enable_contact)
int num_max_contacts, bool exhaustive, bool enable_contact)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
......@@ -84,7 +84,7 @@ bool initialize(MeshCollisionTraversalNodeRSS& node, const BVHModel<RSS>& model1
node.exhaustive = exhaustive;
node.enable_contact = enable_contact;
relativeTransform(R1, T1, R2, T2, node.R, node.T);
relativeTransform(model1.getRotation(), model1.getTranslation(), model2.getRotation(), model2.getTranslation(), node.R, node.T);
return true;
}
......@@ -92,7 +92,6 @@ bool initialize(MeshCollisionTraversalNodeRSS& node, const BVHModel<RSS>& model1
#if USE_SVMLIGHT
bool initialize(PointCloudCollisionTraversalNodeOBB& node, BVHModel<OBB>& model1, BVHModel<OBB>& model2,
const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
BVH_REAL collision_prob_threshold,
int leaf_size_threshold,
int num_max_contacts,
......@@ -125,14 +124,13 @@ bool initialize(PointCloudCollisionTraversalNodeOBB& node, BVHModel<OBB>& model1
node.collision_prob_threshold = collision_prob_threshold;
node.leaf_size_threshold = leaf_size_threshold;
relativeTransform(R1, T1, R2, T2, node.R, node.T);
relativeTransform(model1.getRotation(), model1.getTranslation(), model2.getRotation(), model2.getTranslation(), node.R, node.T);
return true;
}
bool initialize(PointCloudCollisionTraversalNodeRSS& node, BVHModel<RSS>& model1, BVHModel<RSS>& model2,
const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
BVH_REAL collision_prob_threshold,
int leaf_size_threshold,
int num_max_contacts,
......@@ -165,13 +163,12 @@ bool initialize(PointCloudCollisionTraversalNodeRSS& node, BVHModel<RSS>& model1
node.collision_prob_threshold = collision_prob_threshold;
node.leaf_size_threshold = leaf_size_threshold;
relativeTransform(R1, T1, R2, T2, node.R, node.T);
relativeTransform(model1.getRotation(), model1.getTranslation(), model2.getRotation(), model2.getTranslation(), node.R, node.T);
return true;
}
bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node, BVHModel<OBB>& model1, const BVHModel<OBB>& model2,
const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
BVH_REAL collision_prob_threshold,
int leaf_size_threshold,
int num_max_contacts,
......@@ -201,14 +198,13 @@ bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node, BVHModel<OBB>& mo
node.collision_prob_threshold = collision_prob_threshold;
node.leaf_size_threshold = leaf_size_threshold;
relativeTransform(R1, T1, R2, T2, node.R, node.T);
relativeTransform(model1.getRotation(), model1.getTranslation(), model2.getRotation(), model2.getTranslation(), node.R, node.T);
return true;
}
bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node, BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
BVH_REAL collision_prob_threshold,
int leaf_size_threshold,
int num_max_contacts,
......@@ -238,15 +234,14 @@ bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node, BVHModel<RSS>& mo
node.collision_prob_threshold = collision_prob_threshold;
node.leaf_size_threshold = leaf_size_threshold;
relativeTransform(R1, T1, R2, T2, node.R, node.T);
relativeTransform(model1.getRotation(), model1.getTranslation(), model2.getRotation(), model2.getTranslation(), node.R, node.T);
return true;
}
#endif
bool initialize(MeshDistanceTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2)
bool initialize(MeshDistanceTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
......@@ -260,7 +255,7 @@ bool initialize(MeshDistanceTraversalNodeRSS& node, const BVHModel<RSS>& model1,
node.tri_indices1 = model1.tri_indices;
node.tri_indices2 = model2.tri_indices;
relativeTransform(R1, T1, R2, T2, node.R, node.T);
relativeTransform(model1.getRotation(), model1.getTranslation(), model2.getRotation(), model2.getTranslation(), node.R, node.T);
return true;
}
......
......@@ -637,9 +637,12 @@ bool collide_Test(const Transform& tf,
R2[2] = Vec3f(0, 0, 1);
Vec3f T2;
m1.setTransform(tf.R, tf.T);
m2.setTransform(R2, T2);
MeshCollisionTraversalNode<BV> node;
if(!initialize<BV>(node, m1, m2, tf.R, tf.T, R2, T2))
if(!initialize<BV>(node, m1, m2))
std::cout << "initialize error" << std::endl;
node.enable_statistics = verbose;
......@@ -707,8 +710,11 @@ bool collide_Test_OBB(const Transform& tf,
R2[2] = Vec3f(0, 0, 1);
Vec3f T2;
m1.setTransform(tf.R, tf.T);
m2.setTransform(R2, T2);
MeshCollisionTraversalNodeOBB node;
if(!initialize(node, (const BVHModel<OBB>&)m1, (const BVHModel<OBB>&)m2, tf.R, tf.T, R2, T2))
if(!initialize(node, (const BVHModel<OBB>&)m1, (const BVHModel<OBB>&)m2))
std::cout << "initialize error" << std::endl;
node.enable_statistics = verbose;
......@@ -776,8 +782,11 @@ bool collide_Test_RSS(const Transform& tf,
R2[2] = Vec3f(0, 0, 1);
Vec3f T2;
m1.setTransform(tf.R, tf.T);
m2.setTransform(R2, T2);
MeshCollisionTraversalNodeRSS node;
if(!initialize(node, (const BVHModel<RSS>&)m1, (const BVHModel<RSS>&)m2, tf.R, tf.T, R2, T2))
if(!initialize(node, (const BVHModel<RSS>&)m1, (const BVHModel<RSS>&)m2))
std::cout << "initialize error" << std::endl;
node.enable_statistics = verbose;
......
......@@ -167,9 +167,12 @@ bool collide_Test_PP(const Transform& tf,
R2[2] = Vec3f(0, 0, 1);
Vec3f T2;
m1.setTransform(tf.R, tf.T);
m2.setTransform(R2, T2);
PointCloudCollisionTraversalNode<BV> node;
if(!initialize<BV, false, false>(node, m1, m2, tf.R, tf.T, R2, T2, 0.6, 20))
if(!initialize<BV, false, false>(node, m1, m2, 0.6, 20))
std::cout << "initialize error" << std::endl;
node.enable_statistics = verbose;
......@@ -229,9 +232,12 @@ bool collide_Test_PP_OBB(const Transform& tf,
R2[2] = Vec3f(0, 0, 1);
Vec3f T2;
m1.setTransform(tf.R, tf.T);
m2.setTransform(R2, T2);
PointCloudCollisionTraversalNodeOBB node;
if(!initialize(node, m1, m2, tf.R, tf.T, R2, T2, 0.6, 20))
if(!initialize(node, m1, m2, 0.6, 20))
std::cout << "initialize error" << std::endl;
node.enable_statistics = verbose;
......@@ -295,9 +301,12 @@ bool collide_Test_MP(const Transform& tf,
R2[2] = Vec3f(0, 0, 1);
Vec3f T2;
m1.setTransform(tf.R, tf.T);
m2.setTransform(R2, T2);
PointCloudMeshCollisionTraversalNode<BV> node;
if(!initialize<BV, false, false>(node, m2, m1, R2, T2, tf.R, tf.T, 0.6, 20))
if(!initialize<BV, false, false>(node, m2, m1, 0.6, 20))
std::cout << "initialize error" << std::endl;
node.enable_statistics = verbose;
......@@ -360,9 +369,12 @@ bool collide_Test_MP_OBB(const Transform& tf,
R2[2] = Vec3f(0, 0, 1);
Vec3f T2;
m1.setTransform(tf.R, tf.T);
m2.setTransform(R2, T2);
PointCloudMeshCollisionTraversalNodeOBB node;
if(!initialize(node, m2, m1, R2, T2, tf.R, tf.T, 0.6, 20))
if(!initialize(node, m2, m1, 0.6, 20))
std::cout << "initialize error" << std::endl;
node.enable_statistics = verbose;
......@@ -426,9 +438,12 @@ bool collide_Test_PM(const Transform& tf,
R2[2] = Vec3f(0, 0, 1);
Vec3f T2;
m1.setTransform(tf.R, tf.T);
m2.setTransform(R2, T2);
PointCloudMeshCollisionTraversalNode<BV> node;
if(!initialize<BV, false, false>(node, m1, m2, tf.R, tf.T, R2, T2, 0.6, 20))
if(!initialize<BV, false, false>(node, m1, m2, 0.6, 20))
std::cout << "initialize error" << std::endl;
node.enable_statistics = verbose;
......@@ -490,9 +505,12 @@ bool collide_Test_PM_OBB(const Transform& tf,
R2[2] = Vec3f(0, 0, 1);
Vec3f T2;
m1.setTransform(tf.R, tf.T);
m2.setTransform(R2, T2);
PointCloudMeshCollisionTraversalNodeOBB node;
if(!initialize(node, m1, m2, tf.R, tf.T, R2, T2, 0.6, 20))
if(!initialize(node, m1, m2, 0.6, 20))
std::cout << "initialize error" << std::endl;
node.enable_statistics = verbose;
......
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* All rights reserved.
*