void BeliefCollisionChecker::CheckShapeSigmaHullCast(btCollisionShape* shape, const vector<btTransform>& tf0i, const vector<btTransform>& tf1i,
CollisionObjectWrapper* cow, btCollisionWorld* world, vector<BeliefCollision>& collisions) {
if (btConvexShape* convex = dynamic_cast<btConvexShape*>(shape)) {
vector<btTransform> t0i(tf0i.size());
vector<btTransform> t1i(tf1i.size());
// transform all the points with respect to the first transform
for (int i=0; i<tf0i.size(); i++) t0i[i] = tf0i[0].inverseTimes(tf0i[i]);
for (int i=0; i<tf1i.size(); i++) t1i[i] = tf0i[0].inverseTimes(tf1i[i]);
SigmaHullCastShape* shape = new SigmaHullCastShape(convex, t0i, t1i);
CollisionObjectWrapper* obj = new CollisionObjectWrapper(cow->m_link);
obj->setCollisionShape(shape);
obj->setWorldTransform(tf0i[0]);
obj->m_index = cow->m_index;
BeliefContinuousCollisionCollector cc(collisions, obj, this);
cc.m_collisionFilterMask = KinBodyFilter;
cc.m_collisionFilterGroup = RobotFilter;
world->contactTest(obj, cc);
delete obj;
delete shape;
}
else if (btCompoundShape* compound = dynamic_cast<btCompoundShape*>(shape)) {
for (int child_ind = 0; child_ind < compound->getNumChildShapes(); ++child_ind) {
vector<btTransform> tf0i_child(tf0i.size());
vector<btTransform> tf1i_child(tf1i.size());
for (int i=0; i<tf0i.size(); i++) tf0i_child[i] = tf0i[i]*compound->getChildTransform(child_ind);
for (int i=0; i<tf1i.size(); i++) tf1i_child[i] = tf1i[i]*compound->getChildTransform(child_ind);
CheckShapeSigmaHullCast(compound->getChildShape(child_ind), tf0i_child, tf1i_child, cow, world, collisions);
}
}
else {
throw std::runtime_error("I can only continuous collision check convex shapes and compound shapes made of convex shapes");
}
}
void BeliefCollisionChecker::PlotSigmaHull(btCollisionShape* shape, const vector<btTransform>& tfi,
CollisionObjectWrapper* cow, vector<OpenRAVE::GraphHandlePtr>& handles, OR::RaveVector<float> color) {
if (btConvexShape* convex = dynamic_cast<btConvexShape*>(shape)) {
vector<btTransform> t0i(tfi.size());
// transform all the points with respect to the first transform
for (int i=0; i<tfi.size(); i++) t0i[i] = tfi[0].inverseTimes(tfi[i]);
SigmaHullShape* shape = new SigmaHullShape(convex, t0i);
RenderCollisionShape(shape, tfi[0], *boost::const_pointer_cast<OpenRAVE::EnvironmentBase>(m_env), handles, color);
SetTransparency(handles.back(), 0.2);
delete shape;
} else if (btCompoundShape* compound = dynamic_cast<btCompoundShape*>(shape)) {
for (int child_ind = 0; child_ind < compound->getNumChildShapes(); ++child_ind) {
vector<btTransform> tfi_child(tfi.size());
for (int i=0; i<tfi.size(); i++) tfi_child[i] = tfi[i]*compound->getChildTransform(child_ind);
PlotSigmaHull(compound->getChildShape(child_ind), tfi_child, cow, handles, color);
}
} else {
throw std::runtime_error("I can only plot convex shapes and compound shapes made of convex shapes");
}
}
void QHull3d::createInitialTetrahedron()
{
float d;
float dmax;
int epidx[6];
int tetraidx[4];
// Get extreme points (EP)
for (int i = 0; i < 6; ++i)
epidx[i] = -1;
for (int i = 0; i < _pointcount; ++i)
{
const gk::Point& p = _points[i];
if (epidx[0] < 0 || p.x < _points[epidx[0]].x)
epidx[0] = i;
if (epidx[1] < 0 || p.x > _points[epidx[1]].x)
epidx[1] = i;
if (epidx[2] < 0 || p.y < _points[epidx[2]].y)
epidx[2] = i;
if (epidx[3] < 0 || p.y > _points[epidx[3]].y)
epidx[3] = i;
if (epidx[4] < 0 || p.z < _points[epidx[4]].z)
epidx[4] = i;
if (epidx[5] < 0 || p.z > _points[epidx[5]].z)
epidx[5] = i;
}
// Find the most distant EP pair to build base triangle's first edge
dmax = 0.f;
for (int i = 0; i < 5; ++i)
{
for (int j = i + 1; j < 6; ++j)
{
gk::Vector vij(_points[epidx[i]], _points[epidx[j]]);
if ((d = vij.LengthSquared()) > dmax)
{
tetraidx[0] = epidx[i];
tetraidx[1] = epidx[j];
dmax = d;
}
}
}
// Find the most distant EP from the first edge's support line to complete the base triangle
dmax = 0.f;
tetraidx[2] = -1;
const gk::Point& t0 = _points[tetraidx[0]];
gk::Vector t01 = gk::Normalize(gk::Vector(t0, _points[tetraidx[1]]));
for (int i = 0; i < 6; ++i)
{
if (epidx[i] == tetraidx[0] || epidx[i] == tetraidx[1])
continue;
gk::Vector t0i(t0, _points[epidx[i]]);
float pprojlength = gk::Dot(t0i, t01);
d = t0i.LengthSquared() - (pprojlength * pprojlength);
if (d > dmax)
{
tetraidx[2] = epidx[i];
dmax = d;
}
}
// Special case where there are only 2 extreme points => Pick any remaining point
if (tetraidx[2] < 0)
{
for (int i = 0; i < _pointcount; ++i)
{
if (i != tetraidx[0] && i != tetraidx[1])
{
tetraidx[2] = i;
break;
}
}
}
// Find the most distant point from the base triangle within the point cloud to complete the initial tetrahedron
dmax = 0.f;
HEFace* tetrabase = createFace(tetraidx[0], tetraidx[1], tetraidx[2]);
for (int i = 0; i < _pointcount; ++i)
{
if (i == tetraidx[0] || i == tetraidx[1] || i == tetraidx[2])
continue;
//if (fabs(d = tetrabase->distance(_points[i])) > fabs(dmax))
if (fabs(d = tetrabase->distance(_points[i])) >= fabs(dmax))
{
tetraidx[3] = i;
dmax = d;
}
}
// Coplanarity detection
if (dmax == 0)
{
initialize2d();
return;
}
// Reverse the base triangle if not counter clockwise oriented according to the tetrahedron outer surface
if (dmax > 0)
tetrabase->reverse();
// Complete the tetrahedron's mesh
std::vector<HEFace*> tetrafaces = extrudeOut(tetrabase, tetraidx[3]);
tetrafaces.insert(tetrafaces.begin(), tetrabase);
// Assign remaining points to their corresponding face
for (int i = 0; i < _pointcount; ++i)
{
if (i == tetraidx[0] || i == tetraidx[1] || i == tetraidx[2] || i == tetraidx[3])
continue;
for (int f = 0; f < (int)tetrafaces.size(); ++f)
{
if (tetrafaces[f]->tryAssignVertex(_vertices[i].get()))
break;
}
}
//! Add the tetrahedron's not empty faces to the processing stack
for (int i = 0; i < (int)tetrafaces.size(); ++i)
if (!tetrafaces[i]->vertices.empty())
_processingfaces.push(tetrafaces[i]);
// Store hull first vertex
_hull = _vertices[tetraidx[0]].get();
//////////////////////////////////////////////////////////////////////////
// ToDo JRA: Remove this test code
assertManifoldValidity(_hull);
}
