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