virtual void btcSetAabb(btcAabbSpace bp, btcAabbProxy aabbHandle, float minX,float minY,float minZ, float maxX,float maxY, float maxZ)
{
btBroadphaseProxy* proxy = (btBroadphaseProxy*) aabbHandle;
btVector3 aabbMin(minX,minY,minZ);
btVector3 aabbMax(maxX,maxY,maxZ);
m_simpleBP->setAabb(proxy,aabbMin,aabbMax,0);
}
static
int addIslandToLargeTriMesh(PfxLargeTriMesh &lmesh,PfxTriMesh &island)
{
SCE_PFX_ASSERT(island.m_numFacets <= SCE_PFX_NUMMESHFACETS);
int newIsland = lmesh.m_numIslands++;
lmesh.m_islands[newIsland] = island;
// アイランドローカルのAABBを計算
if(island.m_numFacets > 0) {
PfxVector3 aabbMin(SCE_PFX_FLT_MAX),aabbMax(-SCE_PFX_FLT_MAX);
for(PfxUInt32 i=0;i<island.m_numFacets;i++) {
aabbMin = minPerElem(aabbMin,pfxReadVector3(island.m_facets[i].m_center)-pfxReadVector3(island.m_facets[i].m_half));
aabbMax = maxPerElem(aabbMax,pfxReadVector3(island.m_facets[i].m_center)+pfxReadVector3(island.m_facets[i].m_half));
}
PfxVecInt3 aabbMinL,aabbMaxL;
lmesh.getLocalPosition(aabbMin,aabbMax,aabbMinL,aabbMaxL);
pfxSetXMin(lmesh.m_aabbList[newIsland],aabbMinL.getX());
pfxSetXMax(lmesh.m_aabbList[newIsland],aabbMaxL.getX());
pfxSetYMin(lmesh.m_aabbList[newIsland],aabbMinL.getY());
pfxSetYMax(lmesh.m_aabbList[newIsland],aabbMaxL.getY());
pfxSetZMin(lmesh.m_aabbList[newIsland],aabbMinL.getZ());
pfxSetZMax(lmesh.m_aabbList[newIsland],aabbMaxL.getZ());
}
return newIsland;
}
TriangleMeshPhysicsObject::TriangleMeshPhysicsObject(BaseMesh* baseMesh, float mass,
float restitution, float friction, bool dentable, PlasticityMaterial* material)
: RigidPhysicsObject(baseMesh)
{
this->material = material;
this->dentable = dentable;
int numTriangles = baseMesh->elementArray.size()/3;
int vertexStride = sizeof(Vertex);
int indexStride = 3*sizeof(int);
int numVertices = baseMesh->numVertices;
btTriangleIndexVertexArray* indexVertexArrays = new btTriangleIndexVertexArray(
numTriangles,attachedMesh->getElements(),indexStride,numVertices,
(float*)attachedMesh->getVertices(),vertexStride);
if(mass == 0)
{
float dim = 100000;
btVector3 aabbMin(-dim,-dim,-dim);
btVector3 aabbMax(dim,dim,dim);
btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(indexVertexArrays,true,aabbMin,aabbMax);
trimesh->setUserPointer(this);
this->collisionShape = trimesh;
}
else
{
btGImpactMeshShape* trimesh = new btGImpactMeshShape(indexVertexArrays);
trimesh->updateBound();
trimesh->setUserPointer(this);
this->collisionShape = trimesh;
}
this->createRigidBody(mass,friction,restitution);
//this->collisionObject->activate(true);
}
void BulletPhysics::AddPointCloud(Vec3* verts, int numPoints, WeakGameObjectPtr pGameObject, const std::string& densityStr, const std::string& physicsMaterial)
{
// used to create a convex mesh shape
StrongGameObjectPtr pStrongObject = MakeStrongPtr(pGameObject);
if (!pStrongObject)
{
CB_ERROR("Must attach a game object to the point cloud");
return;
}
btConvexHullShape* shape = new btConvexHullShape();
// add points to the shape one at a time
for (int i = 0; i < numPoints; i++)
{
shape->addPoint(Vec3_to_btVector3(verts[i]));
}
// approximate mass using bounding box
btVector3 aabbMin(0, 0, 0), aabbMax(0, 0, 0);
shape->getAabb(btTransform::getIdentity(), aabbMin, aabbMax);
const btVector3 aabbExtents = aabbMax - aabbMin;
float specificGravity = LookupSpecificGravity(densityStr);
const float volume = aabbExtents.x() * aabbExtents.y() * aabbExtents.z();
const btScalar mass = volume * specificGravity;
AddShape(pStrongObject, shape, mass, physicsMaterial);
}
void FeatherstoneMultiBodyDemo::clientMoveAndDisplay()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
//simple dynamics world doesn't handle fixed-time-stepping
float ms = getDeltaTimeMicroseconds();
///step the simulation
if (m_dynamicsWorld)
{
m_dynamicsWorld->stepSimulation(ms / 1000000.f);
//optional but useful: debug drawing
m_dynamicsWorld->debugDrawWorld();
btVector3 aabbMin(1,1,1);
btVector3 aabbMax(2,2,2);
}
renderme();
glFlush();
swapBuffers();
}
//--------------------------------------------------------------
void ofxBulletTriMeshShape::updateMesh( btDiscreteDynamicsWorld* a_world, ofMesh& aMesh ) {
if( aMesh.getNumVertices() != totalVerts || aMesh.getNumIndices() != totalIndices ) {
ofLogWarning() << "updateMesh :: the verts or the indices are not the correct size, not updating";
return;
}
auto& tverts = aMesh.getVertices();
btVector3 aabbMin(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
btVector3 aabbMax(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT);
for( int i = 0; i < totalVerts; i++ ) {
auto& v = tverts[i];
bullet_vertices[i].setValue( v.x, v.y, v.z );
aabbMin.setMin( bullet_vertices[i] );
aabbMax.setMax( bullet_vertices[i] );
}
btBvhTriangleMeshShape* triShape = (btBvhTriangleMeshShape*)_shape;
// triShape->partialRefitTree( aabbMin, aabbMax );
triShape->refitTree( aabbMin, aabbMax );
//clear all contact points involving mesh proxy. Note: this is a slow/unoptimized operation.
a_world->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs( getRigidBody()->getBroadphaseHandle(), a_world->getDispatcher());
}
void BasicDemo::clientMoveAndDisplay()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
//simple dynamics world doesn't handle fixed-time-stepping
float ms = getDeltaTimeMicroseconds();
///step the simulation
if (m_dynamicsWorld)
{
m_dynamicsWorld->stepSimulation(ms / 1000000.f);
//optional but useful: debug drawing
m_dynamicsWorld->debugDrawWorld();
btVector3 aabbMin(1,1,1);
btVector3 aabbMax(2,2,2);
MyOverlapCallback aabbOverlap(aabbMin,aabbMax);
m_dynamicsWorld->getBroadphase()->aabbTest(aabbMin,aabbMax,aabbOverlap);
//if (aabbOverlap.m_numOverlap)
// printf("#aabb overlap = %d\n", aabbOverlap.m_numOverlap);
}
renderme();
glFlush();
swapBuffers();
}
int CLPhysicsDemo::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userIndex)
{
btVector3 aabbMin(0,0,0),aabbMax(0,0,0);
if (collidableIndex>=0)
{
btAABBHost hostLocalAabbMin = m_data->m_localShapeAABBCPU->at(collidableIndex*2);
btAABBHost hostLocalAabbMax = m_data->m_localShapeAABBCPU->at(collidableIndex*2+1);
btVector3 localAabbMin(hostLocalAabbMin.fx,hostLocalAabbMin.fy,hostLocalAabbMin.fz);
btVector3 localAabbMax(hostLocalAabbMax.fx,hostLocalAabbMax.fy,hostLocalAabbMax.fz);
btScalar margin = 0.01f;
btTransform t;
t.setIdentity();
t.setOrigin(btVector3(position[0],position[1],position[2]));
t.setRotation(btQuaternion(orientation[0],orientation[1],orientation[2],orientation[3]));
btTransformAabb(localAabbMin,localAabbMax, margin,t,aabbMin,aabbMax);
//(position[0],position[0],position[0]);
//aabbMin -= btVector3(400.f,410.f,400.f);
//aabbMax += btVector3(400.f,410.f,400.f);
//btBroadphaseProxy* proxy = m_data->m_Broadphase->createProxy(aabbMin,aabbMax,collisionShapeIndex,userPointer,1,1,0,0);//m_dispatcher);
if (useSapGpuBroadphase)
m_data->m_BroadphaseSap->createProxy(aabbMin,aabbMax,userIndex,1,1);//m_dispatcher);
else
{
void* userPtr = (void*)userIndex;
m_data->m_BroadphaseGrid->createProxy(aabbMin,aabbMax,collidableIndex,userPtr ,1,1);//m_dispatcher);
}
}
bool writeToGpu = false;
int bodyIndex = -1;
m_data->m_linVelHost.push_back(btVector3(0,0,0));
m_data->m_angVelHost.push_back(btVector3(0,0,0));
m_data->m_bodyTimesHost.push_back(0.f);
if (narrowphaseAndSolver)
{
//bodyIndex = narrowphaseAndSolver->registerRigidBody(collisionShapeIndex,CollisionShape::SHAPE_CONVEX_HEIGHT_FIELD,mass,position,orientation,&aabbMin.getX(),&aabbMax.getX(),writeToGpu);
bodyIndex = narrowphaseAndSolver->registerRigidBody(collidableIndex,mass,position,orientation,&aabbMin.getX(),&aabbMax.getX(),writeToGpu);
}
if (mass>0.f)
m_numDynamicPhysicsInstances++;
m_numPhysicsInstances++;
return bodyIndex;
}
void TSRODERigidBody::DebugRender()
{
Graphics()->SetRasterizerState( Graphics()->m_FillWireFrameState );
TSRMatrix4 bodyTransform;
TSRMatrix4 geomTransform;
const float* pBodyPosition = dBodyGetPosition( m_BodyID );
const float* pBodyRotation = dBodyGetRotation( m_BodyID );
ODEToMatrix4( bodyTransform, pBodyPosition, pBodyRotation );
TSRGlobalConstants.PushMatrix();
TSRGlobalConstants.MultMatrix( bodyTransform.d );
TSRDebugDraw::RenderAxis( 1.0f );
TSRDebugDraw::RenderSphere( 0.25f );
for ( unsigned int i = 0; i < m_GeomIDs.size(); i++ )
{
dGeomID currGeomTransformID = m_GeomIDs[ i ];
dGeomID geomID = dGeomTransformGetGeom( currGeomTransformID );
const float* pGeomPosition = dGeomGetPosition( geomID );
const float* pGeomRotation = dGeomGetRotation( geomID );
ODEToMatrix4( bodyTransform, pGeomPosition, pGeomRotation );
TSRGlobalConstants.PushMatrix();
TSRGlobalConstants.MultMatrix( bodyTransform.d );
switch( dGeomGetClass( geomID ) )
{
case dBoxClass:
{
dVector3 extents;
dGeomBoxGetLengths( geomID, extents );
TSRVector3 aabbMin( -extents[ 0 ], -extents[ 1 ], -extents[ 2 ] );
TSRVector3 aabbMax( +extents[ 0 ], +extents[ 1 ], +extents[ 2 ] );
aabbMin *= 0.5f;
aabbMax *= 0.5f;
TSRDebugDraw::RenderAABB( aabbMin, aabbMax );
}
break;
case dSphereClass:
{
float radius;
radius = dGeomSphereGetRadius( geomID );
TSRDebugDraw::RenderSphere( radius );
}
break;
case dCylinderClass:
{
float radius,length;
dGeomCylinderGetParams( geomID, &radius, &length );
TSRDebugDraw::RenderCylinder( length, radius, TSRVector3( 0.0f, 0.0f, 1.0f ) );
}
break;
}
TSRGlobalConstants.PopMatrix();
}
TSRGlobalConstants.PopMatrix();
Graphics()->SetRasterizerState( Graphics()->m_FillSolidState );
}
void ConcaveConvexcastDemo::clientMoveAndDisplay()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
float dt = getDeltaTimeMicroseconds() * 0.000001f;
if (m_animatedMesh)
{
static float offset=0.f;
offset+=0.01f;
int i;
int j;
btVector3 aabbMin(1e30,1e30,1e30);
btVector3 aabbMax(-1e30,-1e30,-1e30);
for ( i=NUM_VERTS_X/2-3;i<NUM_VERTS_X/2+2;i++)
{
for (j=NUM_VERTS_X/2-3;j<NUM_VERTS_Y/2+2;j++)
{
aabbMax.setMax(gVertices[i+j*NUM_VERTS_X]);
aabbMin.setMin(gVertices[i+j*NUM_VERTS_X]);
gVertices[i+j*NUM_VERTS_X].setValue((i-NUM_VERTS_X*0.5f)*TRIANGLE_SIZE,
//0.f,
waveheight*sinf((float)i+offset)*cosf((float)j+offset),
(j-NUM_VERTS_Y*0.5f)*TRIANGLE_SIZE);
aabbMin.setMin(gVertices[i+j*NUM_VERTS_X]);
aabbMax.setMax(gVertices[i+j*NUM_VERTS_X]);
}
}
trimeshShape->partialRefitTree(aabbMin,aabbMax);
//clear all contact points involving mesh proxy. Note: this is a slow/unoptimized operation.
m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(staticBody->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher());
}
m_dynamicsWorld->stepSimulation(dt);
//optional but useful: debug drawing
m_dynamicsWorld->debugDrawWorld();
convexcastBatch.move (dt);
convexcastBatch.cast (m_dynamicsWorld);
renderme();
convexcastBatch.draw ();
glFlush();
glutSwapBuffers();
}
virtual btcAabbProxy btcCreateAabbProxy(btcAabbSpace bp, void* clientData, float minX,float minY,float minZ, float maxX,float maxY, float maxZ)
{
btVector3 aabbMin(minX,minY,minZ);
btVector3 aabbMax(maxX,maxY,maxZ);
void* multiSapProxy=0;
btDispatcher* dispatcher = 0;
int shapeType = 0;
unsigned short int collisionFilterGroup = 1;
unsigned short int collisionFilterMask = 1;
return (btcAabbProxy) m_simpleBP->createProxy(aabbMin,aabbMax,shapeType, clientData, collisionFilterGroup, collisionFilterMask, dispatcher,multiSapProxy);
// virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy);
}
bool HeightmapCollisionShape::drawWireFrame(DebugLines *wire,
const Ogre::Vector3 &pos = Ogre::Vector3::ZERO,
const Ogre::Quaternion &quat= Ogre::Quaternion::IDENTITY) const
{
btHeightfieldTerrainShape* pHeightShape = static_cast<btHeightfieldTerrainShape*>(mShape);
btTransform bt;
bt.setIdentity();
btVector3 colour(255.0, 255.0, 255.0);
DebugHelper ddraw(wire);
DebugTriangleDrawCallback cb(&ddraw, bt, colour);
btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
pHeightShape->processAllTriangles(&cb, aabbMin, aabbMax);
return true;
}
void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
{
//use 'w' component of supportVerticesOut?
{
for (int i = 0; i < numVectors; i++)
{
supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
}
}
///@todo: could do the batch inside the callback!
for (int j = 0; j < numVectors; j++)
{
const btVector3& vec = vectors[j];
LocalSupportVertexCallback supportCallback(vec);
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
m_stridingMesh->InternalProcessAllTriangles(&supportCallback, -aabbMax, aabbMax);
supportVerticesOut[j] = supportCallback.GetSupportVertexLocal();
}
}
btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
btVector3 vec = vec0;
btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001))
{
vec.setValue(1,0,0);
} else
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen;
}
LocalSupportVertexCallback supportCallback(vec);
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
supVec = supportCallback.GetSupportVertexLocal();
return supVec;
}
SimdVector3 ConvexTriangleMeshShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec0)const
{
SimdVector3 supVec(0.f,0.f,0.f);
SimdVector3 vec = vec0;
SimdScalar lenSqr = vec.length2();
if (lenSqr < 0.0001f)
{
vec.setValue(1,0,0);
} else
{
float rlen = 1.f / SimdSqrt(lenSqr );
vec *= rlen;
}
LocalSupportVertexCallback supportCallback(vec);
SimdVector3 aabbMax(1e30f,1e30f,1e30f);
m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
supVec = supportCallback.GetSupportVertexLocal();
return supVec;
}
void ConvexTriangleMeshShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const
{
//use 'w' component of supportVerticesOut?
{
for (int i=0;i<numVectors;i++)
{
supportVerticesOut[i][3] = -1e30f;
}
}
//todo: could do the batch inside the callback!
for (int j=0;j<numVectors;j++)
{
const SimdVector3& vec = vectors[j];
LocalSupportVertexCallback supportCallback(vec);
SimdVector3 aabbMax(1e30f,1e30f,1e30f);
m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
supportVerticesOut[j] = supportCallback.GetSupportVertexLocal();
}
}
void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax)
{
m_useQuantization = useQuantizedAabbCompression;
// NodeArray triangleNodes;
struct NodeTriangleCallback : public btInternalTriangleIndexCallback
{
NodeArray& m_triangleNodes;
NodeTriangleCallback& operator=(NodeTriangleCallback& other)
{
m_triangleNodes.copyFromArray(other.m_triangleNodes);
return *this;
}
NodeTriangleCallback(NodeArray& triangleNodes)
:m_triangleNodes(triangleNodes)
{
}
virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
{
btOptimizedBvhNode node;
btVector3 aabbMin,aabbMax;
aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
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;
//for child nodes
node.m_subPart = partId;
node.m_triangleIndex = triangleIndex;
m_triangleNodes.push_back(node);
}
};
struct QuantizedNodeTriangleCallback : public btInternalTriangleIndexCallback
{
QuantizedNodeArray& m_triangleNodes;
const btQuantizedBvh* m_optimizedTree; // for quantization
QuantizedNodeTriangleCallback& operator=(QuantizedNodeTriangleCallback& other)
{
m_triangleNodes.copyFromArray(other.m_triangleNodes);
m_optimizedTree = other.m_optimizedTree;
return *this;
}
QuantizedNodeTriangleCallback(QuantizedNodeArray& triangleNodes,const btQuantizedBvh* tree)
:m_triangleNodes(triangleNodes),m_optimizedTree(tree)
{
}
virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
{
// The partId and triangle index must fit in the same (positive) integer
btAssert(partId < (1<<MAX_NUM_PARTS_IN_BITS));
btAssert(triangleIndex < (1<<(31-MAX_NUM_PARTS_IN_BITS)));
//negative indices are reserved for escapeIndex
btAssert(triangleIndex>=0);
btQuantizedBvhNode node;
btVector3 aabbMin,aabbMax;
aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
aabbMin.setMin(triangle[0]);
aabbMax.setMax(triangle[0]);
aabbMin.setMin(triangle[1]);
aabbMax.setMax(triangle[1]);
aabbMin.setMin(triangle[2]);
aabbMax.setMax(triangle[2]);
//PCK: add these checks for zero dimensions of aabb
const btScalar MIN_AABB_DIMENSION = btScalar(0.002);
const btScalar MIN_AABB_HALF_DIMENSION = btScalar(0.001);
if (aabbMax.x() - aabbMin.x() < MIN_AABB_DIMENSION)
{
aabbMax.setX(aabbMax.x() + MIN_AABB_HALF_DIMENSION);
aabbMin.setX(aabbMin.x() - MIN_AABB_HALF_DIMENSION);
}
if (aabbMax.y() - aabbMin.y() < MIN_AABB_DIMENSION)
{
aabbMax.setY(aabbMax.y() + MIN_AABB_HALF_DIMENSION);
aabbMin.setY(aabbMin.y() - MIN_AABB_HALF_DIMENSION);
}
if (aabbMax.z() - aabbMin.z() < MIN_AABB_DIMENSION)
{
aabbMax.setZ(aabbMax.z() + MIN_AABB_HALF_DIMENSION);
aabbMin.setZ(aabbMin.z() - MIN_AABB_HALF_DIMENSION);
}
m_optimizedTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0);
m_optimizedTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1);
node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex;
m_triangleNodes.push_back(node);
}
};
int numLeafNodes = 0;
if (m_useQuantization)
{
//initialize quantization values
setQuantizationValues(bvhAabbMin,bvhAabbMax);
QuantizedNodeTriangleCallback callback(m_quantizedLeafNodes,this);
triangles->InternalProcessAllTriangles(&callback,m_bvhAabbMin,m_bvhAabbMax);
//now we have an array of leafnodes in m_leafNodes
numLeafNodes = m_quantizedLeafNodes.size();
m_quantizedContiguousNodes.resize(2*numLeafNodes);
} else
{
NodeTriangleCallback callback(m_leafNodes);
btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax);
//now we have an array of leafnodes in m_leafNodes
numLeafNodes = m_leafNodes.size();
m_contiguousNodes.resize(2*numLeafNodes);
}
m_curNodeIndex = 0;
buildTree(0,numLeafNodes);
///if the entire tree is small then subtree size, we need to create a header info for the tree
if(m_useQuantization && !m_SubtreeHeaders.size())
{
btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand();
subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]);
subtree.m_rootNodeIndex = 0;
subtree.m_subtreeSize = m_quantizedContiguousNodes[0].isLeafNode() ? 1 : m_quantizedContiguousNodes[0].getEscapeIndex();
}
//PCK: update the copy of the size
m_subtreeHeaderCount = m_SubtreeHeaders.size();
//PCK: clear m_quantizedLeafNodes and m_leafNodes, they are temporary
m_quantizedLeafNodes.clear();
m_leafNodes.clear();
}
/// Writes a .ae3d model to a file.
/// \param aOutFile File name to save the model into.
void WriteAe3d( const std::string& aOutFile )
{
static_assert( sizeof( Vertex ) == 64, "" );
static_assert( sizeof( ae3d::Vec3 ) == 12, "" );
static_assert( sizeof( VertexInd ) == 6, "" );
std::ofstream ofs( aOutFile.c_str(), std::ios::binary );
if (!ofs.is_open())
{
std::cerr << "Couldn't open file for writing!" << std::endl;
exit( 1 );
}
for (std::size_t m = 0; m < gMeshes.size(); ++m)
{
gMeshes[ m ].SolveAABB();
if (gMeshes[ m ].vnormal.empty())
{
gMeshes[ m ].SolveVertexNormals();
}
gMeshes[ m ].Interleave();
gMeshes[ m ].SolveFaceNormals();
gMeshes[ m ].SolveFaceTangents();
gMeshes[ m ].SolveVertexTangents();
}
// Calculates model's AABB by finding extreme values from meshes' AABBs.
const float maxValue = 99999999.0f;
ae3d::Vec3 aabbMin( maxValue, maxValue, maxValue );
ae3d::Vec3 aabbMax( -maxValue, -maxValue, -maxValue );
for (std::size_t m = 0; m < gMeshes.size(); ++m)
{
aabbMin = ae3d::Vec3::Min2( aabbMin, gMeshes[ m ].aabbMin );
aabbMax = ae3d::Vec3::Max2( aabbMax, gMeshes[ m ].aabbMax );
}
// The file starts with identification bytes.
const char* gAe3dVersion = "a9";
ofs.write( gAe3dVersion, 2 );
ofs.write( reinterpret_cast< char* >( &aabbMin.x ), 3 * 4 );
ofs.write( reinterpret_cast< char* >( &aabbMax.x ), 3 * 4 );
const std::size_t meshes = gMeshes.size();
// # of meshes.
ofs.write( reinterpret_cast< const char* >( &meshes ), 2 );
for (std::size_t m = 0; m < meshes; ++m)
{
assert( gMeshes[ m ].fnormal.size() == gMeshes[ m ].indices.size() );
// AABB.
ofs.write( reinterpret_cast< char* >( &gMeshes[ m ].aabbMin.x ), 3 * 4 );
ofs.write( reinterpret_cast< char* >( &gMeshes[ m ].aabbMax.x ), 3 * 4 );
// Writes name's length.
const unsigned short nameLength = (unsigned short)gMeshes[ m ].name.length();
ofs.write( reinterpret_cast< char* >((char*)&nameLength ), 2 );
// Writes name.
ofs.write(reinterpret_cast<char*>((char*)gMeshes[m].name.data()),
nameLength);
// Writes # of vertices.
const unsigned short nVertices = (unsigned short)gMeshes[ m ].interleavedVertices.size();
ofs.write( reinterpret_cast< char* >((char*)&nVertices), 2 );
// Writes vertex data.
ofs.write( (char*)&gMeshes[m].interleavedVertices[ 0 ], gMeshes[m].interleavedVertices.size() * sizeof( Vertex ) );
// Writes # of indices.
const unsigned short nIndices = (unsigned short)gMeshes[m].indices.size();
ofs.write( reinterpret_cast< char* >((char*)&nIndices), 2 );
// Writes indices.
ofs.write( (char*)&gMeshes[m].indices[ 0 ], gMeshes[m].indices.size() * sizeof( VertexInd ) );
}
// Terminator.
const unsigned char byte = 100;
ofs.write( (char*)&byte, 1 );
std::cout << "Wrote " << aOutFile << std::endl;
}
int b3GpuNarrowPhase::registerRigidBody(int collidableIndex, float mass, const float* position, const float* orientation , const float* aabbMinPtr, const float* aabbMaxPtr,bool writeToGpu)
{
b3Vector3 aabbMin(aabbMinPtr[0],aabbMinPtr[1],aabbMinPtr[2]);
b3Vector3 aabbMax (aabbMaxPtr[0],aabbMaxPtr[1],aabbMaxPtr[2]);
if (m_data->m_numAcceleratedRigidBodies >= (m_data->m_config.m_maxConvexBodies))
{
b3Error("registerRigidBody: exceeding the number of rigid bodies, %d > %d \n",m_data->m_numAcceleratedRigidBodies,m_data->m_config.m_maxConvexBodies);
return -1;
}
m_data->m_bodyBufferGPU->resize(m_data->m_numAcceleratedRigidBodies+1);
b3RigidBodyCL& body = m_data->m_bodyBufferCPU->at(m_data->m_numAcceleratedRigidBodies);
float friction = 1.f;
float restitution = 0.f;
body.m_frictionCoeff = friction;
body.m_restituitionCoeff = restitution;
body.m_angVel.setZero();
body.m_linVel.setValue(0,0,0);//.setZero();
body.m_pos.setValue(position[0],position[1],position[2]);
body.m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]);
body.m_collidableIdx = collidableIndex;
if (collidableIndex>=0)
{
// body.m_shapeType = m_data->m_collidablesCPU.at(collidableIndex).m_shapeType;
} else
{
// body.m_shapeType = CollisionShape::SHAPE_PLANE;
m_planeBodyIndex = m_data->m_numAcceleratedRigidBodies;
}
//body.m_shapeType = shapeType;
body.m_invMass = mass? 1.f/mass : 0.f;
if (writeToGpu)
{
m_data->m_bodyBufferGPU->copyFromHostPointer(&body,1,m_data->m_numAcceleratedRigidBodies);
}
b3InertiaCL& shapeInfo = m_data->m_inertiaBufferCPU->at(m_data->m_numAcceleratedRigidBodies);
if (mass==0.f)
{
if (m_data->m_numAcceleratedRigidBodies==0)
m_static0Index = 0;
shapeInfo.m_initInvInertia.setValue(0,0,0,0,0,0,0,0,0);
shapeInfo.m_invInertiaWorld.setValue(0,0,0,0,0,0,0,0,0);
} else
{
b3Assert(body.m_collidableIdx>=0);
//approximate using the aabb of the shape
//Aabb aabb = (*m_data->m_shapePointers)[shapeIndex]->m_aabb;
b3Vector3 halfExtents = (aabbMax-aabbMin);//*0.5f;//fake larger inertia makes demos more stable ;-)
b3Vector3 localInertia;
float lx=2.f*halfExtents[0];
float ly=2.f*halfExtents[1];
float lz=2.f*halfExtents[2];
localInertia.setValue( (mass/12.0f) * (ly*ly + lz*lz),
(mass/12.0f) * (lx*lx + lz*lz),
(mass/12.0f) * (lx*lx + ly*ly));
b3Vector3 invLocalInertia;
invLocalInertia[0] = 1.f/localInertia[0];
invLocalInertia[1] = 1.f/localInertia[1];
invLocalInertia[2] = 1.f/localInertia[2];
invLocalInertia[3] = 0.f;
shapeInfo.m_initInvInertia.setValue(
invLocalInertia[0], 0, 0,
0, invLocalInertia[1], 0,
0, 0, invLocalInertia[2]);
b3Matrix3x3 m (body.m_quat);
shapeInfo.m_invInertiaWorld = m.scaled(invLocalInertia) * m.transpose();
}
if (writeToGpu)
m_data->m_inertiaBufferGPU->copyFromHostPointer(&shapeInfo,1,m_data->m_numAcceleratedRigidBodies);
return m_data->m_numAcceleratedRigidBodies++;
}
/*! Initializes the physics for the Ramp
*
* @param pWorld Pointer to physics world
* @returns ICRESULT Success/failure of physics initialization
**/
ICRESULT Ramp::InitPhysics(btDiscreteDynamicsWorld* pWorld)
{
if (!pWorld) return IC_OK;;
m_pworld = pWorld;
if (m_verts)
delete[] m_verts;
if (m_ib)
delete[] m_ib;
//m_pContent->Load("Resource/models/skeeball_collision.icm",&model);
//// try to open the model file
icFile file;
if (ICEFAIL(file.Open("Resource/models/skeeball_collision.icm", ICFMREAD_EXISTING)))
{
icWarningf("Content loader could not open: %s", "Resource/models/skeeball_collision.icm");
return IC_FAIL_GEN;
}
_ICE_MODEL model_header = {0};
size_t sizeread = 0;
// read the ice model header
if (ICEFAIL(file.Read(&model_header, sizeof(_ICE_MODEL), &sizeread)))
{
icWarningf("Content loader could not read: %s", "Resource/models/skeeball_collision.icm");
return IC_FAIL_GEN;
}
m_verts = NULL;
int totalTriangles = model_header.numInd/3;
int totalVerts = model_header.numVerts;
m_verts = new icVector3[model_header.numVerts];
m_ib = new int [model_header.numInd];
m_vb = malloc(sizeof(ICVRTNRM_DIF)*model_header.numVerts);
size_t read;
// CREATE VERTEX BUFFER
switch(model_header.modelVersion)
{
case ICE_MODEL_FORMAT:
{
switch (model_header.vertType)
{
case IC_VERT_DIF:
{
ICVRT_DIF cur_vert;
for (int i=0; i<model_header.numVerts; ++i)
{
file.Read(&cur_vert,sizeof(ICVRT_DIF),&read);
m_verts[i] = cur_vert.pos;
}
}break;
case IC_VERT_NRM_DIF:
{
ICVRTNRM_DIF cur_vert;
for (int i=0; i<model_header.numVerts; ++i)
{
file.Read(&cur_vert,sizeof(ICVRTNRM_DIF),&read);
m_verts[i] = cur_vert.pos;
}
}break;
default:
;
}
}break;
case 101:
{
switch (model_header.vertType)
{
case 2:
{
ICVRT_DIF cur_vert;
for (int i=0; i<model_header.numVerts; ++i)
{
file.Read(&cur_vert,sizeof(ICVRT_DIF),&read);
m_verts[i] = cur_vert.pos;
}
}break;
case 8:
{
ICVRTNRM_DIF cur_vert;
for (int i=0; i<model_header.numVerts; ++i)
{
file.Read(&cur_vert,sizeof(ICVRT_DIF),&read);
m_verts[i] = cur_vert.pos;
}
}break;
default:
{
ICVRTNRM_DIF cur_vert;
for (int i=0; i<model_header.numVerts; ++i)
{
file.Read(&cur_vert,sizeof(ICVRT_DIF),&read);
m_verts[i] = cur_vert.pos;
}
}
}
}break;
case 100:
{
file.SetPos(sizeof(_ICE_MODEL_100)); // rewind
ICVRT_DIF cur_vert;
for (int i=0; i<model_header.numVerts; ++i)
{
file.Read(&cur_vert,sizeof(ICVRT_DIF),&read);
m_verts[i] = cur_vert.pos;
}
}break;
default:
icWarningf("Invalid model format: %s", "Resource/models/skeeball_collision.icm");
return IC_FAIL_GEN;
}
file.Read(m_ib,model_header.numInd*sizeof(int),&read);
m_indexVertexArrays = new btTriangleIndexVertexArray(totalTriangles,
m_ib,
3*sizeof(int),
model_header.numVerts,(btScalar*)m_verts,sizeof(icVector3));
btVector3 aabbMin(-1000,-1000,-1000),aabbMax(1000,1000,1000);
m_colShape = new btBvhTriangleMeshShape(m_indexVertexArrays,true,aabbMin,aabbMax);
m_colObject = new btCollisionObject();
m_colObject->setCollisionShape(m_colShape);
m_colObject2 = new btCollisionObject();
m_colObject2->setCollisionShape(m_colShape);
m_colObject3 = new btCollisionObject();
m_colObject3->setCollisionShape(m_colShape);
btTransform transform;
transform.setFromOpenGLMatrix((btScalar*)m_Trans);
m_colObject->setWorldTransform(transform);
transform.setOrigin(btVector3(-105.422f,0.0f,0.0f));
m_colObject2->setWorldTransform(transform);
transform.setOrigin(btVector3(105.422f,0.0f,0.0f));
m_colObject3->setWorldTransform(transform);
pWorld->addCollisionObject(m_colObject);//, 0x0001, 0x00FF);
pWorld->addCollisionObject(m_colObject2);//, 0x0001, 0x00FF);
pWorld->addCollisionObject(m_colObject3);//, 0x0001, 0x00FF);
return IC_OK;
}// END FUNCTION InitPhysics(b2World* pWorld)
void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
{
// Draw a small simplex at the center of the object
getDebugDrawer()->drawTransform(worldTransform,1);
if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
{
const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
{
btTransform childTrans = compoundShape->getChildTransform(i);
const btCollisionShape* colShape = compoundShape->getChildShape(i);
debugDrawObject(worldTransform*childTrans,colShape,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--)
{
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;
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);
}
}
}
}
}
}
void BtApplication::renderscene(int pass)
{
btScalar m[16];
btMatrix3x3 rot;rot.setIdentity();
const 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();
myMotionState->m_graphicsWorldTrans.getOpenGLMatrix(m);
rot=myMotionState->m_graphicsWorldTrans.getBasis();
}
else
{
colObj->getWorldTransform().getOpenGLMatrix(m);
rot=colObj->getWorldTransform().getBasis();
}
btVector3 wireColor(1.f,1.0f,0.5f); //wants deactivation
if(i&1) wireColor=btVector3(0.f,0.0f,1.f);
///color differently for active, sleeping, wantsdeactivation states
if (colObj->getActivationState() == 1) //active
{
if (i & 1)
{
wireColor += btVector3 (1.f,0.f,0.f);
}
else
{
wireColor += btVector3 (.5f,0.f,0.f);
}
}
if(colObj->getActivationState()==2) //ISLAND_SLEEPING
{
if(i&1)
{
wireColor += btVector3 (0.f,1.f, 0.f);
}
else
{
wireColor += btVector3 (0.f,0.5f,0.f);
}
}
btVector3 aabbMin(0,0,0),aabbMax(0,0,0);
//m_dynamicsWorld->getBroadphase()->getBroadphaseAabb(aabbMin,aabbMax);
aabbMin-=btVector3(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
aabbMax+=btVector3(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
// printf("aabbMin=(%f,%f,%f)\n",aabbMin.getX(),aabbMin.getY(),aabbMin.getZ());
// printf("aabbMax=(%f,%f,%f)\n",aabbMax.getX(),aabbMax.getY(),aabbMax.getZ());
// m_dynamicsWorld->getDebugDrawer()->drawAabb(aabbMin,aabbMax,btVector3(1,1,1));
if (!(getDebugMode()& btIDebugDraw::DBG_DrawWireframe))
{
switch(pass)
{
case 0: m_shapeDrawer->drawOpenGL(m,colObj->getCollisionShape(),wireColor,getDebugMode(),aabbMin,aabbMax);break;
case 1: m_shapeDrawer->drawShadow(m,m_sundirection*rot,colObj->getCollisionShape(),aabbMin,aabbMax);break;
case 2: m_shapeDrawer->drawOpenGL(m,colObj->getCollisionShape(),wireColor*btScalar(0.3),0,aabbMin,aabbMax);break;
}
}
}
}
void InternalEdgeDemo::clientMoveAndDisplay()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
float dt = getDeltaTimeMicroseconds() * 0.000001f;
if (m_animatedMesh)
{
static float offset=0.f;
offset+=0.01f;
// setVertexPositions(waveheight,offset);
#if 0 ///not currently supported, we need to update the btInternalTriangleInfoMap
int i;
int j;
btVector3 aabbMin(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
btVector3 aabbMax(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT);
for ( i=NUM_VERTS_X/2-3;i<NUM_VERTS_X/2+2;i++)
{
for (j=NUM_VERTS_X/2-3;j<NUM_VERTS_Y/2+2;j++)
{
aabbMax.setMax(gVertices[i+j*NUM_VERTS_X]);
aabbMin.setMin(gVertices[i+j*NUM_VERTS_X]);
gVertices[i+j*NUM_VERTS_X].setValue((i-NUM_VERTS_X*0.5f)*TRIANGLE_SIZE,
0.f,
//waveheight*sinf((float)i+offset)*cosf((float)j+offset),
(j-NUM_VERTS_Y*0.5f)*TRIANGLE_SIZE);
aabbMin.setMin(gVertices[i+j*NUM_VERTS_X]);
aabbMax.setMax(gVertices[i+j*NUM_VERTS_X]);
}
}
trimeshShape->partialRefitTree(aabbMin,aabbMax);
#else
btVector3 aabbMin,aabbMax;
trimeshShape->getMeshInterface()->calculateAabbBruteForce(aabbMin,aabbMax);
trimeshShape->refitTree(aabbMin,aabbMax);
#endif
//for debugging: clear all contact points involving mesh proxy. Note: this is a slow/unoptimized operation.
//m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(staticBody->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher());
}
m_dynamicsWorld->stepSimulation(dt);
///enable one of the following to debug (render debug lines each frame)
//m_dynamicsWorld->stepSimulation(1./800.,0);
//m_dynamicsWorld->stepSimulation(1./60.,100,1./800.);
//m_dynamicsWorld->stepSimulation(1./60.,0);
int lineWidth=450;
int xStart = m_glutScreenWidth - lineWidth;
int yStart = 20;
#ifndef __QNX__
if((getDebugMode() & btIDebugDraw::DBG_DrawText)!=0)
{
setOrthographicProjection();
glDisable(GL_LIGHTING);
glColor3f(0, 0, 0);
char buf[124];
glRasterPos3f(xStart, yStart, 0);
if (enable)
{
sprintf(buf,"InternalEdgeUtility enabled");
} else
{
sprintf(buf,"InternalEdgeUtility disabled");
}
GLDebugDrawString(xStart,20,buf);
yStart+=20;
glRasterPos3f(xStart, yStart, 0);
sprintf(buf,"Press 'n' to toggle InternalEdgeUtility");
yStart+=20;
GLDebugDrawString(xStart,yStart,buf);
glRasterPos3f(xStart, yStart, 0);
resetPerspectiveProjection();
glEnable(GL_LIGHTING);
}
#endif
renderme();
//optional but useful: debug drawing
m_dynamicsWorld->debugDrawWorld();
glFlush();
swapBuffers();
}
void ConcaveDemo::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);
int index=0;
for ( i=0;i<NUM_VERTS_X-1;i++)
{
for (int j=0;j<NUM_VERTS_Y-1;j++)
{
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;
}
}
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)
#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);
int maxSerializeBufferSize = 1024*1024*5;
btDefaultSerializer* serializer = new btDefaultSerializer(maxSerializeBufferSize);
//serializer->setSerializationFlags(BT_SERIALIZE_NO_BVH);// or BT_SERIALIZE_NO_TRIANGLEINFOMAP
serializer->startSerialization();
//registering a name is optional, it allows you to retrieve the shape by name
//serializer->registerNameForPointer(trimeshShape,"mymesh");
#ifdef SERIALIZE_SHAPE
trimeshShape->serializeSingleShape(serializer);
#else
trimeshShape->serializeSingleBvh(serializer);
#endif
serializer->finishSerialization();
FILE* f2 = fopen("myShape.bullet","wb");
fwrite(serializer->getBufferPointer(),serializer->getCurrentBufferSize(),1,f2);
fclose(f2);
#else
btBulletWorldImporter import(0);//don't store info into the world
if (import.loadFile("myShape.bullet"))
{
int numBvh = import.getNumBvhs();
if (numBvh)
{
btOptimizedBvh* bvh = import.getBvhByIndex(0);
btVector3 aabbMin(-1000,-1000,-1000),aabbMax(1000,1000,1000);
trimeshShape = new btBvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression,aabbMin,aabbMax,false);
trimeshShape->setOptimizedBvh(bvh);
//trimeshShape = new btBvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression,aabbMin,aabbMax);
//trimeshShape->setOptimizedBvh(bvh);
}
int numShape = import.getNumCollisionShapes();
if (numShape)
{
trimeshShape = (btBvhTriangleMeshShape*)import.getCollisionShapeByIndex(0);
//if you know the name, you can also try to get the shape by name:
const char* meshName = import.getNameForPointer(trimeshShape);
if (meshName)
trimeshShape = (btBvhTriangleMeshShape*)import.getCollisionShapeByName(meshName);
}
}
#endif
btCollisionShape* groundShape = trimeshShape;
#else
btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
m_collisionShapes.push_back(groundShape);
#endif //USE_TRIMESH_SHAPE
m_collisionConfiguration = new btDefaultCollisionConfiguration();
#ifdef USE_PARALLEL_DISPATCHER
#ifdef USE_WIN32_THREADING
int maxNumOutstandingTasks = 4;//number of maximum outstanding tasks
Win32ThreadSupport* threadSupport = new Win32ThreadSupport(Win32ThreadSupport::Win32ThreadConstructionInfo(
"collision",
processCollisionTask,
createCollisionLocalStoreMemory,
maxNumOutstandingTasks));
#else
///@todo show other platform threading
///Playstation 3 SPU (SPURS) version is available through PS3 Devnet
///Libspe2 SPU support will be available soon
///pthreads version
///you can hook it up to your custom task scheduler by deriving from btThreadSupportInterface
#endif
m_dispatcher = new SpuGatheringCollisionDispatcher(threadSupport,maxNumOutstandingTasks,m_collisionConfiguration);
#else
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
#endif//USE_PARALLEL_DISPATCHER
btVector3 worldMin(-1000,-1000,-1000);
btVector3 worldMax(1000,1000,1000);
m_broadphase = new btAxisSweep3(worldMin,worldMax);
m_solver = new btSequentialImpulseConstraintSolver();
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
#ifdef USE_PARALLEL_DISPATCHER
m_dynamicsWorld->getDispatchInfo().m_enableSPU=true;
#endif //USE_PARALLEL_DISPATCHER
float mass = 0.f;
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,-2,0));
#ifdef USE_BOX_SHAPE
btCollisionShape* colShape = new btBoxShape(btVector3(1,1,1));
#else
btCompoundShape* colShape = new btCompoundShape;
btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(4,1,1));
btCollisionShape* boxShape = new btBoxShape(btVector3(4,1,1));
btTransform localTransform;
localTransform.setIdentity();
colShape->addChildShape(localTransform,boxShape);
btQuaternion orn(SIMD_HALF_PI,0,0);
localTransform.setRotation(orn);
colShape->addChildShape(localTransform,cylinderShape);
#endif //USE_BOX_SHAPE
m_collisionShapes.push_back(colShape);
{
for (int i=0;i<10;i++)
{
startTransform.setOrigin(btVector3(2,10+i*2,1));
localCreateRigidBody(1, startTransform,colShape);
}
}
startTransform.setIdentity();
staticBody = localCreateRigidBody(mass, startTransform,groundShape);
staticBody->setCollisionFlags(staticBody->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);//STATIC_OBJECT);
//enable custom material callback
staticBody->setCollisionFlags(staticBody->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
}
void render(void)
{
render_begin();
const PfxVector3 colorWhite(1.0f);
const PfxVector3 colorGray(0.7f);
for(int i=0;i<physics_get_num_rigidbodies();i++) {
const PfxRigidState &state = physics_get_state(i);
const PfxCollidable &coll = physics_get_collidable(i);
PfxVector3 color = state.isAsleep()?colorGray:colorWhite;
PfxTransform3 rbT(state.getOrientation(), state.getPosition());
PfxShapeIterator itrShape(coll);
for(PfxUInt32 j=0;j<coll.getNumShapes();j++,++itrShape) {
const PfxShape &shape = *itrShape;
PfxTransform3 offsetT = shape.getOffsetTransform();
PfxTransform3 worldT = rbT * offsetT;
switch(shape.getType()) {
case kPfxShapeSphere:
render_sphere(
worldT,
color,
PfxFloatInVec(shape.getSphere().m_radius));
break;
case kPfxShapeBox:
render_box(
worldT,
color,
shape.getBox().m_half);
break;
case kPfxShapeCapsule:
render_capsule(
worldT,
color,
PfxFloatInVec(shape.getCapsule().m_radius),
PfxFloatInVec(shape.getCapsule().m_halfLen));
break;
case kPfxShapeCylinder:
render_cylinder(
worldT,
color,
PfxFloatInVec(shape.getCylinder().m_radius),
PfxFloatInVec(shape.getCylinder().m_halfLen));
break;
case kPfxShapeConvexMesh:
render_mesh(
worldT,
color,
convexMeshId);
break;
case kPfxShapeLargeTriMesh:
render_mesh(
worldT,
color,
landscapeMeshId);
break;
default:
break;
}
}
}
render_debug_begin();
#ifdef ENABLE_DEBUG_DRAW_CONTACT
for(int i=0;i<physics_get_num_contacts();i++) {
const PfxContactManifold &contact = physics_get_contact(i);
const PfxRigidState &stateA = physics_get_state(contact.getRigidBodyIdA());
const PfxRigidState &stateB = physics_get_state(contact.getRigidBodyIdB());
for(int j=0;j<contact.getNumContacts();j++) {
const PfxContactPoint &cp = contact.getContactPoint(j);
PfxVector3 pA = stateA.getPosition()+rotate(stateA.getOrientation(),pfxReadVector3(cp.m_localPointA));
const float w = 0.05f;
render_debug_line(pA+PfxVector3(-w,0.0f,0.0f),pA+PfxVector3(w,0.0f,0.0f),PfxVector3(0,0,1));
render_debug_line(pA+PfxVector3(0.0f,-w,0.0f),pA+PfxVector3(0.0f,w,0.0f),PfxVector3(0,0,1));
render_debug_line(pA+PfxVector3(0.0f,0.0f,-w),pA+PfxVector3(0.0f,0.0f,w),PfxVector3(0,0,1));
}
}
#endif
#ifdef ENABLE_DEBUG_DRAW_AABB
for(int i=0;i<physics_get_num_rigidbodies();i++) {
const PfxRigidState &state = physics_get_state(i);
const PfxCollidable &coll = physics_get_collidable(i);
PfxVector3 center = state.getPosition() + coll.getCenter();
PfxVector3 half = absPerElem(PfxMatrix3(state.getOrientation())) * coll.getHalf();
render_debug_box(center,half,PfxVector3(1,0,0));
}
#endif
#ifdef ENABLE_DEBUG_DRAW_ISLAND
const PfxIsland *island = physics_get_islands();
if(island) {
for(PfxUInt32 i=0;i<pfxGetNumIslands(island);i++) {
PfxIslandUnit *islandUnit = pfxGetFirstUnitInIsland(island,i);
PfxVector3 aabbMin(SCE_PFX_FLT_MAX);
PfxVector3 aabbMax(-SCE_PFX_FLT_MAX);
for(;islandUnit!=NULL;islandUnit=pfxGetNextUnitInIsland(islandUnit)) {
const PfxRigidState &state = physics_get_state(pfxGetUnitId(islandUnit));
const PfxCollidable &coll = physics_get_collidable(pfxGetUnitId(islandUnit));
PfxVector3 center = state.getPosition() + coll.getCenter();
PfxVector3 half = absPerElem(PfxMatrix3(state.getOrientation())) * coll.getHalf();
aabbMin = minPerElem(aabbMin,center-half);
aabbMax = maxPerElem(aabbMax,center+half);
}
render_debug_box((aabbMax+aabbMin)*0.5f,(aabbMax-aabbMin)*0.5f,PfxVector3(0,1,0));
}
}
#endif
for(int i=0;i<physics_get_num_rays();i++) {
const PfxRayInput& rayInput = physics_get_rayinput(i);
const PfxRayOutput& rayOutput = physics_get_rayoutput(i);
if(rayOutput.m_contactFlag) {
render_debug_line(
rayInput.m_startPosition,
rayOutput.m_contactPoint,
PfxVector3(1.0f,0.0f,1.0f));
render_debug_line(
rayOutput.m_contactPoint,
rayOutput.m_contactPoint+rayOutput.m_contactNormal,
PfxVector3(1.0f,0.0f,1.0f));
}
else {
render_debug_line(rayInput.m_startPosition,
rayInput.m_startPosition+rayInput.m_direction,
PfxVector3(0.5f,0.0f,0.5f));
}
}
extern bool doAreaRaycast;
extern PfxVector3 areaCenter;
extern PfxVector3 areaExtent;
if(doAreaRaycast) {
render_debug_box(areaCenter,areaExtent,PfxVector3(0,0,1));
}
render_debug_end();
render_end();
}
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 btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const
{
class CenterCallback : public btInternalTriangleIndexCallback
{
bool first;
btVector3 ref;
btVector3 sum;
btScalar volume;
public:
CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0)
{
}
virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
{
(void)triangleIndex;
(void)partId;
if (first)
{
ref = triangle[0];
first = false;
}
else
{
btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref));
sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref));
volume += vol;
}
}
btVector3 getCenter()
{
return (volume > 0) ? sum / volume : ref;
}
btScalar getVolume()
{
return volume * btScalar(1. / 6);
}
};
class InertiaCallback : public btInternalTriangleIndexCallback
{
btMatrix3x3 sum;
btVector3 center;
public:
InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center)
{
}
virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
{
(void)triangleIndex;
(void)partId;
btMatrix3x3 i;
btVector3 a = triangle[0] - center;
btVector3 b = triangle[1] - center;
btVector3 c = triangle[2] - center;
btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6);
for (int j = 0; j < 3; j++)
{
for (int k = 0; k <= j; k++)
{
i[j][k] = i[k][j] = volNeg * (btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k]) + btScalar(0.05) * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j]));
}
}
btScalar i00 = -i[0][0];
btScalar i11 = -i[1][1];
btScalar i22 = -i[2][2];
i[0][0] = i11 + i22;
i[1][1] = i22 + i00;
i[2][2] = i00 + i11;
sum[0] += i[0];
sum[1] += i[1];
sum[2] += i[2];
}
btMatrix3x3& getInertia()
{
return sum;
}
};
CenterCallback centerCallback;
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
m_stridingMesh->InternalProcessAllTriangles(¢erCallback, -aabbMax, aabbMax);
btVector3 center = centerCallback.getCenter();
principal.setOrigin(center);
volume = centerCallback.getVolume();
InertiaCallback inertiaCallback(center);
m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax);
btMatrix3x3& i = inertiaCallback.getInertia();
i.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
inertia.setValue(i[0][0], i[1][1], i[2][2]);
inertia /= volume;
}
void BasicDemo::initPhysics()
{
#ifdef FORCE_ZAXIS_UP
m_cameraUp = btVector3(0,0,1);
m_forwardAxis = 1;
#endif
gContactAddedCallback = CustomMaterialCombinerCallback;
setTexturing(true);
setShadows(false);
setCameraDistance(btScalar(SCALING*20.));
this->m_azi = 90;
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btDefaultCollisionConfiguration();
//m_collisionConfiguration->setConvexConvexMultipointIterations();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
m_solver = sol;
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
#ifdef FORCE_ZAXIS_UP
m_dynamicsWorld->setGravity(btVector3(0,0,-10));
#else
m_dynamicsWorld->setGravity(btVector3(0,-10,0));
#endif
m_dynamicsWorld->getSolverInfo().m_solverMode |= SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION+SOLVER_USE_2_FRICTION_DIRECTIONS;
m_dynamicsWorld->getSolverInfo().m_solverMode |= SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;
#if 1
m_blendReader = new BasicBlendReader(m_dynamicsWorld,this);
//const char* fileName = "clubsilo_packed.blend";
const char* fileName = "PhysicsAnimationBakingDemo.blend";
char fullPath[512];
if(!m_blendReader->openFile(fileName))
{
sprintf(fullPath,"../../%s",fileName);
m_blendReader->openFile(fullPath);
}
if (m_blendReader)
{
m_blendReader->convertAllObjects();
} else
{
printf("file not found\n");
}
#endif
///create a few basic rigid bodies
#if 0
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-60,0));
//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
{
btScalar mass(0.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
groundShape->calculateLocalInertia(mass,localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
//enable custom material callback
body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
//add the body to the dynamics world
m_dynamicsWorld->addRigidBody(body);
}
#endif
#if 0
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
//btCollisionShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1));
//btCollisionShape* colShape = new btBoxShape(btVector3(SCALING*.1,SCALING*.1,SCALING*.1));
btCollisionShape* colShape = new btSphereShape(SCALING*btScalar(1.));
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.f);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
colShape->calculateLocalInertia(mass,localInertia);
float start_x = -ARRAY_SIZE_X;
float start_y = -ARRAY_SIZE_Y;
float start_z = - ARRAY_SIZE_Z;
for (int k=0;k<ARRAY_SIZE_Y;k++)
{
for (int i=0;i<ARRAY_SIZE_X;i++)
{
for(int j = 0;j<ARRAY_SIZE_Z;j++)
{
startTransform.setOrigin(1.*SCALING*btVector3(
2.0*i + start_x,
2.0*k + start_y,
2.0*j + start_z));
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
//body->setContactProcessingThreshold(colShape->getContactBreakingThreshold());
body->setActivationState(ISLAND_SLEEPING);
body->setCollisionFlags(btCollisionObject::CF_NO_CONTACT_RESPONSE);
// m_dynamicsWorld->addRigidBody(body);
body->setActivationState(ISLAND_SLEEPING);
}
}
}
}
btTriangleIndexVertexArray* meshInterface = new btTriangleIndexVertexArray();
btIndexedMesh indexMesh;
indexMesh.m_numTriangles = BUNNY_NUM_TRIANGLES ;
indexMesh.m_numVertices = BUNNY_NUM_VERTICES;
indexMesh.m_vertexBase = (const unsigned char*) &gVerticesBunny[0];
indexMesh.m_vertexStride = 3*sizeof(REAL);
indexMesh.m_triangleIndexBase = (const unsigned char*)&gIndicesBunny[0];
indexMesh.m_triangleIndexStride = 3*sizeof(int);
meshInterface->addIndexedMesh(indexMesh);
btBvhTriangleMeshShape* bunny = new btBvhTriangleMeshShape(meshInterface,true);
bunny->setLocalScaling(btVector3(2,2,2));
btCollisionObject* obj = new btCollisionObject();
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(0,2,-20));
obj ->setWorldTransform(tr);
obj->setCollisionShape(bunny);
m_dynamicsWorld->addCollisionObject(obj);
#endif
#if 0
btConvexTriangleMeshShape* convexBun = new btConvexTriangleMeshShape(meshInterface);
obj = new btCollisionObject();
tr.setOrigin(btVector3(0,2,-14));
obj ->setWorldTransform(tr);
obj->setCollisionShape(convexBun);
m_dynamicsWorld->addCollisionObject(obj);
#endif
#if 0
btConvexTriangleMeshShape* convexBun = new btConvexTriangleMeshShape(meshInterface);
obj = new btCollisionObject();
tr.setOrigin(btVector3(0,2,-14));
obj ->setWorldTransform(tr);
obj->setCollisionShape(convexBun);
m_dynamicsWorld->addCollisionObject(obj);
//btDiscreteCollisionDetectorInterface::ClosestPointInput input;
//input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);///@todo: tighter bounds
//input.m_transformA = sphereObj->getWorldTransform();
//input.m_transformB = triObj->getWorldTransform();
//bool swapResults = m_swapped;
//detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw,swapResults);
for (int v=1;v<10;v++)
{
float VOXEL_SIZE = VOXEL_SIZE_START * v;
btVoxelizationCallback voxelizationCallback;
btCompoundShape* compoundBunny = new btCompoundShape();
voxelizationCallback.m_bunnyCompound = compoundBunny;
voxelizationCallback.m_sphereChildShape = new btSphereShape(VOXEL_SIZE);
#if 1
float start_x = -ARRAY_SIZE_X;
float start_y = -ARRAY_SIZE_Y;
float start_z = - ARRAY_SIZE_Z;
for (int k=0;k<ARRAY_SIZE_Y;k++)
{
for (int i=0;i<ARRAY_SIZE_X;i++)
{
for(int j = 0;j<ARRAY_SIZE_Z;j++)
{
btVector3 pos =VOXEL_SIZE*SCALING*btVector3(
2.0*i + start_x,
2.0*k + start_y,
2.0*j + start_z);
btVector3 aabbMin(pos-btVector3(VOXEL_SIZE,VOXEL_SIZE,VOXEL_SIZE));
btVector3 aabbMax(pos+btVector3(VOXEL_SIZE,VOXEL_SIZE,VOXEL_SIZE));
voxelizationCallback.m_curSpherePos = pos;
voxelizationCallback.m_oncePerSphere = false;
bunny->processAllTriangles(&voxelizationCallback,aabbMin,aabbMax);
}
}
}
//btCollisionObject* obj2 = new btCollisionObject();
//obj2->setCollisionShape(compoundBunny);
//m_dynamicsWorld->addCollisionObject(obj2);
btVector3 localInertia;
compoundBunny->calculateLocalInertia(1,localInertia);
btRigidBody* body = new btRigidBody(1,0,compoundBunny,localInertia);
//m_dynamicsWorld->addRigidBody(body);
btTransform start;
start.setIdentity();
start.setOrigin(btVector3(0,2,-12+6*v));
localCreateRigidBody(1.,start,compoundBunny);
printf("compoundBunny with %d spheres\n",compoundBunny->getNumChildShapes());
#endif
}
#endif
clientResetScene();
}
void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
{
// Draw a small simplex at the center of the object
{
btVector3 start = worldTransform.getOrigin();
getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0));
getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0));
getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1));
}
if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
{
const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
{
btTransform childTrans = compoundShape->getChildTransform(i);
const btCollisionShape* colShape = compoundShape->getChildShape(i);
debugDrawObject(worldTransform*childTrans,colShape,color);
}
} else
{
switch (shape->getShapeType())
{
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
debugDrawSphere(radius, worldTransform, color);
break;
}
case MULTI_SPHERE_SHAPE_PROXYTYPE:
{
const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
{
btTransform childTransform = worldTransform;
childTransform.getOrigin() += multiSphereShape->getSpherePosition(i);
debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color);
}
break;
}
case CAPSULE_SHAPE_PROXYTYPE:
{
const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
btScalar radius = capsuleShape->getRadius();
btScalar halfHeight = capsuleShape->getHalfHeight();
// Draw the ends
{
btTransform childTransform = worldTransform;
childTransform.getOrigin() = worldTransform * btVector3(0,halfHeight,0);
debugDrawSphere(radius, childTransform, color);
}
{
btTransform childTransform = worldTransform;
childTransform.getOrigin() = worldTransform * btVector3(0,-halfHeight,0);
debugDrawSphere(radius, childTransform, color);
}
// Draw some additional lines
btVector3 start = worldTransform.getOrigin();
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(-radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(-radius,-halfHeight,0), color);
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(radius,-halfHeight,0), color);
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,-radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,-radius), color);
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,radius), 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();
btVector3 start = worldTransform.getOrigin();
int upAxis= coneShape->getConeUpIndex();
btVector3 offsetHeight(0,0,0);
offsetHeight[upAxis] = height * btScalar(0.5);
btVector3 offsetRadius(0,0,0);
offsetRadius[(upAxis+1)%3] = radius;
btVector3 offset2Radius(0,0,0);
offset2Radius[(upAxis+2)%3] = radius;
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),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];
btVector3 start = worldTransform.getOrigin();
btVector3 offsetHeight(0,0,0);
offsetHeight[upAxis] = halfHeight;
btVector3 offsetRadius(0,0,0);
offsetRadius[(upAxis+1)%3] = radius;
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
break;
}
case STATIC_PLANE_PROXYTYPE:
{
const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
btScalar planeConst = staticPlaneShape->getPlaneConstant();
const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
btVector3 planeOrigin = planeNormal * planeConst;
btVector3 vec0,vec1;
btPlaneSpace1(planeNormal,vec0,vec1);
btScalar vecLen = 100.f;
btVector3 pt0 = planeOrigin + vec0*vecLen;
btVector3 pt1 = planeOrigin - vec0*vecLen;
btVector3 pt2 = planeOrigin + vec1*vecLen;
btVector3 pt3 = planeOrigin - vec1*vecLen;
getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color);
getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color);
break;
}
default:
{
if (shape->isConcave())
{
btConcaveShape* concaveMesh = (btConcaveShape*) shape;
//todo pass camera, for some culling
btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
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(1e30),btScalar(1e30),btScalar(1e30));
btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
//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;
for (i=0;i<polyshape->getNumEdges();i++)
{
btPoint3 a,b;
polyshape->getEdge(i,a,b);
btVector3 wa = worldTransform * a;
btVector3 wb = worldTransform * b;
getDebugDrawer()->drawLine(wa,wb,color);
}
}
}
}
}
}
//--------------------------------------------------------------
void ofxBulletTriMeshShape::create( btDiscreteDynamicsWorld* a_world, ofMesh& aMesh, btTransform &a_bt_tr, float a_mass, glm::vec3 aAAbbMin, glm::vec3 aAAbbMax ) {
if( aMesh.getMode() != OF_PRIMITIVE_TRIANGLES ) {
ofLogWarning() << " ofxBulletTriMeshShape :: create : mesh must be using triangles, not creating!!" << endl;
return;
}
if( aMesh.getNumIndices() < 3 ) {
ofLogWarning() << " ofxBulletTriMeshShape :: create : mesh must have indices, not creating!" << endl;
return;
}
if( !_bInited || _shape == NULL ) {
int vertStride = sizeof(btVector3);
int indexStride = 3*sizeof(int);
totalVerts = (int)aMesh.getNumVertices();
totalIndices = (int)aMesh.getNumIndices();
const int totalTriangles = totalIndices / 3;
if( bullet_indices != NULL ) {
removeShape();
}
if( bullet_vertices != NULL ) {
removeShape();
}
if( bullet_indexVertexArrays != NULL ) {
removeShape();
}
if( _shape != NULL ) {
removeShape();
}
bullet_vertices = new btVector3[ totalVerts ];
bullet_indices = new int[ totalIndices ];
auto& tverts = aMesh.getVertices();
auto& tindices = aMesh.getIndices();
for( int i = 0; i < totalVerts; i++ ) {
bullet_vertices[i].setValue( tverts[i].x, tverts[i].y, tverts[i].z );
}
for( int i = 0; i < totalIndices; i++ ) {
bullet_indices[i] = (int)tindices[i];
}
bullet_indexVertexArrays = new btTriangleIndexVertexArray(totalTriangles, bullet_indices, indexStride,
totalVerts, (btScalar*) &bullet_vertices[0].x(), vertStride);
// if you are having trouble with objects falling through, try passing in smaller or larger aabbMin and aabbMax
// to something closer to the size of your object //
// btVector3 aabbMin(-10000,-10000,-10000),aabbMax(10000,10000,10000);
if( aAAbbMin.length() > 0 && aAAbbMax.length() > 0 ) {
btVector3 aabbMin( aAAbbMin.x, aAAbbMin.y, aAAbbMin.z );
btVector3 aabbMax( aAAbbMax.x, aAAbbMax.y, aAAbbMax.z );
_shape = new btBvhTriangleMeshShape(bullet_indexVertexArrays, true, aabbMin, aabbMax );
} else {
_shape = new btBvhTriangleMeshShape(bullet_indexVertexArrays, true, true );
}
}
ofxBulletRigidBody::create( a_world, _shape, a_bt_tr, a_mass );
createInternalUserData();
updateMesh( a_world, aMesh );
}