// Constructor
_Collision::_Collision(const SpawnStruct &Object)
: _Object(),
TriangleIndexVertexArray(NULL),
TriangleInfoMap(NULL),
VertexList(NULL),
FaceList(NULL) {
gContactAddedCallback = CustomMaterialCallback;
// Load collision mesh file
_File MeshFile;
if(MeshFile.OpenForRead(Object.Template->CollisionFile.c_str())) {
// Read header
int VertCount = MeshFile.ReadInt();
int FaceCount = MeshFile.ReadInt();
// Allocate memory for lists
VertexList = new float[VertCount * 3];
FaceList = new int[FaceCount * 3];
// Read vertices
int VertexIndex = 0;
for(int i = 0; i < VertCount; i++) {
VertexList[VertexIndex++] = MeshFile.ReadFloat();
VertexList[VertexIndex++] = MeshFile.ReadFloat();
VertexList[VertexIndex++] = -MeshFile.ReadFloat();
}
// Read faces
int FaceIndex = 0;
for(int i = 0; i < FaceCount; i++) {
FaceList[FaceIndex+2] = MeshFile.ReadInt();
FaceList[FaceIndex+1] = MeshFile.ReadInt();
FaceList[FaceIndex+0] = MeshFile.ReadInt();
FaceIndex += 3;
}
// Create triangle array
TriangleIndexVertexArray = new btTriangleIndexVertexArray(FaceCount, FaceList, 3 * sizeof(int), VertCount * 3, VertexList, 3 * sizeof(float));
// Create bvh shape
btBvhTriangleMeshShape *Shape = new btBvhTriangleMeshShape(TriangleIndexVertexArray, true);
TriangleInfoMap = new btTriangleInfoMap();
btGenerateInternalEdgeInfo(Shape, TriangleInfoMap);
// Create physics body
CreateRigidBody(Object, Shape);
SetProperties(Object);
RigidBody->setCollisionFlags(RigidBody->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
MeshFile.Close();
}
}
btCollisionShape* Bullet::CreateBulletShape(const PINT_SHAPE_CREATE& desc)
{
btCollisionShape* BulletShape = null;
if(desc.mType==PINT_SHAPE_SPHERE)
{
const PINT_SPHERE_CREATE& SphereCreate = static_cast<const PINT_SPHERE_CREATE&>(desc);
BulletShape = FindSphereShape(SphereCreate);
}
else if(desc.mType==PINT_SHAPE_BOX)
{
const PINT_BOX_CREATE& BoxCreate = static_cast<const PINT_BOX_CREATE&>(desc);
BulletShape = FindBoxShape(BoxCreate);
}
else if(desc.mType==PINT_SHAPE_CAPSULE)
{
const PINT_CAPSULE_CREATE& CapsuleCreate = static_cast<const PINT_CAPSULE_CREATE&>(desc);
BulletShape = FindCapsuleShape(CapsuleCreate);
}
else if(desc.mType==PINT_SHAPE_CONVEX)
{
const PINT_CONVEX_CREATE& ConvexCreate = static_cast<const PINT_CONVEX_CREATE&>(desc);
BulletShape = FindConvexShape(ConvexCreate);
}
else if(desc.mType==PINT_SHAPE_MESH)
{
const PINT_MESH_CREATE& MeshCreate = static_cast<const PINT_MESH_CREATE&>(desc);
// ### share meshes
btTriangleIndexVertexArray* m_indexVertexArrays = new btTriangleIndexVertexArray(
MeshCreate.mSurface.mNbFaces,
(int*)MeshCreate.mSurface.mDFaces,
3*sizeof(udword),
MeshCreate.mSurface.mNbVerts,
(float*)&MeshCreate.mSurface.mVerts->x,
sizeof(Point));
btBvhTriangleMeshShape* shape = new btBvhTriangleMeshShape(m_indexVertexArrays, true, true);
ASSERT(shape);
btTriangleInfoMap* triangleInfoMap = null;
if(gUseInternalEdgeUtility)
{
triangleInfoMap = new btTriangleInfoMap();
btGenerateInternalEdgeInfo(shape, triangleInfoMap);
}
InternalMeshShape MeshData;
MeshData.mShape = shape;
MeshData.mMeshData = m_indexVertexArrays;
MeshData.mTriangleInfoMap = triangleInfoMap;
mMeshShapes.push_back(MeshData);
mCollisionShapes.push_back(shape);
if(desc.mRenderer)
shape->setUserPointer(desc.mRenderer);
shape->setMargin(gCollisionMargin);
BulletShape = shape;
}
else ASSERT(0);
return BulletShape;
}
int main(int argc, char * argv[])
{
btCollisionConfiguration * CollisionConfiguration = new btDefaultCollisionConfiguration();
btCollisionDispatcher * Dispatcher = new btCollisionDispatcher(CollisionConfiguration);
btBroadphaseInterface * OverlappingPairCache = new btDbvtBroadphase();
btConstraintSolver * Solver = new btSequentialImpulseConstraintSolver();
btDynamicsWorld * World = new btDiscreteDynamicsWorld(Dispatcher, OverlappingPairCache, Solver, CollisionConfiguration);
World->setGravity(btVector3(0, -20, 0));
btTriangleMesh Trimesh;
for(float x = 0; x < 100; x += 1)
{
for(float z = -3; z < 3; z += 1)
{
btVector3 v1(x+0, 0, z+0);
btVector3 v2(x+0, 0, z+1);
btVector3 v3(x+1, 0, z+0);
btVector3 v4(x+1, 0, z+1);
Trimesh.addTriangle(v1, v2, v3);
Trimesh.addTriangle(v2, v3, v4);
}
}
btBvhTriangleMeshShape TrimeshShape(&Trimesh, true);
btRigidBody * body = new btRigidBody(0, new btDefaultMotionState, &TrimeshShape);
World->addRigidBody(body);
btTriangleInfoMap * triinfomap = new btTriangleInfoMap();
btGenerateInternalEdgeInfo(&TrimeshShape, triinfomap);
gContactAddedCallback = CustomMaterialCombinerCallback;
body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK | btCollisionObject::CF_STATIC_OBJECT);
btVector3 inertia;
btBoxShape box(btVector3(0.3, 0.3, 0.3));
box.calculateLocalInertia(10, inertia);
btRigidBody * cube = new btRigidBody(10, new btDefaultMotionState(btTransform(btQuaternion::getIdentity(), btVector3(1, 0.3, 0))), &box, inertia);
World->addRigidBody(cube);
cube->setLinearVelocity(btVector3(15,0,0.1));
//cube->setFriction(0);
body->setContactProcessingThreshold(0);
cube->setContactProcessingThreshold(0);
std::fstream f("out.txt", std::fstream::out);
const float dt = 1.0 / 60.0;
for(float t = 0; t < 10; t += dt)
{
World->stepSimulation(1.0/60.0,1000,1.0/60.0);
btVector3 pos = cube->getCenterOfMassPosition();
f.width(12);
f << std::left << t;
f.width(12);
f << pos.x();
f.width(12);
f << pos.y();
f.width(12);
f << pos.z();
f << "\n";
}
return 0;
}
void InternalEdgeDemo::initPhysics()
{
setTexturing(true);
setShadows(false);//true);
#define TRISIZE 10.f
gContactAddedCallback = CustomMaterialCombinerCallback;
#define USE_TRIMESH_SHAPE 1
#ifdef USE_TRIMESH_SHAPE
int vertStride = sizeof(btVector3);
int indexStride = 3*sizeof(int);
const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1);
gVertices = new btVector3[totalVerts];
gIndices = new int[totalTriangles*3];
int i;
setVertexPositions(waveheight,0.f);
//gVertices[1].setY(21.1);
//gVertices[1].setY(121.1);
gVertices[1].setY(.1f);
#ifdef ROTATE_GROUND
//gVertices[1].setY(-1.1);
#else
//gVertices[1].setY(0.1);
//gVertices[1].setY(-0.1);
//gVertices[1].setY(-20.1);
//gVertices[1].setY(-20);
#endif
int index=0;
for ( i=0;i<NUM_VERTS_X-1;i++)
{
for (int j=0;j<NUM_VERTS_Y-1;j++)
{
#ifdef SWAP_WINDING
#ifdef SHIFT_INDICES
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
#else
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i;
#endif //SHIFT_INDICES
#else //SWAP_WINDING
#ifdef SHIFT_INDICES
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = j*NUM_VERTS_X+i+1;
#ifdef TEST_INCONSISTENT_WINDING
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
#else //TEST_INCONSISTENT_WINDING
gIndices[index++] = (j+1)*NUM_VERTS_X+i;
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
#endif //TEST_INCONSISTENT_WINDING
#else //SHIFT_INDICES
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = j*NUM_VERTS_X+i+1;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = (j+1)*NUM_VERTS_X+i;
#endif //SHIFT_INDICES
#endif //SWAP_WINDING
}
}
m_indexVertexArrays = new btTriangleIndexVertexArray(totalTriangles,
gIndices,
indexStride,
totalVerts,(btScalar*) &gVertices[0].x(),vertStride);
bool useQuantizedAabbCompression = true;
//comment out the next line to read the BVH from disk (first run the demo once to create the BVH)
#define SERIALIZE_TO_DISK 1
#ifdef SERIALIZE_TO_DISK
btVector3 aabbMin(-1000,-1000,-1000),aabbMax(1000,1000,1000);
trimeshShape = new btBvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression,aabbMin,aabbMax);
m_collisionShapes.push_back(trimeshShape);
///we can serialize the BVH data
void* buffer = 0;
int numBytes = trimeshShape->getOptimizedBvh()->calculateSerializeBufferSize();
buffer = btAlignedAlloc(numBytes,16);
bool swapEndian = false;
trimeshShape->getOptimizedBvh()->serialize(buffer,numBytes,swapEndian);
#ifdef __QNX__
FILE* file = fopen("app/native/bvh.bin","wb");
#else
FILE* file = fopen("bvh.bin","wb");
#endif
fwrite(buffer,1,numBytes,file);
fclose(file);
btAlignedFree(buffer);
#else
trimeshShape = new btBvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression,false);
char* fileName = "bvh.bin";
#ifdef __QNX__
char* fileName = "app/native/bvh.bin";
#else
char* fileName = "bvh.bin";
#endif
int size=0;
btOptimizedBvh* bvh = 0;
if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET)) { /* File operations denied? ok, just close and return failure */
printf("Error: cannot get filesize from %s\n", fileName);
exit(0);
} else
{
fseek(file, 0, SEEK_SET);
int buffersize = size+btOptimizedBvh::getAlignmentSerializationPadding();
void* buffer = btAlignedAlloc(buffersize,16);
int read = fread(buffer,1,size,file);
fclose(file);
bool swapEndian = false;
bvh = btOptimizedBvh::deSerializeInPlace(buffer,buffersize,swapEndian);
}
trimeshShape->setOptimizedBvh(bvh);
#endif
btCollisionShape* groundShape = trimeshShape;
btTriangleInfoMap* triangleInfoMap = new btTriangleInfoMap();
btGenerateInternalEdgeInfo(trimeshShape,triangleInfoMap);
#else
btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
m_collisionShapes.push_back(groundShape);
#endif //USE_TRIMESH_SHAPE
m_collisionConfiguration = new btDefaultCollisionConfiguration();
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
m_solver = new btSequentialImpulseConstraintSolver();
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
/*
m_dynamicsWorld->getSolverInfo().m_splitImpulse = true;
m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold = 1e30f;
m_dynamicsWorld->getSolverInfo().m_maxErrorReduction = 1e30f;
m_dynamicsWorld->getSolverInfo().m_erp =1.f;
m_dynamicsWorld->getSolverInfo().m_erp2 = 1.f;
*/
m_dynamicsWorld->setGravity(btVector3(0,-10,0));
float mass = 0.f;
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,-2,0));
btConvexHullShape* colShape = new btConvexHullShape();
for (int i=0;i<TaruVtxCount;i++)
{
btVector3 vtx(TaruVtx[i*3],TaruVtx[i*3+1],TaruVtx[i*3+2]);
colShape->addPoint(vtx);
}
//this will enable polyhedral contact clipping, better quality, slightly slower
colShape->initializePolyhedralFeatures();
//the polyhedral contact clipping can use either GJK or SAT test to find the separating axis
m_dynamicsWorld->getDispatchInfo().m_enableSatConvex=false;
m_collisionShapes.push_back(colShape);
{
for (int i=0;i<1;i++)
{
startTransform.setOrigin(btVector3(-10.f+i*3.f,2.2f+btScalar(i)*0.1f,-1.3f));
btRigidBody* body = localCreateRigidBody(10, startTransform,colShape);
body->setActivationState(DISABLE_DEACTIVATION);
body->setLinearVelocity(btVector3(0,0,-1));
//body->setContactProcessingThreshold(0.f);
}
}
{
btBoxShape* colShape = new btBoxShape(btVector3(1,1,1));
colShape->initializePolyhedralFeatures();
m_collisionShapes.push_back(colShape);
startTransform.setOrigin(btVector3(-16.f+i*3.f,1.f+btScalar(i)*0.1f,-1.3f));
btRigidBody* body = localCreateRigidBody(10, startTransform,colShape);
body->setActivationState(DISABLE_DEACTIVATION);
body->setLinearVelocity(btVector3(0,0,-1));
}
startTransform.setIdentity();
#ifdef ROTATE_GROUND
btQuaternion orn(btVector3(0,0,1),SIMD_PI);
startTransform.setOrigin(btVector3(-20,0,0));
startTransform.setRotation(orn);
#endif //ROTATE_GROUND
staticBody = localCreateRigidBody(mass, startTransform,groundShape);
//staticBody->setContactProcessingThreshold(-0.031f);
staticBody->setCollisionFlags(staticBody->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);//STATIC_OBJECT);
//enable custom material callback
staticBody->setCollisionFlags(staticBody->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
getDynamicsWorld()->setDebugDrawer(&gDebugDrawer);
setDebugMode(btIDebugDraw::DBG_DrawText|btIDebugDraw::DBG_NoHelpText+btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints);
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btSetDebugDrawer(&gDebugDrawer);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
}
void CollisionMesh::loadImpl (void)
{
APP_ASSERT(masterShape==NULL);
Ogre::DataStreamPtr file;
try {
file = Ogre::ResourceGroupManager::getSingleton().openResource(name.substr(1), "GRIT");
} catch (Ogre::Exception &e) {
GRIT_EXCEPT(e.getDescription());
}
std::string ext = name.substr(name.length()-5);
uint32_t fourcc = 0;
for (int i=0 ; i<4 ; ++i) {
unsigned char c;
file->read(&c, 1);
fourcc |= c << (i*8);
}
file->seek(0);
std::string dir = grit_dirname(name);
const btVector3 ZV(0,0,0);
const btQuaternion ZQ(0,0,0,1);
bool compute_inertia = false;
bool is_static = false;
if (fourcc==0x4c4f4342) { //BCOL
Ogre::MemoryDataStreamPtr mem =
Ogre::MemoryDataStreamPtr(OGRE_NEW Ogre::MemoryDataStream(name,file));
BColFile &bcol = *reinterpret_cast<BColFile*>(mem->getPtr());
is_static = bcol.mass == 0.0f; // static
masterShape = new btCompoundShape();
BColMaterialMap mmap(dir,name);
for (unsigned i=0 ; i<bcol.hullNum ; ++i) {
BColHull &p = *bcol.hulls(i);
btConvexHullShape *s2 = new btConvexHullShape();
s2->setMargin(p.margin);
for (unsigned j=0 ; j<p.vertNum ; ++j) {
BColVert &v = *p.verts(j);
s2->addPoint(btVector3(v.x, v.y, v.z));
}
masterShape->addChildShape(btTransform(ZQ,ZV), s2);
partMaterials.push_back(mmap(p.mat.name()));
}
for (unsigned i=0 ; i<bcol.boxNum ; ++i) {
BColBox &p = *bcol.boxes(i);
btBoxShape *s2 = new btBoxShape(btVector3(p.dx/2,p.dy/2,p.dz/2));
s2->setMargin(p.margin);
masterShape->addChildShape(btTransform(btQuaternion(p.qx,p.qy,p.qz,p.qw),
btVector3(p.px,p.py,p.pz)), s2);
partMaterials.push_back(mmap(p.mat.name()));
}
for (unsigned i=0 ; i<bcol.cylNum ; ++i) {
BColCyl &p = *bcol.cyls(i);
btCylinderShape *s2 = new btCylinderShapeZ(btVector3(p.dx/2,p.dy/2,p.dz/2));
s2->setMargin(p.margin);
masterShape->addChildShape(btTransform(btQuaternion(p.qx,p.qy,p.qz,p.qw),
btVector3(p.px,p.py,p.pz)), s2);
partMaterials.push_back(mmap(p.mat.name()));
}
for (unsigned i=0 ; i<bcol.coneNum ; ++i) {
BColCone &p = *bcol.cones(i);
btConeShape *s2 = new btConeShapeZ(p.radius,p.height);
s2->setMargin(p.margin);
masterShape->addChildShape(btTransform(btQuaternion(p.qx,p.qy,p.qz,p.qw),
btVector3(p.px,p.py,p.pz)), s2);
partMaterials.push_back(mmap(p.mat.name()));
}
for (unsigned i=0 ; i<bcol.planeNum ; ++i) {
BColPlane &p = *bcol.planes(i);
btStaticPlaneShape *s2 = new btStaticPlaneShape(btVector3(p.nx,p.ny,p.nz),p.d);
masterShape->addChildShape(btTransform(ZQ,ZV), s2);
partMaterials.push_back(mmap(p.mat.name()));
}
for (unsigned i=0 ; i<bcol.sphereNum ; ++i) {
BColSphere &p = *bcol.spheres(i);
btSphereShape *s2 = new btSphereShape(p.radius);
masterShape->addChildShape(btTransform(ZQ, btVector3(p.px,p.py,p.pz)), s2);
partMaterials.push_back(mmap(p.mat.name()));
}
if (bcol.triMeshFaceNum > 0) {
bcolVerts.resize(bcol.triMeshVertNum);
bcolFaces.resize(bcol.triMeshFaceNum);
memcpy(&bcolVerts[0], bcol.triMeshVerts(0), bcol.triMeshVertNum * sizeof(BColVert));
memcpy(&bcolFaces[0], bcol.triMeshFaces(0), bcol.triMeshFaceNum * sizeof(BColFace));
faceMaterials.reserve(bcol.triMeshFaceNum);
int counter = 0;
float accum_area = 0;
for (unsigned i=0 ; i<bcol.triMeshFaceNum ; ++i) {
BColFace &face = *bcol.triMeshFaces(i);
PhysicalMaterial *mat = mmap(face.mat.name());
faceMaterials.push_back(mat);
CollisionMesh::ProcObjFace po_face(to_v3(bcolVerts[face.v1]),
to_v3(bcolVerts[face.v2]),
to_v3(bcolVerts[face.v3]));
procObjFaceDB[mat->id].faces.push_back(po_face);
float area = (po_face.AB.cross(po_face.AC)).length();
APP_ASSERT(area>=0);
procObjFaceDB[mat->id].areas.push_back(area);
procObjFaceDB[mat->id].totalArea += area;
if (++counter = 10) {
counter = 0;
accum_area = 0;
procObjFaceDB[mat->id].areas10.push_back(accum_area);
}
accum_area += area;
}
btTriangleIndexVertexArray *v = new btTriangleIndexVertexArray(
bcolFaces.size(), reinterpret_cast<int*>(&(bcolFaces[0].v1)), sizeof(BColFace),
bcolVerts.size(), &(bcolVerts[0].x), sizeof(BColVert));
if (is_static) {
btBvhTriangleMeshShape *tm = new btBvhTriangleMeshShape(v,true,true);
tm->setMargin(bcol.triMeshMargin);
btTriangleInfoMap* tri_info_map = new btTriangleInfoMap();
tri_info_map->m_edgeDistanceThreshold = bcol.triMeshEdgeDistanceThreshold;
btGenerateInternalEdgeInfo(tm,tri_info_map);
masterShape->addChildShape(btTransform::getIdentity(), tm);
} else {
// skip over dynamic trimesh
}
}
setMass(bcol.mass);
setLinearDamping(bcol.linearDamping);
setAngularDamping(bcol.angularDamping);
setLinearSleepThreshold(bcol.linearSleepThreshold);
setAngularSleepThreshold(bcol.angularSleepThreshold);
setCCDMotionThreshold(bcol.ccdMotionThreshold);
setCCDSweptSphereRadius(bcol.ccdSweptSphereRadius);
setInertia(Vector3(bcol.inertia[0],bcol.inertia[1],bcol.inertia[2]));
compute_inertia = !bcol.inertiaProvided;
} else if (fourcc==0x4c4f4354) { //TCOL
ProxyStreamBuf proxy(file);
std::istream stream(&proxy);
quex::tcol_lexer qlex(&stream);
TColFile tcol;
parse_tcol_1_0(name,&qlex,tcol);
is_static = tcol.mass == 0.0f; // static
masterShape = new btCompoundShape();
if (tcol.usingCompound) {
TColCompound &c = tcol.compound;
for (size_t i=0 ; i<c.hulls.size() ; ++i) {
const TColHull &h = c.hulls[i];
btConvexHullShape *s2 = new btConvexHullShape();
s2->setMargin(h.margin);
for (unsigned j=0 ; j<h.vertexes.size() ; ++j) {
const Vector3 &v = h.vertexes[j];
s2->addPoint(to_bullet(v));
}
masterShape->addChildShape(btTransform(ZQ,ZV), s2);
partMaterials.push_back(phys_mats.getMaterial(dir,name,h.material));
}
for (size_t i=0 ; i<c.boxes.size() ; ++i) {
const TColBox &b = c.boxes[i];
/* implement with hulls
btConvexHullShape *s2 = new btConvexHullShape();
s2->addPoint(btVector3(-b.dx/2+b.margin, -b.dy/2+b.margin, -b.dz/2+b.margin));
s2->addPoint(btVector3(-b.dx/2+b.margin, -b.dy/2+b.margin, b.dz/2-b.margin));
s2->addPoint(btVector3(-b.dx/2+b.margin, b.dy/2-b.margin, -b.dz/2+b.margin));
s2->addPoint(btVector3(-b.dx/2+b.margin, b.dy/2-b.margin, b.dz/2-b.margin));
s2->addPoint(btVector3( b.dx/2-b.margin, -b.dy/2+b.margin, -b.dz/2+b.margin));
s2->addPoint(btVector3( b.dx/2-b.margin, -b.dy/2+b.margin, b.dz/2-b.margin));
s2->addPoint(btVector3( b.dx/2-b.margin, b.dy/2-b.margin, -b.dz/2+b.margin));
s2->addPoint(btVector3( b.dx/2-b.margin, b.dy/2-b.margin, b.dz/2-b.margin));
*/
btBoxShape *s2 =new btBoxShape(btVector3(b.dx/2,b.dy/2,b.dz/2));
s2->setMargin(b.margin);
masterShape->addChildShape(btTransform(btQuaternion(b.qx,b.qy,b.qz,b.qw),
btVector3(b.px,b.py,b.pz)), s2);
partMaterials.push_back(phys_mats.getMaterial(dir,name,b.material));
}
for (size_t i=0 ; i<c.cylinders.size() ; ++i) {
const TColCylinder &cyl = c.cylinders[i];
btCylinderShape *s2 =
new btCylinderShapeZ(btVector3(cyl.dx/2,cyl.dy/2,cyl.dz/2));
s2->setMargin(cyl.margin);
masterShape->addChildShape(
btTransform(btQuaternion(cyl.qx,cyl.qy,cyl.qz,cyl.qw),
btVector3(cyl.px,cyl.py,cyl.pz)), s2);
partMaterials.push_back(phys_mats.getMaterial(dir,name,cyl.material));
}
for (size_t i=0 ; i<c.cones.size() ; ++i) {
const TColCone &cone = c.cones[i];
btConeShapeZ *s2 = new btConeShapeZ(cone.radius,cone.height);
s2->setMargin(cone.margin);
masterShape->addChildShape(
btTransform(btQuaternion(cone.qx,cone.qy,cone.qz,cone.qw),
btVector3(cone.px,cone.py,cone.pz)), s2);
partMaterials.push_back(phys_mats.getMaterial(dir,name,cone.material));
}
for (size_t i=0 ; i<c.planes.size() ; ++i) {
const TColPlane &p = c.planes[i];
btStaticPlaneShape *s2 =
new btStaticPlaneShape(btVector3(p.nx,p.ny,p.nz),p.d);
masterShape->addChildShape(btTransform(ZQ,ZV), s2);
partMaterials.push_back(phys_mats.getMaterial(dir,name,p.material));
}
for (size_t i=0 ; i<c.spheres.size() ; ++i) {
const TColSphere &sp = c.spheres[i];
btSphereShape *s2 = new btSphereShape(sp.radius);
masterShape->addChildShape(btTransform(ZQ,
btVector3(sp.px,sp.py,sp.pz)), s2);
partMaterials.push_back(phys_mats.getMaterial(dir,name,sp.material));
}
}
if (tcol.usingTriMesh) {
TColTriMesh &t = tcol.triMesh;
std::swap(verts, t.vertexes);
std::swap(faces, t.faces);
faceMaterials.reserve(faces.size());
int counter = 0;
float accum_area = 0;
for (TColFaces::const_iterator i=faces.begin(), i_=faces.end() ; i!=i_ ; ++i) {
//optimisation possible here by changing the TCol struct to be more liek what
//bullet wants, and then re-using memory
PhysicalMaterial *mat = phys_mats.getMaterial(dir,name,i->material);
faceMaterials.push_back(mat);
CollisionMesh::ProcObjFace po_face(verts[i->v1], verts[i->v2], verts[i->v3]);
procObjFaceDB[mat->id].faces.push_back(po_face);
float area = (po_face.AB.cross(po_face.AC)).length();
APP_ASSERT(area>=0);
procObjFaceDB[mat->id].areas.push_back(area);
procObjFaceDB[mat->id].totalArea += area;
if (++counter = 10) {
counter = 0;
accum_area = 0;
procObjFaceDB[mat->id].areas10.push_back(accum_area);
}
accum_area += area;
}
btTriangleIndexVertexArray *v = new btTriangleIndexVertexArray(
faces.size(), &(faces[0].v1), sizeof(TColFace),
verts.size(), &(verts[0].x), sizeof(Vector3));
if (is_static) {
btBvhTriangleMeshShape *tm = new btBvhTriangleMeshShape(v,true,true);
tm->setMargin(t.margin);
btTriangleInfoMap* tri_info_map = new btTriangleInfoMap();
tri_info_map->m_edgeDistanceThreshold = t.edgeDistanceThreshold;
btGenerateInternalEdgeInfo(tm,tri_info_map);
masterShape->addChildShape(btTransform::getIdentity(), tm);
} else {
// Skip over dynamic trimesh
}
}
setMass(tcol.mass);
setInertia(Vector3(tcol.inertia_x,tcol.inertia_y,tcol.inertia_z));
setLinearDamping(tcol.linearDamping);
setAngularDamping(tcol.angularDamping);
setLinearSleepThreshold(tcol.linearSleepThreshold);
setAngularSleepThreshold(tcol.angularSleepThreshold);
setCCDMotionThreshold(tcol.ccdMotionThreshold);
setCCDSweptSphereRadius(tcol.ccdSweptSphereRadius);
compute_inertia = !tcol.hasInertia;
} else {
GRIT_EXCEPT("Collision mesh \""+name+"\" seems to be corrupt.");
}
if (is_static) {
setInertia(Vector3(0,0,0));
} else {
if (faceMaterials.size() > 0) {
CERR << "While loading \"" + name + "\": Dynamic trimesh not supported." << std::endl;
}
if (compute_inertia) {
btVector3 i;
masterShape->calculateLocalInertia(mass,i);
setInertia(from_bullet(i));
}
}
}
int main(int ArgumentCount, char **Arguments) {
// Set up physics modules
CollisionConfiguration = new btDefaultCollisionConfiguration();
//BroadPhase = new btAxisSweep3(btVector3(-1000, -1000, -1000), btVector3(1000, 1000, 1000));
BroadPhase = new btDbvtBroadphase();
Dispatcher = new btCollisionDispatcher(CollisionConfiguration);
Solver = new btSequentialImpulseConstraintSolver();
World = new btDiscreteDynamicsWorld(Dispatcher, BroadPhase, Solver, CollisionConfiguration);
World->setGravity(btVector3(0.0, -9.81, 0.0));
btContactSolverInfo &SolverInfo = World->getSolverInfo();
//SolverInfo.m_splitImpulseTurnErp = 0.0f;
//SolverInfo.m_splitImpulse = 1;
//SolverInfo.m_splitImpulsePenetrationThreshold = -0.02f;
gContactAddedCallback = CustomMaterialCallback;
{
btScalar Mass = 1.0;
btCollisionShape *Shape = new btSphereShape(0.5);
btVector3 LocalInertia(0.0, 0.0, 0.0);
Shape->calculateLocalInertia(Mass, LocalInertia);
SphereBody = new btRigidBody(Mass, NULL, Shape, LocalInertia);
SphereBody->setFriction(1.0);
SphereBody->setDamping(0.1, 0.3);
SphereBody->setActivationState(DISABLE_DEACTIVATION);
SphereBody->translate(btVector3(0.71, 2.5, -0.8));
World->addRigidBody(SphereBody);
}
// Set to 1 to test static box shape
if(USE_STATIC_BOX) {
btScalar Mass = 0.0;
btCollisionShape *Shape = new btBoxShape(btVector3(1, 1, 1));
btVector3 LocalInertia(0.0, 0.0, 0.0);
Shape->calculateLocalInertia(Mass, LocalInertia);
BoxBody = new btRigidBody(Mass, NULL, Shape, LocalInertia);
BoxBody->setFriction(1.0);
BoxBody->setDamping(0.1, 0.3);
BoxBody->setActivationState(DISABLE_DEACTIVATION);
BoxBody->translate(btVector3(0, -2, 0));
World->addRigidBody(BoxBody);
}
else {
btScalar Mass = 0.0;
int VertexCount = sizeof(Vertices) / sizeof(btScalar);
int FaceCount = sizeof(Indices) / sizeof(int) / 3;
TriangleIndexVertexArray = new btTriangleIndexVertexArray(FaceCount, Indices, 3 * sizeof(int), VertexCount, Vertices, 3 * sizeof(btScalar));
btBvhTriangleMeshShape *Shape = new btBvhTriangleMeshShape(TriangleIndexVertexArray, true);
btTriangleInfoMap *TriangleInfoMap = new btTriangleInfoMap();
btGenerateInternalEdgeInfo(Shape, TriangleInfoMap);
MeshBody = new btRigidBody(Mass, NULL, Shape);
MeshBody->setFriction(1.0);
MeshBody->setDamping(0.1, 0.3);
MeshBody->setCollisionFlags(MeshBody->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
// Add body
World->addRigidBody(MeshBody);
}
for(int i = 0; i < 100; i++) {
World->stepSimulation(TIMESTEP, 0, 0);
const btTransform &Transform = SphereBody->getCenterOfMassTransform();
const btVector3 &Position = Transform.getOrigin();
printf("%f %f %f\n", Position.getX(), Position.getY(), Position.getZ());
}
World->removeRigidBody(SphereBody);
if(MeshBody)
World->removeRigidBody(MeshBody);
if(BoxBody)
World->removeRigidBody(BoxBody);
delete SphereBody->getCollisionShape();
delete SphereBody;
if(MeshBody) {
delete ((btBvhTriangleMeshShape *)MeshBody->getCollisionShape())->getTriangleInfoMap();
delete ((btBvhTriangleMeshShape *)MeshBody->getCollisionShape())->getMeshInterface();
delete MeshBody->getCollisionShape();
delete MeshBody;
}
if(BoxBody) {
delete BoxBody->getCollisionShape();
delete BoxBody;
}
delete World;
delete Solver;
delete Dispatcher;
delete BroadPhase;
delete CollisionConfiguration;
return 0;
}
