void clientDisplay(void) {
updateCamera();
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
//GL_ShapeDrawer::drawCoordSystem();
ATTRIBUTE_ALIGNED16(btScalar) m[16];
int i;
#ifdef USE_GJK
btGjkEpaPenetrationDepthSolver epa;
btGjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,&epa);
btVector3 seperatingAxis(0.00000000f,0.059727669f,0.29259586f);
convexConvex.setCachedSeperatingAxis(seperatingAxis);
btPointCollector gjkOutput;
btGjkPairDetector::ClosestPointInput input;
input.m_transformA = tr[0];
input.m_transformB = tr[1];
convexConvex.getClosestPoints(input ,gjkOutput,0);
if (gjkOutput.m_hasResult)
{
btVector3 endPt = gjkOutput.m_pointInWorld +
gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;
glBegin(GL_LINES);
glColor3f(1, 0, 0);
glVertex3d(gjkOutput.m_pointInWorld.x(), gjkOutput.m_pointInWorld.y(),gjkOutput.m_pointInWorld.z());
glVertex3d(endPt.x(),endPt.y(),endPt.z());
glEnd();
}
#else //USE_GJK
struct MyContactResultCallback : public btCollisionWorld::ContactResultCallback
{
virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1)
{
glBegin(GL_LINES);
glColor3f(1, 0, 0);
glVertex3d(cp.m_positionWorldOnA.getX(),cp.m_positionWorldOnA.getY(),cp.m_positionWorldOnA.getZ());
glVertex3d(cp.m_positionWorldOnB.getX(),cp.m_positionWorldOnB.getY(),cp.m_positionWorldOnB.getZ());
glEnd();
return 1.f;
}
};
btDefaultCollisionConfiguration collisionConfiguration;
btCollisionDispatcher dispatcher(&collisionConfiguration);
btDbvtBroadphase pairCache;
btCollisionWorld world (&dispatcher,&pairCache,&collisionConfiguration);
gContactBreakingThreshold=1e10f;
MyContactResultCallback result;
btCollisionObject obA;
obA.setCollisionShape(shapePtr[0]);
obA.setWorldTransform(tr[0]);
btCollisionObject obB;
obB.setCollisionShape(shapePtr[1]);
obB.setWorldTransform(tr[1]);
world.contactPairTest(&obA,&obB,result);
#endif//USE_GJK
btVector3 worldMin(-1000,-1000,-1000);
btVector3 worldMax(1000,1000,1000);
for (i=0;i<numObjects;i++)
{
tr[i].getOpenGLMatrix( m );
if (debugMode)
{
/// for polyhedral shapes
if (shapePtr[i]->isPolyhedral())
{
if (!shapePtr[i]->getUserPointer())
{
btConvexHullComputer* convexUtil = new btConvexHullComputer();
shapePtr[i]->setUserPointer(convexUtil);
btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shapePtr[i];
btAlignedObjectArray<btVector3> vertices;
vertices.resize(polyshape->getNumVertices());
for (int i=0;i<polyshape->getNumVertices();i++)
{
polyshape->getVertex(i,vertices[i]);
}
bool useDoublePrecision = false;
convexUtil->compute(&vertices[0].getX(),sizeof(btVector3), polyshape->getNumVertices(),0,0);
}
if (shapePtr[i]->getUserPointer())
{
btConvexHullComputer* convexUtil = (btConvexHullComputer*)shapePtr[i]->getUserPointer();
//printf("num faces = %d\n",convexUtil->faces.size());
for (int j=0;j<convexUtil->faces.size();j++)
{
int face = convexUtil->faces[j];
//printf("face=%d\n",face);
const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
const btConvexHullComputer::Edge* edge = firstEdge;
do
{
int src = edge->getSourceVertex();
int targ = edge->getTargetVertex();
//printf("src=%d target = %d\n", src,targ);
btVector3 wa = tr[i] * convexUtil->vertices[src];
btVector3 wb = tr[i] * convexUtil->vertices[targ];
glBegin(GL_LINES);
glColor3f(1, 1, 1);
glVertex3f(wa.getX(),wa.getY(),wa.getZ());
glVertex3f(wb.getX(),wb.getY(),wb.getZ());
glEnd();
edge = edge->getNextEdgeOfFace();
} while (edge!=firstEdge);
}
}
}
} else
{
shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(1,1,1),debugMode, worldMin, worldMax);
}
}
simplex.setSimplexSolver(&sGjkSimplexSolver);
btVector3 ybuf[4],pbuf[4],qbuf[4];
int numpoints = sGjkSimplexSolver.getSimplex(pbuf,qbuf,ybuf);
simplex.reset();
for (i=0;i<numpoints;i++)
simplex.addVertex(ybuf[i]);
btTransform ident;
ident.setIdentity();
ident.getOpenGLMatrix(m);
shapeDrawer.drawOpenGL(m,&simplex,btVector3(1,1,1),debugMode, worldMin,worldMax);
btQuaternion orn;
orn.setEuler(yaw,pitch,roll);
tr[0].setRotation(orn);
tr[1].setRotation(orn);
pitch += 0.005f;
yaw += 0.01f;
glFlush();
glutSwapBuffers();
}
void VehicleDemo::initPhysics()
{
#ifdef FORCE_ZAXIS_UP
m_cameraUp = btVector3(0,0,1);
m_forwardAxis = 1;
#endif
btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
m_collisionShapes.push_back(groundShape);
m_collisionConfiguration = new btDefaultCollisionConfiguration();
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
btVector3 worldMin(-1000,-1000,-1000);
btVector3 worldMax(1000,1000,1000);
m_overlappingPairCache = new btAxisSweep3(worldMin,worldMax);
m_constraintSolver = new btSequentialImpulseConstraintSolver();
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_constraintSolver,m_collisionConfiguration);
#ifdef FORCE_ZAXIS_UP
m_dynamicsWorld->setGravity(btVector3(0,0,-10));
#endif
//m_dynamicsWorld->setGravity(btVector3(0,0,0));
btTransform tr;
tr.setIdentity();
//either use heightfield or triangle mesh
#define USE_TRIMESH_GROUND 1
#ifdef USE_TRIMESH_GROUND
int i;
const float TRIANGLE_SIZE=20.f;
//create a triangle-mesh ground
int vertStride = sizeof(btVector3);
int indexStride = 3*sizeof(int);
const int NUM_VERTS_X = 20;
const int NUM_VERTS_Y = 20;
const int totalVerts = NUM_VERTS_X*NUM_VERTS_Y;
const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1);
m_vertices = new btVector3[totalVerts];
int* gIndices = new int[totalTriangles*3];
for ( i=0; i<NUM_VERTS_X; i++)
{
for (int j=0; j<NUM_VERTS_Y; j++)
{
float wl = .2f;
//height set to zero, but can also use curved landscape, just uncomment out the code
float height = 0.f;//20.f*sinf(float(i)*wl)*cosf(float(j)*wl);
#ifdef FORCE_ZAXIS_UP
m_vertices[i+j*NUM_VERTS_X].setValue(
(i-NUM_VERTS_X*0.5f)*TRIANGLE_SIZE,
(j-NUM_VERTS_Y*0.5f)*TRIANGLE_SIZE,
height
);
#else
m_vertices[i+j*NUM_VERTS_X].setValue(
(i-NUM_VERTS_X*0.5f)*TRIANGLE_SIZE,
height,
(j-NUM_VERTS_Y*0.5f)*TRIANGLE_SIZE);
#endif
}
}
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*) &m_vertices[0].x(),vertStride);
bool useQuantizedAabbCompression = true;
groundShape = new btBvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression);
tr.setOrigin(btVector3(0,-4.5f,0));
#else
//testing btHeightfieldTerrainShape
int width=128;
int length=128;
unsigned char* heightfieldData = new unsigned char[width*length];
{
for (int i=0; i<width*length; i++)
{
heightfieldData[i]=0;
}
}
char* filename="heightfield128x128.raw";
FILE* heightfieldFile = fopen(filename,"r");
if (!heightfieldFile)
{
filename="../../heightfield128x128.raw";
heightfieldFile = fopen(filename,"r");
}
if (heightfieldFile)
{
int numBytes =fread(heightfieldData,1,width*length,heightfieldFile);
//btAssert(numBytes);
if (!numBytes)
{
printf("couldn't read heightfield at %s\n",filename);
}
fclose (heightfieldFile);
}
btScalar maxHeight = 20000.f;
bool useFloatDatam=false;
bool flipQuadEdges=false;
btHeightfieldTerrainShape* heightFieldShape = new btHeightfieldTerrainShape(width,length,heightfieldData,maxHeight,upIndex,useFloatDatam,flipQuadEdges);;
groundShape = heightFieldShape;
heightFieldShape->setUseDiamondSubdivision(true);
btVector3 localScaling(20,20,20);
localScaling[upIndex]=1.f;
groundShape->setLocalScaling(localScaling);
tr.setOrigin(btVector3(0,-64.5f,0));
#endif //
m_collisionShapes.push_back(groundShape);
//create ground object
localCreateRigidBody(0,tr,groundShape);
#ifdef FORCE_ZAXIS_UP
// indexRightAxis = 0;
// indexUpAxis = 2;
// indexForwardAxis = 1;
btCollisionShape* chassisShape = new btBoxShape(btVector3(1.f,2.f, 0.5f));
btCompoundShape* compound = new btCompoundShape();
btTransform localTrans;
localTrans.setIdentity();
//localTrans effectively shifts the center of mass with respect to the chassis
localTrans.setOrigin(btVector3(0,0,1));
#else
btCollisionShape* chassisShape = new btBoxShape(btVector3(1.f,0.5f,2.f));
m_collisionShapes.push_back(chassisShape);
btCompoundShape* compound = new btCompoundShape();
m_collisionShapes.push_back(compound);
btTransform localTrans;
localTrans.setIdentity();
//localTrans effectively shifts the center of mass with respect to the chassis
localTrans.setOrigin(btVector3(0,1,0));
#endif
compound->addChildShape(localTrans,chassisShape);
tr.setOrigin(btVector3(0,0.f,0));
m_carChassis = localCreateRigidBody(800,tr,compound);//chassisShape);
//m_carChassis->setDamping(0.2,0.2);
clientResetScene();
/// create vehicle
{
m_vehicleRayCaster = new btDefaultVehicleRaycaster(m_dynamicsWorld);
m_vehicle = new btRaycastVehicle(m_tuning,m_carChassis,m_vehicleRayCaster);
///never deactivate the vehicle
m_carChassis->setActivationState(DISABLE_DEACTIVATION);
m_dynamicsWorld->addVehicle(m_vehicle);
float connectionHeight = 1.2f;
bool isFrontWheel=true;
//choose coordinate system
m_vehicle->setCoordinateSystem(rightIndex,upIndex,forwardIndex);
#ifdef FORCE_ZAXIS_UP
btVector3 connectionPointCS0(CUBE_HALF_EXTENTS-(0.3*wheelWidth),2*CUBE_HALF_EXTENTS-wheelRadius, connectionHeight);
#else
btVector3 connectionPointCS0(CUBE_HALF_EXTENTS-(0.3*wheelWidth),connectionHeight,2*CUBE_HALF_EXTENTS-wheelRadius);
#endif
m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);
#ifdef FORCE_ZAXIS_UP
connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),2*CUBE_HALF_EXTENTS-wheelRadius, connectionHeight);
#else
connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),connectionHeight,2*CUBE_HALF_EXTENTS-wheelRadius);
#endif
m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);
#ifdef FORCE_ZAXIS_UP
connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),-2*CUBE_HALF_EXTENTS+wheelRadius, connectionHeight);
#else
connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),connectionHeight,-2*CUBE_HALF_EXTENTS+wheelRadius);
#endif //FORCE_ZAXIS_UP
isFrontWheel = false;
m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);
#ifdef FORCE_ZAXIS_UP
connectionPointCS0 = btVector3(CUBE_HALF_EXTENTS-(0.3*wheelWidth),-2*CUBE_HALF_EXTENTS+wheelRadius, connectionHeight);
#else
connectionPointCS0 = btVector3(CUBE_HALF_EXTENTS-(0.3*wheelWidth),connectionHeight,-2*CUBE_HALF_EXTENTS+wheelRadius);
#endif
m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);
for (int i=0; i<m_vehicle->getNumWheels(); i++)
{
btWheelInfo& wheel = m_vehicle->getWheelInfo(i);
wheel.m_suspensionStiffness = suspensionStiffness;
wheel.m_wheelsDampingRelaxation = suspensionDamping;
wheel.m_wheelsDampingCompression = suspensionCompression;
wheel.m_frictionSlip = wheelFriction;
wheel.m_rollInfluence = rollInfluence;
}
}
setCameraDistance(26.f);
}
void ConcaveRaycastDemo::initPhysics()
{
#define TRISIZE 10.f
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;
trimeshShape = new btBvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression);
m_collisionShapes.push_back(trimeshShape);
btCollisionShape* groundShape = trimeshShape;
m_collisionConfiguration = new btDefaultCollisionConfiguration();
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
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);
m_dynamicsWorld->getSolverInfo().m_splitImpulse=true;
m_dynamicsWorld->setDebugDrawer(&sDebugDraw);
float mass = 0.f;
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,-2,0));
btCollisionShape* colShape = new btBoxShape(btVector3(1,1,1));
m_collisionShapes.push_back(colShape);
{
for (int i=0;i<10;i++)
{
//btCollisionShape* colShape = new btCapsuleShape(0.5,2.0);//boxShape = new btSphereShape(1.f);
startTransform.setOrigin(btVector3(2*i,10,1));
localCreateRigidBody(1, startTransform,colShape);
}
}
startTransform.setIdentity();
staticBody = localCreateRigidBody(mass, startTransform,groundShape);
staticBody->setCollisionFlags(staticBody->getCollisionFlags() | btCollisionObject::CF_STATIC_OBJECT);
#ifdef BATCH_RAYCASTER
int maxNumOutstandingTasks = 4;
#ifdef USE_WIN32_THREADING
Win32ThreadSupport::Win32ThreadConstructionInfo tci("batch raycast",
processRaycastTask,
createRaycastLocalStoreMemory,
maxNumOutstandingTasks);
m_threadSupportRaycast = new Win32ThreadSupport(tci);
printf("m_threadSupportRaycast = %p\n", m_threadSupportRaycast);
#endif
gBatchRaycaster = new SpuBatchRaycaster (m_threadSupportRaycast, maxNumOutstandingTasks, m_dynamicsWorld->getCollisionObjectArray(), m_dynamicsWorld->getNumCollisionObjects());
#endif
raycastBar = btRaycastBar (4000.0, 0.0);
//raycastBar = btRaycastBar (true, 40.0, -50.0, 50.0);
}
void ForkLiftDemo::initPhysics()
{
int upAxis = 1;
m_guiHelper->setUpAxis(upAxis);
btVector3 groundExtents(50,50,50);
groundExtents[upAxis]=3;
btCollisionShape* groundShape = new btBoxShape(groundExtents);
m_collisionShapes.push_back(groundShape);
m_collisionConfiguration = new btDefaultCollisionConfiguration();
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
btVector3 worldMin(-1000,-1000,-1000);
btVector3 worldMax(1000,1000,1000);
m_overlappingPairCache = new btAxisSweep3(worldMin,worldMax);
if (useMCLPSolver)
{
btDantzigSolver* mlcp = new btDantzigSolver();
//btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel;
btMLCPSolver* sol = new btMLCPSolver(mlcp);
m_constraintSolver = sol;
} else
{
m_constraintSolver = new btSequentialImpulseConstraintSolver();
}
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_constraintSolver,m_collisionConfiguration);
if (useMCLPSolver)
{
m_dynamicsWorld ->getSolverInfo().m_minimumSolverBatchSize = 1;//for direct solver it is better to have a small A matrix
} else
{
m_dynamicsWorld ->getSolverInfo().m_minimumSolverBatchSize = 128;//for direct solver, it is better to solve multiple objects together, small batches have high overhead
}
m_dynamicsWorld->getSolverInfo().m_globalCfm = 0.00001;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
//m_dynamicsWorld->setGravity(btVector3(0,0,0));
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(0,-3,0));
//either use heightfield or triangle mesh
//create ground object
localCreateRigidBody(0,tr,groundShape);
btCollisionShape* chassisShape = new btBoxShape(btVector3(1.f,0.5f,2.f));
m_collisionShapes.push_back(chassisShape);
btCompoundShape* compound = new btCompoundShape();
m_collisionShapes.push_back(compound);
btTransform localTrans;
localTrans.setIdentity();
//localTrans effectively shifts the center of mass with respect to the chassis
localTrans.setOrigin(btVector3(0,1,0));
compound->addChildShape(localTrans,chassisShape);
{
btCollisionShape* suppShape = new btBoxShape(btVector3(0.5f,0.1f,0.5f));
btTransform suppLocalTrans;
suppLocalTrans.setIdentity();
//localTrans effectively shifts the center of mass with respect to the chassis
suppLocalTrans.setOrigin(btVector3(0,1.0,2.5));
compound->addChildShape(suppLocalTrans, suppShape);
}
tr.setOrigin(btVector3(0,0.f,0));
m_carChassis = localCreateRigidBody(800,tr,compound);//chassisShape);
//m_carChassis->setDamping(0.2,0.2);
m_wheelShape = new btCylinderShapeX(btVector3(wheelWidth,wheelRadius,wheelRadius));
m_guiHelper->createCollisionShapeGraphicsObject(m_wheelShape);
int wheelGraphicsIndex = m_wheelShape->getUserIndex();
const float position[4]={0,10,10,0};
const float quaternion[4]={0,0,0,1};
const float color[4]={0,1,0,1};
const float scaling[4] = {1,1,1,1};
for (int i=0;i<4;i++)
{
m_wheelInstances[i] = m_guiHelper->registerGraphicsInstance(wheelGraphicsIndex, position, quaternion, color, scaling);
}
{
btCollisionShape* liftShape = new btBoxShape(btVector3(0.5f,2.0f,0.05f));
m_collisionShapes.push_back(liftShape);
btTransform liftTrans;
m_liftStartPos = btVector3(0.0f, 2.5f, 3.05f);
liftTrans.setIdentity();
liftTrans.setOrigin(m_liftStartPos);
m_liftBody = localCreateRigidBody(10,liftTrans, liftShape);
btTransform localA, localB;
localA.setIdentity();
localB.setIdentity();
localA.getBasis().setEulerZYX(0, M_PI_2, 0);
localA.setOrigin(btVector3(0.0, 1.0, 3.05));
localB.getBasis().setEulerZYX(0, M_PI_2, 0);
localB.setOrigin(btVector3(0.0, -1.5, -0.05));
m_liftHinge = new btHingeConstraint(*m_carChassis,*m_liftBody, localA, localB);
// m_liftHinge->setLimit(-LIFT_EPS, LIFT_EPS);
m_liftHinge->setLimit(0.0f, 0.0f);
m_dynamicsWorld->addConstraint(m_liftHinge, true);
btCollisionShape* forkShapeA = new btBoxShape(btVector3(1.0f,0.1f,0.1f));
m_collisionShapes.push_back(forkShapeA);
btCompoundShape* forkCompound = new btCompoundShape();
m_collisionShapes.push_back(forkCompound);
btTransform forkLocalTrans;
forkLocalTrans.setIdentity();
forkCompound->addChildShape(forkLocalTrans, forkShapeA);
btCollisionShape* forkShapeB = new btBoxShape(btVector3(0.1f,0.02f,0.6f));
m_collisionShapes.push_back(forkShapeB);
forkLocalTrans.setIdentity();
forkLocalTrans.setOrigin(btVector3(-0.9f, -0.08f, 0.7f));
forkCompound->addChildShape(forkLocalTrans, forkShapeB);
btCollisionShape* forkShapeC = new btBoxShape(btVector3(0.1f,0.02f,0.6f));
m_collisionShapes.push_back(forkShapeC);
forkLocalTrans.setIdentity();
forkLocalTrans.setOrigin(btVector3(0.9f, -0.08f, 0.7f));
forkCompound->addChildShape(forkLocalTrans, forkShapeC);
btTransform forkTrans;
m_forkStartPos = btVector3(0.0f, 0.6f, 3.2f);
forkTrans.setIdentity();
forkTrans.setOrigin(m_forkStartPos);
m_forkBody = localCreateRigidBody(5, forkTrans, forkCompound);
localA.setIdentity();
localB.setIdentity();
localA.getBasis().setEulerZYX(0, 0, M_PI_2);
localA.setOrigin(btVector3(0.0f, -1.9f, 0.05f));
localB.getBasis().setEulerZYX(0, 0, M_PI_2);
localB.setOrigin(btVector3(0.0, 0.0, -0.1));
m_forkSlider = new btSliderConstraint(*m_liftBody, *m_forkBody, localA, localB, true);
m_forkSlider->setLowerLinLimit(0.1f);
m_forkSlider->setUpperLinLimit(0.1f);
// m_forkSlider->setLowerAngLimit(-LIFT_EPS);
// m_forkSlider->setUpperAngLimit(LIFT_EPS);
m_forkSlider->setLowerAngLimit(0.0f);
m_forkSlider->setUpperAngLimit(0.0f);
m_dynamicsWorld->addConstraint(m_forkSlider, true);
btCompoundShape* loadCompound = new btCompoundShape();
m_collisionShapes.push_back(loadCompound);
btCollisionShape* loadShapeA = new btBoxShape(btVector3(2.0f,0.5f,0.5f));
m_collisionShapes.push_back(loadShapeA);
btTransform loadTrans;
loadTrans.setIdentity();
loadCompound->addChildShape(loadTrans, loadShapeA);
btCollisionShape* loadShapeB = new btBoxShape(btVector3(0.1f,1.0f,1.0f));
m_collisionShapes.push_back(loadShapeB);
loadTrans.setIdentity();
loadTrans.setOrigin(btVector3(2.1f, 0.0f, 0.0f));
loadCompound->addChildShape(loadTrans, loadShapeB);
btCollisionShape* loadShapeC = new btBoxShape(btVector3(0.1f,1.0f,1.0f));
m_collisionShapes.push_back(loadShapeC);
loadTrans.setIdentity();
loadTrans.setOrigin(btVector3(-2.1f, 0.0f, 0.0f));
loadCompound->addChildShape(loadTrans, loadShapeC);
loadTrans.setIdentity();
m_loadStartPos = btVector3(0.0f, 3.5f, 7.0f);
loadTrans.setOrigin(m_loadStartPos);
m_loadBody = localCreateRigidBody(loadMass, loadTrans, loadCompound);
}
/// create vehicle
{
m_vehicleRayCaster = new btDefaultVehicleRaycaster(m_dynamicsWorld);
m_vehicle = new btRaycastVehicle(m_tuning,m_carChassis,m_vehicleRayCaster);
///never deactivate the vehicle
m_carChassis->setActivationState(DISABLE_DEACTIVATION);
m_dynamicsWorld->addVehicle(m_vehicle);
float connectionHeight = 1.2f;
bool isFrontWheel=true;
//choose coordinate system
m_vehicle->setCoordinateSystem(rightIndex,upIndex,forwardIndex);
btVector3 connectionPointCS0(CUBE_HALF_EXTENTS-(0.3*wheelWidth),connectionHeight,2*CUBE_HALF_EXTENTS-wheelRadius);
m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);
connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),connectionHeight,2*CUBE_HALF_EXTENTS-wheelRadius);
m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);
connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),connectionHeight,-2*CUBE_HALF_EXTENTS+wheelRadius);
isFrontWheel = false;
m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);
connectionPointCS0 = btVector3(CUBE_HALF_EXTENTS-(0.3*wheelWidth),connectionHeight,-2*CUBE_HALF_EXTENTS+wheelRadius);
m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);
for (int i=0;i<m_vehicle->getNumWheels();i++)
{
btWheelInfo& wheel = m_vehicle->getWheelInfo(i);
wheel.m_suspensionStiffness = suspensionStiffness;
wheel.m_wheelsDampingRelaxation = suspensionDamping;
wheel.m_wheelsDampingCompression = suspensionCompression;
wheel.m_frictionSlip = wheelFriction;
wheel.m_rollInfluence = rollInfluence;
}
}
resetForklift();
// setCameraDistance(26.f);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void CharacterDemo::initPhysics()
{
btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
m_collisionShapes.push_back(groundShape);
m_collisionConfiguration = new btDefaultCollisionConfiguration();
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
btVector3 worldMin(-1000,-1000,-1000);
btVector3 worldMax(1000,1000,1000);
btAxisSweep3* sweepBP = new btAxisSweep3(worldMin,worldMax);
m_overlappingPairCache = sweepBP;
m_constraintSolver = new btSequentialImpulseConstraintSolver();
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_constraintSolver,m_collisionConfiguration);
m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration=0.0001f;
#ifdef DYNAMIC_CHARACTER_CONTROLLER
m_character = new DynamicCharacterController ();
#else
btTransform startTransform;
startTransform.setIdentity ();
//startTransform.setOrigin (btVector3(0.0, 4.0, 0.0));
startTransform.setOrigin (btVector3(10.210098,-1.6433364,16.453260));
m_ghostObject = new btPairCachingGhostObject();
m_ghostObject->setWorldTransform(startTransform);
sweepBP->getOverlappingPairCache()->setInternalGhostPairCallback(new btGhostPairCallback());
btScalar characterHeight=1.75;
btScalar characterWidth =1.75;
btConvexShape* capsule = new btCapsuleShape(characterWidth,characterHeight);
m_ghostObject->setCollisionShape (capsule);
m_ghostObject->setCollisionFlags (btCollisionObject::CF_CHARACTER_OBJECT);
btScalar stepHeight = btScalar(0.35);
m_character = new btKinematicCharacterController (m_ghostObject,capsule,stepHeight);
#endif
////////////////
/// Create some basic environment from a Quake level
//m_dynamicsWorld->setGravity(btVector3(0,0,0));
btTransform tr;
tr.setIdentity();
const char* bspfilename = "BspDemo.bsp";
void* memoryBuffer = 0;
FILE* file = fopen(bspfilename,"r");
if (!file)
{
//cmake generated visual studio projects need 4 levels back
bspfilename = "../../../../BspDemo.bsp";
file = fopen(bspfilename,"r");
}
if (!file)
{
//visual studio leaves the current working directory in the projectfiles folder
bspfilename = "../../BspDemo.bsp";
file = fopen(bspfilename,"r");
}
if (!file)
{
//visual studio leaves the current working directory in the projectfiles folder
bspfilename = "BspDemo.bsp";
file = fopen(bspfilename,"r");
}
if (file)
{
BspLoader bspLoader;
int size=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", bspfilename);
} else
{
//how to detect file size?
memoryBuffer = malloc(size+1);
fread(memoryBuffer,1,size,file);
bspLoader.loadBSPFile( memoryBuffer);
BspToBulletConverter bsp2bullet(this);
float bspScaling = 0.1f;
bsp2bullet.convertBsp(bspLoader,bspScaling);
}
fclose(file);
}
///only collide with static for now (no interaction with dynamic objects)
m_dynamicsWorld->addCollisionObject(m_ghostObject,btBroadphaseProxy::CharacterFilter, btBroadphaseProxy::StaticFilter|btBroadphaseProxy::DefaultFilter);
m_dynamicsWorld->addAction(m_character);
///////////////
clientResetScene();
setCameraDistance(56.f);
}
bool BtWorld::initWorld( bool isServer, ProcessList *processList )
{
// Collision configuration contains default setup for memory, collision setup.
//.logicking >> - soft body support
//mCollisionConfiguration = new btDefaultCollisionConfiguration();
mCollisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
//.logicking <<
// TODO: There is something wrong with multithreading
// and compound convex shapes... so disable it for now.
static const U32 smMaxThreads = 1;
// Different initialization with threading enabled.
if ( smMaxThreads > 1 )
{
// TODO: ifdef assumes smMaxThread is always one at this point. MACOSX support to be decided
#ifdef WIN32
mThreadSupportCollision = new Win32ThreadSupport(
Win32ThreadSupport::Win32ThreadConstructionInfo( isServer ? "bt_servercol" : "bt_clientcol",
processCollisionTask,
createCollisionLocalStoreMemory,
smMaxThreads ) );
mDispatcher = new SpuGatheringCollisionDispatcher( mThreadSupportCollision,
smMaxThreads,
mCollisionConfiguration );
#endif // WIN32
}
else
{
mThreadSupportCollision = NULL;
mDispatcher = new btCollisionDispatcher( mCollisionConfiguration );
}
btVector3 worldMin( -2000, -2000, -1000 );
btVector3 worldMax( 2000, 2000, 1000 );
btAxisSweep3 *sweepBP = new btAxisSweep3( worldMin, worldMax );
mBroadphase = sweepBP;
sweepBP->getOverlappingPairCache()->setInternalGhostPairCallback( new btGhostPairCallback() );
// The default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded).
mSolver = new btSequentialImpulseConstraintSolver;
//.logicking >>
mDynamicsWorld = new btSoftRigidDynamicsWorld(mDispatcher,mBroadphase,mSolver,mCollisionConfiguration);
mSoftBodyWorldInfo.m_broadphase = mBroadphase;
mSoftBodyWorldInfo.m_dispatcher = mDispatcher;
mSoftBodyWorldInfo.m_gravity = btCast<btVector3>( mGravity );
mSoftBodyWorldInfo.m_sparsesdf.Initialize();
//mDynamicsWorld = new btDiscreteDynamicsWorld( mDispatcher, mBroadphase, mSolver, mCollisionConfiguration );
//.logicking <<
if ( !mDynamicsWorld )
{
Con::errorf( "BtWorld - %s failed to create dynamics world!", isServer ? "Server" : "Client" );
return false;
}
// Removing the randomization in the solver is required
// to make the simulation deterministic.
mDynamicsWorld->getSolverInfo().m_solverMode &= ~SOLVER_RANDMIZE_ORDER;
mDynamicsWorld->setGravity( btCast<btVector3>( mGravity ) );
AssertFatal( processList, "BtWorld::init() - We need a process list to create the world!" );
mProcessList = processList;
mProcessList->preTickSignal().notify( this, &BtWorld::getPhysicsResults );
mProcessList->postTickSignal().notify( this, &BtWorld::tickPhysics, 1000.0f );
return true;
}
void clientDisplay(void) {
updateCamera();
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
//GL_ShapeDrawer::drawCoordSystem();
float m[16];
int i;
#ifdef USE_GJK
btGjkEpaPenetrationDepthSolver epa;
btGjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,&epa);
btVector3 seperatingAxis(0.00000000f,0.059727669f,0.29259586f);
convexConvex.setCachedSeperatingAxis(seperatingAxis);
btPointCollector gjkOutput;
btGjkPairDetector::ClosestPointInput input;
input.m_transformA = tr[0];
input.m_transformB = tr[1];
convexConvex.getClosestPoints(input ,gjkOutput,0);
if (gjkOutput.m_hasResult)
{
btVector3 endPt = gjkOutput.m_pointInWorld +
gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;
glBegin(GL_LINES);
glColor3f(1, 0, 0);
glVertex3d(gjkOutput.m_pointInWorld.x(), gjkOutput.m_pointInWorld.y(),gjkOutput.m_pointInWorld.z());
glVertex3d(endPt.x(),endPt.y(),endPt.z());
glEnd();
}
#else //USE_GJK
struct MyContactResultCallback : public btCollisionWorld::ContactResultCallback
{
virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1)
{
glBegin(GL_LINES);
glColor3f(1, 0, 0);
glVertex3d(cp.m_positionWorldOnA.getX(),cp.m_positionWorldOnA.getY(),cp.m_positionWorldOnA.getZ());
glVertex3d(cp.m_positionWorldOnB.getX(),cp.m_positionWorldOnB.getY(),cp.m_positionWorldOnB.getZ());
glEnd();
return 1.f;
}
};
btDefaultCollisionConfiguration collisionConfiguration;
btCollisionDispatcher dispatcher(&collisionConfiguration);
btDbvtBroadphase pairCache;
btCollisionWorld world (&dispatcher,&pairCache,&collisionConfiguration);
world.getDispatchInfo().m_convexMaxDistanceUseCPT = true;
MyContactResultCallback result;
btCollisionObject obA;
obA.setCollisionShape(shapePtr[0]);
obA.setWorldTransform(tr[0]);
btCollisionObject obB;
obB.setCollisionShape(shapePtr[1]);
obB.setWorldTransform(tr[1]);
world.contactPairTest(&obA,&obB,result);
#endif//USE_GJK
btVector3 worldMin(-1000,-1000,-1000);
btVector3 worldMax(1000,1000,1000);
for (i=0;i<numObjects;i++)
{
tr[i].getOpenGLMatrix( m );
shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(1,1,1),debugMode, worldMin, worldMax);
}
simplex.setSimplexSolver(&sGjkSimplexSolver);
btVector3 ybuf[4],pbuf[4],qbuf[4];
int numpoints = sGjkSimplexSolver.getSimplex(pbuf,qbuf,ybuf);
simplex.reset();
for (i=0;i<numpoints;i++)
simplex.addVertex(ybuf[i]);
btTransform ident;
ident.setIdentity();
ident.getOpenGLMatrix(m);
shapeDrawer.drawOpenGL(m,&simplex,btVector3(1,1,1),debugMode, worldMin,worldMax);
btQuaternion orn;
orn.setEuler(yaw,pitch,roll);
tr[0].setRotation(orn);
tr[1].setRotation(orn);
pitch += 0.005f;
yaw += 0.01f;
glFlush();
glutSwapBuffers();
}
void ConcaveConvexcastDemo::initPhysics()
{
#define TRISIZE 10.f
setCameraDistance(100.f);
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;
trimeshShape = new btBvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression);
m_collisionShapes.push_back(trimeshShape);
btCollisionShape* groundShape = trimeshShape;
m_collisionConfiguration = new btDefaultCollisionConfiguration();
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
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);
float mass = 0.f;
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,-2,0));
btCollisionShape* colShape = new btBoxShape(btVector3(1,1,1));
m_collisionShapes.push_back(colShape);
{
for (int j=0;j<NUM_DYNAMIC_BOXES_X;j++)
for (int i=0;i<NUM_DYNAMIC_BOXES_Y;i++)
{
//btCollisionShape* colShape = new btCapsuleShape(0.5,2.0);//boxShape = new btSphereShape(1.f);
startTransform.setOrigin(btVector3(5*(i-NUM_DYNAMIC_BOXES_X/2),10,5*(j-NUM_DYNAMIC_BOXES_Y/2)));
localCreateRigidBody(1, startTransform,colShape);
}
}
startTransform.setIdentity();
//startTransform = btTransform(btQuaternion (btVector3(1,1,1), 1.5));
staticBody = localCreateRigidBody(mass, startTransform,groundShape);
staticBody->setCollisionFlags(staticBody->getCollisionFlags() | btCollisionObject::CF_STATIC_OBJECT);
//enable custom material callback
staticBody->setCollisionFlags(staticBody->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
convexcastBatch = btConvexcastBatch (40.0, 0.0, -10.0,80.0);
//convexcastBatch = btConvexcastBatch (true, 40.0, -50.0, 50.0);
}
void BtPhysics::run() {
btVector3 worldMin(-1000,-1000,-1000);
btVector3 worldMax(1000,1000,1000);
m_broadphase = new btAxisSweep3(worldMin,worldMax); //TODO: get bounds from world information
m_collision_config = new btDefaultCollisionConfiguration;
m_dispatcher = new btCollisionDispatcher(m_collision_config);
m_solver = new btSequentialImpulseConstraintSolver;
m_world = new btDiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collision_config);
m_world->setGravity(btVector3(0, 0, -9.8));
m_gpcallback = new btGhostPairCallback();
m_broadphase->getOverlappingPairCache()->setInternalGhostPairCallback(m_gpcallback);
{
boost::mutex::scoped_lock lock(m_status_mutex);
m_running = true;
}
m_status_cond.notify_all();
while(!m_finish) {
boost::system_time t = boost::get_system_time();
t += boost::posix_time::millisec(1000/30);
ServerMsg* msg;
while(m_msg_queue.try_pop(msg))
parseMessage(msg);
updateCharacterObjects();
m_world->stepSimulation(1.f/30.f, 1, 1.f/30.f);
LocalServer::ClientTransList ct;
ct.reserve(m_characters.size());
for(auto i = m_characters.begin(); i != m_characters.end(); ++i) {
float *mat = new float[16];
i->second->m_object->getWorldTransform().getOpenGLMatrix(mat);
ct.push_back(std::make_pair(i->first, mat));
}
m_server->runTickCallbacks();
m_server->pushClientTransforms(ct);
boost::this_thread::sleep(t);
}
for(auto i = m_characters.begin(); i != m_characters.end(); ++i) {
CharacterObject *character = i->second;
m_world->removeAction(character->m_controller);
m_world->removeCollisionObject(character->m_object);
delete character->m_controller;
delete character->m_object;
delete character->m_collision;
delete character;
}
for(auto i = m_objects.begin(); i != m_objects.end(); ++i) {
PhysicsObject *object = i->second;
m_world->removeRigidBody(object->m_body);
delete object->m_body->getMotionState();
delete object->m_body;
delete object->m_collision;
delete object;
}
delete m_world;
delete m_solver;
delete m_dispatcher;
delete m_collision_config;
delete m_broadphase;
delete m_gpcallback;
{
boost::mutex::scoped_lock lock(m_status_mutex);
m_running = false;
}
m_status_cond.notify_all();
}
void DynamicsSolver::initPhysics()
{
m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
btSoftBodyWorldInfo worldInfo;
worldInfo.m_dispatcher = m_dispatcher;
btVector3 worldMin(-1000,-1000,-1000);
btVector3 worldMax(1000,1000,1000);
m_broadphase = new btAxisSweep3(worldMin,worldMax, 1000);
worldInfo.m_broadphase = m_broadphase;
worldInfo.m_sparsesdf.Initialize();
worldInfo.m_gravity.setValue(0,0,0);
m_constraintSolver = new btSequentialImpulseConstraintSolver();
m_dynamicsWorld = new btSoftRigidDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
m_dynamicsWorld->getDispatchInfo().m_enableSPU = true;
m_dynamicsWorld->setGravity(btVector3(0,-1,0));
btCollisionShape* groundShape = new btBoxShape(btVector3(75,1,75));
m_collisionShapes.push_back(groundShape);
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(0,-5,0));
btRigidBody* body = localCreateRigidBody(0.f,tr,groundShape);
m_dynamicsWorld->addRigidBody(body);
btCollisionShape* cubeShape = new btBoxShape(btVector3(1.f,1.f,1.f));
m_collisionShapes.push_back(cubeShape);
btTransform trans;
trans.setIdentity();
trans.setOrigin(btVector3(0.0, 15.0, 0.0));
btCollisionShape* cubeShape1 = new btBoxShape(btVector3(1.f,.1f,.2f));
m_collisionShapes.push_back(cubeShape1);
btRigidBody* body0 = localCreateRigidBody(1.f, trans, cubeShape1);
m_dynamicsWorld->addRigidBody(body0);
/*
btCollisionShape* cubeShape2 = new btBoxShape(btVector3(2.f,.2f,.5f));
m_collisionShapes.push_back(cubeShape2);
trans.setOrigin(btVector3(5.0, 9.0, 3.0));
btRigidBody* clavicle = localCreateRigidBody(1.f, trans, cubeShape2);
m_dynamicsWorld->addRigidBody(clavicle);
btCollisionShape* cubeShape3 = new btBoxShape(btVector3(4.f,.2f,.5f));
m_collisionShapes.push_back(cubeShape3);
trans.setOrigin(btVector3(20.0, 9.0, 4.0));
btRigidBody* body2 = localCreateRigidBody(1.f, trans, cubeShape3);
m_dynamicsWorld->addRigidBody(body2);
trans.setOrigin(btVector3(25.0, 9.0, 6.0));
btRigidBody* body3 = localCreateRigidBody(1.f, trans, cubeShape3);
m_dynamicsWorld->addRigidBody(body3);
btCollisionShape* cubeShape4 = new btBoxShape(btVector3(.5f,.5f,.5f));
m_collisionShapes.push_back(cubeShape4);
trans.setOrigin(btVector3(28.0, 9.0, 12.0));
btRigidBody* body4 = localCreateRigidBody(1.f, trans, cubeShape4);
m_dynamicsWorld->addRigidBody(body4);
btMatrix3x3 flip(1.f, 0.f, 0.f, 0.f, 0.f, -1.f, 0.f, 1.f, 0.f);
btTransform frameInA(flip), frameInB(flip);
frameInA.setOrigin(btVector3(2., 4., 0.));
frameInB.setOrigin(btVector3(-2.5, 0., 0.));
btGeneric6DofConstraint* d6f = new btGeneric6DofConstraint(*body0, *clavicle, frameInA, frameInB, true);
d6f->setAngularLowerLimit(btVector3(0., -SIMD_PI/4., -SIMD_PI/4.));
d6f->setAngularUpperLimit(btVector3(0., SIMD_PI/4., SIMD_PI/4.));
m_dynamicsWorld->addConstraint(d6f);
frameInA.setOrigin(btVector3(2.5, 0., 0.));
frameInB.setOrigin(btVector3(-6., 0., 0.));
btGeneric6DofConstraint* d6f1 = new btGeneric6DofConstraint(*body2, *clavicle, frameInB, frameInA, true);
d6f1->setAngularLowerLimit(btVector3(-SIMD_PI/2.3, -SIMD_PI/2.1, -SIMD_PI/22.3));
d6f1->setAngularUpperLimit(btVector3(SIMD_PI/2.3, SIMD_PI/12.3, SIMD_PI/1.8));
m_dynamicsWorld->addConstraint(d6f1);
frameInA.setOrigin(btVector3(6., 0., 0.));
frameInB.setOrigin(btVector3(0., 0., 0.));
btGeneric6DofConstraint* d6f2 = new btGeneric6DofConstraint(*body2, *body4, frameInA, frameInB, true);
//d6f2->setAngularLowerLimit(btVector3(0., 0., -SIMD_PI* .75));
//d6f2->setAngularUpperLimit(btVector3(0., 0., 0.));
//d6f2->setAngularLowerLimit(btVector3(0., 0., 0.));
//d6f2->setAngularUpperLimit(btVector3(0., 0., 0.));
d6f2->setLinearLowerLimit(btVector3(-33.3, 0., 0.));
d6f2->setLinearUpperLimit(btVector3(33.3, 0., 0.));
m_dynamicsWorld->addConstraint(d6f2);
frameInA.setOrigin(btVector3(6., 0., 0.));
frameInB.setOrigin(btVector3(-2., 0., 0.));
btGeneric6DofConstraint* d6f3 = new btGeneric6DofConstraint(*body3, *body4, frameInA, frameInB, true);
m_dynamicsWorld->addConstraint(d6f3);
body0->setDamping(.99f, .99f);
clavicle->setDamping(.99f, .99f);
body2->setDamping(.99f, .99f);
body3->setDamping(.99f, .99f);
body4->setDamping(.99f, .99f);
btCollisionShape* scapulaShape = new btBoxShape(btVector3(1.f,1.5f,.25f));
m_collisionShapes.push_back(scapulaShape);
trans.setOrigin(btVector3(6.0, 7.0, 4.0));
btRigidBody* scapula = localCreateRigidBody(1.f, trans, scapulaShape);
m_dynamicsWorld->addRigidBody(scapula);
scapula->setDamping(.99f, .99f);
frameInA.setOrigin(btVector3(2.5, 0., 0.));
frameInB.setOrigin(btVector3(1.2, 1.99, 0.5));
btGeneric6DofConstraint* c2s = new btGeneric6DofConstraint(*clavicle, *scapula, frameInA, frameInB, true);
c2s->setAngularLowerLimit(btVector3(-SIMD_PI/2.3, -SIMD_PI/2.1, -SIMD_PI/22.3));
c2s->setAngularUpperLimit(btVector3(SIMD_PI/2.3, SIMD_PI/12.3, SIMD_PI/1.8));
m_dynamicsWorld->addConstraint(c2s);
frameInA.setOrigin(btVector3(-1.2, 1.99, 0.));
frameInB.setOrigin(btVector3(3., 2., -3.));
btGeneric6DofConstraint* r2s = new btGeneric6DofConstraint(*scapula, *body0, frameInA, frameInB, true);
r2s->setLinearLowerLimit(btVector3(-3.3, -3.1, -3.3));
r2s->setLinearUpperLimit(btVector3(3.3, 3.3, 3.8));
m_dynamicsWorld->addConstraint(r2s);
frameInA.setOrigin(btVector3(-1.2, -1.99, 0.));
frameInB.setOrigin(btVector3(3., -2., -3.));
btGeneric6DofConstraint* r2s2 = new btGeneric6DofConstraint(*scapula, *body0, frameInA, frameInB, true);
r2s2->setLinearLowerLimit(btVector3(-3.3, -3.1, -3.3));
r2s2->setLinearUpperLimit(btVector3(3.3, 3.3, 3.8));
m_dynamicsWorld->addConstraint(r2s2);
*/
//initRope();
}
void MultiMaterialDemo::initPhysics()
{
#define TRISIZE 50.f
gContactAddedCallback = CustomMaterialCombinerCallback;
// The number of triangles
const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1);
// The number of materials
const int totalMaterials = 2;
int vertStride = sizeof(btVector3);
int indexStride = 3*sizeof(int);
int materialStride = sizeof(CustomMaterial);
int triangleMaterialStride = sizeof(int);
gVertices = new btVector3[totalVerts];
gIndices = new int[totalTriangles*3];
gMaterials = new CustomMaterial[totalMaterials];
gFaceMaterialIndices = new int[totalTriangles];
// Explicitly set up the materials. It's a small array so let's do it bit by bit.
gMaterials[0].m_friction = 0;
gMaterials[0].m_restitution = 0.9;
gMaterials[0].foo1 = 5;
gMaterials[0].foo2 = 7;
gMaterials[1].m_friction = 0.9;
gMaterials[1].m_restitution = 0.1;
gMaterials[1].foo1 = 53;
gMaterials[1].foo2 = 15;
int i;
// Set up the vertex data
setVertexPositions(waveheight,0.f);
int index=0;
// Set up the face data
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;
}
}
// Set up the face->material index data
for(int a = 0; a < totalTriangles; a++)
{
// This will give the first half of the faces low friction and high restitution
// and the second half of the faces high friction and low restitution
if(a > totalTriangles*0.5f)
gFaceMaterialIndices[a] = 0;
else
gFaceMaterialIndices[a] = 1;
}
// Create the array structure
m_indexVertexArrays = new btTriangleIndexVertexMaterialArray(
totalTriangles, gIndices, indexStride,
totalVerts,(btScalar*) &gVertices[0].x(),vertStride,
totalMaterials, (unsigned char *)gMaterials, sizeof(CustomMaterial),
gFaceMaterialIndices, sizeof(int));
bool useQuantizedAabbCompression = true;
// Create the multimaterial mesh shape
trimeshShape = new btMultimaterialTriangleMeshShape((btTriangleIndexVertexMaterialArray*)m_indexVertexArrays,useQuantizedAabbCompression);
m_collisionShapes.push_back(trimeshShape);
btCollisionShape* groundShape = trimeshShape;
m_collisionConfiguration = new btDefaultCollisionConfiguration();
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
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);
float mass = 0.f;
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,-2,0));
btCollisionShape* colShape = new btBoxShape(btVector3(0.5f,0.5f,0.5f));
m_collisionShapes.push_back(colShape);
{
for (int i=0;i<1;i++)
{
startTransform.setOrigin(btVector3(10,10,-20));
btRigidBody* body = localCreateRigidBody(1, startTransform,colShape);
body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
body->setFriction(0.9f);
body->setGravity(btVector3(0,-20.f,0));
body->applyCentralImpulse(btVector3(-7.7f,0,0));
}
}
startTransform.setIdentity();
staticBody = localCreateRigidBody(mass, startTransform,groundShape);
staticBody->setCollisionFlags(staticBody->getCollisionFlags() | btCollisionObject::CF_STATIC_OBJECT);
//enable custom material callback
staticBody->setCollisionFlags(staticBody->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
}
void SliderConstraintDemo::initPhysics()
{
setTexturing(true);
setShadows(true);
setCameraDistance(26.f);
// init world
m_collisionConfiguration = new btDefaultCollisionConfiguration();
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
btVector3 worldMin(-1000,-1000,-1000);
btVector3 worldMax(1000,1000,1000);
m_overlappingPairCache = new btAxisSweep3(worldMin,worldMax);
#if SLIDER_DEMO_USE_ODE_SOLVER
m_constraintSolver = new btOdeQuickstepConstraintSolver();
#else
m_constraintSolver = new btSequentialImpulseConstraintSolver();
#endif
btDiscreteDynamicsWorld* wp = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_constraintSolver,m_collisionConfiguration);
// wp->getSolverInfo().m_numIterations = 20; // default is 10
m_dynamicsWorld = wp;
// wp->getSolverInfo().m_erp = 0.8;
// add floor
//btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-76,0));
btRigidBody* groundBody;
groundBody = localCreateRigidBody(0, groundTransform, groundShape);
// add box shape (will be reused for all bodies)
btCollisionShape* shape = new btBoxShape(btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS));
m_collisionShapes.push_back(shape);
// mass of dymamic bodies
btScalar mass = btScalar(1.);
// add dynamic rigid body A1
btTransform trans;
trans.setIdentity();
btVector3 worldPos(-20,0,0);
trans.setOrigin(worldPos);
btTransform frameInA, frameInB;
frameInA = btTransform::getIdentity();
frameInB = btTransform::getIdentity();
#if SLIDER_ENABLE_ALL_DEMOS
btRigidBody* pRbA1 = localCreateRigidBody(mass, trans, shape);
// btRigidBody* pRbA1 = localCreateRigidBody(0.f, trans, shape);
pRbA1->setActivationState(DISABLE_DEACTIVATION);
// add dynamic rigid body B1
worldPos.setValue(-30,0,0);
trans.setOrigin(worldPos);
btRigidBody* pRbB1 = localCreateRigidBody(mass, trans, shape);
// btRigidBody* pRbB1 = localCreateRigidBody(0.f, trans, shape);
pRbB1->setActivationState(DISABLE_DEACTIVATION);
// create slider constraint between A1 and B1 and add it to world
#if SLIDER_DEMO_USE_6DOF
spSlider1 = new btGeneric6DofConstraint(*pRbA1, *pRbB1, frameInA, frameInB, true);
btVector3 lowerSliderLimit = btVector3(-20,0,0);
btVector3 hiSliderLimit = btVector3(-10,0,0);
// btVector3 lowerSliderLimit = btVector3(-20,-5,-5);
// btVector3 hiSliderLimit = btVector3(-10,5,5);
spSlider1->setLinearLowerLimit(lowerSliderLimit);
spSlider1->setLinearUpperLimit(hiSliderLimit);
spSlider1->setAngularLowerLimit(btVector3(0,0,0));
spSlider1->setAngularUpperLimit(btVector3(0,0,0));
#else
spSlider1 = new btSliderConstraint(*pRbA1, *pRbB1, frameInA, frameInB, true);
// spSlider1 = new btSliderConstraint(*pRbA1, *pRbB1, frameInA, frameInB, false);
spSlider1->setLowerLinLimit(-15.0F);
spSlider1->setUpperLinLimit(-5.0F);
// spSlider1->setLowerLinLimit(5.0F);
// spSlider1->setUpperLinLimit(15.0F);
// spSlider1->setLowerLinLimit(-10.0F);
// spSlider1->setUpperLinLimit(-10.0F);
spSlider1->setLowerAngLimit(-SIMD_PI / 3.0F);
spSlider1->setUpperAngLimit( SIMD_PI / 3.0F);
#endif
m_dynamicsWorld->addConstraint(spSlider1, true);
spSlider1->setDbgDrawSize(btScalar(5.f));
#endif
#if SLIDER_ENABLE_ALL_DEMOS
// add kinematic rigid body A2
// worldPos.setValue(20,4,0);
worldPos.setValue(5,-20,0);
trans.setOrigin(worldPos);
btRigidBody* pRbA2 = localCreateRigidBody(0., trans, shape);
// btRigidBody* pRbA2 = localCreateRigidBody(mass, trans, shape);
// btRigidBody* pRbA2 = localCreateRigidBody(mass * 10000, trans, shape);
pRbA2->setActivationState(DISABLE_DEACTIVATION);
// add dynamic rigid body B2
// worldPos.setValue(-20,4,0);
worldPos.setValue(-5,-20,0);
trans.setOrigin(worldPos);
// btRigidBody* pRbB2 = localCreateRigidBody(0., trans, shape);
btRigidBody* pRbB2 = localCreateRigidBody(mass, trans, shape);
// btRigidBody* pRbB2 = localCreateRigidBody(mass * 10000, trans, shape);
pRbB2->setActivationState(DISABLE_DEACTIVATION);
// frameInA.getBasis().setEulerZYX(1.f, 1.f, 1.f);
// frameInB.getBasis().setEulerZYX(1.f, 1.f, 1.f);
// frameInA.getBasis().setEulerZYX(1.f, 1.f, 1.f);
// frameInB.getBasis().setEulerZYX(1.f, 1.f, 1.f);
// frameInA.setOrigin(btVector3(-20., 5., 0));
// frameInB.setOrigin(btVector3( 20., 5., 0));
frameInA.setOrigin(btVector3(-5., 20., 0));
frameInB.setOrigin(btVector3( 5., 20., 0));
// create slider constraint between A2 and B2 and add it to world
#if SLIDER_DEMO_USE_6DOF
spSlider2 = new btGeneric6DofConstraint(*pRbA2, *pRbB2, frameInA, frameInB, true);
spSlider2->setLinearLowerLimit(lowerSliderLimit);
spSlider2->setLinearUpperLimit(hiSliderLimit);
spSlider2->setAngularLowerLimit(btVector3(0,0,0));
spSlider2->setAngularUpperLimit(btVector3(0,0,0));
#else
spSlider2 = new btSliderConstraint(*pRbA2, *pRbB2, frameInA, frameInB, true);
// spSlider2 = new btSliderConstraint(*pRbA2, *pRbB2, frameInA, frameInB, false);
// spSlider2->setLowerLinLimit(0.0F);
// spSlider2->setUpperLinLimit(0.0F);
spSlider2->setLowerLinLimit(-2.0F);
spSlider2->setUpperLinLimit(2.0F);
// spSlider2->setLowerLinLimit(5.0F);
// spSlider2->setUpperLinLimit(25.0F);
// spSlider2->setUpperLinLimit(-5.0F);
// spSlider2->setUpperLinLimit(-9.99F);
// spSlider2->setLowerAngLimit(SIMD_PI / 2.0F);
// spSlider2->setUpperAngLimit(-SIMD_PI / 2.0F);
// spSlider2->setLowerAngLimit(-SIMD_PI / 2.0F);
// spSlider2->setUpperAngLimit(SIMD_PI / 2.0F);
// spSlider2->setLowerAngLimit(-SIMD_PI);
// spSlider2->setUpperAngLimit(SIMD_PI *0.8F);
// spSlider2->setLowerAngLimit(-0.01F);
// spSlider2->setUpperAngLimit(0.01F);
spSlider2->setLowerAngLimit(-1.570796326F * 0.5f);
spSlider2->setUpperAngLimit(1.570796326F * 0.5f);
// spSlider2->setLowerAngLimit(1.F);
// spSlider2->setUpperAngLimit(-1.F);
// spSlider2->setDampingLimLin(0.5f);
#if 0
// add motors
spSlider2->setPoweredLinMotor(true);
spSlider2->setMaxLinMotorForce(0.1);
spSlider2->setTargetLinMotorVelocity(-5.0);
spSlider2->setPoweredAngMotor(true);
// spSlider2->setMaxAngMotorForce(0.01);
spSlider2->setMaxAngMotorForce(10.0);
spSlider2->setTargetAngMotorVelocity(1.0);
// change default damping and restitution
spSlider2->setDampingDirLin(0.005F);
spSlider2->setRestitutionLimLin(1.1F);
#endif
// various ODE tests
// spSlider2->setDampingLimLin(0.1F); // linear bounce factor for ODE == 1.0 - DampingLimLin;
// spSlider2->setDampingLimAng(0.1F); // angular bounce factor for ODE == 1.0 - DampingLimAng;
// spSlider2->setSoftnessOrthoAng(0.1);
// spSlider2->setSoftnessOrthoLin(0.1);
// spSlider2->setSoftnessLimLin(0.1);
// spSlider2->setSoftnessLimAng(0.1);
#endif
m_dynamicsWorld->addConstraint(spSlider2, true);
spSlider2->setDbgDrawSize(btScalar(5.f));
#endif
#if SLIDER_ENABLE_ALL_DEMOS
{
// add dynamic rigid body A1
trans.setIdentity();
worldPos.setValue(20,0,0);
trans.setOrigin(worldPos);
btRigidBody* pRbA3 = localCreateRigidBody(0.0F, trans, shape);
pRbA3->setActivationState(DISABLE_DEACTIVATION);
// add dynamic rigid body B1
worldPos.setValue(25,0,0);
trans.setOrigin(worldPos);
btRigidBody* pRbB3 = localCreateRigidBody(mass, trans, shape);
pRbB3->setActivationState(DISABLE_DEACTIVATION);
btVector3 pivA( 2.5, 0., 0.);
btVector3 pivB(-2.5, 0., 0.);
spP2PConst = new btPoint2PointConstraint(*pRbA3, *pRbB3, pivA, pivB);
m_dynamicsWorld->addConstraint(spP2PConst, true);
spP2PConst->setDbgDrawSize(btScalar(5.f));
}
#endif
#if 0 // SLIDER_ENABLE_ALL_DEMOS
// add dynamic rigid body A4
trans.setIdentity();
worldPos.setValue(20,10,0);
trans.setOrigin(worldPos);
btRigidBody* pRbA4 = localCreateRigidBody(0.0F, trans, shape);
pRbA4->setActivationState(DISABLE_DEACTIVATION);
// add dynamic rigid body B1
worldPos.setValue(27,10,0);
trans.setOrigin(worldPos);
btRigidBody* pRbB4 = localCreateRigidBody(mass, trans, shape);
pRbB1->setActivationState(DISABLE_DEACTIVATION);
btVector3 pivA( 2., 0., 0.);
btVector3 pivB(-5., 0., 0.);
btVector3 axisA(0., 0., 1.);
btVector3 axisB(0., 0., 1.);
spHingeConst = new btHingeConstraint(*pRbA4, *pRbB4, pivA, pivB, axisA, axisB);
// spHingeConst->setLimit(-1.57, 1.57);
spHingeConst->setLimit(1.57, -1.57);
spHingeConst->enableAngularMotor(true, 10.0, 0.19);
m_dynamicsWorld->addConstraint(spHingeConst, true);
spHingeConst->setDbgDrawSize(btScalar(5.f));
#endif
} // SliderConstraintDemo::initPhysics()
