void MultipleBoxesExample::initPhysics() { m_guiHelper->setUpAxis(1); createEmptyDynamicsWorld(); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); if (m_dynamicsWorld->getDebugDrawer()) m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints); ///create a few basic rigid bodies btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); groundTransform.setOrigin(btVector3(0,-50,0)); { btScalar mass(0.); createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1)); } { //create a few dynamic rigidbodies // Re-using the same collision is better for memory usage and performance btBoxShape* colShape = createBoxShape(btVector3(1,1,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); for(int i=0;i<TOTAL_BOXES;++i) { startTransform.setOrigin(btVector3( btScalar(0), btScalar(20+i*2), btScalar(0))); createRigidBody(mass,startTransform,colShape); } } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }
BSPSceneObject::BSPSceneObject(char * filename, int curveTesselation) : RigidSceneObject() { hasVisibility = true; bspObject = new BSP(); bspObject->Load(filename,curveTesselation); bspObject->buildSingleCluster(); bind(bspObject->clusterObject); // bspObject->disableVIS(true); // bspObject->showAll(true); setMass(0); setMargin(0.01f); createRigidBody(); mRigidBody->setActivationState(WANTS_DEACTIVATION); segmentMask = new BITSET; segmentMask->Init(bspObject->clusterObject.numSegments); segmentMask->SetAll(); // createRigidBody(); // colShape->setMargin(0.005); }
void WorldObjectAbstract::initWorldObject(Ogre::SceneManager* m_pSceneMgr, PhysicsWrapper* physics) { createRigidBody(physics); createSceneNode(m_pSceneMgr); if(physics != NULL) attachToDynamicWorld(physics); }
void ConstraintPhysicsSetup::initPhysics() { m_guiHelper->setUpAxis(1); createEmptyDynamicsWorld(); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); int mode = btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawConstraints + btIDebugDraw::DBG_DrawConstraintLimits; m_dynamicsWorld->getDebugDrawer()->setDebugMode(mode); { SliderParams slider("target vel", &targetVel); slider.m_minVal = -4; slider.m_maxVal = 4; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { SliderParams slider("max impulse", &maxImpulse); slider.m_minVal = 0; slider.m_maxVal = 1000; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { SliderParams slider("actual vel", &actualHingeVelocity); slider.m_minVal = -4; slider.m_maxVal = 4; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } val = 1.f; { SliderParams slider("angle", &val); slider.m_minVal = -720; slider.m_maxVal = 720; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { // create a door using hinge constraint attached to the world btCollisionShape* pDoorShape = new btBoxShape(btVector3(2.0f, 5.0f, 0.2f)); m_collisionShapes.push_back(pDoorShape); btTransform doorTrans; doorTrans.setIdentity(); doorTrans.setOrigin(btVector3(-5.0f, -2.0f, 0.0f)); btRigidBody* pDoorBody = createRigidBody(1.0, doorTrans, pDoorShape); pDoorBody->setActivationState(DISABLE_DEACTIVATION); const btVector3 btPivotA(10.f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside spDoorHinge = new btHingeAccumulatedAngleConstraint(*pDoorBody, btPivotA, btAxisA); m_dynamicsWorld->addConstraint(spDoorHinge); spDoorHinge->setDbgDrawSize(btScalar(5.f)); } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }
btRigidBody* AWPhysics::PhysicsManager::createBox(float x, float y, float z) { return createRigidBody( 10.f, btTransform( btTransform().getRotation(), btVector3(x, y, z) ), new btBoxShape( btVector3(1.0, 1.0, 1.0) ) ); mLogger << "creating Box rigid body"; }
void CoordinateFrameDemoPhysicsSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) { createEmptyDynamicsWorld(); m_dynamicsWorld->setGravity(btVector3(0,0,0)); gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld); m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints); m_dynamicsWorld->getDebugDrawer()->setDebugMode(m_dynamicsWorld->getDebugDrawer()->getDebugMode() + btIDebugDraw::DBG_DrawFrames); btScalar sqr2 = btSqrt(2); btVector3 tetraVerts[] = { btVector3(1.f, 0.f, -1/sqr2), btVector3(-1.f, 0.f, -1/sqr2), btVector3(0, 1.f, 1/sqr2), btVector3(0, -1.f, 1/sqr2), }; { //create a few dynamic rigidbodies // Re-using the same collision is better for memory usage and performance btCompoundShape* hull = new btCompoundShape(); btConvexHullShape* childHull = new btConvexHullShape(&tetraVerts[0].getX(),sizeof(tetraVerts)/sizeof(btVector3),sizeof(btVector3)); childHull->initializePolyhedralFeatures(); btTransform childTrans; childTrans.setIdentity(); childTrans.setOrigin(btVector3(2,0,0)); hull->addChildShape(childTrans,childHull); gfxBridge.createCollisionShapeGraphicsObject(hull); m_collisionShapes.push_back(hull); /// 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) hull->calculateLocalInertia(mass,localInertia); startTransform.setOrigin(btVector3(0,0,0)); btRigidBody* body = createRigidBody(mass,startTransform,hull); gfxBridge.createRigidBodyGraphicsObject(body, btVector3(1, 1, 0)); } }
ShipPhysicsObject::ShipPhysicsObject(GameObject* go){ // f-16 mass 12000 kg fully loaded const float mass = 2.0f; collisionShape = new btBoxShape(btVector3(1.0, 0.5, 1.5)); // Add a pointer to the calling GameObject for collisions collisionShape->setUserPointer(go); btVector3 localInertia(0,0,0); collisionShape->calculateLocalInertia(mass, localInertia); rigidBody = createRigidBody(collisionShape, mass, btVector3(0,0,0), 0); rigidBody->setActivationState(DISABLE_DEACTIVATION); }
btRigidBody* AWPhysics::PhysicsManager::createPlane(float x, float y, float z) { return createRigidBody( 0.f, // zero mass == static object btTransform( btQuaternion::getIdentity(), // wow valgrind memory analyzer found the old bug here... btVector3(x, y, z) ), // normal constant new btStaticPlaneShape( btVector3(0.0, 1.0, 0.0), btScalar(0.0))); // assume that the plane is placed at: (x, y, z) = scalar*normal mLogger << "creating Plane rigid body"; }
void StaticObject::instantiateCollisionObject() { if(mCompoundShape != NULL) { const StaticObjectSettings* staticObjectSettings = static_cast<const StaticObjectSettings*>(mObjectSettings); btTransform startTransform = btTransform(convert(staticObjectSettings->mInitialOrientation), convert(staticObjectSettings->mInitialPosition)); mMyMotionState = new MyMotionState(mSceneNode, startTransform); mRigidBody = createRigidBody(startTransform, mCompoundShape, mMyMotionState, 0.f); mRigidBody->setRestitution(DEFAULT_RESTITUTION_VALUE); mDynamicWorld->addRigidBody(mRigidBody); } }
void MotorDemo::initPhysics() { m_guiHelper->setUpAxis(1); // Setup the basic world m_Time = 0; m_fCyclePeriod = 2000.f; // in milliseconds // m_fMuscleStrength = 0.05f; // new SIMD solver for joints clips accumulated impulse, so the new limits for the motor // should be (numberOfsolverIterations * oldLimits) // currently solver uses 10 iterations, so: m_fMuscleStrength = 0.5f; m_collisionConfiguration = new btDefaultCollisionConfiguration(); m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); btVector3 worldAabbMin(-10000,-10000,-10000); btVector3 worldAabbMax(10000,10000,10000); m_broadphase = new btAxisSweep3 (worldAabbMin, worldAabbMax); m_solver = new btSequentialImpulseConstraintSolver; m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration); m_dynamicsWorld->setInternalTickCallback(motorPreTickCallback,this,true); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); // Setup a big ground box { btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(200.),btScalar(10.),btScalar(200.))); m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); groundTransform.setOrigin(btVector3(0,-10,0)); createRigidBody(btScalar(0.),groundTransform,groundShape); } // Spawn one ragdoll btVector3 startOffset(1,0.5,0); spawnTestRig(startOffset, false); startOffset.setValue(-2,0.5,0); spawnTestRig(startOffset, true); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }
SmellSensor::SmellSensor(Fixation *fixation, QString typeName, SensorType type, RigidBodyOrigin::RigidBodyType smellType, btScalar smellRadius) : Sensor(fixation) { this->typeName = typeName; this->type = type; this->smellType = smellType; this->radiusOfSmell = smellRadius; intensityInput = new BrainIn(0, smellRadius); brainInputs.append(intensityInput); createRigidBody(smellRadius); }
void MultiBodyVehicleSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) { int upAxis = 1; btVector4 colors[4] = { btVector4(1,0,0,1), btVector4(0,1,0,1), btVector4(0,1,1,1), btVector4(1,1,0,1), }; int curColor = 0; gfxBridge.setUpAxis(upAxis); this->createEmptyDynamicsWorld(); gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld); m_dynamicsWorld->getDebugDrawer()->setDebugMode( //btIDebugDraw::DBG_DrawConstraints +btIDebugDraw::DBG_DrawWireframe +btIDebugDraw::DBG_DrawContactPoints +btIDebugDraw::DBG_DrawAabb );//+btIDebugDraw::DBG_DrawConstraintLimits); createMultiBodyVehicle(); if (1) { btVector3 groundHalfExtents(20,20,20); groundHalfExtents[upAxis]=1.f; btBoxShape* box = new btBoxShape(groundHalfExtents); box->initializePolyhedralFeatures(); gfxBridge.createCollisionShapeGraphicsObject(box); btTransform start; start.setIdentity(); btVector3 groundOrigin(0,0,0); groundOrigin[upAxis]=-1.5; start.setOrigin(groundOrigin); btRigidBody* body = createRigidBody(0,start,box); btVector4 color = colors[curColor]; curColor++; curColor&=3; gfxBridge.createRigidBodyGraphicsObject(body,color); } }
// To create from serialization data SmellSensor::SmellSensor(QVariant data, RigidBodyOrigin::RigidBodyType smellType, Fixation * fixation) : Sensor(data, fixation) { QVariantMap map = data.toMap(); this->smellType = smellType; intensityInput = new BrainIn(map["intensityInput"]); // the max value equals radius of smell ! radiusOfSmell = map["radiusOfSmell"].toFloat(); //intensityInput->setMax(radiusOfSmell); brainInputs.append(intensityInput); createRigidBody(radiusOfSmell); }
void btWorldImporter::convertRigidBodyDouble( btRigidBodyDoubleData* colObjData) { btScalar mass = btScalar(colObjData->m_inverseMass? 1.f/colObjData->m_inverseMass : 0.f); btVector3 localInertia; localInertia.setZero(); btCollisionShape** shapePtr = m_shapeMap.find(colObjData->m_collisionObjectData.m_collisionShape); if (shapePtr && *shapePtr) { btTransform startTransform; colObjData->m_collisionObjectData.m_worldTransform.m_origin.m_floats[3] = 0.f; startTransform.deSerializeDouble(colObjData->m_collisionObjectData.m_worldTransform); // startTransform.setBasis(btMatrix3x3::getIdentity()); btCollisionShape* shape = (btCollisionShape*)*shapePtr; if (shape->isNonMoving()) { mass = 0.f; } if (mass) { shape->calculateLocalInertia(mass,localInertia); } bool isDynamic = mass!=0.f; btRigidBody* body = createRigidBody(isDynamic,mass,startTransform,shape,colObjData->m_collisionObjectData.m_name); body->setFriction(static_cast<btScalar>(colObjData->m_collisionObjectData.m_friction)); body->setRestitution(static_cast<btScalar>(colObjData->m_collisionObjectData.m_restitution)); #ifdef USE_INTERNAL_EDGE_UTILITY if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) { btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; if (trimesh->getTriangleInfoMap()) { body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); } } #endif //USE_INTERNAL_EDGE_UTILITY m_bodyMap.insert(colObjData,body); } else { printf("error: no shape found\n"); } }
boing::boing(MObject inputShape,MString inname, MString inTypeName, MVector pos, MVector vel, MVector rot, MVector av, float mass) { node = inputShape; name = inname; typeName = inTypeName; m_initial_velocity = vel; m_initial_position = pos; m_initial_rotation = rot; m_initial_angularvelocity = av; m_mass = mass; attrArray = MStringArray(); dataArray = MStringArray(); m_collision_shape = createCollisionShape(node); /*cout<<"m_collision_shape : "<<m_collision_shape<<endl; std::cout<<"creating a new (boing::boing) boing node : "<<inname<<" type "<<inTypeName<<"from node "<<MFnDependencyNode(inputShape).name()<<" in pos "<<m_initial_position<<" vel "<<m_initial_velocity<<" rotation "<<m_initial_rotation<<" and av "<<m_initial_angularvelocity<<endl;*/ count++; std::cout<<"count = "<<count<<std::endl; createRigidBody(); }
void ColladaConverter::PreparePhysicsObject(struct btRigidBodyInput& input, bool isDynamics, btScalar mass,btCollisionShape* colShape) { btTransform startTransform; startTransform.setIdentity(); btVector3 startScale(1.f,1.f,1.f); //The 'target' points to a graphics element/node, which contains the start (world) transform daeElementRef elem = input.m_instanceRigidBodyRef->getTarget().getElement(); if (elem) { domNodeRef node = *(domNodeRef*)&elem; m_colladadomNodes[m_numObjects] = node; //find transform of the node that this rigidbody maps to startTransform = GetbtTransformFromCOLLADA_DOM( node->getMatrix_array(), node->getRotate_array(), node->getTranslate_array(), m_unitMeterScaling ); unsigned int i; for (i=0;i<node->getScale_array().getCount();i++) { domScaleRef scaleRef = node->getScale_array()[i]; domFloat3 fl3 = scaleRef->getValue(); startScale = btVector3((btScalar)fl3.get(0),(btScalar)fl3.get(1),(btScalar)fl3.get(2)); } btRigidBody* body= createRigidBody(elem->getID(), isDynamics, (float) mass,startTransform,colShape); if (body) { //bodyName is used as identifier for constraints body->setUserPointer((void*)input.m_bodyName); m_rigidBodies[m_numObjects] = body; m_numObjects++; } } }
// virtual void Mechanism::initialize(stateStruct& startState){ // Set necessary parameters from startState before initializing physics setStartWithState(startState); // Virtual function: not defined in this file // Initialize Physics // set up world broadphase_ = new btDbvtBroadphase(); collisionConfiguration_ = new btDefaultCollisionConfiguration(); dispatcher_ = new btCollisionDispatcher(collisionConfiguration_); solver_ = new btSequentialImpulseConstraintSolver; dynamicsWorld_ = new btDiscreteDynamicsWorld(dispatcher_,broadphase_,solver_,collisionConfiguration_); // set gravity. y is apparently up. dynamicsWorld_->setGravity(btVector3(0,-10,0)); //----------------OBJECT CREATION SECTION-------------------// //make the ground plane gndCS_ = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); btTransform gndBT; gndBT.setIdentity(); gndBT.setOrigin(btVector3(0.0f,-50.0f,0.0f)); btScalar gndMass(0.0); //create the ground plane rigid body gndRB_ = createRigidBody(gndCS_,gndMass,gndBT); //add it to the dynamics world dynamicsWorld_->addRigidBody(gndRB_); //--------------END OBJECT CREATION SECTION-----------------// //---------INITIALIZE REMAINING PARAMETERS SECTION----------// //set controller values pGains_.setValue(5.0f,50.0f,5.0f); dGains_.setValue(2.0f,2.0f,2.0f); // change friction gndRB_->setFriction(0.1); //-------END INITIALIZE REMAINING PARAMETERS SECTION-------// }
void TestJointTorqueSetup::initPhysics() { int upAxis = 1; gJointFeedbackInWorldSpace = true; gJointFeedbackInJointFrame = true; m_guiHelper->setUpAxis(upAxis); btVector4 colors[4] = { btVector4(1,0,0,1), btVector4(0,1,0,1), btVector4(0,1,1,1), btVector4(1,1,0,1), }; int curColor = 0; this->createEmptyDynamicsWorld(); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_dynamicsWorld->getDebugDrawer()->setDebugMode( //btIDebugDraw::DBG_DrawConstraints +btIDebugDraw::DBG_DrawWireframe +btIDebugDraw::DBG_DrawContactPoints +btIDebugDraw::DBG_DrawAabb );//+btIDebugDraw::DBG_DrawConstraintLimits); //create a static ground object if (1) { btVector3 groundHalfExtents(1,1,0.2); groundHalfExtents[upAxis]=1.f; btBoxShape* box = new btBoxShape(groundHalfExtents); box->initializePolyhedralFeatures(); m_guiHelper->createCollisionShapeGraphicsObject(box); btTransform start; start.setIdentity(); btVector3 groundOrigin(-0.4f, 3.f, 0.f); groundOrigin[upAxis] -=.5; groundOrigin[2]-=0.6; start.setOrigin(groundOrigin); btQuaternion groundOrn(btVector3(0,1,0),0.25*SIMD_PI); // start.setRotation(groundOrn); btRigidBody* body = createRigidBody(0,start,box); body->setFriction(0); btVector4 color = colors[curColor]; curColor++; curColor&=3; m_guiHelper->createRigidBodyGraphicsObject(body,color); } { bool floating = false; bool damping = false; bool gyro = false; 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); btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f); //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 = 1.f; if(baseMass) { //btCollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); btCollisionShape *shape = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); shape->calculateLocalInertia(baseMass, baseInertiaDiag); delete shape; } bool isMultiDof = true; btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof); m_multiBody = pMultiBody; btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); // baseOriQuat.setEulerZYX(-.25*SIMD_PI,0,-1.75*SIMD_PI); pMultiBody->setBasePos(basePosition); pMultiBody->setWorldToBaseRot(baseOriQuat); btVector3 vel(0, 0, 0); // pMultiBody->setBaseVel(vel); //init the links btVector3 hingeJointAxis(1, 0, 0); //y-axis assumed up btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset ////// btScalar q0 = 0.f * SIMD_PI/ 180.f; btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0); quat0.normalize(); ///// for(int i = 0; i < numLinks; ++i) { float linkMass = 1.f; //if (i==3 || i==2) // linkMass= 1000; btVector3 linkInertiaDiag(0.f, 0.f, 0.f); btCollisionShape* shape = 0; if (i==0) { shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));// } else { shape = new btSphereShape(radius); } shape->calculateLocalInertia(linkMass, linkInertiaDiag); delete shape; if(!spherical) { //pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); if (i==0) { pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); } else { btVector3 parentComToCurrentCom(0, -radius * 2.f, 0); //par body's COM to cur body's COM offset btVector3 currentPivotToCurrentCom(0, -radius, 0); //cur body's COM to cur body's PIV offset btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false); } //pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false); } else { //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false); pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false); } } pMultiBody->finalizeMultiDof(); //for (int i=pMultiBody->getNumLinks()-1;i>=0;i--)// for (int i=0;i<pMultiBody->getNumLinks();i++) { btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback(); pMultiBody->getLink(i).m_jointFeedback = fb; m_jointFeedbacks.push_back(fb); //break; } btMultiBodyDynamicsWorld* world = m_dynamicsWorld; /// world->addMultiBody(pMultiBody); btMultiBody* mbC = pMultiBody; mbC->setCanSleep(canSleep); mbC->setHasSelfCollision(selfCollide); mbC->setUseGyroTerm(gyro); // if(!damping) { mbC->setLinearDamping(0.f); mbC->setAngularDamping(0.f); }else { mbC->setLinearDamping(0.1f); mbC->setAngularDamping(0.9f); } // m_dynamicsWorld->setGravity(btVector3(0,0,-10)); ////////////////////////////////////////////// if(0)//numLinks > 0) { btScalar q0 = 45.f * SIMD_PI/ 180.f; if(!spherical) if(mbC->isMultiDof()) mbC->setJointPosMultiDof(0, &q0); else mbC->setJointPos(0, q0); else { btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0); quat0.normalize(); mbC->setJointPosMultiDof(0, quat0); } } /// btAlignedObjectArray<btQuaternion> world_to_local; world_to_local.resize(pMultiBody->getNumLinks() + 1); btAlignedObjectArray<btVector3> local_origin; local_origin.resize(pMultiBody->getNumLinks() + 1); world_to_local[0] = pMultiBody->getWorldToBaseRot(); local_origin[0] = pMultiBody->getBasePos(); // double friction = 1; { // float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; // btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; if (1) { btCollisionShape* shape = new btBoxShape(btVector3(baseHalfExtents[0],baseHalfExtents[1],baseHalfExtents[2]));//new btSphereShape(baseHalfExtents[0]); m_guiHelper->createCollisionShapeGraphicsObject(shape); btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1); col->setCollisionShape(shape); btTransform tr; tr.setIdentity(); //if we don't set the initial pose of the btCollisionObject, the simulator will do this //when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider tr.setOrigin(local_origin[0]); btQuaternion orn(btVector3(0,0,1),0.25*3.1415926538); tr.setRotation(orn); col->setWorldTransform(tr); bool isDynamic = (baseMass > 0 && floating); short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//, 2,1+2); btVector3 color(0.0,0.0,0.5); m_guiHelper->createCollisionObjectGraphicsObject(col,color); // col->setFriction(friction); pMultiBody->setBaseCollider(col); } } for (int i=0; i < pMultiBody->getNumLinks(); ++i) { const int parent = pMultiBody->getParent(i); world_to_local[i+1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent+1]; local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , pMultiBody->getRVector(i))); } for (int i=0; i < pMultiBody->getNumLinks(); ++i) { btVector3 posr = local_origin[i+1]; // float pos[4]={posr.x(),posr.y(),posr.z(),1}; btScalar quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; btCollisionShape* shape =0; if (i==0) { shape = new btBoxShape(btVector3(linkHalfExtents[0],linkHalfExtents[1],linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]); } else { shape = new btSphereShape(radius); } m_guiHelper->createCollisionShapeGraphicsObject(shape); btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i); col->setCollisionShape(shape); btTransform tr; tr.setIdentity(); tr.setOrigin(posr); tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); col->setWorldTransform(tr); // col->setFriction(friction); bool isDynamic = 1;//(linkMass > 0); short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); //if (i==0||i>numLinks-2) { world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//,2,1+2); btVector4 color = colors[curColor]; curColor++; curColor&=3; m_guiHelper->createCollisionObjectGraphicsObject(col,color); pMultiBody->getLink(i).m_collider=col; } } } btSerializer* s = new btDefaultSerializer; m_dynamicsWorld->serialize(s); b3ResourcePath p; char resourcePath[1024]; if (p.findResourcePath("multibody.bullet",resourcePath,1024)) { FILE* f = fopen(resourcePath,"wb"); fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f); fclose(f); } }
PxActor* World::addPlane(const ofVec3f& pos, const ofQuaternion& rot, float mass) { assert(inited); return createRigidBody(PxPlaneGeometry(), mass, pos, rot); }
PxActor* World::addCapsule(const float radius, const float height, const ofVec3f& pos, const ofQuaternion& rot, float mass) { assert(inited); return createRigidBody(PxCapsuleGeometry(radius, height), mass, pos, rot); }
PxActor* World::addSphere(const float size, const ofVec3f& pos, const ofQuaternion& rot, float mass) { assert(inited); return createRigidBody(PxSphereGeometry(size), mass, pos, rot); }
PxActor* World::addBox(const ofVec3f& size, const ofVec3f& pos, const ofQuaternion& rot, float mass) { assert(inited); return createRigidBody(PxBoxGeometry(toPx(size)), mass, pos, rot); }
void BasicExample::initPhysics() { m_guiHelper->setUpAxis(1); createEmptyDynamicsWorld(); //m_dynamicsWorld->setGravity(btVector3(0,0,0)); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); if (m_dynamicsWorld->getDebugDrawer()) m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints); ///create a few basic rigid bodies btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); //groundShape->initializePolyhedralFeatures(); //btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50); m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); groundTransform.setOrigin(btVector3(0,-50,0)); { btScalar mass(0.); createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1)); } { //create a few dynamic rigidbodies // Re-using the same collision is better for memory usage and performance btBoxShape* colShape = createBoxShape(btVector3(.1,.1,.1)); //btCollisionShape* colShape = new btSphereShape(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); 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(btVector3( btScalar(0.2*i), btScalar(2+.2*k), btScalar(0.2*j))); createRigidBody(mass,startTransform,colShape); } } } } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }
void ImportUrdfSetup::initPhysics() { m_guiHelper->setUpAxis(m_upAxis); this->createEmptyDynamicsWorld(); //m_dynamicsWorld->getSolverInfo().m_numIterations = 100; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_dynamicsWorld->getDebugDrawer()->setDebugMode( btIDebugDraw::DBG_DrawConstraints +btIDebugDraw::DBG_DrawContactPoints +btIDebugDraw::DBG_DrawAabb );//+btIDebugDraw::DBG_DrawConstraintLimits); if (m_guiHelper->getParameterInterface()) { SliderParams slider("Gravity", &m_grav); slider.m_minVal = -10; slider.m_maxVal = 10; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } BulletURDFImporter u2b(m_guiHelper, 0); bool loadOk = u2b.loadURDF(m_fileName); #ifdef TEST_MULTIBODY_SERIALIZATION //test to serialize a multibody to disk or shared memory, with base, link and joint names btSerializer* s = new btDefaultSerializer; #endif //TEST_MULTIBODY_SERIALIZATION if (loadOk) { //printTree(u2b,u2b.getRootLinkIndex()); //u2b.printTree(); btTransform identityTrans; identityTrans.setIdentity(); { //todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user //int rootLinkIndex = u2b.getRootLinkIndex(); //b3Printf("urdf root link index = %d\n",rootLinkIndex); MyMultiBodyCreator creation(m_guiHelper); ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix()); m_data->m_rb = creation.getRigidBody(); m_data->m_mb = creation.getBulletMultiBody(); btMultiBody* mb = m_data->m_mb; for (int i = 0; i < u2b.getNumAllocatedCollisionShapes(); i++) { m_collisionShapes.push_back(u2b.getAllocatedCollisionShape(i)); } if (m_useMultiBody && mb ) { std::string* name = new std::string(u2b.getLinkName(u2b.getRootLinkIndex())); m_nameMemory.push_back(name); #ifdef TEST_MULTIBODY_SERIALIZATION s->registerNameForPointer(name->c_str(),name->c_str()); #endif//TEST_MULTIBODY_SERIALIZATION mb->setBaseName(name->c_str()); //create motors for each btMultiBody joint int numLinks = mb->getNumLinks(); for (int i=0;i<numLinks;i++) { int mbLinkIndex = i; int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex]; std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex)); std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str()); #ifdef TEST_MULTIBODY_SERIALIZATION s->registerNameForPointer(jointName->c_str(),jointName->c_str()); s->registerNameForPointer(linkName->c_str(),linkName->c_str()); #endif//TEST_MULTIBODY_SERIALIZATION m_nameMemory.push_back(jointName); m_nameMemory.push_back(linkName); mb->getLink(i).m_linkName = linkName->c_str(); mb->getLink(i).m_jointName = jointName->c_str(); if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute ||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic ) { if (m_data->m_numMotors<MAX_NUM_MOTORS) { char motorName[1024]; sprintf(motorName,"%s q'", jointName->c_str()); btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors]; *motorVel = 0.f; SliderParams slider(motorName,motorVel); slider.m_minVal=-4; slider.m_maxVal=4; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); float maxMotorImpulse = 10.1f; btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse); //motor->setMaxAppliedImpulse(0); m_data->m_jointMotors[m_data->m_numMotors]=motor; m_dynamicsWorld->addMultiBodyConstraint(motor); m_data->m_numMotors++; } } } } else { if (1) { //create motors for each generic joint int num6Dof = creation.getNum6DofConstraints(); for (int i=0;i<num6Dof;i++) { btGeneric6DofSpring2Constraint* c = creation.get6DofConstraint(i); if (c->getUserConstraintPtr()) { GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)c->getUserConstraintPtr(); if ((jointInfo->m_urdfJointType ==URDFRevoluteJoint) || (jointInfo->m_urdfJointType ==URDFPrismaticJoint) || (jointInfo->m_urdfJointType ==URDFContinuousJoint)) { int urdfLinkIndex = jointInfo->m_urdfIndex; std::string jointName = u2b.getJointName(urdfLinkIndex); char motorName[1024]; sprintf(motorName,"%s q'", jointName.c_str()); btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors]; *motorVel = 0.f; SliderParams slider(motorName,motorVel); slider.m_minVal=-4; slider.m_maxVal=4; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); m_data->m_generic6DofJointMotors[m_data->m_numMotors]=c; bool motorOn = true; c->enableMotor(jointInfo->m_jointAxisIndex,motorOn); c->setMaxMotorForce(jointInfo->m_jointAxisIndex,10000); c->setTargetVelocity(jointInfo->m_jointAxisIndex,0); m_data->m_numMotors++; } } } } } } //the btMultiBody support is work-in-progress :-) for (int i=0;i<m_dynamicsWorld->getNumMultiBodyConstraints();i++) { m_dynamicsWorld->getMultiBodyConstraint(i)->finalizeMultiDof(); } bool createGround=true; if (createGround) { btVector3 groundHalfExtents(20,20,20); groundHalfExtents[m_upAxis]=1.f; btBoxShape* box = new btBoxShape(groundHalfExtents); m_collisionShapes.push_back(box); box->initializePolyhedralFeatures(); m_guiHelper->createCollisionShapeGraphicsObject(box); btTransform start; start.setIdentity(); btVector3 groundOrigin(0,0,0); groundOrigin[m_upAxis]=-2.5; start.setOrigin(groundOrigin); btRigidBody* body = createRigidBody(0,start,box); //m_dynamicsWorld->removeRigidBody(body); // m_dynamicsWorld->addRigidBody(body,2,1); btVector3 color(0.5,0.5,0.5); m_guiHelper->createRigidBodyGraphicsObject(body,color); } } #ifdef TEST_MULTIBODY_SERIALIZATION m_dynamicsWorld->serialize(s); b3ResourcePath p; char resourcePath[1024]; if (p.findResourcePath("r2d2_multibody.bullet",resourcePath,1024)) { FILE* f = fopen(resourcePath,"wb"); fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f); fclose(f); } #endif//TEST_MULTIBODY_SERIALIZATION }
void Hinge2Vehicle::initPhysics() { m_guiHelper->setUpAxis(1); 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_broadphase = new btAxisSweep3(worldMin,worldMax); if (useMCLPSolver) { btDantzigSolver* mlcp = new btDantzigSolver(); //btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel; btMLCPSolver* sol = new btMLCPSolver(mlcp); m_solver = sol; } else { m_solver = new btSequentialImpulseConstraintSolver(); } m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,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_numIterations = 100; 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)); btScalar chassisMass = 800; m_carChassis = localCreateRigidBody(chassisMass,tr,compound);//chassisShape); //m_carChassis->setDamping(0.2,0.2); //m_wheelShape = new btCylinderShapeX(btVector3(wheelWidth,wheelRadius,wheelRadius)); m_wheelShape = new btCylinderShapeX(btVector3(wheelWidth,wheelRadius,wheelRadius)); //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}; btVector3 wheelPos[4] = { btVector3(btScalar(-1.), btScalar(-0.25), btScalar(1.25)), btVector3(btScalar(1.), btScalar(-0.25), btScalar(1.25)), btVector3(btScalar(1.), btScalar(-0.25), btScalar(-1.25)), btVector3(btScalar(-1.), btScalar(-0.25), btScalar(-1.25)) }; for (int i=0;i<4;i++) { // create a Hinge2 joint // create two rigid bodies // static bodyA (parent) on top: btRigidBody* pBodyA = this->m_carChassis;//m_chassis;//createRigidBody( 0.0, tr, m_wheelShape); pBodyA->setActivationState(DISABLE_DEACTIVATION); // dynamic bodyB (child) below it : btTransform tr; tr.setIdentity(); tr.setOrigin(wheelPos[i]); btRigidBody* pBodyB = createRigidBody(10.0, tr, m_wheelShape); pBodyB->setFriction(1110); pBodyB->setActivationState(DISABLE_DEACTIVATION); // add some data to build constraint frames btVector3 parentAxis(0.f, 1.f, 0.f); btVector3 childAxis(1.f, 0.f, 0.f); btVector3 anchor = tr.getOrigin();//(0.f, 0.f, 0.f); btHinge2Constraint* pHinge2 = new btHinge2Constraint(*pBodyA, *pBodyB, anchor, parentAxis, childAxis); //m_guiHelper->get2dCanvasInterface(); pHinge2->setLowerLimit(-SIMD_HALF_PI * 0.5f); pHinge2->setUpperLimit( SIMD_HALF_PI * 0.5f); // add constraint to world m_dynamicsWorld->addConstraint(pHinge2, true); // draw constraint frames and limits for debugging { int motorAxis = 3; pHinge2->enableMotor(motorAxis,true); pHinge2->setMaxMotorForce(motorAxis,1000); pHinge2->setTargetVelocity(motorAxis,-1); } { int motorAxis = 5; pHinge2->enableMotor(motorAxis,true); pHinge2->setMaxMotorForce(motorAxis,1000); pHinge2->setTargetVelocity(motorAxis,0); } pHinge2->setDbgDrawSize(btScalar(5.f)); } { 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); } resetForklift(); // setCameraDistance(26.f); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }
void ImportSDFSetup::initPhysics() { int upAxis = 2; m_guiHelper->setUpAxis(upAxis); this->createEmptyDynamicsWorld(); //m_dynamicsWorld->getSolverInfo().m_numIterations = 100; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_dynamicsWorld->getDebugDrawer()->setDebugMode( btIDebugDraw::DBG_DrawConstraints +btIDebugDraw::DBG_DrawContactPoints +btIDebugDraw::DBG_DrawAabb );//+btIDebugDraw::DBG_DrawConstraintLimits); btVector3 gravity(0,0,0); gravity[upAxis]=-9.8; m_dynamicsWorld->setGravity(gravity); BulletURDFImporter u2b(m_guiHelper); bool loadOk = u2b.loadSDF(m_fileName); if (loadOk) { //printTree(u2b,u2b.getRootLinkIndex()); //u2b.printTree(); btTransform identityTrans; identityTrans.setIdentity(); for (int m =0; m<u2b.getNumModels();m++) { u2b.activateModel(m); btMultiBody* mb = 0; //todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user int rootLinkIndex = u2b.getRootLinkIndex(); b3Printf("urdf root link index = %d\n",rootLinkIndex); MyMultiBodyCreator creation(m_guiHelper); ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix()); mb = creation.getBulletMultiBody(); } for (int i=0;i<m_dynamicsWorld->getNumMultiBodyConstraints();i++) { m_dynamicsWorld->getMultiBodyConstraint(i)->finalizeMultiDof(); } bool createGround=true; if (createGround) { btVector3 groundHalfExtents(20,20,20); groundHalfExtents[upAxis]=1.f; btBoxShape* box = new btBoxShape(groundHalfExtents); box->initializePolyhedralFeatures(); m_guiHelper->createCollisionShapeGraphicsObject(box); btTransform start; start.setIdentity(); btVector3 groundOrigin(0,0,0); groundOrigin[upAxis]=-2.5; start.setOrigin(groundOrigin); btRigidBody* body = createRigidBody(0,start,box); //m_dynamicsWorld->removeRigidBody(body); // m_dynamicsWorld->addRigidBody(body,2,1); btVector3 color(0.5,0.5,0.5); m_guiHelper->createRigidBodyGraphicsObject(body,color); } ///this extra stepSimulation call makes sure that all the btMultibody transforms are properly propagates. m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.); } }
void SimpleJointExample::initPhysics() { m_guiHelper->setUpAxis(1); createEmptyDynamicsWorld(); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); if (m_dynamicsWorld->getDebugDrawer()) m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints); ///create a few basic rigid bodies btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); groundTransform.setOrigin(btVector3(0,-50,0)); { btScalar mass(0.); createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1)); } { //create a few dynamic rigidbodies // Re-using the same collision is better for memory usage and performance btBoxShape* colShape = createBoxShape(btVector3(1,1,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); startTransform.setOrigin(btVector3( btScalar(0), btScalar(10), btScalar(0))); btRigidBody* dynamicBox = createRigidBody(mass,startTransform,colShape); //create a static rigid body mass = 0; startTransform.setOrigin(btVector3( btScalar(0), btScalar(20), btScalar(0))); btRigidBody* staticBox = createRigidBody(mass,startTransform,colShape); //create a simple p2pjoint constraint btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*dynamicBox, *staticBox, btVector3(0,3,0), btVector3(0,0,0)); p2p->m_setting.m_damping = 0.0625; p2p->m_setting.m_impulseClamp = 0.95; m_dynamicsWorld->addConstraint(p2p); } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }
btCollisionObject* btBulletWorldImporter::createCollisionObject(const btTransform& startTransform,btCollisionShape* shape, const char* bodyName) { return createRigidBody(false,0,startTransform,shape,bodyName); }
void RigidBodyFromObjExample::initPhysics() { m_guiHelper->setUpAxis(1); createEmptyDynamicsWorld(); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); //if (m_dynamicsWorld->getDebugDrawer()) // m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints); ///create a few basic rigid bodies btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); groundTransform.setOrigin(btVector3(0,-50,0)); { btScalar mass(0.); createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1)); } //load our obj mesh const char* fileName = "teddy.obj";//sphere8.obj";//sponza_closed.obj";//sphere8.obj"; char relativeFileName[1024]; if (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024)) { char pathPrefix[1024]; b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); } GLInstanceGraphicsShape* glmesh = LoadMeshFromObj(relativeFileName, ""); printf("[INFO] Obj loaded: Extracted %d verticed from obj file [%s]\n", glmesh->m_numvertices, fileName); const GLInstanceVertex& v = glmesh->m_vertices->at(0); btConvexHullShape* shape = new btConvexHullShape((const btScalar*)(&(v.xyzw[0])), glmesh->m_numvertices, sizeof(GLInstanceVertex)); float scaling[4] = {0.1,0.1,0.1,1}; btVector3 localScaling(scaling[0],scaling[1],scaling[2]); shape->setLocalScaling(localScaling); if (m_options & OptimizeConvexObj) { shape->optimizeConvexHull(); } if (m_options & ComputePolyhedralFeatures) { shape->initializePolyhedralFeatures(); } //shape->setMargin(0.001); m_collisionShapes.push_back(shape); btTransform startTransform; startTransform.setIdentity(); btScalar mass(1.f); bool isDynamic = (mass != 0.f); btVector3 localInertia(0,0,0); if (isDynamic) shape->calculateLocalInertia(mass,localInertia); float color[4] = {1,1,1,1}; float orn[4] = {0,0,0,1}; float pos[4] = {0,3,0,0}; btVector3 position(pos[0],pos[1],pos[2]); startTransform.setOrigin(position); btRigidBody* body = createRigidBody(mass,startTransform,shape); bool useConvexHullForRendering = ((m_options & ObjUseConvexHullForRendering)!=0); if (!useConvexHullForRendering) { int shapeId = m_guiHelper->registerGraphicsShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, &glmesh->m_indices->at(0), glmesh->m_numIndices, B3_GL_TRIANGLES, -1); shape->setUserIndex(shapeId); int renderInstance = m_guiHelper->registerGraphicsInstance(shapeId,pos,orn,color,scaling); body->setUserIndex(renderInstance); } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }
bool btBulletWorldImporter::convertAllObjects( bParse::btBulletFile* bulletFile2) { int i; for (i=0;i<bulletFile2->m_bvhs.size();i++) { btOptimizedBvh* bvh = createOptimizedBvh(); if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION) { btQuantizedBvhDoubleData* bvhData = (btQuantizedBvhDoubleData*)bulletFile2->m_bvhs[i]; bvh->deSerializeDouble(*bvhData); } else { btQuantizedBvhFloatData* bvhData = (btQuantizedBvhFloatData*)bulletFile2->m_bvhs[i]; bvh->deSerializeFloat(*bvhData); } m_bvhMap.insert(bulletFile2->m_bvhs[i],bvh); } btHashMap<btHashPtr,btCollisionShape*> shapeMap; for (i=0;i<bulletFile2->m_collisionShapes.size();i++) { btCollisionShapeData* shapeData = (btCollisionShapeData*)bulletFile2->m_collisionShapes[i]; btCollisionShape* shape = convertCollisionShape(shapeData); if (shape) { // printf("shapeMap.insert(%x,%x)\n",shapeData,shape); shapeMap.insert(shapeData,shape); } if (shape&& shapeData->m_name) { char* newname = duplicateName(shapeData->m_name); m_objectNameMap.insert(shape,newname); m_nameShapeMap.insert(newname,shape); } } btHashMap<btHashPtr,btCollisionObject*> bodyMap; for (i=0;i<bulletFile2->m_rigidBodies.size();i++) { if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION) { btRigidBodyDoubleData* colObjData = (btRigidBodyDoubleData*)bulletFile2->m_rigidBodies[i]; btScalar mass = btScalar(colObjData->m_inverseMass? 1.f/colObjData->m_inverseMass : 0.f); btVector3 localInertia; localInertia.setZero(); btCollisionShape** shapePtr = shapeMap.find(colObjData->m_collisionObjectData.m_collisionShape); if (shapePtr && *shapePtr) { btTransform startTransform; startTransform.deSerializeDouble(colObjData->m_collisionObjectData.m_worldTransform); // startTransform.setBasis(btMatrix3x3::getIdentity()); btCollisionShape* shape = (btCollisionShape*)*shapePtr; if (shape->isNonMoving()) { mass = 0.f; } if (mass) { shape->calculateLocalInertia(mass,localInertia); } bool isDynamic = mass!=0.f; btRigidBody* body = createRigidBody(isDynamic,mass,startTransform,shape,colObjData->m_collisionObjectData.m_name); #ifdef USE_INTERNAL_EDGE_UTILITY if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) { btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; if (trimesh->getTriangleInfoMap()) { body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); } } #endif //USE_INTERNAL_EDGE_UTILITY bodyMap.insert(colObjData,body); } else { printf("error: no shape found\n"); } } else { btRigidBodyFloatData* colObjData = (btRigidBodyFloatData*)bulletFile2->m_rigidBodies[i]; btScalar mass = btScalar(colObjData->m_inverseMass? 1.f/colObjData->m_inverseMass : 0.f); btVector3 localInertia; localInertia.setZero(); btCollisionShape** shapePtr = shapeMap.find(colObjData->m_collisionObjectData.m_collisionShape); if (shapePtr && *shapePtr) { btTransform startTransform; startTransform.deSerializeFloat(colObjData->m_collisionObjectData.m_worldTransform); // startTransform.setBasis(btMatrix3x3::getIdentity()); btCollisionShape* shape = (btCollisionShape*)*shapePtr; if (shape->isNonMoving()) { mass = 0.f; } if (mass) { shape->calculateLocalInertia(mass,localInertia); } bool isDynamic = mass!=0.f; btRigidBody* body = createRigidBody(isDynamic,mass,startTransform,shape,colObjData->m_collisionObjectData.m_name); #ifdef USE_INTERNAL_EDGE_UTILITY if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) { btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; if (trimesh->getTriangleInfoMap()) { body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); } } #endif //USE_INTERNAL_EDGE_UTILITY bodyMap.insert(colObjData,body); } else { printf("error: no shape found\n"); } } } for (i=0;i<bulletFile2->m_collisionObjects.size();i++) { if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION) { btCollisionObjectDoubleData* colObjData = (btCollisionObjectDoubleData*)bulletFile2->m_collisionObjects[i]; btCollisionShape** shapePtr = shapeMap.find(colObjData->m_collisionShape); if (shapePtr && *shapePtr) { btTransform startTransform; startTransform.deSerializeDouble(colObjData->m_worldTransform); btCollisionShape* shape = (btCollisionShape*)*shapePtr; btCollisionObject* body = createCollisionObject(startTransform,shape,colObjData->m_name); #ifdef USE_INTERNAL_EDGE_UTILITY if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) { btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; if (trimesh->getTriangleInfoMap()) { body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); } } #endif //USE_INTERNAL_EDGE_UTILITY bodyMap.insert(colObjData,body); } else { printf("error: no shape found\n"); } } else { btCollisionObjectFloatData* colObjData = (btCollisionObjectFloatData*)bulletFile2->m_collisionObjects[i]; btCollisionShape** shapePtr = shapeMap.find(colObjData->m_collisionShape); if (shapePtr && *shapePtr) { btTransform startTransform; startTransform.deSerializeFloat(colObjData->m_worldTransform); btCollisionShape* shape = (btCollisionShape*)*shapePtr; btCollisionObject* body = createCollisionObject(startTransform,shape,colObjData->m_name); #ifdef USE_INTERNAL_EDGE_UTILITY if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) { btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; if (trimesh->getTriangleInfoMap()) { body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); } } #endif //USE_INTERNAL_EDGE_UTILITY bodyMap.insert(colObjData,body); } else { printf("error: no shape found\n"); } } printf("bla"); } for (i=0;i<bulletFile2->m_constraints.size();i++) { btTypedConstraintData* constraintData = (btTypedConstraintData*)bulletFile2->m_constraints[i]; btCollisionObject** colAptr = bodyMap.find(constraintData->m_rbA); btCollisionObject** colBptr = bodyMap.find(constraintData->m_rbB); btRigidBody* rbA = 0; btRigidBody* rbB = 0; if (colAptr) { rbA = btRigidBody::upcast(*colAptr); if (!rbA) rbA = &getFixedBody(); } if (colBptr) { rbB = btRigidBody::upcast(*colBptr); if (!rbB) rbB = &getFixedBody(); } btTypedConstraint* constraint = 0; switch (constraintData->m_objectType) { case POINT2POINT_CONSTRAINT_TYPE: { if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION) { btPoint2PointConstraintDoubleData* p2pData = (btPoint2PointConstraintDoubleData*)constraintData; if (rbA && rbB) { btVector3 pivotInA,pivotInB; pivotInA.deSerializeDouble(p2pData->m_pivotInA); pivotInB.deSerializeDouble(p2pData->m_pivotInB); constraint = createPoint2PointConstraint(*rbA,*rbB,pivotInA,pivotInB); } else { btVector3 pivotInA; pivotInA.deSerializeDouble(p2pData->m_pivotInA); constraint = createPoint2PointConstraint(*rbA,pivotInA); } } else { btPoint2PointConstraintFloatData* p2pData = (btPoint2PointConstraintFloatData*)constraintData; if (rbA&& rbB) { btVector3 pivotInA,pivotInB; pivotInA.deSerializeFloat(p2pData->m_pivotInA); pivotInB.deSerializeFloat(p2pData->m_pivotInB); constraint = createPoint2PointConstraint(*rbA,*rbB,pivotInA,pivotInB); } else { btVector3 pivotInA; pivotInA.deSerializeFloat(p2pData->m_pivotInA); constraint = createPoint2PointConstraint(*rbA,pivotInA); } } break; } case HINGE_CONSTRAINT_TYPE: { btHingeConstraint* hinge = 0; if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION) { btHingeConstraintDoubleData* hingeData = (btHingeConstraintDoubleData*)constraintData; if (rbA&& rbB) { btTransform rbAFrame,rbBFrame; rbAFrame.deSerializeDouble(hingeData->m_rbAFrame); rbBFrame.deSerializeDouble(hingeData->m_rbBFrame); hinge = createHingeConstraint(*rbA,*rbB,rbAFrame,rbBFrame,hingeData->m_useReferenceFrameA!=0); } else { btTransform rbAFrame; rbAFrame.deSerializeDouble(hingeData->m_rbAFrame); hinge = createHingeConstraint(*rbA,rbAFrame,hingeData->m_useReferenceFrameA!=0); } if (hingeData->m_enableAngularMotor) { hinge->enableAngularMotor(true,hingeData->m_motorTargetVelocity,hingeData->m_maxMotorImpulse); } hinge->setAngularOnly(hingeData->m_angularOnly!=0); hinge->setLimit(btScalar(hingeData->m_lowerLimit),btScalar(hingeData->m_upperLimit),btScalar(hingeData->m_limitSoftness),btScalar(hingeData->m_biasFactor),btScalar(hingeData->m_relaxationFactor)); } else { btHingeConstraintFloatData* hingeData = (btHingeConstraintFloatData*)constraintData; if (rbA&& rbB) { btTransform rbAFrame,rbBFrame; rbAFrame.deSerializeFloat(hingeData->m_rbAFrame); rbBFrame.deSerializeFloat(hingeData->m_rbBFrame); hinge = createHingeConstraint(*rbA,*rbB,rbAFrame,rbBFrame,hingeData->m_useReferenceFrameA!=0); } else { btTransform rbAFrame; rbAFrame.deSerializeFloat(hingeData->m_rbAFrame); hinge = createHingeConstraint(*rbA,rbAFrame,hingeData->m_useReferenceFrameA!=0); } if (hingeData->m_enableAngularMotor) { hinge->enableAngularMotor(true,hingeData->m_motorTargetVelocity,hingeData->m_maxMotorImpulse); } hinge->setAngularOnly(hingeData->m_angularOnly!=0); hinge->setLimit(btScalar(hingeData->m_lowerLimit),btScalar(hingeData->m_upperLimit),btScalar(hingeData->m_limitSoftness),btScalar(hingeData->m_biasFactor),btScalar(hingeData->m_relaxationFactor)); } constraint = hinge; break; } case CONETWIST_CONSTRAINT_TYPE: { btConeTwistConstraintData* coneData = (btConeTwistConstraintData*)constraintData; btConeTwistConstraint* coneTwist = 0; if (rbA&& rbB) { btTransform rbAFrame,rbBFrame; rbAFrame.deSerializeFloat(coneData->m_rbAFrame); rbBFrame.deSerializeFloat(coneData->m_rbBFrame); coneTwist = createConeTwistConstraint(*rbA,*rbB,rbAFrame,rbBFrame); } else { btTransform rbAFrame; rbAFrame.deSerializeFloat(coneData->m_rbAFrame); coneTwist = createConeTwistConstraint(*rbA,rbAFrame); } coneTwist->setLimit(coneData->m_swingSpan1,coneData->m_swingSpan2,coneData->m_twistSpan,coneData->m_limitSoftness,coneData->m_biasFactor,coneData->m_relaxationFactor); coneTwist->setDamping(coneData->m_damping); constraint = coneTwist; break; } case D6_CONSTRAINT_TYPE: { btGeneric6DofConstraintData* dofData = (btGeneric6DofConstraintData*)constraintData; btGeneric6DofConstraint* dof = 0; if (rbA&& rbB) { btTransform rbAFrame,rbBFrame; rbAFrame.deSerializeFloat(dofData->m_rbAFrame); rbBFrame.deSerializeFloat(dofData->m_rbBFrame); dof = createGeneric6DofConstraint(*rbA,*rbB,rbAFrame,rbBFrame,dofData->m_useLinearReferenceFrameA!=0); } else { if (rbB) { btTransform rbBFrame; rbBFrame.deSerializeFloat(dofData->m_rbBFrame); dof = createGeneric6DofConstraint(*rbB,rbBFrame,dofData->m_useLinearReferenceFrameA!=0); } else { printf("Error in btWorldImporter::createGeneric6DofConstraint: missing rbB\n"); } } if (dof) { btVector3 angLowerLimit,angUpperLimit, linLowerLimit,linUpperlimit; angLowerLimit.deSerializeFloat(dofData->m_angularLowerLimit); angUpperLimit.deSerializeFloat(dofData->m_angularUpperLimit); linLowerLimit.deSerializeFloat(dofData->m_linearLowerLimit); linUpperlimit.deSerializeFloat(dofData->m_linearUpperLimit); dof->setAngularLowerLimit(angLowerLimit); dof->setAngularUpperLimit(angUpperLimit); dof->setLinearLowerLimit(linLowerLimit); dof->setLinearUpperLimit(linUpperlimit); } constraint = dof; break; } case SLIDER_CONSTRAINT_TYPE: { btSliderConstraintData* sliderData = (btSliderConstraintData*)constraintData; btSliderConstraint* slider = 0; if (rbA&& rbB) { btTransform rbAFrame,rbBFrame; rbAFrame.deSerializeFloat(sliderData->m_rbAFrame); rbBFrame.deSerializeFloat(sliderData->m_rbBFrame); slider = createSliderConstraint(*rbA,*rbB,rbAFrame,rbBFrame,sliderData->m_useLinearReferenceFrameA!=0); } else { btTransform rbBFrame; rbBFrame.deSerializeFloat(sliderData->m_rbBFrame); slider = createSliderConstraint(*rbB,rbBFrame,sliderData->m_useLinearReferenceFrameA!=0); } slider->setLowerLinLimit(sliderData->m_linearLowerLimit); slider->setUpperLinLimit(sliderData->m_linearUpperLimit); slider->setLowerAngLimit(sliderData->m_angularLowerLimit); slider->setUpperAngLimit(sliderData->m_angularUpperLimit); slider->setUseFrameOffset(sliderData->m_useOffsetForConstraintFrame!=0); constraint = slider; break; } default: { printf("unknown constraint type\n"); } }; if (constraint) { constraint->setDbgDrawSize(constraintData->m_dbgDrawSize); if (constraintData->m_name) { char* newname = duplicateName(constraintData->m_name); m_nameConstraintMap.insert(newname,constraint); m_objectNameMap.insert(constraint,newname); } if(m_dynamicsWorld) m_dynamicsWorld->addConstraint(constraint,constraintData->m_disableCollisionsBetweenLinkedBodies!=0); } } return true; }