Skip to content

Commit

Permalink
Pass test_fcl_collision
Browse files Browse the repository at this point in the history
  • Loading branch information
@jslee02
jslee02 committed Jul 27, 2016
1 parent 074bca3 commit 62eaf25
Show file tree
Hide file tree
Showing 8 changed files with 47 additions and 43 deletions.
2 changes: 1 addition & 1 deletion include/fcl/BV/BV.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -188,7 +188,7 @@ class Converter<BV1, OBB>
static void convert(const BV1& bv1, const Transform3f& tf1, OBB& bv2)
{
AABB bv;
Converter<BV1, AABB>::convert(bv1, Transform3f(), bv);
Converter<BV1, AABB>::convert(bv1, Transform3f::Identity(), bv);
Converter<AABB, OBB>::convert(bv, tf1, bv2);
}
};
Expand Down
10 changes: 5 additions & 5 deletions include/fcl/shape/geometric_shapes.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -317,7 +317,7 @@ class Convex : public ShapeBase
polygons = polygons_;
edges = NULL;

Vec3f sum;
Vec3f sum = Vec3f::Zero();
for(int i = 0; i < num_points; ++i)
{
sum += points[i];
Expand Down Expand Up @@ -389,7 +389,7 @@ class Convex : public ShapeBase
int* index = polygons + 1;
for(int i = 0; i < num_planes; ++i)
{
Vec3f plane_center;
Vec3f plane_center = Vec3f::Zero();

// compute the center of the polygon
for(int j = 0; j < *points_in_poly; ++j)
Expand Down Expand Up @@ -428,13 +428,13 @@ class Convex : public ShapeBase

Vec3f computeCOM() const
{
Vec3f com;
Vec3f com = Vec3f::Zero();
FCL_REAL vol = 0;
int* points_in_poly = polygons;
int* index = polygons + 1;
for(int i = 0; i < num_planes; ++i)
{
Vec3f plane_center;
Vec3f plane_center = Vec3f::Zero();

// compute the center of the polygon
for(int j = 0; j < *points_in_poly; ++j)
Expand Down Expand Up @@ -468,7 +468,7 @@ class Convex : public ShapeBase
int* index = polygons + 1;
for(int i = 0; i < num_planes; ++i)
{
Vec3f plane_center;
Vec3f plane_center = Vec3f::Zero();

// compute the center of the polygon
for(int j = 0; j < *points_in_poly; ++j)
Expand Down
4 changes: 2 additions & 2 deletions include/fcl/traversal/traversal_node_octree.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -175,7 +175,7 @@ class OcTreeSolver
cresult = &result_;

AABB bv2;
computeBV<AABB>(s, Transform3f(), bv2);
computeBV<AABB>(s, Transform3f::Identity(), bv2);
OBB obb2;
convertBV(bv2, tf2, obb2);
OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(),
Expand All @@ -195,7 +195,7 @@ class OcTreeSolver
cresult = &result_;

AABB bv1;
computeBV<AABB>(s, Transform3f(), bv1);
computeBV<AABB>(s, Transform3f::Identity(), bv1);
OBB obb1;
convertBV(bv1, tf1, obb1);
OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(),
Expand Down
16 changes: 8 additions & 8 deletions include/fcl/traversal/traversal_node_setup.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1114,8 +1114,8 @@ bool initialize(ShapeConservativeAdvancementTraversalNode<S1, S2, NarrowPhaseSol
node.tf2 = tf2;
node.nsolver = nsolver;

computeBV<RSS, S1>(shape1, Transform3f(), node.model1_bv);
computeBV<RSS, S2>(shape2, Transform3f(), node.model2_bv);
computeBV<RSS, S1>(shape1, Transform3f::Identity(), node.model1_bv);
computeBV<RSS, S2>(shape2, Transform3f::Identity(), node.model2_bv);

return true;
}
Expand Down Expand Up @@ -1152,7 +1152,7 @@ bool initialize(MeshShapeConservativeAdvancementTraversalNode<BV, S, NarrowPhase
node.nsolver = nsolver;
node.w = w;

computeBV<BV, S>(model2, Transform3f(), node.model2_bv);
computeBV<BV, S>(model2, Transform3f::Identity(), node.model2_bv);

return true;
}
Expand All @@ -1173,7 +1173,7 @@ bool initialize(MeshShapeConservativeAdvancementTraversalNodeRSS<S, NarrowPhaseS

node.w = w;

computeBV<RSS, S>(model2, Transform3f(), node.model2_bv);
computeBV<RSS, S>(model2, Transform3f::Identity(), node.model2_bv);

return true;
}
Expand All @@ -1194,7 +1194,7 @@ bool initialize(MeshShapeConservativeAdvancementTraversalNodeOBBRSS<S, NarrowPha

node.w = w;

computeBV<OBBRSS, S>(model2, Transform3f(), node.model2_bv);
computeBV<OBBRSS, S>(model2, Transform3f::Identity(), node.model2_bv);

return true;
}
Expand Down Expand Up @@ -1232,7 +1232,7 @@ bool initialize(ShapeMeshConservativeAdvancementTraversalNode<S, BV, NarrowPhase
node.nsolver = nsolver;
node.w = w;

computeBV<BV, S>(model1, Transform3f(), node.model1_bv);
computeBV<BV, S>(model1, Transform3f::Identity(), node.model1_bv);

return true;
}
Expand All @@ -1253,7 +1253,7 @@ bool initialize(ShapeMeshConservativeAdvancementTraversalNodeRSS<S, NarrowPhaseS

node.w = w;

computeBV<RSS, S>(model1, Transform3f(), node.model1_bv);
computeBV<RSS, S>(model1, Transform3f::Identity(), node.model1_bv);

return true;
}
Expand All @@ -1274,7 +1274,7 @@ bool initialize(ShapeMeshConservativeAdvancementTraversalNodeOBBRSS<S, NarrowPha

node.w = w;

computeBV<OBBRSS, S>(model1, Transform3f(), node.model1_bv);
computeBV<OBBRSS, S>(model1, Transform3f::Identity(), node.model1_bv);

return true;
}
Expand Down
6 changes: 3 additions & 3 deletions src/broadphase/broadphase_dynamic_AABB_tree.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
if(!obj1->isFree())
{
OBB obb1, obb2;
convertBV(root1->bv, Transform3f(), obb1);
convertBV(root1->bv, Transform3f::Identity(), obb1);
convertBV(root2_bv, tf2, obb2);

if(obb1.overlap(obb2))
Expand Down Expand Up @@ -96,7 +96,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
if(!tree2->isNodeFree(root2) && !obj1->isFree())
{
OBB obb1, obb2;
convertBV(root1->bv, Transform3f(), obb1);
convertBV(root1->bv, Transform3f::Identity(), obb1);
convertBV(root2_bv, tf2, obb2);

if(obb1.overlap(obb2))
Expand All @@ -117,7 +117,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
}

OBB obb1, obb2;
convertBV(root1->bv, Transform3f(), obb1);
convertBV(root1->bv, Transform3f::Identity(), obb1);
convertBV(root2_bv, tf2, obb2);

if(tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false;
Expand Down
6 changes: 3 additions & 3 deletions src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* n
if(!obj1->isFree())
{
OBB obb1, obb2;
convertBV(root1->bv, Transform3f(), obb1);
convertBV(root1->bv, Transform3f::Identity(), obb1);
convertBV(root2_bv, tf2, obb2);

if(obb1.overlap(obb2))
Expand Down Expand Up @@ -95,7 +95,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* n
if(!tree2->isNodeFree(root2) && !obj1->isFree())
{
OBB obb1, obb2;
convertBV(root1->bv, Transform3f(), obb1);
convertBV(root1->bv, Transform3f::Identity(), obb1);
convertBV(root2_bv, tf2, obb2);

if(obb1.overlap(obb2))
Expand All @@ -116,7 +116,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* n
}

OBB obb1, obb2;
convertBV(root1->bv, Transform3f(), obb1);
convertBV(root1->bv, Transform3f::Identity(), obb1);
convertBV(root2_bv, tf2, obb2);

if(tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false;
Expand Down
20 changes: 10 additions & 10 deletions src/shape/geometric_shapes.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -133,70 +133,70 @@ void Plane::unitNormalTest()

void Box::computeLocalAABB()
{
computeBV<AABB>(*this, Transform3f(), aabb_local);
computeBV<AABB>(*this, Transform3f::Identity(), aabb_local);
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).norm();
}

void Sphere::computeLocalAABB()
{
computeBV<AABB>(*this, Transform3f(), aabb_local);
computeBV<AABB>(*this, Transform3f::Identity(), aabb_local);
aabb_center = aabb_local.center();
aabb_radius = radius;
}

void Ellipsoid::computeLocalAABB()
{
computeBV<AABB>(*this, Transform3f(), aabb_local);
computeBV<AABB>(*this, Transform3f::Identity(), aabb_local);
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).norm();
}

void Capsule::computeLocalAABB()
{
computeBV<AABB>(*this, Transform3f(), aabb_local);
computeBV<AABB>(*this, Transform3f::Identity(), aabb_local);
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).norm();
}

void Cone::computeLocalAABB()
{
computeBV<AABB>(*this, Transform3f(), aabb_local);
computeBV<AABB>(*this, Transform3f::Identity(), aabb_local);
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).norm();
}

void Cylinder::computeLocalAABB()
{
computeBV<AABB>(*this, Transform3f(), aabb_local);
computeBV<AABB>(*this, Transform3f::Identity(), aabb_local);
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).norm();
}

void Convex::computeLocalAABB()
{
computeBV<AABB>(*this, Transform3f(), aabb_local);
computeBV<AABB>(*this, Transform3f::Identity(), aabb_local);
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).norm();
}

void Halfspace::computeLocalAABB()
{
computeBV<AABB>(*this, Transform3f(), aabb_local);
computeBV<AABB>(*this, Transform3f::Identity(), aabb_local);
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).norm();
}

void Plane::computeLocalAABB()
{
computeBV<AABB>(*this, Transform3f(), aabb_local);
computeBV<AABB>(*this, Transform3f::Identity(), aabb_local);
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).norm();
}

void TriangleP::computeLocalAABB()
{
computeBV<AABB>(*this, Transform3f(), aabb_local);
computeBV<AABB>(*this, Transform3f::Identity(), aabb_local);
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).norm();
}
Expand Down
26 changes: 15 additions & 11 deletions test/test_fcl_collision.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -208,7 +208,7 @@ GTEST_TEST(FCL_COLLISION, OBB_AABB_test)
aabb1.max_ = Vec3f(600, 600, 600);

OBB obb1;
convertBV(aabb1, Transform3f(), obb1);
convertBV(aabb1, Transform3f::Identity(), obb1);

for(std::size_t i = 0; i < transforms.size(); ++i)
{
Expand All @@ -219,16 +219,16 @@ GTEST_TEST(FCL_COLLISION, OBB_AABB_test)
AABB aabb2 = translate(aabb, transforms[i].translation());

OBB obb2;
convertBV(aabb, Transform3f(transforms[i].translation()), obb2);
convertBV(aabb, Transform3f(Eigen::Translation3d(transforms[i].translation())), obb2);

bool overlap_aabb = aabb1.overlap(aabb2);
bool overlap_obb = obb1.overlap(obb2);
if(overlap_aabb != overlap_obb)
{
std::cout << aabb1.min_ << " " << aabb1.max_ << std::endl;
std::cout << aabb2.min_ << " " << aabb2.max_ << std::endl;
std::cout << obb1.To << " " << obb1.extent << " " << obb1.axis[0] << " " << obb1.axis[1] << " " << obb1.axis[2] << std::endl;
std::cout << obb2.To << " " << obb2.extent << " " << obb2.axis[0] << " " << obb2.axis[1] << " " << obb2.axis[2] << std::endl;
std::cout << aabb1.min_.transpose() << " " << aabb1.max_.transpose() << std::endl;
std::cout << aabb2.min_.transpose() << " " << aabb2.max_.transpose() << std::endl;
std::cout << obb1.To.transpose() << " " << obb1.extent.transpose() << " " << obb1.axis.col(0).transpose() << " " << obb1.axis.col(1).transpose() << " " << obb1.axis.col(2).transpose() << std::endl;
std::cout << obb2.To.transpose() << " " << obb2.extent.transpose() << " " << obb2.axis.col(0).transpose() << " " << obb2.axis.col(1).transpose() << " " << obb2.axis.col(2).transpose() << std::endl;
}

EXPECT_TRUE(overlap_aabb == overlap_obb);
Expand Down Expand Up @@ -822,7 +822,7 @@ bool collide_Test2(const Transform3f& tf,
std::vector<Vec3f> vertices1_new(vertices1.size());
for(unsigned int i = 0; i < vertices1_new.size(); ++i)
{
vertices1_new[i] = tf.transform(vertices1[i]);
vertices1_new[i] = tf * vertices1[i];
}


Expand All @@ -834,7 +834,8 @@ bool collide_Test2(const Transform3f& tf,
m2.addSubModel(vertices2, triangles2);
m2.endModel();

Transform3f pose1, pose2;
Transform3f pose1 = Transform3f::Identity();
Transform3f pose2 = Transform3f::Identity();

CollisionResult local_result;
MeshCollisionTraversalNode<BV> node;
Expand Down Expand Up @@ -893,7 +894,8 @@ bool collide_Test(const Transform3f& tf,
m2.addSubModel(vertices2, triangles2);
m2.endModel();

Transform3f pose1(tf), pose2;
Transform3f pose1(tf);
Transform3f pose2 = Transform3f::Identity();

CollisionResult local_result;
MeshCollisionTraversalNode<BV> node;
Expand Down Expand Up @@ -951,7 +953,8 @@ bool collide_Test_Oriented(const Transform3f& tf,
m2.addSubModel(vertices2, triangles2);
m2.endModel();

Transform3f pose1(tf), pose2;
Transform3f pose1(tf);
Transform3f pose2 = Transform3f::Identity();

CollisionResult local_result;
TraversalNode node;
Expand Down Expand Up @@ -1008,7 +1011,8 @@ bool test_collide_func(const Transform3f& tf,
m2.addSubModel(vertices2, triangles2);
m2.endModel();

Transform3f pose1(tf), pose2;
Transform3f pose1(tf);
Transform3f pose2 = Transform3f::Identity();

std::vector<Contact> contacts;

Expand Down

0 comments on commit 62eaf25

Please sign in to comment.