I am a researcher working on path planning in 3D maps for robots. I generate trimeshes from range images and then try to find a collision-free path (or multiple actually for an RRT). For this, I sweep a sphere through the map. The map is static and divided into some patches, which are the result of our algorithms. I am adding each patch as its own trimesh because I figured with the two phase (broad/narrow) algorithms of Bullet this would save time. Later on, some of the patches might be updated. One potentially important feature of the map: Due to messurement errors and approximations fromt the algorithms
(i.e. we just don't care), the patches of the map might intersect each other. However, that is of no interest and should not be reported by the collision detection.
My problem is that while some collisions between my trimeshes and the swept sphere are detected, some are not. I am looking for pointers on how to start debuggin this.
This is how I setup the trimesh from my own datastructure:
Code: Select all
int numTriangles = pPolygonCloud->size();
int * triangleIndexBase = new int[numTriangles*3];
int triangleIndexStride = sizeof(int)*3;
int numVertices=pPolygonCloud->numVertices();
btScalar * vertexBase=new btScalar[numVertices*3];
int vertexStride=sizeof(btScalar)*3;
for( int i=0;i<numTriangles; ++i ) {
triangleIndexBase[i*3 + 0] = pPolygonCloud->getVertexIndices()[i][0];
triangleIndexBase[i*3 + 1] = pPolygonCloud->getVertexIndices()[i][1];
triangleIndexBase[i*3 + 2] = pPolygonCloud->getVertexIndices()[i][2];
}
for( int i=0;i<numVertices;++i ) {
vertexBase[i*3 + 0] = pPolygonCloud->getVertices()[i].x();
vertexBase[i*3 + 1] = pPolygonCloud->getVertices()[i].y();
vertexBase[i*3 + 2] = pPolygonCloud->getVertices()[i].z();
}
btTriangleIndexVertexArray* tiva = new btTriangleIndexVertexArray(numTriangles, triangleIndexBase, triangleIndexStride, numVertices, vertexBase, vertexStride);
bool useQuantizedAabbCompression=false;
btBvhTriangleMeshShape* btms = new btBvhTriangleMeshShape(tiva, useQuantizedAabbCompression);
btScalar mass=0;
btMotionState *motionState=0;
btCollisionShape *collisionShape=btms;
btVector3 inertia(0,0,0);
collisionShape->calculateLocalInertia(mass,inertia);
btRigidBody::btRigidBodyConstructionInfo rbci( mass,motionState,collisionShape, inertia );
btRigidBody* rb= new btRigidBody(rbci);
rb->setCollisionShape(collisionShape);
btTransform worldTransform;
worldTransform.setIdentity();
rb->setWorldTransform(worldTransform);
aWorld.addCollisionObject(rb);
Code: Select all
btVector3 fromVector(pFrom.x(),pFrom.y(),pFrom.z());
btVector3 toVector(pTo.x(),pTo.y(),pTo.z());
btSphereShape sphere(pCapsuleRadius);
btTransform from, to;
from.setIdentity();
from.setOrigin(fromVector);
to.setIdentity();
to.setOrigin(toVector);
btVector3 zeroVector(0,0,0);
btCollisionWorld::ClosestConvexResultCallback callback(fromVector, toVector);
btScalar allowedCcdPenetration=0.1;
aWorld.convexSweepTest( &sphere, from, to, callback, allowedCcdPenetration );
if( callback.hasHit() ) {
return true;
} else {
return false;
}
