TEST(BBoxTest, mergeWithBBox) {
BBox3f bounds1(Vec3f(-12.0f, -3.0f, 4.0f), Vec3f(7.0f, 8.0f, 9.0f));
const BBox3f bounds2(Vec3f(-10.0f, -5.0f, 3.0f), Vec3f(9.0f, 9.0f, 5.0f));
const BBox3f merged( Vec3f(-12.0f, -5.0f, 3.0f), Vec3f(9.0f, 9.0f, 9.0f));
ASSERT_EQ(merged, bounds1.mergeWith(bounds2));
}
void ompl::control::OpenDEStateSpace::setDefaultBounds()
{
// limit all velocities to 1 m/s, 1 rad/s, respectively
base::RealVectorBounds bounds1(3);
bounds1.setLow(-1);
bounds1.setHigh(1);
setLinearVelocityBounds(bounds1);
setAngularVelocityBounds(bounds1);
// find the bounding box that contains all geoms included in the collision spaces
double mX, mY, mZ, MX, MY, MZ;
mX = mY = mZ = std::numeric_limits<double>::infinity();
MX = MY = MZ = -std::numeric_limits<double>::infinity();
bool found = false;
std::queue<dSpaceID> spaces;
for (auto &collisionSpace : env_->collisionSpaces_)
spaces.push(collisionSpace);
while (!spaces.empty())
{
dSpaceID space = spaces.front();
spaces.pop();
int n = dSpaceGetNumGeoms(space);
for (int j = 0; j < n; ++j)
{
dGeomID geom = dSpaceGetGeom(space, j);
if (dGeomIsSpace(geom) != 0)
spaces.push((dSpaceID)geom);
else
{
bool valid = true;
dReal aabb[6];
dGeomGetAABB(geom, aabb);
// things like planes are infinite; we want to ignore those
for (double k : aabb)
if (fabs(k) >= std::numeric_limits<dReal>::max())
{
valid = false;
break;
}
if (valid)
{
found = true;
if (aabb[0] < mX)
mX = aabb[0];
if (aabb[1] > MX)
MX = aabb[1];
if (aabb[2] < mY)
mY = aabb[2];
if (aabb[3] > MY)
MY = aabb[3];
if (aabb[4] < mZ)
mZ = aabb[4];
if (aabb[5] > MZ)
MZ = aabb[5];
}
}
}
}
if (found)
{
double dx = MX - mX;
double dy = MY - mY;
double dz = MZ - mZ;
double dM = std::max(dx, std::max(dy, dz));
// add 10% in each dimension + 1% of the max dimension
dx = dx / 10.0 + dM / 100.0;
dy = dy / 10.0 + dM / 100.0;
dz = dz / 10.0 + dM / 100.0;
bounds1.low[0] = mX - dx;
bounds1.high[0] = MX + dx;
bounds1.low[1] = mY - dy;
bounds1.high[1] = MY + dy;
bounds1.low[2] = mZ - dz;
bounds1.high[2] = MZ + dz;
setVolumeBounds(bounds1);
}
}