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);
}
}
