int ORCEnvironmentGetBodies(void* env, void** bodies) { EnvironmentBasePtr penv = GetEnvironment(env); std::vector<KinBodyPtr> vbodies; penv->GetBodies(vbodies); if( !bodies ) { return static_cast<int>(vbodies.size()); } for(size_t i = 0; i < vbodies.size(); ++i) { bodies[i] = new InterfaceBasePtr(vbodies[i]); } return vbodies.size(); }
TEST(collision_checker, box_distance) { EnvironmentBasePtr env = RaveCreateEnvironment(); ASSERT_TRUE(env->Load(data_dir() + "/box.xml")); KinBodyPtr box0 = env->GetKinBody("box"); box0->SetName("box0"); ASSERT_TRUE(env->Load(DATA_DIR + string("/box.xml"))); KinBodyPtr box1 = env->GetKinBody("box"); box1->SetName("box1"); vector<KinBodyPtr> bodies; env->GetBodies(bodies); RAVELOG_DEBUG("%i kinbodies in rave env\n", bodies.size()); CollisionCheckerPtr checker = CreateCollisionChecker(env); #define EXPECT_NUM_COLLISIONS(n)\ vector<Collision> collisions;\ checker->AllVsAll(collisions);\ EXPECT_EQ(collisions.size(), n);\ collisions.clear();\ checker->BodyVsAll(*box0, collisions);\ EXPECT_EQ(collisions.size(), n); { checker->SetContactDistance(0); box1->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(.9,0,0))); box0->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(0,0,0))); EXPECT_NUM_COLLISIONS(1); } { checker->SetContactDistance(0); box1->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(1.1,0,0))); box0->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(0,0,0))); EXPECT_NUM_COLLISIONS(0); } { checker->SetContactDistance(.04); box1->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(1.1,0,0))); box0->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(0,0,0))); EXPECT_NUM_COLLISIONS(0); } { checker->SetContactDistance(.1); box1->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(1.09,0,0))); box0->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(0,0,0))); EXPECT_NUM_COLLISIONS(1); } { checker->SetContactDistance(.2); box1->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(1.1,0,0))); box0->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(0,0,0))); EXPECT_NUM_COLLISIONS(1); } { env->Remove(box1); EXPECT_NUM_COLLISIONS(0); } }