btMultiBody* MultiDofDemo::createFeatherstoneMultiBody_testMultiDof(btMultiBodyDynamicsWorld *pWorld, int numLinks, const btVector3 &basePosition, const btVector3 &baseHalfExtents, const btVector3 &linkHalfExtents, bool spherical, bool floating) { //init the base btVector3 baseInertiaDiag(0.f, 0.f, 0.f); float baseMass = 1.f; if(baseMass) { btCollisionShape *pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag); delete pTempBox; } bool canSleep = false; btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep); btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); pMultiBody->setBasePos(basePosition); pMultiBody->setWorldToBaseRot(baseOriQuat); btVector3 vel(0, 0, 0); // pMultiBody->setBaseVel(vel); //init the links btVector3 hingeJointAxis(1, 0, 0); float linkMass = 1.f; btVector3 linkInertiaDiag(0.f, 0.f, 0.f); btCollisionShape *pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2])); pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag); delete pTempBox; //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) { if(!spherical) pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true); 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, true); } pMultiBody->finalizeMultiDof(); /// pWorld->addMultiBody(pMultiBody); /// return pMultiBody; }
void llquat_test_object_t::test<6>() { LLQuaternion quat1(3.0f, 2.0f, 6.0f, 0.0f), quat2(1.0f, 1.0f, 1.0f, 1.0f); ensure("1. The two values are different", llround(12.000000f, 2) == llround(dot(quat1, quat2), 2)); LLQuaternion quat0(3.0f, 9.334f, 34.5f, 23.0f), quat(34.5f, 23.23f, 2.0f, 45.5f); ensure("2. The two values are different", llround(1435.828807f, 2) == llround(dot(quat0, quat), 2)); }
void llquat_test_object_t::test<7>() { F32 radian = 60.0f; LLQuaternion quat(3.0f, 2.0f, 6.0f, 0.0f); LLQuaternion quat1; quat1 = quat.constrain(radian); ensure("1. LLQuaternion::constrain(F32 radians) failed", is_approx_equal_fraction(-0.423442f, quat1.mQ[0], 8) && is_approx_equal_fraction(-0.282295f, quat1.mQ[1], 8) && is_approx_equal_fraction(-0.846884f, quat1.mQ[2], 8) && is_approx_equal_fraction(0.154251f, quat1.mQ[3], 8)); radian = 30.0f; LLQuaternion quat0(37.50f, 12.0f, 86.023f, 40.32f); quat1 = quat0.constrain(radian); ensure("2. LLQuaternion::constrain(F32 radians) failed", is_approx_equal_fraction(37.500000f, quat1.mQ[0], 8) && is_approx_equal_fraction(12.0000f, quat1.mQ[1], 8) && is_approx_equal_fraction(86.0230f, quat1.mQ[2], 8) && is_approx_equal_fraction(40.320000f, quat1.mQ[3], 8)); }
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); } }
void MultiDofDemo::initPhysics() { m_guiHelper->setUpAxis(1); if(g_firstInit) { m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraDistance(btScalar(10.*scaling)); m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraPitch(50); g_firstInit = false; } ///collision configuration contains default setup for memory, collision setup m_collisionConfiguration = new btDefaultCollisionConfiguration(); ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); m_broadphase = new btDbvtBroadphase(); //Use the btMultiBodyConstraintSolver for Featherstone btMultiBody support btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver; m_solver = sol; //use btMultiBodyDynamicsWorld for Featherstone btMultiBody support btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher,m_broadphase,sol,m_collisionConfiguration); m_dynamicsWorld = world; // m_dynamicsWorld->setDebugDrawer(&gDebugDraw); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_dynamicsWorld->setGravity(btVector3(0,-10,0)); ///create a few basic rigid bodies btVector3 groundHalfExtents(50,50,50); btCollisionShape* groundShape = new btBoxShape(groundHalfExtents); //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,00)); ///////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////// bool damping = true; bool gyro = true; int numLinks = 5; bool spherical = true; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals bool multibodyOnly = false; bool canSleep = true; bool selfCollide = true; bool multibodyConstraint = false; btVector3 linkHalfExtents(0.05, 0.37, 0.1); btVector3 baseHalfExtents(0.05, 0.37, 0.1); btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(world, numLinks, btVector3(-0.4f, 3.f, 0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase); //mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm g_floatingBase = ! g_floatingBase; 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, -9.81 ,0)); //m_dynamicsWorld->getSolverInfo().m_numIterations = 100; ////////////////////////////////////////////// if(numLinks > 0) { btScalar q0 = 45.f * SIMD_PI/ 180.f; if(!spherical) { mbC->setJointPosMultiDof(0, &q0); } else { btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0); quat0.normalize(); mbC->setJointPosMultiDof(0, quat0); } } /// addColliders_testMultiDof(mbC, world, baseHalfExtents, linkHalfExtents); ///////////////////////////////////////////////////////////////// btScalar groundHeight = -51.55; if (!multibodyOnly) { btScalar mass(0.); //rigidbody is dynamic if and only if mass is non zero, otherwise static bool isDynamic = (mass != 0.f); btVector3 localInertia(0,0,0); if (isDynamic) groundShape->calculateLocalInertia(mass,localInertia); //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects groundTransform.setIdentity(); groundTransform.setOrigin(btVector3(0,groundHeight,0)); btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia); btRigidBody* body = new btRigidBody(rbInfo); //add the body to the dynamics world m_dynamicsWorld->addRigidBody(body,1,1+2);//,1,1+2); } ///////////////////////////////////////////////////////////////// if(!multibodyOnly) { btVector3 halfExtents(.5,.5,.5); btBoxShape* colShape = new btBoxShape(halfExtents); //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); startTransform.setOrigin(btVector3( btScalar(0.0), 0.0, btScalar(0.0))); //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); m_dynamicsWorld->addRigidBody(body);//,1,1+2); if (multibodyConstraint) { btVector3 pointInA = -linkHalfExtents; // btVector3 pointInB = halfExtents; btMatrix3x3 frameInA; btMatrix3x3 frameInB; frameInA.setIdentity(); frameInB.setIdentity(); btVector3 jointAxis(1.0,0.0,0.0); //btMultiBodySliderConstraint* p2p = new btMultiBodySliderConstraint(mbC,numLinks-1,body,pointInA,pointInB,frameInA,frameInB,jointAxis); btMultiBodyFixedConstraint* p2p = new btMultiBodyFixedConstraint(mbC,numLinks-1,mbC,numLinks-4,pointInA,pointInA,frameInA,frameInB); p2p->setMaxAppliedImpulse(2.0); m_dynamicsWorld->addMultiBodyConstraint(p2p); } } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); ///////////////////////////////////////////////////////////////// }
void NewtonEulerFrom1DLocalFrameR::computeJachq(double time, Interaction& inter, SP::BlockVector q0) { DEBUG_BEGIN("NewtonEulerFrom1DLocalFrameR::computeJachq(double time, Interaction& inter, SP::BlockVector q0 ) \n"); DEBUG_PRINTF("with time = %f\n",time); DEBUG_PRINTF("with inter = %p\n",&inter); _jachq->setValue(0, 0, _Nc->getValue(0)); _jachq->setValue(0, 1, _Nc->getValue(1)); _jachq->setValue(0, 2, _Nc->getValue(2)); if (inter.has2Bodies()) { _jachq->setValue(0, 7, -_Nc->getValue(0)); _jachq->setValue(0, 8, -_Nc->getValue(1)); _jachq->setValue(0, 9, -_Nc->getValue(2)); } for (unsigned int iDS =0 ; iDS < q0->getNumberOfBlocks() ; iDS++) { SP::SiconosVector q = (q0->getAllVect())[iDS]; double sign = 1.0; DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq : ds%d->q :", iDS); DEBUG_EXPR_WE(q->display();); ::boost::math::quaternion<double> quatGP; if (iDS == 0) { ::boost::math::quaternion<double> quatAux(0, _Pc1->getValue(0) - q->getValue(0), _Pc1->getValue(1) - q->getValue(1), _Pc1->getValue(2) - q->getValue(2)); quatGP = quatAux; } else { sign = -1.0; //cout<<"NewtonEulerFrom1DLocalFrameR::computeJachq sign is -1 \n"; ::boost::math::quaternion<double> quatAux(0, _Pc2->getValue(0) - q->getValue(0), _Pc2->getValue(1) - q->getValue(1), _Pc2->getValue(2) - q->getValue(2)); quatGP = quatAux; } DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq :GP :%lf, %lf, %lf\n", quatGP.R_component_2(), quatGP.R_component_3(), quatGP.R_component_4()); DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq :Q :%e,%e, %e, %e\n", q->getValue(3), q->getValue(4), q->getValue(5), q->getValue(6)); ::boost::math::quaternion<double> quatQ(q->getValue(3), q->getValue(4), q->getValue(5), q->getValue(6)); ::boost::math::quaternion<double> quatcQ(q->getValue(3), -q->getValue(4), -q->getValue(5), -q->getValue(6)); ::boost::math::quaternion<double> quat0(1, 0, 0, 0); ::boost::math::quaternion<double> quatBuff; ::boost::math::quaternion<double> _2qiquatGP; _2qiquatGP = quatGP; _2qiquatGP *= 2 * (q->getValue(3)); quatBuff = (quatGP * quatQ) + (quatcQ * quatGP) - _2qiquatGP; DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq :quattBuuf : %e,%e,%e \n", quatBuff.R_component_2(), quatBuff.R_component_3(), quatBuff.R_component_4()); _jachq->setValue(0, 7 * iDS + 3, sign * (quatBuff.R_component_2()*_Nc->getValue(0) + quatBuff.R_component_3()*_Nc->getValue(1) + quatBuff.R_component_4()*_Nc->getValue(2))); //cout<<"WARNING NewtonEulerFrom1DLocalFrameR set jachq \n"; //_jachq->setValue(0,7*iDS+3,0); for (unsigned int i = 1; i < 4; i++) { ::boost::math::quaternion<double> quatei(0, (i == 1) ? 1 : 0, (i == 2) ? 1 : 0, (i == 3) ? 1 : 0); _2qiquatGP = quatGP; _2qiquatGP *= 2 * (q->getValue(3 + i)); quatBuff = quatei * quatcQ * quatGP - quatGP * quatQ * quatei - _2qiquatGP; _jachq->setValue(0, 7 * iDS + 3 + i, sign * (quatBuff.R_component_2()*_Nc->getValue(0) + quatBuff.R_component_3()*_Nc->getValue(1) + quatBuff.R_component_4()*_Nc->getValue(2))); } }
void TestJointTorqueSetup::initPhysics() { int upAxis = 2; 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 (0) { 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]=-1.5; start.setOrigin(groundOrigin); btRigidBody* body = createRigidBody(0,start,box); btVector4 color = colors[curColor]; curColor++; curColor&=3; m_guiHelper->createRigidBodyGraphicsObject(body,color); } { bool floating = false; bool damping = true; bool gyro = true; int numLinks = 5; 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 *pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag); delete pTempBox; } bool isMultiDof = false; btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof); m_multiBody = pMultiBody; btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); pMultiBody->setBasePos(basePosition); pMultiBody->setWorldToBaseRot(baseOriQuat); btVector3 vel(0, 0, 0); // pMultiBody->setBaseVel(vel); //init the links btVector3 hingeJointAxis(1, 0, 0); float linkMass = 1.f; btVector3 linkInertiaDiag(0.f, 0.f, 0.f); btCollisionShape *pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2])); pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag); delete pTempBox; //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) { if(!spherical) pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, 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(); 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); } // btVector3 gravity(0,0,0); //gravity[upAxis] = -9.81; m_dynamicsWorld->setGravity(gravity); ////////////////////////////////////////////// if(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}; float 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* box = new btBoxShape(baseHalfExtents); m_guiHelper->createCollisionShapeGraphicsObject(box); btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1); col->setCollisionShape(box); 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]); tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); col->setWorldTransform(tr); world->addCollisionObject(col, 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}; float 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* box = new btBoxShape(linkHalfExtents); m_guiHelper->createCollisionShapeGraphicsObject(box); btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i); col->setCollisionShape(box); btTransform tr; tr.setIdentity(); tr.setOrigin(posr); tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); col->setWorldTransform(tr); col->setFriction(friction); world->addCollisionObject(col,2,1+2); btVector4 color = colors[curColor]; curColor++; curColor&=3; m_guiHelper->createCollisionObjectGraphicsObject(col,color); pMultiBody->getLink(i).m_collider=col; } } }