Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

- Added compressed/quantized AABB tree, 16 bytes per node, while supporting 32-bit (triangle) indices. Should be faster and smaller then original version (quantized aabb check is done in integer space)

Browse Source 
Original aabb nodes are 44 bytes (with full floating point precision and additional part index)
- added meter-unit scaling support in ColladaConverter.cpp
This commit is contained in:
ejcoumans
2007-02-25 06:11:23 +00:00
parent 7efef8e394
commit e610598d33
10 changed files with 571 additions and 184 deletions
Download Patch File Download Diff File Expand all files Collapse all files

66
Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp
View File

@@ -17,6 +17,7 @@ subject to the following restrictions:
//#define REGISTER_CUSTOM_COLLISION_ALGORITHM 1
//#define USER_DEFINED_FRICTION_MODEL 1
#define USE_CUSTOM_NEAR_CALLBACK 1
//#define CENTER_OF_MASS_SHIFT 1
//following define allows to compare/replace Bullet's constraint solver with ODE quickstep
//this define requires to either add the libquickstep library (win32, see msvc/8/libquickstep.vcproj) or manually add the files from Extras/quickstep
@@ -46,6 +47,8 @@ float gCollisionMargin = 0.05f;
#include "GlutStuff.h"
btTransform comOffset;
btVector3 comOffsetVec(0,2,0);
extern float eye[3];
extern int glutScreenWidth;
@@ -55,7 +58,7 @@ const int maxProxies = 32766;
const int maxOverlap = 65535;
bool createConstraint = true;//false;
bool useCompound = false;//true;//false;
bool useCompound = false;
@@ -91,6 +94,7 @@ btCollisionShape* shapePtr[numShapes] =
#endif
new btCylinderShape (btVector3(CUBE_HALF_EXTENTS-gCollisionMargin,CUBE_HALF_EXTENTS-gCollisionMargin,CUBE_HALF_EXTENTS-gCollisionMargin)),
//new btSphereShape (CUBE_HALF_EXTENTS),
//new btCapsuleShape(0.5*CUBE_HALF_EXTENTS-gCollisionMargin,CUBE_HALF_EXTENTS-gCollisionMargin),
//new btCylinderShape (btVector3(1-gCollisionMargin,CUBE_HALF_EXTENTS-gCollisionMargin,1-gCollisionMargin)),
//new btBoxShape (btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)),
@@ -245,6 +249,48 @@ void CcdPhysicsDemo::clientMoveAndDisplay()
renderme();
//render the graphics objects, with center of mass shift
updateCamera();
#ifdef CENTER_OF_MASS_SHIFT
btScalar m[16];
if (m_dynamicsWorld)
{
int numObjects = m_dynamicsWorld->getNumCollisionObjects();
btVector3 wireColor(1,0,0);
for (int i=0;i<numObjects;i++)
{
btCollisionObject* colObj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(colObj);
if (body && body->getMotionState())
{
btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState();
btTransform offsetTrans = myMotionState->m_graphicsWorldTrans;
btVector3 worldShift = offsetTrans.getBasis() * comOffsetVec;
offsetTrans.setOrigin(offsetTrans.getOrigin() + worldShift);
offsetTrans.getOpenGLMatrix(m);
btVector3 wireColor(1.f,1.0f,1.f); //wants deactivation
btSphereShape sphereTmp(CUBE_HALF_EXTENTS);
GL_ShapeDrawer::drawOpenGL(m,&sphereTmp,wireColor,0);
}
}
}
#endif //CENTER_OF_MASS_SHIFT
#ifdef USE_QUICKPROF
btProfiler::endBlock("render");
#endif
@@ -363,13 +409,19 @@ void CcdPhysicsDemo::initPhysics()
btCompoundShape* compoundShape = new btCompoundShape();
btCollisionShape* oldShape = shapePtr[1];
shapePtr[1] = compoundShape;
btVector3 sphereOffset(0,0,2);
btTransform ident;
ident.setIdentity();
ident.setOrigin(btVector3(0,0,0));
compoundShape->addChildShape(ident,oldShape);//
ident.setOrigin(btVector3(0,0,2));
compoundShape->addChildShape(ident,new btSphereShape(0.9));//
comOffset.setIdentity();
#ifdef CENTER_OF_MASS_SHIFT
comOffset.setOrigin(comOffsetVec);
compoundShape->addChildShape(comOffset,oldShape);
#else
compoundShape->addChildShape(tr,oldShape);
tr.setOrigin(sphereOffset);
compoundShape->addChildShape(tr,new btSphereShape(0.9));
#endif
}
for (i=0;i<gNumObjects;i++)

58
Demos/ColladaDemo/ColladaConverter.cpp
View File

@@ -67,9 +67,11 @@ domMatrix_Array emptyMatrixArray;
domNodeRef m_colladadomNodes[COLLADA_CONVERTER_MAX_NUM_OBJECTS];
///This code is actually wrong: the order of transformations is lost, so we need to rewrite this!
btTransform GetbtTransformFromCOLLADA_DOM(domMatrix_Array& matrixArray,
domRotate_Array& rotateArray,
domTranslate_Array& translateArray
domTranslate_Array& translateArray,
float meterScaling
)
{
@@ -83,7 +85,7 @@ btTransform GetbtTransformFromCOLLADA_DOM(domMatrix_Array& matrixArray,
domMatrixRef matrixRef = matrixArray[i];
domFloat4x4 fl16 = matrixRef->getValue();
btVector3 origin(fl16.get(3),fl16.get(7),fl16.get(11));
startTransform.setOrigin(origin);
startTransform.setOrigin(origin*meterScaling);
btMatrix3x3 basis(fl16.get(0),fl16.get(1),fl16.get(2),
fl16.get(4),fl16.get(5),fl16.get(6),
fl16.get(8),fl16.get(9),fl16.get(10));
@@ -103,7 +105,8 @@ btTransform GetbtTransformFromCOLLADA_DOM(domMatrix_Array& matrixArray,
{
domTranslateRef translateRef = translateArray[i];
domFloat3 fl3 = translateRef->getValue();
startTransform.getOrigin() += btVector3(fl3.get(0),fl3.get(1),fl3.get(2));
btVector3 orgTrans(fl3.get(0),fl3.get(1),fl3.get(2));
startTransform.getOrigin() += orgTrans*meterScaling;
}
return startTransform;
}
@@ -116,7 +119,8 @@ ColladaConverter::ColladaConverter()
:m_collada(0),
m_dom(0),
m_filename(0),
m_numObjects(0)
m_numObjects(0),
m_unitMeterScaling(1.f)
{
//Collada-m_dom
m_collada = new DAE;
@@ -179,6 +183,15 @@ bool ColladaConverter::convert()
//succesfully loaded file, now convert data
if (m_dom->getAsset() && m_dom->getAsset()->getUnit())
{
domAsset::domUnitRef unit = m_dom->getAsset()->getUnit();
domFloat meter = unit->getMeter();
printf("asset unit meter=%f\n",meter);
m_unitMeterScaling = meter;
}
if ( m_dom->getAsset() && m_dom->getAsset()->getUp_axis() )
{
domAsset::domUp_axis * up = m_dom->getAsset()->getUp_axis();
@@ -593,7 +606,8 @@ void ColladaConverter::prepareConstraints(ConstraintInput& input)
(
emptyMatrixArray,
attachRefBody->getRotate_array(),
attachRefBody->getTranslate_array());
attachRefBody->getTranslate_array(),
m_unitMeterScaling);
}
btTransform attachFrameOther;
@@ -605,7 +619,8 @@ void ColladaConverter::prepareConstraints(ConstraintInput& input)
(
emptyMatrixArray,
attachBody1->getRotate_array(),
attachBody1->getTranslate_array()
attachBody1->getTranslate_array(),
m_unitMeterScaling
);
}
@@ -718,7 +733,8 @@ void ColladaConverter::PreparePhysicsObject(struct btRigidBodyInput& input, bool
startTransform = GetbtTransformFromCOLLADA_DOM(
node->getMatrix_array(),
node->getRotate_array(),
node->getTranslate_array()
node->getTranslate_array(),
m_unitMeterScaling
);
unsigned int i;
@@ -994,7 +1010,7 @@ void ColladaConverter::ConvertRigidBodyRef( btRigidBodyInput& rbInput,btRigidBod
{
const domFloat4 planeEq = planeRef->getEquation()->getValue();
btVector3 planeNormal(planeEq.get(0),planeEq.get(1),planeEq.get(2));
btScalar planeConstant = planeEq.get(3);
btScalar planeConstant = planeEq.get(3)*m_unitMeterScaling;
rbOutput.m_colShape = new btStaticPlaneShape(planeNormal,planeConstant);
}
@@ -1005,25 +1021,25 @@ void ColladaConverter::ConvertRigidBodyRef( btRigidBodyInput& rbInput,btRigidBod
domBoxRef boxRef = shapeRef->getBox();
domBox::domHalf_extentsRef domHalfExtentsRef = boxRef->getHalf_extents();
domFloat3& halfExtents = domHalfExtentsRef->getValue();
float x = halfExtents.get(0);
float y = halfExtents.get(1);
float z = halfExtents.get(2);
float x = halfExtents.get(0)*m_unitMeterScaling;
float y = halfExtents.get(1)*m_unitMeterScaling;
float z = halfExtents.get(2)*m_unitMeterScaling;
rbOutput.m_colShape = new btBoxShape(btVector3(x,y,z));
}
if (shapeRef->getSphere())
{
domSphereRef sphereRef = shapeRef->getSphere();
domSphere::domRadiusRef radiusRef = sphereRef->getRadius();
domFloat radius = radiusRef->getValue();
domFloat radius = radiusRef->getValue()*m_unitMeterScaling;
rbOutput.m_colShape = new btSphereShape(radius);
}
if (shapeRef->getCylinder())
{
domCylinderRef cylinderRef = shapeRef->getCylinder();
domFloat height = cylinderRef->getHeight()->getValue();
domFloat height = cylinderRef->getHeight()->getValue()*m_unitMeterScaling;
domFloat2 radius2 = cylinderRef->getRadius()->getValue();
domFloat radius0 = radius2.get(0);
domFloat radius0 = radius2.get(0)*m_unitMeterScaling;
//Cylinder around the local Y axis
rbOutput.m_colShape = new btCylinderShape(btVector3(radius0,height,radius0));
@@ -1117,7 +1133,7 @@ void ColladaConverter::ConvertRigidBodyRef( btRigidBodyInput& rbInput,btRigidBod
domFloat fl1 = listFloats.get(index0+1);
domFloat fl2 = listFloats.get(index0+2);
k+=meshPart.m_triangleIndexStride;
verts[i].setValue(fl0,fl1,fl2);
verts[i].setValue(fl0*m_unitMeterScaling,fl1*m_unitMeterScaling,fl2*m_unitMeterScaling);
}
trimesh->addTriangle(verts[0],verts[1],verts[2]);
}
@@ -1143,7 +1159,8 @@ void ColladaConverter::ConvertRigidBodyRef( btRigidBodyInput& rbInput,btRigidBod
} else
{
printf("static concave triangle <mesh> added\n");
rbOutput.m_colShape = new btBvhTriangleMeshShape(trimesh);
bool useQuantizedAabbCompression = false;
rbOutput.m_colShape = new btBvhTriangleMeshShape(trimesh,useQuantizedAabbCompression);
//rbOutput.m_colShape = new btBvhTriangleMeshShape(trimesh);
//rbOutput.m_colShape = new btConvexTriangleMeshShape(trimesh);
@@ -1182,7 +1199,7 @@ void ColladaConverter::ConvertRigidBodyRef( btRigidBodyInput& rbInput,btRigidBod
domFloat fl0 = listFloats.get(vertIndex);
domFloat fl1 = listFloats.get(vertIndex+1);
domFloat fl2 = listFloats.get(vertIndex+2);
convexHull->addPoint(btPoint3(fl0,fl1,fl2));
convexHull->addPoint(btPoint3(fl0,fl1,fl2) * m_unitMeterScaling);
}
}
}
@@ -1280,7 +1297,7 @@ void ColladaConverter::ConvertRigidBodyRef( btRigidBodyInput& rbInput,btRigidBod
domFloat fl2 = listFloats.get(k+2);
//printf("float %f %f %f\n",fl0,fl1,fl2);
convexHullShape->addPoint(btPoint3(fl0,fl1,fl2));
convexHullShape->addPoint(btPoint3(fl0,fl1,fl2) * m_unitMeterScaling);
}
}
@@ -1328,7 +1345,7 @@ void ColladaConverter::ConvertRigidBodyRef( btRigidBodyInput& rbInput,btRigidBod
domFloat fl2 = listFloats.get(k+2);
//printf("float %f %f %f\n",fl0,fl1,fl2);
convexHullShape->addPoint(btPoint3(fl0,fl1,fl2));
convexHullShape->addPoint(btPoint3(fl0,fl1,fl2)*m_unitMeterScaling);
}
}
@@ -1383,7 +1400,8 @@ void ColladaConverter::ConvertRigidBodyRef( btRigidBodyInput& rbInput,btRigidBod
localTransform = GetbtTransformFromCOLLADA_DOM(
emptyMatrixArray,
shapeRef->getRotate_array(),
shapeRef->getTranslate_array()
shapeRef->getTranslate_array(),
m_unitMeterScaling
);
}

1
Demos/ColladaDemo/ColladaConverter.h
View File

@@ -41,6 +41,7 @@ protected:
class DAE* m_collada;
class domCOLLADA* m_dom;
const char* m_filename;
float m_unitMeterScaling;
int m_numObjects;
btRigidBody* m_rigidBodies[COLLADA_CONVERTER_MAX_NUM_OBJECTS];

4
Demos/ConcaveDemo/ConcavePhysicsDemo.cpp
View File

@@ -149,7 +149,8 @@ void ConcaveDemo::initPhysics()
indexStride,
totalVerts,(btScalar*) &gVertices[0].x(),vertStride);
btCollisionShape* trimeshShape = new btBvhTriangleMeshShape(indexVertexArrays);
bool useQuantizedAabbCompression = true;
btCollisionShape* trimeshShape = new btBvhTriangleMeshShape(indexVertexArrays,useQuantizedAabbCompression);
// btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
@@ -159,6 +160,7 @@ void ConcaveDemo::initPhysics()
btOverlappingPairCache* pairCache = new btAxisSweep3(worldMin,worldMax);
btConstraintSolver* constraintSolver = new btSequentialImpulseConstraintSolver();
m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver);
m_dynamicsWorld->setDebugDrawer(&debugDrawer);
float mass = 0.f;
btTransform startTransform;

3
Demos/VehicleDemo/VehicleDemo.cpp
View File

@@ -185,7 +185,8 @@ const float TRIANGLE_SIZE=20.f;
indexStride,
totalVerts,(btScalar*) &gVertices[0].x(),vertStride);
groundShape = new btBvhTriangleMeshShape(indexVertexArrays);
bool useQuantizedAabbCompression = true;
groundShape = new btBvhTriangleMeshShape(indexVertexArrays,useQuantizedAabbCompression);
#endif //

9
src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp
View File

@@ -110,14 +110,7 @@ btAxisSweep3::~btAxisSweep3()
void btAxisSweep3::quantize(unsigned short* out, const btPoint3& point, int isMax) const
{
btPoint3 clampedPoint(point);
/*
if (isMax)
clampedPoint += btVector3(10,10,10);
else
{
clampedPoint -= btVector3(10,10,10);
}
*/
clampedPoint.setMax(m_worldAabbMin);

18
src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp
View File

@@ -20,14 +20,14 @@ subject to the following restrictions:
///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization.
///Uses an interface to access the triangles to allow for sharing graphics/physics triangles.
btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface)
:btTriangleMeshShape(meshInterface)
btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression)
:btTriangleMeshShape(meshInterface),m_useQuantizedAabbCompression(useQuantizedAabbCompression)
{
//construct bvh from meshInterface
#ifndef DISABLE_BVH
m_bvh = new btOptimizedBvh();
m_bvh->build(meshInterface);
m_bvh->build(meshInterface,m_useQuantizedAabbCompression);
#endif //DISABLE_BVH
@@ -63,7 +63,7 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co
{
}
virtual void processNode(const btOptimizedBvhNode* node)
virtual void processNode(int nodeSubPart, int nodeTriangleIndex)
{
const unsigned char *vertexbase;
int numverts;
@@ -84,9 +84,9 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co
indexstride,
numfaces,
indicestype,
node->m_subPart);
nodeSubPart);
int* gfxbase = (int*)(indexbase+node->m_triangleIndex*indexstride);
int* gfxbase = (int*)(indexbase+nodeTriangleIndex*indexstride);
const btVector3& meshScaling = m_meshInterface->getScaling();
for (int j=2;j>=0;j--)
@@ -107,8 +107,8 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co
#endif //DEBUG_TRIANGLE_MESH
}
m_callback->processTriangle(m_triangle,node->m_subPart,node->m_triangleIndex);
m_meshInterface->unLockReadOnlyVertexBase(node->m_subPart);
m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex);
m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart);
}
};
@@ -131,7 +131,7 @@ void btBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling)
btTriangleMeshShape::setLocalScaling(scaling);
delete m_bvh;
m_bvh = new btOptimizedBvh();
m_bvh->build(m_meshInterface);
m_bvh->build(m_meshInterface,m_useQuantizedAabbCompression);
//rebuild the bvh...
}
}

6
src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h
View File

@@ -25,10 +25,10 @@ class btBvhTriangleMeshShape : public btTriangleMeshShape
{
btOptimizedBvh* m_bvh;
bool m_useQuantizedAabbCompression;
public:
btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface);
btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression);
virtual ~btBvhTriangleMeshShape();

390
src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp
View File

@@ -18,17 +18,16 @@ subject to the following restrictions:
#include "LinearMath/btAabbUtil2.h"
btOptimizedBvh::btOptimizedBvh() :m_rootNode1(0), m_numNodes(0)
btOptimizedBvh::btOptimizedBvh() : m_contiguousNodes(0), m_quantizedContiguousNodes(0), m_useQuantization(false)
{
}
void btOptimizedBvh::build(btStridingMeshInterface* triangles)
void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantizedAabbCompression)
{
//int countTriangles = 0;
m_useQuantization = useQuantizedAabbCompression;
// NodeArray triangleNodes;
@@ -39,61 +38,148 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles)
NodeTriangleCallback(NodeArray& triangleNodes)
:m_triangleNodes(triangleNodes)
{
}
virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
{
btOptimizedBvhNode node;
node.m_aabbMin = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30));
node.m_aabbMax = btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
node.m_aabbMin.setMin(triangle[0]);
node.m_aabbMax.setMax(triangle[0]);
node.m_aabbMin.setMin(triangle[1]);
node.m_aabbMax.setMax(triangle[1]);
node.m_aabbMin.setMin(triangle[2]);
node.m_aabbMax.setMax(triangle[2]);
btVector3 aabbMin,aabbMax;
aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
aabbMin.setMin(triangle[0]);
aabbMax.setMax(triangle[0]);
aabbMin.setMin(triangle[1]);
aabbMax.setMax(triangle[1]);
aabbMin.setMin(triangle[2]);
aabbMax.setMax(triangle[2]);
//with quantization?
node.m_aabbMinOrg = aabbMin;
node.m_aabbMaxOrg = aabbMax;
node.m_escapeIndex = -1;
node.m_leftChild = 0;
node.m_rightChild = 0;
//for child nodes
node.m_subPart = partId;
node.m_triangleIndex = triangleIndex;
m_triangleNodes.push_back(node);
}
};
struct QuantizedNodeTriangleCallback : public btInternalTriangleIndexCallback
{
QuantizedNodeArray& m_triangleNodes;
const btOptimizedBvh* m_optimizedTree; // for quantization
QuantizedNodeTriangleCallback(QuantizedNodeArray& triangleNodes,const btOptimizedBvh* tree)
:m_triangleNodes(triangleNodes),m_optimizedTree(tree)
{
}
virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
{
btAssert(partId==0);
//negative indices are reserved for escapeIndex
btAssert(triangleIndex>=0);
btQuantizedBvhNode node;
btVector3 aabbMin,aabbMax;
aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
aabbMin.setMin(triangle[0]);
aabbMax.setMax(triangle[0]);
aabbMin.setMin(triangle[1]);
aabbMax.setMax(triangle[1]);
aabbMin.setMin(triangle[2]);
aabbMax.setMax(triangle[2]);
m_optimizedTree->quantizeWithClamp(&node.m_quantizedAabbMin[0],aabbMin);
m_optimizedTree->quantizeWithClamp(&node.m_quantizedAabbMax[0],aabbMax);
node.m_escapeIndexOrTriangleIndex = triangleIndex;
m_triangleNodes.push_back(node);
}
};
struct AabbCalculationCallback : public btInternalTriangleIndexCallback
{
btVector3 m_aabbMin;
btVector3 m_aabbMax;
AabbCalculationCallback()
{
m_aabbMin.setValue(1e30,1e30,1e30);
m_aabbMax.setValue(-1e30,-1e30,-1e30);
}
virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
{
m_aabbMin.setMin(triangle[0]);
m_aabbMax.setMax(triangle[0]);
m_aabbMin.setMin(triangle[1]);
m_aabbMax.setMax(triangle[1]);
m_aabbMin.setMin(triangle[2]);
m_aabbMax.setMax(triangle[2]);
}
};
int numLeafNodes = 0;
if (m_useQuantization)
{
//first calculate the total aabb for all triangles
AabbCalculationCallback aabbCallback;
btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
triangles->InternalProcessAllTriangles(&aabbCallback,aabbMin,aabbMax);
NodeTriangleCallback callback(m_leafNodes);
//initialize quantization values
m_bvhAabbMin = aabbCallback.m_aabbMin;
m_bvhAabbMax = aabbCallback.m_aabbMax;
btVector3 aabbSize = m_bvhAabbMax - m_bvhAabbMin;
m_bvhQuantization = btVector3(btScalar(65535.0),btScalar(65535.0),btScalar(65535.0)) / aabbSize;
btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
QuantizedNodeTriangleCallback callback(m_quantizedLeafNodes,this);
triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax);
triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax);
//now we have an array of leafnodes in m_leafNodes
//now we have an array of leafnodes in m_leafNodes
numLeafNodes = m_quantizedLeafNodes.size();
m_quantizedContiguousNodes = new btQuantizedBvhNode[2*numLeafNodes];
} else
{
NodeTriangleCallback callback(m_leafNodes);
btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax);
//now we have an array of leafnodes in m_leafNodes
numLeafNodes = m_leafNodes.size();
m_contiguousNodes = new btOptimizedBvhNode[2*numLeafNodes];
}
m_contiguousNodes = new btOptimizedBvhNode[2*m_leafNodes.size()];
m_curNodeIndex = 0;
m_rootNode1 = buildTree(m_leafNodes,0,m_leafNodes.size());
buildTree(0,numLeafNodes);
///create the leafnodes first
// btOptimizedBvhNode* leafNodes = new btOptimizedBvhNode;
}
btOptimizedBvh::~btOptimizedBvh()
{
if (m_contiguousNodes)
delete []m_contiguousNodes;
if (m_quantizedContiguousNodes)
delete []m_quantizedContiguousNodes;
}
#ifdef DEBUG_TREE_BUILDING
@@ -101,7 +187,7 @@ int gStackDepth = 0;
int gMaxStackDepth = 0;
#endif //DEBUG_TREE_BUILDING
btOptimizedBvhNode* btOptimizedBvh::buildTree (NodeArray& leafNodes,int startIndex,int endIndex)
void btOptimizedBvh::buildTree (int startIndex,int endIndex)
{
#ifdef DEBUG_TREE_BUILDING
gStackDepth++;
@@ -109,7 +195,6 @@ btOptimizedBvhNode* btOptimizedBvh::buildTree (NodeArray& leafNodes,int startInd
gMaxStackDepth = gStackDepth;
#endif //DEBUG_TREE_BUILDING
btOptimizedBvhNode* internalNode;
int splitAxis, splitIndex, i;
int numIndices =endIndex-startIndex;
@@ -122,38 +207,48 @@ btOptimizedBvhNode* btOptimizedBvh::buildTree (NodeArray& leafNodes,int startInd
#ifdef DEBUG_TREE_BUILDING
gStackDepth--;
#endif //DEBUG_TREE_BUILDING
return new (&m_contiguousNodes[m_curNodeIndex++]) btOptimizedBvhNode(leafNodes[startIndex]);
assignInternalNodeFromLeafNode(m_curNodeIndex,startIndex);
m_curNodeIndex++;
return;
}
//calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
splitAxis = calcSplittingAxis(leafNodes,startIndex,endIndex);
splitAxis = calcSplittingAxis(startIndex,endIndex);
splitIndex = sortAndCalcSplittingIndex(leafNodes,startIndex,endIndex,splitAxis);
splitIndex = sortAndCalcSplittingIndex(startIndex,endIndex,splitAxis);
internalNode = &m_contiguousNodes[m_curNodeIndex++];
int internalNodeIndex = m_curNodeIndex;
internalNode->m_aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
internalNode->m_aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
setInternalNodeAabbMax(m_curNodeIndex,btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)));
setInternalNodeAabbMin(m_curNodeIndex,btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30)));
for (i=startIndex;i<endIndex;i++)
{
internalNode->m_aabbMax.setMax(leafNodes[i].m_aabbMax);
internalNode->m_aabbMin.setMin(leafNodes[i].m_aabbMin);
mergeInternalNodeAabb(m_curNodeIndex,getAabbMin(i),getAabbMax(i));
}
m_curNodeIndex++;
//internalNode->m_escapeIndex;
internalNode->m_leftChild = buildTree(leafNodes,startIndex,splitIndex);
internalNode->m_rightChild = buildTree(leafNodes,splitIndex,endIndex);
//build left child tree
buildTree(startIndex,splitIndex);
//build right child tree
buildTree(splitIndex,endIndex);
#ifdef DEBUG_TREE_BUILDING
gStackDepth--;
#endif //DEBUG_TREE_BUILDING
internalNode->m_escapeIndex = m_curNodeIndex - curIndex;
return internalNode;
setInternalNodeEscapeIndex(internalNodeIndex,m_curNodeIndex - curIndex);
}
int btOptimizedBvh::sortAndCalcSplittingIndex(NodeArray& leafNodes,int startIndex,int endIndex,int splitAxis)
int btOptimizedBvh::sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis)
{
int i;
int splitIndex =startIndex;
@@ -163,7 +258,7 @@ int btOptimizedBvh::sortAndCalcSplittingIndex(NodeArray& leafNodes,int startInde
btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
for (i=startIndex;i<endIndex;i++)
{
btVector3 center = btScalar(0.5)*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin);
btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i));
means+=center;
}
means *= (btScalar(1.)/(btScalar)numIndices);
@@ -173,13 +268,11 @@ int btOptimizedBvh::sortAndCalcSplittingIndex(NodeArray& leafNodes,int startInde
//sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
for (i=startIndex;i<endIndex;i++)
{
btVector3 center = btScalar(0.5)*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin);
btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i));
if (center[splitAxis] > splitValue)
{
//swap
btOptimizedBvhNode tmp = leafNodes[i];
leafNodes[i] = leafNodes[splitIndex];
leafNodes[splitIndex] = tmp;
swapLeafNodes(i,splitIndex);
splitIndex++;
}
}
@@ -208,7 +301,7 @@ int btOptimizedBvh::sortAndCalcSplittingIndex(NodeArray& leafNodes,int startInde
}
int btOptimizedBvh::calcSplittingAxis(NodeArray& leafNodes,int startIndex,int endIndex)
int btOptimizedBvh::calcSplittingAxis(int startIndex,int endIndex)
{
int i;
@@ -218,15 +311,14 @@ int btOptimizedBvh::calcSplittingAxis(NodeArray& leafNodes,int startIndex,int en
for (i=startIndex;i<endIndex;i++)
{
btOptimizedBvhNode& node = leafNodes[i];
btVector3 center = btScalar(0.5)*(node.m_aabbMax+node.m_aabbMin);
btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i));
means+=center;
}
means *= (btScalar(1.)/(btScalar)numIndices);
for (i=startIndex;i<endIndex;i++)
{
btVector3 center = btScalar(0.5)*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin);
btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i));
btVector3 diff2 = center-means;
diff2 = diff2 * diff2;
variance += diff2;
@@ -242,11 +334,104 @@ void btOptimizedBvh::reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallb
{
//either choose recursive traversal (walkTree) or stackless (walkStacklessTree)
//walkTree(m_rootNode1,nodeCallback,aabbMin,aabbMax);
walkStacklessTree(m_rootNode1,nodeCallback,aabbMin,aabbMax);
if (m_useQuantization)
{
walkStacklessQuantizedTree(nodeCallback,aabbMin,aabbMax);
} else
{
walkStacklessTree(nodeCallback,aabbMin,aabbMax);
}
}
int maxIterations = 0;
void btOptimizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
{
btAssert(!m_useQuantization);
btOptimizedBvhNode* rootNode = &m_contiguousNodes[0];
int escapeIndex, curIndex = 0;
int walkIterations = 0;
bool aabbOverlap, isLeafNode;
while (curIndex < m_curNodeIndex)
{
//catch bugs in tree data
assert (walkIterations < m_curNodeIndex);
walkIterations++;
aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMinOrg,rootNode->m_aabbMaxOrg);
isLeafNode = rootNode->m_escapeIndex == -1;
if (isLeafNode && aabbOverlap)
{
nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex);
}
if (aabbOverlap || isLeafNode)
{
rootNode++;
curIndex++;
} else
{
escapeIndex = rootNode->m_escapeIndex;
rootNode += escapeIndex;
curIndex += escapeIndex;
}
}
if (maxIterations < walkIterations)
maxIterations = walkIterations;
}
void btOptimizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
{
btAssert(m_useQuantization);
unsigned short int quantizedQueryAabbMin[3];
unsigned short int quantizedQueryAabbMax[3];
quantizeWithClamp(quantizedQueryAabbMin,aabbMin);
quantizeWithClamp(quantizedQueryAabbMax,aabbMax);
btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[0];
int escapeIndex, curIndex = 0;
int walkIterations = 0;
bool aabbOverlap, isLeafNode;
while (curIndex < m_curNodeIndex)
{
//catch bugs in tree data
assert (walkIterations < m_curNodeIndex);
walkIterations++;
aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax);
isLeafNode = rootNode->isLeafNode();
if (isLeafNode && aabbOverlap)
{
nodeCallback->processNode(0,rootNode->getTriangleIndex());
}
if (aabbOverlap || isLeafNode)
{
rootNode++;
curIndex++;
} else
{
escapeIndex = rootNode->getEscapeIndex();
rootNode += escapeIndex;
curIndex += escapeIndex;
}
}
if (maxIterations < walkIterations)
maxIterations = walkIterations;
}
/*
///this was the original recursive traversal, before we optimized towards stackless traversal
void btOptimizedBvh::walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
{
bool isLeafNode, aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax);
@@ -264,46 +449,8 @@ void btOptimizedBvh::walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback
}
}
*/
int maxIterations = 0;
void btOptimizedBvh::walkStacklessTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
{
int escapeIndex, curIndex = 0;
int walkIterations = 0;
bool aabbOverlap, isLeafNode;
while (curIndex < m_curNodeIndex)
{
//catch bugs in tree data
assert (walkIterations < m_curNodeIndex);
walkIterations++;
aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax);
isLeafNode = (!rootNode->m_leftChild && !rootNode->m_rightChild);
if (isLeafNode && aabbOverlap)
{
nodeCallback->processNode(rootNode);
}
if (aabbOverlap || isLeafNode)
{
rootNode++;
curIndex++;
} else
{
escapeIndex = rootNode->m_escapeIndex;
rootNode += escapeIndex;
curIndex += escapeIndex;
}
}
if (maxIterations < walkIterations)
maxIterations = walkIterations;
}
void btOptimizedBvh::reportSphereOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
@@ -311,3 +458,56 @@ void btOptimizedBvh::reportSphereOverlappingNodex(btNodeOverlapCallback* nodeCal
}
void btOptimizedBvh::quantizeWithClamp(unsigned short* out, const btVector3& point) const
{
btAssert(m_useQuantization);
btVector3 clampedPoint(point);
clampedPoint.setMax(m_bvhAabbMin);
clampedPoint.setMin(m_bvhAabbMax);
btVector3 v = (clampedPoint - m_bvhAabbMin) * m_bvhQuantization;
out[0] = (unsigned short)(v.getX()+0.5f);
out[1] = (unsigned short)(v.getY()+0.5f);
out[2] = (unsigned short)(v.getZ()+0.5f);
}
btVector3 btOptimizedBvh::unQuantize(const unsigned short* vecIn) const
{
btVector3 vecOut;
vecOut.setValue(
(btScalar)(vecIn[0]) / (m_bvhQuantization.getX()),
(btScalar)(vecIn[1]) / (m_bvhQuantization.getY()),
(btScalar)(vecIn[2]) / (m_bvhQuantization.getZ()));
vecOut += m_bvhAabbMin;
return vecOut;
}
void btOptimizedBvh::swapLeafNodes(int i,int splitIndex)
{
if (m_useQuantization)
{
btQuantizedBvhNode tmp = m_quantizedLeafNodes[i];
m_quantizedLeafNodes[i] = m_quantizedLeafNodes[splitIndex];
m_quantizedLeafNodes[splitIndex] = tmp;
} else
{
btOptimizedBvhNode tmp = m_leafNodes[i];
m_leafNodes[i] = m_leafNodes[splitIndex];
m_leafNodes[splitIndex] = tmp;
}
}
void btOptimizedBvh::assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex)
{
if (m_useQuantization)
{
m_quantizedContiguousNodes[internalNode] = m_quantizedLeafNodes[leafNodeIndex];
} else
{
m_contiguousNodes[internalNode] = m_leafNodes[leafNodeIndex];
}
}

200
src/BulletCollision/CollisionShapes/btOptimizedBvh.h
View File

@@ -23,95 +23,215 @@ subject to the following restrictions:
//http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp
#include <vector>
class btStridingMeshInterface;
///btQuantizedBvhNode is a compressed aabb node, 16 bytes.
///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
ATTRIBUTE_ALIGNED16 (struct btQuantizedBvhNode)
{
//12 bytes
unsigned short int m_quantizedAabbMin[3];
unsigned short int m_quantizedAabbMax[3];
//4 bytes
int m_escapeIndexOrTriangleIndex;
bool isLeafNode() const
{
//skipindex is negative (internal node), triangleindex >=0 (leafnode)
return (m_escapeIndexOrTriangleIndex >= 0);
}
int getEscapeIndex() const
{
btAssert(!isLeafNode());
return -m_escapeIndexOrTriangleIndex;
}
int getTriangleIndex() const
{
btAssert(isLeafNode());
return m_escapeIndexOrTriangleIndex;
}
};
/// btOptimizedBvhNode contains both internal and leaf node information.
/// It hasn't been optimized yet for storage. Some obvious optimizations are:
/// Removal of the pointers (can already be done, they are not used for traversal)
/// and storing aabbmin/max as quantized integers.
/// 'subpart' doesn't need an integer either. It allows to re-use graphics triangle
/// meshes stored in a non-uniform way (like batches/subparts of triangle-fans
/// Total node size is 44 bytes / node. You can use the compressed version of 16 bytes.
ATTRIBUTE_ALIGNED16 (struct btOptimizedBvhNode)
{
//32 bytes
btVector3 m_aabbMinOrg;
btVector3 m_aabbMaxOrg;
btVector3 m_aabbMin;
btVector3 m_aabbMax;
//these 2 pointers are obsolete, the stackless traversal just uses the escape index
btOptimizedBvhNode* m_leftChild;
btOptimizedBvhNode* m_rightChild;
//4
int m_escapeIndex;
//8
//for child nodes
int m_subPart;
int m_triangleIndex;
};
class btNodeOverlapCallback
{
public:
virtual ~btNodeOverlapCallback() {};
virtual void processNode(const btOptimizedBvhNode* node) = 0;
virtual void processNode(int subPart, int triangleIndex) = 0;
};
#include "../../LinearMath/btAlignedAllocator.h"
#include "../../LinearMath/btAlignedObjectArray.h"
//typedef std::vector< unsigned , allocator_type > container_type;
const unsigned size = (1 << 20);
typedef btAlignedAllocator< btOptimizedBvhNode , size > allocator_type;
//typedef btAlignedObjectArray<btOptimizedBvhNode, allocator_type> NodeArray;
typedef btAlignedObjectArray<btOptimizedBvhNode> NodeArray;
typedef btAlignedObjectArray<btQuantizedBvhNode> QuantizedNodeArray;
///OptimizedBvh store an AABB tree that can be quickly traversed on CPU (and SPU,GPU in future)
class btOptimizedBvh
{
NodeArray m_leafNodes;
btOptimizedBvhNode* m_rootNode1;
btOptimizedBvhNode* m_contiguousNodes;
QuantizedNodeArray m_quantizedLeafNodes;
btQuantizedBvhNode* m_quantizedContiguousNodes;
int m_curNodeIndex;
int m_numNodes;
//quantization data
bool m_useQuantization;
btVector3 m_bvhAabbMin;
btVector3 m_bvhAabbMax;
btVector3 m_bvhQuantization;
//two versions, one for quantized and normal nodes. This allows code-reuse while maintaining readability (no template/macro!)
void setInternalNodeAabbMin(int nodeIndex, const btVector3& aabbMin)
{
if (m_useQuantization)
{
quantizeWithClamp(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] ,aabbMin);
} else
{
m_contiguousNodes[nodeIndex].m_aabbMinOrg = aabbMin;
}
}
void setInternalNodeAabbMax(int nodeIndex,const btVector3& aabbMax)
{
if (m_useQuantization)
{
quantizeWithClamp(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0],aabbMax);
} else
{
m_contiguousNodes[nodeIndex].m_aabbMaxOrg = aabbMax;
}
}
btVector3 getAabbMin(int nodeIndex) const
{
if (m_useQuantization)
{
return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMin[0]);
}
//non-quantized
return m_leafNodes[nodeIndex].m_aabbMinOrg;
}
btVector3 getAabbMax(int nodeIndex) const
{
if (m_useQuantization)
{
return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMax[0]);
}
//non-quantized
return m_leafNodes[nodeIndex].m_aabbMaxOrg;
}
void setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex)
{
if (m_useQuantization)
{
m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = -escapeIndex;
}
else
{
m_contiguousNodes[nodeIndex].m_escapeIndex = escapeIndex;
}
}
void mergeInternalNodeAabb(int nodeIndex,const btVector3& newAabbMin,const btVector3& newAabbMax)
{
if (m_useQuantization)
{
unsigned short int quantizedAabbMin[3];
unsigned short int quantizedAabbMax[3];
quantizeWithClamp(quantizedAabbMin,newAabbMin);
quantizeWithClamp(quantizedAabbMax,newAabbMax);
for (int i=0;i<3;i++)
{
if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] > quantizedAabbMin[i])
m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] = quantizedAabbMin[i];
if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] < quantizedAabbMax[i])
m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] = quantizedAabbMax[i];
}
} else
{
//non-quantized
m_contiguousNodes[nodeIndex].m_aabbMinOrg.setMin(newAabbMin);
m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax);
}
}
void swapLeafNodes(int firstIndex,int secondIndex);
void assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex);
protected:
void buildTree (int startIndex,int endIndex);
int calcSplittingAxis(int startIndex,int endIndex);
int sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis);
void walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
void walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
inline bool testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,unsigned short int* aabbMin2,unsigned short int* aabbMax2) const
{
bool overlap = true;
overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
return overlap;
}
public:
btOptimizedBvh();
virtual ~btOptimizedBvh();
void build(btStridingMeshInterface* triangles);
btOptimizedBvhNode* buildTree (NodeArray& leafNodes,int startIndex,int endIndex);
int calcSplittingAxis(NodeArray& leafNodes,int startIndex,int endIndex);
int sortAndCalcSplittingIndex(NodeArray& leafNodes,int startIndex,int endIndex,int splitAxis);
void walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
void walkStacklessTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
//OptimizedBvhNode* GetRootNode() { return m_rootNode1;}
int getNumNodes() { return m_numNodes;}
void build(btStridingMeshInterface* triangles,bool useQuantizedAabbCompression);
void reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
void reportSphereOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
void quantizeWithClamp(unsigned short* out, const btVector3& point) const;
btVector3 unQuantize(const unsigned short* vecIn) const;
};