Added Pierre Terdiman's 'internal object' optimization to improve performance for separating axis tests.
Make the winding consistent in btConvexHullComputer (and related fixes in btPolyhedralConvexShape), thanks to Ole! Some fixes in the btPolyhedralContactClipping implementation (never report a penetration deeper than GJK/EPA found, to avoid issues due to its approximate contact normal directions) Properly visualize btPolyhedralConvexHullShape that have a btConvexPolyhedron (by calling initializePolyhedralFeatures() method)
This commit is contained in:
erwin.coumans
@@ -1231,159 +1231,164 @@ void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const
|
||||
|
||||
} else
|
||||
{
|
||||
switch (shape->getShapeType())
|
||||
|
||||
/// for polyhedral shapes
|
||||
if (shape->isPolyhedral())
|
||||
{
|
||||
btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
|
||||
|
||||
case BOX_SHAPE_PROXYTYPE:
|
||||
int i;
|
||||
if (polyshape->getConvexPolyhedron())
|
||||
{
|
||||
const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
|
||||
btVector3 halfExtents = boxShape->getHalfExtentsWithMargin();
|
||||
getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color);
|
||||
break;
|
||||
}
|
||||
|
||||
case SPHERE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
|
||||
btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
|
||||
|
||||
getDebugDrawer()->drawSphere(radius, worldTransform, color);
|
||||
break;
|
||||
}
|
||||
case MULTI_SPHERE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
|
||||
|
||||
btTransform childTransform;
|
||||
childTransform.setIdentity();
|
||||
|
||||
for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
|
||||
const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron();
|
||||
for (i=0;i<poly->m_faces.size();i++)
|
||||
{
|
||||
childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
|
||||
getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case CAPSULE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
|
||||
|
||||
btScalar radius = capsuleShape->getRadius();
|
||||
btScalar halfHeight = capsuleShape->getHalfHeight();
|
||||
|
||||
int upAxis = capsuleShape->getUpAxis();
|
||||
getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color);
|
||||
break;
|
||||
}
|
||||
case CONE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
|
||||
btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
|
||||
btScalar height = coneShape->getHeight();//+coneShape->getMargin();
|
||||
|
||||
int upAxis= coneShape->getConeUpIndex();
|
||||
getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color);
|
||||
break;
|
||||
|
||||
}
|
||||
case CYLINDER_SHAPE_PROXYTYPE:
|
||||
{
|
||||
const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
|
||||
int upAxis = cylinder->getUpAxis();
|
||||
btScalar radius = cylinder->getRadius();
|
||||
btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
|
||||
getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color);
|
||||
break;
|
||||
}
|
||||
|
||||
case STATIC_PLANE_PROXYTYPE:
|
||||
{
|
||||
const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
|
||||
btScalar planeConst = staticPlaneShape->getPlaneConstant();
|
||||
const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
|
||||
getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, color);
|
||||
break;
|
||||
|
||||
}
|
||||
default:
|
||||
{
|
||||
|
||||
if (shape->isConcave())
|
||||
{
|
||||
btConcaveShape* concaveMesh = (btConcaveShape*) shape;
|
||||
|
||||
///@todo pass camera, for some culling? no -> we are not a graphics lib
|
||||
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
|
||||
btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
|
||||
|
||||
DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
|
||||
concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
|
||||
|
||||
}
|
||||
|
||||
if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
|
||||
{
|
||||
btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
|
||||
//todo: pass camera for some culling
|
||||
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
|
||||
btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
|
||||
//DebugDrawcallback drawCallback;
|
||||
DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
|
||||
convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
|
||||
}
|
||||
|
||||
|
||||
/// for polyhedral shapes
|
||||
if (shape->isPolyhedral())
|
||||
{
|
||||
btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
|
||||
|
||||
int i;
|
||||
if (polyshape->getConvexPolyhedron())
|
||||
btVector3 centroid(0,0,0);
|
||||
int numVerts = poly->m_faces[i].m_indices.size();
|
||||
if (numVerts)
|
||||
{
|
||||
const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron();
|
||||
for (i=0;i<poly->m_faces.size();i++)
|
||||
int lastV = poly->m_faces[i].m_indices[numVerts-1];
|
||||
for (int v=0;v<poly->m_faces[i].m_indices.size();v++)
|
||||
{
|
||||
btVector3 centroid(0,0,0);
|
||||
int numVerts = poly->m_faces[i].m_indices.size();
|
||||
if (numVerts)
|
||||
{
|
||||
int lastV = poly->m_faces[i].m_indices[numVerts-1];
|
||||
for (int v=0;v<poly->m_faces[i].m_indices.size();v++)
|
||||
{
|
||||
int curVert = poly->m_faces[i].m_indices[v];
|
||||
centroid+=poly->m_vertices[curVert];
|
||||
getDebugDrawer()->drawLine(worldTransform*poly->m_vertices[lastV],worldTransform*poly->m_vertices[curVert],color);
|
||||
lastV = curVert;
|
||||
}
|
||||
}
|
||||
centroid*= 1./btScalar(numVerts);
|
||||
|
||||
btVector3 normalColor(1,1,0);
|
||||
btVector3 faceNormal(poly->m_faces[i].m_plane[0],poly->m_faces[i].m_plane[1],poly->m_faces[i].m_plane[2]);
|
||||
getDebugDrawer()->drawLine(worldTransform*centroid,worldTransform*(centroid+faceNormal),normalColor);
|
||||
|
||||
|
||||
int curVert = poly->m_faces[i].m_indices[v];
|
||||
centroid+=poly->m_vertices[curVert];
|
||||
getDebugDrawer()->drawLine(worldTransform*poly->m_vertices[lastV],worldTransform*poly->m_vertices[curVert],color);
|
||||
lastV = curVert;
|
||||
}
|
||||
}
|
||||
centroid*= btScalar(1.f)/btScalar(numVerts);
|
||||
|
||||
|
||||
} else
|
||||
btVector3 normalColor(1,1,0);
|
||||
btVector3 faceNormal(poly->m_faces[i].m_plane[0],poly->m_faces[i].m_plane[1],poly->m_faces[i].m_plane[2]);
|
||||
getDebugDrawer()->drawLine(worldTransform*centroid,worldTransform*(centroid+faceNormal),normalColor);
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
} else
|
||||
{
|
||||
for (i=0;i<polyshape->getNumEdges();i++)
|
||||
{
|
||||
btVector3 a,b;
|
||||
polyshape->getEdge(i,a,b);
|
||||
btVector3 wa = worldTransform * a;
|
||||
btVector3 wb = worldTransform * b;
|
||||
getDebugDrawer()->drawLine(wa,wb,color);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
switch (shape->getShapeType())
|
||||
{
|
||||
|
||||
case BOX_SHAPE_PROXYTYPE:
|
||||
{
|
||||
const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
|
||||
btVector3 halfExtents = boxShape->getHalfExtentsWithMargin();
|
||||
getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color);
|
||||
break;
|
||||
}
|
||||
|
||||
case SPHERE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
|
||||
btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
|
||||
|
||||
getDebugDrawer()->drawSphere(radius, worldTransform, color);
|
||||
break;
|
||||
}
|
||||
case MULTI_SPHERE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
|
||||
|
||||
btTransform childTransform;
|
||||
childTransform.setIdentity();
|
||||
|
||||
for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
|
||||
{
|
||||
for (i=0;i<polyshape->getNumEdges();i++)
|
||||
{
|
||||
btVector3 a,b;
|
||||
polyshape->getEdge(i,a,b);
|
||||
btVector3 wa = worldTransform * a;
|
||||
btVector3 wb = worldTransform * b;
|
||||
getDebugDrawer()->drawLine(wa,wb,color);
|
||||
}
|
||||
childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
|
||||
getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case CAPSULE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
|
||||
|
||||
btScalar radius = capsuleShape->getRadius();
|
||||
btScalar halfHeight = capsuleShape->getHalfHeight();
|
||||
|
||||
int upAxis = capsuleShape->getUpAxis();
|
||||
getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color);
|
||||
break;
|
||||
}
|
||||
case CONE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
|
||||
btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
|
||||
btScalar height = coneShape->getHeight();//+coneShape->getMargin();
|
||||
|
||||
int upAxis= coneShape->getConeUpIndex();
|
||||
getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color);
|
||||
break;
|
||||
|
||||
}
|
||||
case CYLINDER_SHAPE_PROXYTYPE:
|
||||
{
|
||||
const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
|
||||
int upAxis = cylinder->getUpAxis();
|
||||
btScalar radius = cylinder->getRadius();
|
||||
btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
|
||||
getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color);
|
||||
break;
|
||||
}
|
||||
|
||||
case STATIC_PLANE_PROXYTYPE:
|
||||
{
|
||||
const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
|
||||
btScalar planeConst = staticPlaneShape->getPlaneConstant();
|
||||
const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
|
||||
getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, color);
|
||||
break;
|
||||
|
||||
}
|
||||
default:
|
||||
{
|
||||
|
||||
if (shape->isConcave())
|
||||
{
|
||||
btConcaveShape* concaveMesh = (btConcaveShape*) shape;
|
||||
|
||||
///@todo pass camera, for some culling? no -> we are not a graphics lib
|
||||
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
|
||||
btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
|
||||
|
||||
DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
|
||||
concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
|
||||
|
||||
}
|
||||
|
||||
if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
|
||||
{
|
||||
btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
|
||||
//todo: pass camera for some culling
|
||||
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
|
||||
btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
|
||||
//DebugDrawcallback drawCallback;
|
||||
DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
|
||||
convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user