Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <fcl/shape/geometric_shapes.h>
- #include <fcl/shape/geometric_shapes_utility.h>
- #include <fcl/narrowphase/narrowphase.h>
- #include <iostream>
- #include <fcl/collision.h>
- #include <boost/foreach.hpp>
- using namespace std;
- using namespace fcl;
- using boost::shared_ptr;
- int main(int argc, char** argv) {
- shared_ptr<Box> box0(new Box(1,1,1));
- shared_ptr<Box> box1(new Box(1,1,1));
- // GJKSolver_indep solver;
- GJKSolver_libccd solver;
- Vec3f contact_points;
- FCL_REAL penetration_depth;
- Vec3f normal;
- Transform3f tf0, tf1;
- tf0.setIdentity();
- tf0.setTranslation(Vec3f(.9,0,0));
- tf0.setQuatRotation(Quaternion3f(.6, .8, 0, 0));
- tf1.setIdentity();
- bool res = solver.shapeIntersect(*box0, tf0, *box1, tf1, &contact_points, &penetration_depth, &normal);
- cout << "contact points: " << contact_points << endl;
- cout << "pen depth: " << penetration_depth << endl;
- cout << "normal: " << normal << endl;
- cout << "result: " << res << endl;
- static const int num_max_contacts = std::numeric_limits<int>::max();
- static const bool enable_contact = true;
- fcl::CollisionResult result;
- fcl::CollisionRequest request(num_max_contacts,
- enable_contact);
- CollisionObject co0(box0, tf0);
- CollisionObject co1(box1, tf1);
- fcl::collide(&co0, &co1, request, result);
- vector<Contact> contacts;
- result.getContacts(contacts);
- cout << contacts.size() << " contacts found" << endl;
- BOOST_FOREACH(Contact& contact, contacts) {
- cout << "position: " << contact.pos << endl;
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement