bool ModulePhysics3D::Constraint(PhysBody3D& A, PhysBody3D& B, float a_x, float a_y, float a_z, float b_x, float b_y, float b_z) { btVector3 pivotInA(a_x, a_y, a_z); btVector3 pivotInB(b_x, b_y, b_z); btRigidBody& rbA = *A.body; btRigidBody& rbB = *B.body; world->addConstraint(new btPoint2PointConstraint(rbA, rbB, pivotInA, pivotInB)); return true; }
void elfRecalcJoint(elfJoint* joint) { elfVec3f pos1; elfVec3f pos2; elfVec4f qua1; elfVec4f qua2; elfVec4f aqua1; elfVec4f aqua2; elfVec3f worldCoordPivot1; elfVec3f localCoordPivot2; elfVec3f localAxis1; elfVec3f localAxis2; float matrix1[16]; float matrix2[16]; float matrix3[16]; pos1 = elfGetActorPosition(joint->actor1); pos2 = elfGetActorPosition(joint->actor2); qua1 = elfGetActorOrientation(joint->actor1); qua2 = elfGetActorOrientation(joint->actor2); worldCoordPivot1.x = joint->pivot[0]; worldCoordPivot1.y = joint->pivot[1]; worldCoordPivot1.z = joint->pivot[2]; worldCoordPivot1 = elfAddVec3fVec3f(elfMulQuaVec3f(qua1, worldCoordPivot1), pos1); localCoordPivot2 = elfMulQuaVec3f(elfGetQuaInverted(qua2), elfSubVec3fVec3f(worldCoordPivot1, pos2)); btVector3 pivotInA(joint->pivot[0], joint->pivot[1], joint->pivot[2]); btVector3 pivotInB(localCoordPivot2.x, localCoordPivot2.y, localCoordPivot2.z); aqua1 = elfCreateQuaFromEuler(joint->axis[0], joint->axis[1], joint->axis[2]); localAxis1 = elfMulQuaVec3f(aqua1, elfCreateVec3f(0.0f, 0.0f, 1.0f)); aqua2 = elfMulQuaQua(aqua1, elfMulQuaQua(qua1, elfGetQuaInverted(qua2))); localAxis2 = elfMulQuaVec3f(aqua2, elfCreateVec3f(0.0f, 0.0f, 1.0f)); btVector3 axisInA(localAxis1.x, localAxis1.y, localAxis1.z); btVector3 axisInB(localAxis2.x, localAxis2.y, localAxis2.z); if(joint->jointType == ELF_HINGE) { joint->constraint = new btHingeConstraint(*joint->actor1->object->body,* joint->actor2->object->body, pivotInA, pivotInB, axisInA, axisInB); joint->actor1->scene->world->world->addConstraint(joint->constraint); } else if(joint->jointType == ELF_BALL) { joint->constraint = new btPoint2PointConstraint(*joint->actor1->object->body,* joint->actor2->object->body, pivotInA, pivotInB); joint->actor1->scene->world->world->addConstraint(joint->constraint); } else if(joint->jointType == ELF_CONE_TWIST) { gfxMatrix4SetIdentity(matrix1); matrix1[12] = joint->pivot[0]; matrix1[13] = joint->pivot[1]; matrix1[14] = joint->pivot[2]; gfxQuaToMatrix4(&aqua1.x, matrix2); gfxMulMatrix4Matrix4(matrix1, matrix2, matrix3); btTransform frameInA; frameInA.setFromOpenGLMatrix(matrix3); gfxMatrix4SetIdentity(matrix1); matrix1[12] = localCoordPivot2.x; matrix1[13] = localCoordPivot2.y; matrix1[14] = localCoordPivot2.z; gfxQuaToMatrix4(&aqua2.x, matrix2); gfxMulMatrix4Matrix4(matrix1, matrix2, matrix3); btTransform frameInB; frameInB.setFromOpenGLMatrix(matrix3); joint->constraint = new btConeTwistConstraint(*joint->actor1->object->body, *joint->actor2->object->body, frameInA, frameInB); joint->actor1->scene->world->world->addConstraint(joint->constraint); } }
btTypedConstraint* gkDynamicsWorld::createConstraint(btRigidBody* rbA, btRigidBody*rbB, const gkPhysicsConstraintProperties& props) { btVector3 pivotInA(gkMathUtils::get(props.m_pivot)); btVector3 pivotInB(0,0,0); pivotInB = rbB ? rbB->getCenterOfMassTransform().inverse()(rbA->getCenterOfMassTransform()(pivotInA)) : rbA->getCenterOfMassTransform() * pivotInA; //localConstraintFrameBasis btMatrix3x3 localCFrame; localCFrame.setEulerZYX(props.m_axis.x, props.m_axis.y, props.m_axis.z); btVector3 axisInA = localCFrame.getColumn(0); btVector3 axis1 = localCFrame.getColumn(1); btVector3 axis2 = localCFrame.getColumn(2); bool angularOnly = false; btTypedConstraint* constraint = 0; if (props.m_type == GK_BALL_CONSTRAINT) { btPoint2PointConstraint* p2p = 0; if (rbB) p2p = new btPoint2PointConstraint(*rbA,*rbB,pivotInA,pivotInB); else p2p = new btPoint2PointConstraint(*rbA,pivotInA); constraint = p2p; } else if (props.m_type == GK_HINGE_CONSTRAINT) { btHingeConstraint* hinge = 0; if (rbB) { btVector3 axisInB = rbB->getCenterOfMassTransform().getBasis().inverse() * (rbA->getCenterOfMassTransform().getBasis() * axisInA); hinge = new btHingeConstraint(*rbA, *rbB, pivotInA, pivotInB, axisInA, axisInB); } else { hinge = new btHingeConstraint(*rbA, pivotInA, axisInA); } hinge->setAngularOnly(angularOnly); constraint = hinge; } else if (props.m_type == GK_D6_CONSTRAINT) { btTransform frameInA; btTransform frameInB; if (axis1.length() == 0.0) { btPlaneSpace1(axisInA, axis1, axis2); } frameInA.getBasis().setValue(axisInA.x(), axis1.x(), axis2.x(), axisInA.y(), axis1.y(), axis2.y(), axisInA.z(), axis1.z(), axis2.z()); frameInA.setOrigin( pivotInA ); btTransform inv = rbB ? rbB->getCenterOfMassTransform().inverse() : btTransform::getIdentity(); btTransform globalFrameA = rbA->getCenterOfMassTransform() * frameInA; frameInB = inv * globalFrameA; bool useReferenceFrameA = true; if (!rbB) rbB = getFixedBody(); btGeneric6DofSpringConstraint* genericConstraint = new btGeneric6DofSpringConstraint(*rbA, *rbB, frameInA, frameInB, useReferenceFrameA); //if it is a generic 6DOF constraint, set all the limits accordingly int dof, dofbit=1; for (dof=0;dof<6;dof++) { if (props.m_flag & dofbit) { ///this access is a bloated, will probably make special cases for common arrays btScalar minLimit = props.m_minLimit[dof]; btScalar maxLimit = props.m_maxLimit[dof]; genericConstraint->setLimit(dof, minLimit, maxLimit); } else { //minLimit > maxLimit means free(disabled limit) for this degree of freedom genericConstraint->setLimit(dof, 1, -1); } dofbit<<=1; } constraint = genericConstraint; } else if (props.m_type == GK_CONETWIST_CONSTRAINT) { //TODO: implement conetwist constraint } return constraint; }
//to be implemented by the demo void renderme() { debugDrawer.SetDebugMode(getDebugMode()); //render the hinge axis if (createConstraint) { SimdVector3 color(1,0,0); SimdVector3 dirLocal(0,1,0); SimdVector3 pivotInA(CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS); SimdVector3 pivotInB(-CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS); SimdVector3 from = physObjects[1]->GetRigidBody()->getCenterOfMassTransform()(pivotInA); SimdVector3 fromB = physObjects[2]->GetRigidBody()->getCenterOfMassTransform()(pivotInB); SimdVector3 dirWorldA = physObjects[1]->GetRigidBody()->getCenterOfMassTransform().getBasis() * dirLocal ; SimdVector3 dirWorldB = physObjects[2]->GetRigidBody()->getCenterOfMassTransform().getBasis() * dirLocal ; debugDrawer.DrawLine(from,from+dirWorldA,color); debugDrawer.DrawLine(fromB,fromB+dirWorldB,color); } float m[16]; int i; if (getDebugMode() & IDebugDraw::DBG_DisableBulletLCP) { //don't use Bullet, use quickstep physicsEnvironmentPtr->setSolverType(0); } else { //Bullet LCP solver physicsEnvironmentPtr->setSolverType(1); } if (getDebugMode() & IDebugDraw::DBG_EnableCCD) { physicsEnvironmentPtr->setCcdMode(3); } else { physicsEnvironmentPtr->setCcdMode(0); } bool isSatEnabled = (getDebugMode() & IDebugDraw::DBG_EnableSatComparison); physicsEnvironmentPtr->EnableSatCollisionDetection(isSatEnabled); #ifdef USE_HULL //some testing code for SAT if (isSatEnabled) { for (int s=0;s<numShapes;s++) { CollisionShape* shape = shapePtr[s]; if (shape->IsPolyhedral()) { PolyhedralConvexShape* polyhedron = static_cast<PolyhedralConvexShape*>(shape); if (!polyhedron->m_optionalHull) { //first convert vertices in 'Point3' format int numPoints = polyhedron->GetNumVertices(); Point3* points = new Point3[numPoints+1]; //first 4 points should not be co-planar, so add central point to satisfy MakeHull points[0] = Point3(0.f,0.f,0.f); SimdVector3 vertex; for (int p=0;p<numPoints;p++) { polyhedron->GetVertex(p,vertex); points[p+1] = Point3(vertex.getX(),vertex.getY(),vertex.getZ()); } Hull* hull = Hull::MakeHull(numPoints+1,points); polyhedron->m_optionalHull = hull; } } } } #endif //USE_HULL for (i=0;i<numObjects;i++) { SimdTransform transA; transA.setIdentity(); float pos[3]; float rot[4]; ms[i].getWorldPosition(pos[0],pos[1],pos[2]); ms[i].getWorldOrientation(rot[0],rot[1],rot[2],rot[3]); SimdQuaternion q(rot[0],rot[1],rot[2],rot[3]); transA.setRotation(q); SimdPoint3 dpos; dpos.setValue(pos[0],pos[1],pos[2]); transA.setOrigin( dpos ); transA.getOpenGLMatrix( m ); SimdVector3 wireColor(1.f,1.0f,0.5f); //wants deactivation if (i & 1) { wireColor = SimdVector3(0.f,0.0f,1.f); } ///color differently for active, sleeping, wantsdeactivation states if (physObjects[i]->GetRigidBody()->GetActivationState() == 1) //active { if (i & 1) { wireColor += SimdVector3 (1.f,0.f,0.f); } else { wireColor += SimdVector3 (.5f,0.f,0.f); } } if (physObjects[i]->GetRigidBody()->GetActivationState() == 2) //ISLAND_SLEEPING { if (i & 1) { wireColor += SimdVector3 (0.f,1.f, 0.f); } else { wireColor += SimdVector3 (0.f,0.5f,0.f); } } char extraDebug[125]; sprintf(extraDebug,"islId, Body=%i , %i",physObjects[i]->GetRigidBody()->m_islandTag1,physObjects[i]->GetRigidBody()->m_debugBodyId); physObjects[i]->GetRigidBody()->GetCollisionShape()->SetExtraDebugInfo(extraDebug); GL_ShapeDrawer::DrawOpenGL(m,physObjects[i]->GetRigidBody()->GetCollisionShape(),wireColor,getDebugMode()); ///this block is just experimental code to show some internal issues with replacing shapes on the fly. if (getDebugMode()!=0 && (i>0)) { if (physObjects[i]->GetRigidBody()->GetCollisionShape()->GetShapeType() == EMPTY_SHAPE_PROXYTYPE) { physObjects[i]->GetRigidBody()->SetCollisionShape(shapePtr[1]); //remove the persistent collision pairs that were created based on the previous shape BroadphaseProxy* bpproxy = physObjects[i]->GetRigidBody()->m_broadphaseHandle; physicsEnvironmentPtr->GetBroadphase()->CleanProxyFromPairs(bpproxy); SimdVector3 newinertia; SimdScalar newmass = 10.f; physObjects[i]->GetRigidBody()->GetCollisionShape()->CalculateLocalInertia(newmass,newinertia); physObjects[i]->GetRigidBody()->setMassProps(newmass,newinertia); physObjects[i]->GetRigidBody()->updateInertiaTensor(); } } } if (!(getDebugMode() & IDebugDraw::DBG_NoHelpText)) { float xOffset = 10.f; float yStart = 20.f; float yIncr = -2.f; char buf[124]; glColor3f(0, 0, 0); #ifdef USE_QUICKPROF if ( getDebugMode() & IDebugDraw::DBG_ProfileTimings) { static int counter = 0; counter++; std::map<std::string, hidden::ProfileBlock*>::iterator iter; for (iter = Profiler::mProfileBlocks.begin(); iter != Profiler::mProfileBlocks.end(); ++iter) { char blockTime[128]; sprintf(blockTime, "%s: %lf",&((*iter).first[0]),Profiler::getBlockTime((*iter).first, Profiler::BLOCK_CYCLE_SECONDS));//BLOCK_TOTAL_PERCENT)); glRasterPos3f(xOffset,yStart,0); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),blockTime); yStart += yIncr; } } #endif //USE_QUICKPROF //profiling << Profiler::createStatsString(Profiler::BLOCK_TOTAL_PERCENT); //<< std::endl; glRasterPos3f(xOffset,yStart,0); sprintf(buf,"mouse to interact"); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; glRasterPos3f(xOffset,yStart,0); sprintf(buf,"space to reset"); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; glRasterPos3f(xOffset,yStart,0); sprintf(buf,"cursor keys and z,x to navigate"); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; glRasterPos3f(xOffset,yStart,0); sprintf(buf,"i to toggle simulation, s single step"); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; glRasterPos3f(xOffset,yStart,0); sprintf(buf,"q to quit"); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; glRasterPos3f(xOffset,yStart,0); sprintf(buf,"d to toggle deactivation"); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; glRasterPos3f(xOffset,yStart,0); sprintf(buf,"a to draw temporal AABBs"); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; glRasterPos3f(xOffset,yStart,0); sprintf(buf,"h to toggle help text"); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; bool useBulletLCP = !(getDebugMode() & IDebugDraw::DBG_DisableBulletLCP); bool useCCD = (getDebugMode() & IDebugDraw::DBG_EnableCCD); glRasterPos3f(xOffset,yStart,0); sprintf(buf,"m Bullet GJK = %i",!isSatEnabled); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; glRasterPos3f(xOffset,yStart,0); sprintf(buf,"n Bullet LCP = %i",useBulletLCP); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; glRasterPos3f(xOffset,yStart,0); sprintf(buf,"1 CCD mode (adhoc) = %i",useCCD); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; glRasterPos3f(xOffset,yStart,0); sprintf(buf,"+- shooting speed = %10.2f",bulletSpeed); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; } }
int main(int argc,char** argv) { int i; for (i=0;i<numObjects;i++) { if (i>0) { shapePtr[i] = prebuildShapePtr[1]; shapeIndex[i] = 1;//sphere } else { shapeIndex[i] = 0; shapePtr[i] = prebuildShapePtr[0]; } } ConvexDecomposition::WavefrontObj wo; char* filename = "file.obj"; tcount = wo.loadObj(filename); class MyConvexDecomposition : public ConvexDecomposition::ConvexDecompInterface { public: MyConvexDecomposition (FILE* outputFile) :mBaseCount(0), mHullCount(0), mOutputFile(outputFile) { } virtual void ConvexDecompResult(ConvexDecomposition::ConvexResult &result) { TriangleMesh* trimesh = new TriangleMesh(); SimdVector3 localScaling(6.f,6.f,6.f); //export data to .obj printf("ConvexResult\n"); if (mOutputFile) { fprintf(mOutputFile,"## Hull Piece %d with %d vertices and %d triangles.\r\n", mHullCount, result.mHullVcount, result.mHullTcount ); fprintf(mOutputFile,"usemtl Material%i\r\n",mBaseCount); fprintf(mOutputFile,"o Object%i\r\n",mBaseCount); for (unsigned int i=0; i<result.mHullVcount; i++) { const float *p = &result.mHullVertices[i*3]; fprintf(mOutputFile,"v %0.9f %0.9f %0.9f\r\n", p[0], p[1], p[2] ); } //calc centroid, to shift vertices around center of mass centroids[numObjects] = SimdVector3(0,0,0); if ( 1 ) { const unsigned int *src = result.mHullIndices; for (unsigned int i=0; i<result.mHullTcount; i++) { unsigned int index0 = *src++; unsigned int index1 = *src++; unsigned int index2 = *src++; SimdVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]); SimdVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]); SimdVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]); vertex0 *= localScaling; vertex1 *= localScaling; vertex2 *= localScaling; centroids[numObjects] += vertex0; centroids[numObjects]+= vertex1; centroids[numObjects]+= vertex2; } } centroids[numObjects] *= 1.f/(float(result.mHullTcount) * 3); if ( 1 ) { const unsigned int *src = result.mHullIndices; for (unsigned int i=0; i<result.mHullTcount; i++) { unsigned int index0 = *src++; unsigned int index1 = *src++; unsigned int index2 = *src++; SimdVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]); SimdVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]); SimdVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]); vertex0 *= localScaling; vertex1 *= localScaling; vertex2 *= localScaling; vertex0 -= centroids[numObjects]; vertex1 -= centroids[numObjects]; vertex2 -= centroids[numObjects]; trimesh->AddTriangle(vertex0,vertex1,vertex2); index0+=mBaseCount; index1+=mBaseCount; index2+=mBaseCount; fprintf(mOutputFile,"f %d %d %d\r\n", index0+1, index1+1, index2+1 ); } } shapeIndex[numObjects] = numObjects; shapePtr[numObjects++] = new ConvexTriangleMeshShape(trimesh); mBaseCount+=result.mHullVcount; // advance the 'base index' counter. } } int mBaseCount; int mHullCount; FILE* mOutputFile; }; if (tcount) { numObjects = 1; //always have the ground object first TriangleMesh* trimesh = new TriangleMesh(); SimdVector3 localScaling(6.f,6.f,6.f); for (int i=0;i<wo.mTriCount;i++) { int index0 = wo.mIndices[i*3]; int index1 = wo.mIndices[i*3+1]; int index2 = wo.mIndices[i*3+2]; SimdVector3 vertex0(wo.mVertices[index0*3], wo.mVertices[index0*3+1],wo.mVertices[index0*3+2]); SimdVector3 vertex1(wo.mVertices[index1*3], wo.mVertices[index1*3+1],wo.mVertices[index1*3+2]); SimdVector3 vertex2(wo.mVertices[index2*3], wo.mVertices[index2*3+1],wo.mVertices[index2*3+2]); vertex0 *= localScaling; vertex1 *= localScaling; vertex2 *= localScaling; trimesh->AddTriangle(vertex0,vertex1,vertex2); } shapePtr[numObjects++] = new ConvexTriangleMeshShape(trimesh); } if (tcount) { char outputFileName[512]; strcpy(outputFileName,filename); char *dot = strstr(outputFileName,"."); if ( dot ) *dot = 0; strcat(outputFileName,"_convex.obj"); FILE* outputFile = fopen(outputFileName,"wb"); unsigned int depth = 7; float cpercent = 5; float ppercent = 15; unsigned int maxv = 16; float skinWidth = 0.01; printf("WavefrontObj num triangles read %i",tcount); ConvexDecomposition::DecompDesc desc; desc.mVcount = wo.mVertexCount; desc.mVertices = wo.mVertices; desc.mTcount = wo.mTriCount; desc.mIndices = (unsigned int *)wo.mIndices; desc.mDepth = depth; desc.mCpercent = cpercent; desc.mPpercent = ppercent; desc.mMaxVertices = maxv; desc.mSkinWidth = skinWidth; MyConvexDecomposition convexDecomposition(outputFile); desc.mCallback = &convexDecomposition; //convexDecomposition.performConvexDecomposition(desc); ConvexBuilder cb(desc.mCallback); int ret = cb.process(desc); if (outputFile) fclose(outputFile); } CollisionDispatcher* dispatcher = new CollisionDispatcher(); SimdVector3 worldAabbMin(-10000,-10000,-10000); SimdVector3 worldAabbMax(10000,10000,10000); OverlappingPairCache* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax); //OverlappingPairCache* broadphase = new SimpleBroadphase(); physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase); physicsEnvironmentPtr->setDeactivationTime(2.f); physicsEnvironmentPtr->setGravity(0,-10,0); PHY_ShapeProps shapeProps; shapeProps.m_do_anisotropic = false; shapeProps.m_do_fh = false; shapeProps.m_do_rot_fh = false; shapeProps.m_friction_scaling[0] = 1.; shapeProps.m_friction_scaling[1] = 1.; shapeProps.m_friction_scaling[2] = 1.; shapeProps.m_inertia = 1.f; shapeProps.m_lin_drag = 0.2f; shapeProps.m_ang_drag = 0.1f; shapeProps.m_mass = 10.0f; PHY_MaterialProps materialProps; materialProps.m_friction = 10.5f; materialProps.m_restitution = 0.0f; CcdConstructionInfo ccdObjectCi; ccdObjectCi.m_friction = 0.5f; ccdObjectCi.m_linearDamping = shapeProps.m_lin_drag; ccdObjectCi.m_angularDamping = shapeProps.m_ang_drag; SimdTransform tr; tr.setIdentity(); for (i=0;i<numObjects;i++) { shapeProps.m_shape = shapePtr[shapeIndex[i]]; shapeProps.m_shape->SetMargin(0.05f); bool isDyna = i>0; //if (i==1) // isDyna=false; if (0)//i==1) { SimdQuaternion orn(0,0,0.1*SIMD_HALF_PI); ms[i].setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]); } if (i>0) { switch (i) { case 1: { ms[i].setWorldPosition(0,10,0); //for testing, rotate the ground cube so the stack has to recover a bit break; } case 2: { ms[i].setWorldPosition(0,8,2); break; } default: ms[i].setWorldPosition(0,i*CUBE_HALF_EXTENTS*2 - CUBE_HALF_EXTENTS,0); } float quatIma0,quatIma1,quatIma2,quatReal; SimdQuaternion quat; SimdVector3 axis(0,0,1); SimdScalar angle=0.5f; quat.setRotation(axis,angle); ms[i].setWorldOrientation(quat.getX(),quat.getY(),quat.getZ(),quat[3]); } else { ms[i].setWorldPosition(0,-10+EXTRA_HEIGHT,0); } ccdObjectCi.m_MotionState = &ms[i]; ccdObjectCi.m_gravity = SimdVector3(0,0,0); ccdObjectCi.m_localInertiaTensor =SimdVector3(0,0,0); if (!isDyna) { shapeProps.m_mass = 0.f; ccdObjectCi.m_mass = shapeProps.m_mass; ccdObjectCi.m_collisionFlags = CollisionObject::isStatic; } else { shapeProps.m_mass = 1.f; ccdObjectCi.m_mass = shapeProps.m_mass; ccdObjectCi.m_collisionFlags = 0; } SimdVector3 localInertia; if (shapePtr[shapeIndex[i]]->GetShapeType() == EMPTY_SHAPE_PROXYTYPE) { //take inertia from first shape shapePtr[1]->CalculateLocalInertia(shapeProps.m_mass,localInertia); } else { shapePtr[shapeIndex[i]]->CalculateLocalInertia(shapeProps.m_mass,localInertia); } ccdObjectCi.m_localInertiaTensor = localInertia; ccdObjectCi.m_collisionShape = shapePtr[shapeIndex[i]]; physObjects[i]= new CcdPhysicsController( ccdObjectCi); // Only do CCD if motion in one timestep (1.f/60.f) exceeds CUBE_HALF_EXTENTS physObjects[i]->GetRigidBody()->m_ccdSquareMotionTreshold = CUBE_HALF_EXTENTS; //Experimental: better estimation of CCD Time of Impact: //physObjects[i]->GetRigidBody()->m_ccdSweptShereRadius = 0.5*CUBE_HALF_EXTENTS; physicsEnvironmentPtr->addCcdPhysicsController( physObjects[i]); if (i==1) { //physObjects[i]->SetAngularVelocity(0,0,-2,true); } physicsEnvironmentPtr->setDebugDrawer(&debugDrawer); } //create a constraint if (createConstraint) { //physObjects[i]->SetAngularVelocity(0,0,-2,true); int constraintId; float pivotX=CUBE_HALF_EXTENTS, pivotY=-CUBE_HALF_EXTENTS, pivotZ=CUBE_HALF_EXTENTS; float axisX=1,axisY=0,axisZ=0; HingeConstraint* hinge = 0; SimdVector3 pivotInA(CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS); SimdVector3 pivotInB(-CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS); SimdVector3 axisInA(0,1,0); SimdVector3 axisInB(0,-1,0); RigidBody* rb0 = physObjects[1]->GetRigidBody(); RigidBody* rb1 = physObjects[2]->GetRigidBody(); hinge = new HingeConstraint( *rb0, *rb1,pivotInA,pivotInB,axisInA,axisInB); physicsEnvironmentPtr->m_constraints.push_back(hinge); hinge->SetUserConstraintId(100); hinge->SetUserConstraintType(PHY_LINEHINGE_CONSTRAINT); } clientResetScene(); setCameraDistance(26.f); return glutmain(argc, argv,640,480,"Bullet Physics Demo. http://www.continuousphysics.com/Bullet/phpBB2/"); }
void ChainDemo::initPhysics() { // Bullet2RigidBodyDemo::initPhysics(); m_config = new btDefaultCollisionConfiguration; m_dispatcher = new btCollisionDispatcher(m_config); m_bp = new btDbvtBroadphase(); //m_solver = new btNNCGConstraintSolver(); m_solver = new btSequentialImpulseConstraintSolver(); // btDantzigSolver* mlcp = new btDantzigSolver(); //btLemkeSolver* mlcp = new btLemkeSolver(); //m_solver = new btMLCPSolver(mlcp); // m_solver = new btSequentialImpulseConstraintSolver(); //btMultiBodyConstraintSolver* solver = new btMultiBodyConstraintSolver(); //m_solver = solver; m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_bp,m_solver,m_config); m_dynamicsWorld->getSolverInfo().m_numIterations = 1000; m_dynamicsWorld->getSolverInfo().m_splitImpulse = false; int curColor=0; //create ground btScalar radius=scaling; int unitCubeShapeId = m_glApp->registerCubeShape(); float pos[]={0,0,0}; float orn[]={0,0,0,1}; //eateGround(unitCubeShapeId); int sphereShapeId = m_glApp->registerGraphicsSphereShape(radius,false); { float halfExtents[]={scaling,scaling,scaling,1}; btVector4 colors[4] = { btVector4(1,0,0,1), btVector4(0,1,0,1), btVector4(0,1,1,1), btVector4(1,1,0,1), }; btTransform startTransform; startTransform.setIdentity(); btCollisionShape* colShape = new btSphereShape(scaling); btScalar largeMass[]={1000,10,100,1000}; for (int i=0;i<1;i++) { btAlignedObjectArray<btRigidBody*> bodies; for (int k=0;k<NUM_SPHERES;k++) { btVector3 localInertia(0,0,0); btScalar mass = 0.f; curColor = 1; switch (k) { case 0: { mass = largeMass[i]; curColor = 0; break; } case NUM_SPHERES-1: { mass = 0.f; curColor = 2; break; } default: { curColor = 1; mass = 1.f; } }; if (mass) colShape ->calculateLocalInertia(mass,localInertia); btVector4 color = colors[curColor]; startTransform.setOrigin(btVector3( btScalar(7.5+-i*5), btScalar(6.*scaling+2.0*scaling*k), btScalar(0))); m_glApp->m_instancingRenderer->registerGraphicsInstance(sphereShapeId,startTransform.getOrigin(),startTransform.getRotation(),color,halfExtents); //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); bodies.push_back(body); body->setActivationState(DISABLE_DEACTIVATION); m_dynamicsWorld->addRigidBody(body); } //add constraints btVector3 pivotInA(0,radius,0); btVector3 pivotInB(0,-radius,0); for (int k=0;k<NUM_SPHERES-1;k++) { btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*bodies[k],*bodies[k+1],pivotInA,pivotInB); m_dynamicsWorld->addConstraint(p2p,true); } } } m_glApp->m_instancingRenderer->writeTransforms(); }
void TestHingeTorque::initPhysics() { int upAxis = 1; m_guiHelper->setUpAxis(upAxis); createEmptyDynamicsWorld(); m_dynamicsWorld->getSolverInfo().m_splitImpulse = false; m_dynamicsWorld->setGravity(btVector3(0,0,-10)); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); int mode = btIDebugDraw::DBG_DrawWireframe +btIDebugDraw::DBG_DrawConstraints +btIDebugDraw::DBG_DrawConstraintLimits; m_dynamicsWorld->getDebugDrawer()->setDebugMode(mode); { // create a door using hinge constraint attached to the world int numLinks = 2; bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals bool canSleep = false; bool selfCollide = false; btVector3 linkHalfExtents(0.05, 0.37, 0.1); btVector3 baseHalfExtents(0.05, 0.37, 0.1); btBoxShape* baseBox = new btBoxShape(baseHalfExtents); btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f); btTransform baseWorldTrans; baseWorldTrans.setIdentity(); baseWorldTrans.setOrigin(basePosition); //mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm //init the base btVector3 baseInertiaDiag(0.f, 0.f, 0.f); float baseMass = 0.f; float linkMass = 1.f; btRigidBody* base = createRigidBody(baseMass,baseWorldTrans,baseBox); m_dynamicsWorld->removeRigidBody(base); base->setDamping(0,0); m_dynamicsWorld->addRigidBody(base,collisionFilterGroup,collisionFilterMask); btBoxShape* linkBox1 = new btBoxShape(linkHalfExtents); btSphereShape* linkSphere = new btSphereShape(radius); btRigidBody* prevBody = base; for (int i=0;i<numLinks;i++) { btTransform linkTrans; linkTrans = baseWorldTrans; linkTrans.setOrigin(basePosition-btVector3(0,linkHalfExtents[1]*2.f*(i+1),0)); btCollisionShape* colOb = 0; if (i==0) { colOb = linkBox1; } else { colOb = linkSphere; } btRigidBody* linkBody = createRigidBody(linkMass,linkTrans,colOb); m_dynamicsWorld->removeRigidBody(linkBody); m_dynamicsWorld->addRigidBody(linkBody,collisionFilterGroup,collisionFilterMask); linkBody->setDamping(0,0); btTypedConstraint* con = 0; if (i==0) { //create a hinge constraint btVector3 pivotInA(0,-linkHalfExtents[1],0); btVector3 pivotInB(0,linkHalfExtents[1],0); btVector3 axisInA(1,0,0); btVector3 axisInB(1,0,0); bool useReferenceA = true; btHingeConstraint* hinge = new btHingeConstraint(*prevBody,*linkBody, pivotInA,pivotInB, axisInA,axisInB,useReferenceA); con = hinge; } else { btTransform pivotInA(btQuaternion::getIdentity(),btVector3(0, -radius, 0)); //par body's COM to cur body's COM offset btTransform pivotInB(btQuaternion::getIdentity(),btVector3(0, radius, 0)); //cur body's COM to cur body's PIV offset btGeneric6DofSpring2Constraint* fixed = new btGeneric6DofSpring2Constraint(*prevBody, *linkBody, pivotInA,pivotInB); fixed->setLinearLowerLimit(btVector3(0,0,0)); fixed->setLinearUpperLimit(btVector3(0,0,0)); fixed->setAngularLowerLimit(btVector3(0,0,0)); fixed->setAngularUpperLimit(btVector3(0,0,0)); con = fixed; } btAssert(con); if (con) { btJointFeedback* fb = new btJointFeedback(); m_jointFeedback.push_back(fb); con->setJointFeedback(fb); m_dynamicsWorld->addConstraint(con,true); } prevBody = linkBody; } } if (1) { btVector3 groundHalfExtents(1,1,0.2); groundHalfExtents[upAxis]=1.f; btBoxShape* box = new btBoxShape(groundHalfExtents); box->initializePolyhedralFeatures(); btTransform start; start.setIdentity(); btVector3 groundOrigin(-0.4f, 3.f, 0.f); btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f); btQuaternion groundOrn(btVector3(0,1,0),0.25*SIMD_PI); groundOrigin[upAxis] -=.5; groundOrigin[2]-=0.6; start.setOrigin(groundOrigin); // start.setRotation(groundOrn); btRigidBody* body = createRigidBody(0,start,box); body->setFriction(0); } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }