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 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);
}
}
}
}
}
}
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)
{
if (mass)
{
m_data->m_BroadphaseSap->createProxy(aabbMin,aabbMax,userIndex,1,1);//m_dispatcher);
} else
{
m_data->m_BroadphaseSap->createLargeProxy(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 (m_narrowphaseAndSolver)
{
//bodyIndex = m_narrowphaseAndSolver->registerRigidBody(collisionShapeIndex,CollisionShape::SHAPE_CONVEX_HEIGHT_FIELD,mass,position,orientation,&aabbMin.getX(),&aabbMax.getX(),writeToGpu);
bodyIndex = m_narrowphaseAndSolver->registerRigidBody(collidableIndex,mass,position,orientation,&aabbMin.getX(),&aabbMax.getX(),writeToGpu);
}
if (mass>0.f)
m_numDynamicPhysicsInstances++;
m_numPhysicsInstances++;
return bodyIndex;
}
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 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(int 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));
render_debug_point(pA,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
render_debug_end();
render_end();
}
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 = 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 = 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();
}
//--------------------------------------------------------------
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 );
}
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 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 InitShaders()
{
btOverlappingPairCache* overlappingPairCache=0;
int maxObjects = btMax(256,NUM_OBJECTS);
#ifdef USE_NEW
int maxPairsSmallProxy = 32;
sBroadphase = new btGridBroadphaseCl(overlappingPairCache,btVector3(4.f, 4.f, 4.f), 128, 128, 128,maxObjects, maxObjects, maxPairsSmallProxy, 100.f, 128,
g_cxMainContext ,g_device,g_cqCommandQue, g_deviceCL);
#else
sBroadphase = new btGpu3DGridBroadphase(btVector3(2.f, 2.f, 2.f), 32, 32, 32,maxObjects, maxObjects, 64, 100.f, 64);
#endif
// sBroadphase = new bt3dGridBroadphaseOCL(overlappingPairCache,btVector3(10.f, 10.f, 10.f), 32, 32, 32,NUM_OBJECTS, NUM_OBJECTS, 64, 100.f, 16,
// g_cxMainContext ,g_device,g_cqCommandQue);
bool loadFromFile = false;
instancingShader = gltLoadShaderPair("instancing.vs","instancing.fs", loadFromFile);
glLinkProgram(instancingShader);
glUseProgram(instancingShader);
angle_loc = glGetUniformLocation(instancingShader, "angle");
ModelViewMatrix = glGetUniformLocation(instancingShader, "ModelViewMatrix");
ProjectionMatrix = glGetUniformLocation(instancingShader, "ProjectionMatrix");
uniform_texture_diffuse = glGetUniformLocation(instancingShader, "Diffuse");
GLuint offset = 0;
glGenBuffers(1, &cube_vbo);
glBindBuffer(GL_ARRAY_BUFFER, cube_vbo);
instance_positions_ptr = (GLfloat*)new float[NUM_OBJECTS*4];
instance_quaternion_ptr = (GLfloat*)new float[NUM_OBJECTS*4];
instance_colors_ptr = (GLfloat*)new float[NUM_OBJECTS*4];
instance_scale_ptr = (GLfloat*)new float[NUM_OBJECTS*3];
int index=0;
for (int i=0;i<NUM_OBJECTS_X;i++)
{
for (int j=0;j<NUM_OBJECTS_Y;j++)
{
for (int k=0;k<NUM_OBJECTS_Z;k++)
{
instance_positions_ptr[index*4]=(i*X_GAP-NUM_OBJECTS_X/2);
instance_positions_ptr[index*4+1]=(j*Y_GAP-NUM_OBJECTS_Y/2);
instance_positions_ptr[index*4+2]=(k*Z_GAP-NUM_OBJECTS_Z/2)+(j&1);
instance_positions_ptr[index*4+3]=1;
int shapeType =0;
void* userPtr = 0;
btVector3 aabbMin(
instance_positions_ptr[index*4],
instance_positions_ptr[index*4+1],
instance_positions_ptr[index*4+2]);
btVector3 aabbMax = aabbMin;
aabbMin -= btVector3(1.f,1.f,1.f);
aabbMax += btVector3(1.f,1.f,1.f);
void* myptr = (void*)index;//0;//&mBoxes[i]
btBroadphaseProxy* proxy = sBroadphase->createProxy(aabbMin,aabbMax,shapeType,myptr,1,1,0,0);//m_dispatcher);
proxyArray.push_back(proxy);
instance_quaternion_ptr[index*4]=0;
instance_quaternion_ptr[index*4+1]=0;
instance_quaternion_ptr[index*4+2]=0;
instance_quaternion_ptr[index*4+3]=1;
instance_colors_ptr[index*4]=j<NUM_OBJECTS_Y/2? 0.5f : 1.f;
instance_colors_ptr[index*4+1]=k<NUM_OBJECTS_Y/2? 0.5f : 1.f;
instance_colors_ptr[index*4+2]=i<NUM_OBJECTS_Y/2? 0.5f : 1.f;
instance_colors_ptr[index*4+3]=1.f;
instance_scale_ptr[index*3] = 1;
instance_scale_ptr[index*3+1] = 1;
instance_scale_ptr[index*3+2] = 1;
float mass = 1.f;//j? 1.f : 0.f;
bool writeToGpu = false;
if (narrowphaseAndSolver)
narrowphaseAndSolver->registerRigidBody(gShapeIndex,mass,&instance_positions_ptr[index*4],&instance_quaternion_ptr[index*4],writeToGpu);
index++;
}
}
}
float posZero[4] = {0,-NUM_OBJECTS_Y/2-1,0,0};
float ornZero[4] = {0,0,0,1};
//register a 'plane'
if (narrowphaseAndSolver)
narrowphaseAndSolver->registerRigidBody(-1, 0.f, posZero,ornZero,false);
if (narrowphaseAndSolver)
narrowphaseAndSolver->writeAllBodiesToGpu();
int size = sizeof(cube_vertices) + POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE+SCALE_BUFFER_SIZE;
VBOsize = size;
char* bla = (char*)malloc(size);
int szc = sizeof(cube_vertices);
memcpy(bla,&cube_vertices[0],szc);
memcpy(bla+sizeof(cube_vertices),instance_positions_ptr,POSITION_BUFFER_SIZE);
memcpy(bla+sizeof(cube_vertices)+POSITION_BUFFER_SIZE,instance_quaternion_ptr,ORIENTATION_BUFFER_SIZE);
memcpy(bla+sizeof(cube_vertices)+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE,instance_colors_ptr, COLOR_BUFFER_SIZE);
memcpy(bla+sizeof(cube_vertices)+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE,instance_scale_ptr, SCALE_BUFFER_SIZE);
glBufferData(GL_ARRAY_BUFFER, size, bla, GL_DYNAMIC_DRAW);//GL_STATIC_DRAW);
///initialize parts of the buffer
#ifdef _USE_SUB_DATA
glBufferSubData(GL_ARRAY_BUFFER, 0, sizeof(cube_vertices)+ 16384, bla);//cube_vertices);
#endif
char* dest= (char*)glMapBuffer( GL_ARRAY_BUFFER,GL_WRITE_ONLY);//GL_WRITE_ONLY
memcpy(dest,cube_vertices,sizeof(cube_vertices));
//memcpy(dest+sizeof(cube_vertices),instance_colors,sizeof(instance_colors));
glUnmapBuffer( GL_ARRAY_BUFFER);
writeTransforms();
/*
glBufferSubData(GL_ARRAY_BUFFER, sizeof(cube_vertices) + sizeof(instance_colors), POSITION_BUFFER_SIZE, instance_positions_ptr);
glBufferSubData(GL_ARRAY_BUFFER, sizeof(cube_vertices) + sizeof(instance_colors)+POSITION_BUFFER_SIZE,ORIENTATION_BUFFER_SIZE , instance_quaternion_ptr);
*/
glGenVertexArrays(1, &cube_vao);
glBindVertexArray(cube_vao);
glBindBuffer(GL_ARRAY_BUFFER, cube_vbo);
glBindVertexArray(0);
glGenBuffers(1, &index_vbo);
int indexBufferSize = sizeof(cube_indices);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_vbo);
glBufferData(GL_ELEMENT_ARRAY_BUFFER, indexBufferSize, NULL, GL_STATIC_DRAW);
glBufferSubData(GL_ELEMENT_ARRAY_BUFFER,0,indexBufferSize,cube_indices);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
glBindBuffer(GL_ARRAY_BUFFER,0);
glBindVertexArray(0);
}
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++;
}
void InitShaders()
{
btOverlappingPairCache* overlappingPairCache=0;
#ifdef USE_NEW
sBroadphase = new btGridBroadphaseCl(overlappingPairCache,btVector3(3.f, 3.f, 3.f), 32, 32, 32,NUM_OBJECTS, NUM_OBJECTS, 64, 100.f, 16,
g_cxMainContext ,g_device,g_cqCommandQue);
#else
sBroadphase = new btGpu3DGridBroadphase(btVector3(10.f, 10.f, 10.f), 32, 32, 32,NUM_OBJECTS, NUM_OBJECTS, 64, 100.f, 16);
#endif
// sBroadphase = new bt3dGridBroadphaseOCL(overlappingPairCache,btVector3(10.f, 10.f, 10.f), 32, 32, 32,NUM_OBJECTS, NUM_OBJECTS, 64, 100.f, 16,
// g_cxMainContext ,g_device,g_cqCommandQue);
bool loadFromFile = false;
instancingShader = gltLoadShaderPair("instancing.vs","instancing.fs", loadFromFile);
glLinkProgram(instancingShader);
glUseProgram(instancingShader);
angle_loc = glGetUniformLocation(instancingShader, "angle");
ModelViewMatrix = glGetUniformLocation(instancingShader, "ModelViewMatrix");
ProjectionMatrix = glGetUniformLocation(instancingShader, "ProjectionMatrix");
uniform_texture_diffuse = glGetUniformLocation(instancingShader, "Diffuse");
GLuint offset = 0;
glGenBuffers(1, &cube_vbo);
glBindBuffer(GL_ARRAY_BUFFER, cube_vbo);
instance_positions_ptr = (GLfloat*)new float[NUM_OBJECTS*4];
instance_quaternion_ptr = (GLfloat*)new float[NUM_OBJECTS*4];
instance_colors_ptr = (GLfloat*)new float[NUM_OBJECTS*4];
instance_scale_ptr = (GLfloat*)new float[NUM_OBJECTS*3];
int index=0;
for (int i=0;i<NUM_OBJECTS_X;i++)
{
for (int j=0;j<NUM_OBJECTS_Y;j++)
{
for (int k=0;k<NUM_OBJECTS_Z;k++)
{
instance_positions_ptr[index*4]=-(i-NUM_OBJECTS_X/2)*10;
instance_positions_ptr[index*4+1]=-(j-NUM_OBJECTS_Y/2)*10;
instance_positions_ptr[index*4+2]=-(k-NUM_OBJECTS_Z/2)*10;
instance_positions_ptr[index*4+3]=1;
int shapeType =0;
void* userPtr = 0;
btVector3 aabbMin(
instance_positions_ptr[index*4],
instance_positions_ptr[index*4+1],
instance_positions_ptr[index*4+2]);
btVector3 aabbMax = aabbMin;
aabbMin -= btVector3(1,1,1);
aabbMax += btVector3(1,1,1);
void* myptr = (void*)index;//0;//&mBoxes[i]
btBroadphaseProxy* proxy = sBroadphase->createProxy(aabbMin,aabbMax,shapeType,myptr,1,1,0,0);//m_dispatcher);
proxyArray.push_back(proxy);
instance_quaternion_ptr[index*4]=0;
instance_quaternion_ptr[index*4+1]=0;
instance_quaternion_ptr[index*4+2]=0;
instance_quaternion_ptr[index*4+3]=1;
instance_colors_ptr[index*4]=j<NUM_OBJECTS_Y/2? 0.5f : 1.f;
instance_colors_ptr[index*4+1]=k<NUM_OBJECTS_Y/2? 0.5f : 1.f;
instance_colors_ptr[index*4+2]=i<NUM_OBJECTS_Y/2? 0.5f : 1.f;
instance_colors_ptr[index*4+3]=1.f;
instance_scale_ptr[index*3] = 1;
instance_scale_ptr[index*3+1] = 1;
instance_scale_ptr[index*3+2] = 1;
index++;
}
}
}
int size = sizeof(cube_vertices) + POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE+SCALE_BUFFER_SIZE;
char* bla = (char*)malloc(size);
int szc = sizeof(cube_vertices);
memcpy(bla,&cube_vertices[0],szc);
memcpy(bla+sizeof(cube_vertices),instance_positions_ptr,POSITION_BUFFER_SIZE);
memcpy(bla+sizeof(cube_vertices)+POSITION_BUFFER_SIZE,instance_quaternion_ptr,ORIENTATION_BUFFER_SIZE);
memcpy(bla+sizeof(cube_vertices)+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE,instance_colors_ptr, COLOR_BUFFER_SIZE);
memcpy(bla+sizeof(cube_vertices)+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE,instance_scale_ptr, SCALE_BUFFER_SIZE);
glBufferData(GL_ARRAY_BUFFER, size, bla, GL_DYNAMIC_DRAW);//GL_STATIC_DRAW);
///initialize parts of the buffer
#ifdef _USE_SUB_DATA
glBufferSubData(GL_ARRAY_BUFFER, 0, sizeof(cube_vertices)+ 16384, bla);//cube_vertices);
#endif
char* dest= (char*)glMapBuffer( GL_ARRAY_BUFFER,GL_WRITE_ONLY);//GL_WRITE_ONLY
memcpy(dest,cube_vertices,sizeof(cube_vertices));
//memcpy(dest+sizeof(cube_vertices),instance_colors,sizeof(instance_colors));
glUnmapBuffer( GL_ARRAY_BUFFER);
writeTransforms();
/*
glBufferSubData(GL_ARRAY_BUFFER, sizeof(cube_vertices) + sizeof(instance_colors), POSITION_BUFFER_SIZE, instance_positions_ptr);
glBufferSubData(GL_ARRAY_BUFFER, sizeof(cube_vertices) + sizeof(instance_colors)+POSITION_BUFFER_SIZE,ORIENTATION_BUFFER_SIZE , instance_quaternion_ptr);
*/
glGenVertexArrays(1, &cube_vao);
glBindVertexArray(cube_vao);
glBindBuffer(GL_ARRAY_BUFFER, cube_vbo);
glBindVertexArray(0);
glGenBuffers(1, &index_vbo);
int indexBufferSize = sizeof(cube_indices);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_vbo);
glBufferData(GL_ELEMENT_ARRAY_BUFFER, indexBufferSize, NULL, GL_STATIC_DRAW);
glBufferSubData(GL_ELEMENT_ARRAY_BUFFER,0,indexBufferSize,cube_indices);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
glBindBuffer(GL_ARRAY_BUFFER,0);
glBindVertexArray(0);
}
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);
}
}
}
}
}
}
/*! 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)
