diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index cd1f7746a..bb95483b1 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -197,6 +197,7 @@ project "App_BulletExampleBrowser" "../MultiBody/MultiBodySoftContact.cpp", "../MultiBody/MultiBodyConstraintFeedback.cpp", "../MultiBody/InvertedPendulumPDControl.cpp", + "../MultiBody/KinematicMultiBodyExample.cpp", "../RigidBody/RigidBodySoftContact.cpp", "../RigidBody/KinematicRigidBodyExample.cpp", "../ThirdPartyLibs/stb_image/stb_image.cpp", diff --git a/examples/MultiBody/KinematicMultiBodyExample.cpp b/examples/MultiBody/KinematicMultiBodyExample.cpp index 3daeedca8..aad4845d6 100644 --- a/examples/MultiBody/KinematicMultiBodyExample.cpp +++ b/examples/MultiBody/KinematicMultiBodyExample.cpp @@ -37,6 +37,14 @@ void kinematicPreTickCallback(btDynamicsWorld* world, btScalar deltaTime) btVector3 angularVelocity(0, 0.1, 0); btTransformUtil::integrateTransform(groundBody->getBaseWorldTransform(), linearVelocity, angularVelocity, deltaTime, predictedTrans); groundBody->setBaseWorldTransform(predictedTrans); + + static float time = 0.0; + time += deltaTime; + double old_joint_pos = groundBody->getJointPos(0); + double joint_pos = 0.5 * sin(time * 3.0 - 0.3); + double joint_vel = (joint_pos - old_joint_pos) / deltaTime; + groundBody->setJointPosMultiDof(0, &joint_pos); + groundBody->setJointVelMultiDof(0, &joint_vel); } struct KinematicMultiBodyExample : public CommonMultiBodyBase @@ -79,25 +87,21 @@ void KinematicMultiBodyExample::initPhysics() if (m_dynamicsWorld->getDebugDrawer()) m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints); - ///create a few basic rigid bodies - btScalar halfExtentsX = 10.0; - btScalar halfExtentsY = 0.1; - btScalar halfExtentsZ = 10.0; - + ///create a kinematic multibody btBoxShape* groundShape = createBoxShape(btVector3(btScalar(10.), btScalar(0.1), btScalar(10.))); - btTransform groundTransform; - groundTransform.setIdentity(); - groundTransform.setOrigin(btVector3(0, -halfExtentsY, 0)); m_collisionShapes.push_back(groundShape); - + btBoxShape* secondLevelShape = createBoxShape(btVector3(btScalar(0.5), btScalar(0.1), btScalar(0.5))); + m_collisionShapes.push_back(secondLevelShape); { bool floating = false; - int numLinks = 0; + int numLinks = 1; bool canSleep = false; btVector3 baseInertiaDiag(0.f, 0.f, 0.f); float baseMass = 1.f; + btVector3 secondLevelInertiaDiag(0.f, 0.f, 0.f); + float secondLevelMass = 0.1f; if (baseMass) { @@ -105,25 +109,42 @@ void KinematicMultiBodyExample::initPhysics() pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag); delete pTempBox; } + if (secondLevelMass) + { + btCollisionShape* pTempBox = new btBoxShape(btVector3(0.5, 0.5, 0.5)); + pTempBox->calculateLocalInertia(secondLevelMass, secondLevelInertiaDiag); + delete pTempBox; + } btTransform startTransform; startTransform.setIdentity(); m_groundBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep); m_groundBody->setBasePos(startTransform.getOrigin()); m_groundBody->setWorldToBaseRot(startTransform.getRotation()); + + //init the child link - second level. + btVector3 hingeJointAxis(0, 1, 0); + m_groundBody->setupRevolute(0, secondLevelMass, secondLevelInertiaDiag, -1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, btVector3(0, 0.5, 0), btVector3(0, 0, 0), true); + m_groundBody->finalizeMultiDof(); m_dynamicsWorld->addMultiBody(m_groundBody); + // add collision geometries + bool isDynamic = false; // Kinematic is not treated as dynamic here. + int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter); + int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(m_groundBody, -1); col->setCollisionShape(groundShape); - bool isDynamic = (baseMass > 0 && floating); - int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter); - int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); m_dynamicsWorld->addCollisionObject(col, collisionFilterGroup, collisionFilterMask); //, 2,1+2); m_groundBody->setBaseCollider(col); m_groundBody->setBaseDynamicType(btCollisionObject::CF_KINEMATIC_OBJECT); + btMultiBodyLinkCollider* secondLevelCol = new btMultiBodyLinkCollider(m_groundBody, 0); + secondLevelCol->setCollisionShape(secondLevelShape); + m_dynamicsWorld->addCollisionObject(secondLevelCol, collisionFilterGroup, collisionFilterMask); + m_groundBody->getLink(0).m_collider = secondLevelCol; + m_groundBody->setLinkDynamicType(0, btCollisionObject::CF_KINEMATIC_OBJECT); } m_dynamicsWorld->setInternalTickCallback(kinematicPreTickCallback, m_groundBody, true); diff --git a/src/BulletDynamics/CMakeLists.txt b/src/BulletDynamics/CMakeLists.txt index 3332440f2..b85e3851b 100644 --- a/src/BulletDynamics/CMakeLists.txt +++ b/src/BulletDynamics/CMakeLists.txt @@ -38,6 +38,7 @@ SET(BulletDynamics_SRCS Featherstone/btMultiBodyGearConstraint.cpp Featherstone/btMultiBodyJointLimitConstraint.cpp Featherstone/btMultiBodyJointMotor.cpp + Featherstone/btMultiBodyLink.cpp Featherstone/btMultiBodyMLCPConstraintSolver.cpp Featherstone/btMultiBodyPoint2Point.cpp Featherstone/btMultiBodySliderConstraint.cpp diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 8c361cf8b..b98e8d5e5 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -898,31 +898,53 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar // calculate zhat_i^A // - //external forces - btVector3 linkAppliedForce = isConstraintPass ? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce; - btVector3 linkAppliedTorque = isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque; + if (isLinkStaticOrKinematic(i)) + { + zeroAccSpatFrc[i].setZero(); + } + else{ + //external forces + btVector3 linkAppliedForce = isConstraintPass ? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce; + btVector3 linkAppliedTorque = isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque; - zeroAccSpatFrc[i + 1].setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce)); + zeroAccSpatFrc[i + 1].setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce)); #if 0 - { + { - b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f", - i+1, - zeroAccSpatFrc[i+1].m_topVec[0], - zeroAccSpatFrc[i+1].m_topVec[1], - zeroAccSpatFrc[i+1].m_topVec[2], + b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f", + i+1, + zeroAccSpatFrc[i+1].m_topVec[0], + zeroAccSpatFrc[i+1].m_topVec[1], + zeroAccSpatFrc[i+1].m_topVec[2], - zeroAccSpatFrc[i+1].m_bottomVec[0], - zeroAccSpatFrc[i+1].m_bottomVec[1], - zeroAccSpatFrc[i+1].m_bottomVec[2]); - } + zeroAccSpatFrc[i+1].m_bottomVec[0], + zeroAccSpatFrc[i+1].m_bottomVec[1], + zeroAccSpatFrc[i+1].m_bottomVec[2]); + } #endif - // - //adding damping terms (only) - btScalar linDampMult = 1., angDampMult = 1.; - zeroAccSpatFrc[i + 1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i + 1].getAngular().safeNorm()), - linDampMult * m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm())); + // + //adding damping terms (only) + btScalar linDampMult = 1., angDampMult = 1.; + zeroAccSpatFrc[i + 1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i + 1].getAngular().safeNorm()), + linDampMult * m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm())); + //p += vhat x Ihat vhat - done in a simpler way + if (m_useGyroTerm) + zeroAccSpatFrc[i + 1].addAngular(spatVel[i + 1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular())); + // + zeroAccSpatFrc[i + 1].addLinear(m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear())); + // + //btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()); + ////clamp parent's omega + //btScalar parOmegaMod = temp.length(); + //btScalar parOmegaModMax = 1000; + //if(parOmegaMod > parOmegaModMax) + // temp *= parOmegaModMax / parOmegaMod; + //zeroAccSpatFrc[i+1].addLinear(temp); + //printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length()); + //temp = spatCoriolisAcc[i].getLinear(); + //printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length()); + } // calculate Ihat_i^A //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs) @@ -935,22 +957,6 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0, 0, m_links[i].m_inertiaLocal[1], 0, 0, 0, m_links[i].m_inertiaLocal[2])); - // - //p += vhat x Ihat vhat - done in a simpler way - if (m_useGyroTerm) - zeroAccSpatFrc[i + 1].addAngular(spatVel[i + 1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular())); - // - zeroAccSpatFrc[i + 1].addLinear(m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear())); - //btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()); - ////clamp parent's omega - //btScalar parOmegaMod = temp.length(); - //btScalar parOmegaModMax = 1000; - //if(parOmegaMod > parOmegaModMax) - // temp *= parOmegaModMax / parOmegaMod; - //zeroAccSpatFrc[i+1].addLinear(temp); - //printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length()); - //temp = spatCoriolisAcc[i].getLinear(); - //printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length()); //printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z()); //printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z()); @@ -961,6 +967,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar // (part of TreeForwardDynamics in Mirtich.) for (int i = num_links - 1; i >= 0; --i) { + if(isLinkStaticOrKinematic(i)) + continue; const int parent = m_links[i].m_parent; fromParent.m_rotMat = rot_from_parent[i + 1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; @@ -1107,22 +1115,24 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]); - for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) + if(!isLinkStaticOrKinematic(i)) { - const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; - // - Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof); + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + // + Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof); + } + btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset]; + //D^{-1} * (Y - h^{T}*apar) + mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]); + + spatAcc[i + 1] += spatCoriolisAcc[i]; + + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) + spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof]; } - btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset]; - //D^{-1} * (Y - h^{T}*apar) - mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]); - - spatAcc[i + 1] += spatCoriolisAcc[i]; - - for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) - spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof]; - if (m_links[i].m_jointFeedback) { m_internalNeedsJointFeedback = true; @@ -1477,6 +1487,8 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar // (part of TreeForwardDynamics in Mirtich.) for (int i = num_links - 1; i >= 0; --i) { + if(isLinkStaticOrKinematic(i)) + continue; const int parent = m_links[i].m_parent; fromParent.m_rotMat = rot_from_parent[i + 1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; @@ -1533,6 +1545,8 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar // now do the loop over the m_links for (int i = 0; i < num_links; ++i) { + if(isLinkStaticOrKinematic(i)) + continue; const int parent = m_links[i].m_parent; fromParent.m_rotMat = rot_from_parent[i + 1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; @@ -1676,55 +1690,88 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt) btScalar *pJointPos; pJointPos = &m_links[i].m_jointPos_interpolate[0]; - btScalar *pJointVel = getJointVelMultiDof(i); - - switch (m_links[i].m_jointType) + if(m_links[i].isStaticOrKinematic()) + { + switch (m_links[i].m_jointType) + { + case btMultibodyLink::ePrismatic: + case btMultibodyLink::eRevolute: + { + pJointPos[0] = m_links[i].m_jointPos[0]; + break; + } + case btMultibodyLink::eSpherical: + { + for (int j = 0; j < 4; ++j) + { + pJointPos[j] = m_links[i].m_jointPos[j]; + } + break; + } + case btMultibodyLink::ePlanar: + { + for (int j = 0; j < 3; ++j) + { + pJointPos[j] = m_links[i].m_jointPos[j]; + } + break; + } + default: + break; + } + } + else { - case btMultibodyLink::ePrismatic: - case btMultibodyLink::eRevolute: - { - //reset to current pos - pJointPos[0] = m_links[i].m_jointPos[0]; - btScalar jointVel = pJointVel[0]; - pJointPos[0] += dt * jointVel; - break; - } - case btMultibodyLink::eSpherical: - { - //reset to current pos + btScalar *pJointVel = getJointVelMultiDof(i); - for (int j = 0; j < 4; ++j) - { - pJointPos[j] = m_links[i].m_jointPos[j]; - } - - btVector3 jointVel; - jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); - btQuaternion jointOri; - jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); - pQuatUpdateFun(jointVel, jointOri, false, dt); - pJointPos[0] = jointOri.x(); - pJointPos[1] = jointOri.y(); - pJointPos[2] = jointOri.z(); - pJointPos[3] = jointOri.w(); - break; - } - case btMultibodyLink::ePlanar: + switch (m_links[i].m_jointType) { - for (int j = 0; j < 3; ++j) + case btMultibodyLink::ePrismatic: + case btMultibodyLink::eRevolute: + { + //reset to current pos + pJointPos[0] = m_links[i].m_jointPos[0]; + btScalar jointVel = pJointVel[0]; + pJointPos[0] += dt * jointVel; + break; + } + case btMultibodyLink::eSpherical: + { + //reset to current pos + + for (int j = 0; j < 4; ++j) + { + pJointPos[j] = m_links[i].m_jointPos[j]; + } + + btVector3 jointVel; + jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); + btQuaternion jointOri; + jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); + pQuatUpdateFun(jointVel, jointOri, false, dt); + pJointPos[0] = jointOri.x(); + pJointPos[1] = jointOri.y(); + pJointPos[2] = jointOri.z(); + pJointPos[3] = jointOri.w(); + break; + } + case btMultibodyLink::ePlanar: + { + for (int j = 0; j < 3; ++j) + { + pJointPos[j] = m_links[i].m_jointPos[j]; + } + pJointPos[0] += dt * getJointVelMultiDof(i)[0]; + + btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); + btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2); + pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt; + pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt; + break; + } + default: { - pJointPos[j] = m_links[i].m_jointPos[j]; } - pJointPos[0] += dt * getJointVelMultiDof(i)[0]; - - btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); - btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2); - pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt; - pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt; - break; - } - default: - { } } @@ -1826,48 +1873,51 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd // Finally we can update m_jointPos for each of the m_links for (int i = 0; i < num_links; ++i) { - btScalar *pJointPos; - pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]); - - btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i)); - - switch (m_links[i].m_jointType) + if(!m_links[i].isStaticOrKinematic()) { - case btMultibodyLink::ePrismatic: - case btMultibodyLink::eRevolute: - { - //reset to current pos - btScalar jointVel = pJointVel[0]; - pJointPos[0] += dt * jointVel; - break; - } - case btMultibodyLink::eSpherical: - { - //reset to current pos - btVector3 jointVel; - jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); - btQuaternion jointOri; - jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); - pQuatUpdateFun(jointVel, jointOri, false, dt); - pJointPos[0] = jointOri.x(); - pJointPos[1] = jointOri.y(); - pJointPos[2] = jointOri.z(); - pJointPos[3] = jointOri.w(); - break; - } - case btMultibodyLink::ePlanar: - { - pJointPos[0] += dt * getJointVelMultiDof(i)[0]; + btScalar *pJointPos; + pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]); + + btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i)); - btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); - btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2); - pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt; - pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt; - - break; - } - default: + switch (m_links[i].m_jointType) { + case btMultibodyLink::ePrismatic: + case btMultibodyLink::eRevolute: + { + //reset to current pos + btScalar jointVel = pJointVel[0]; + pJointPos[0] += dt * jointVel; + break; + } + case btMultibodyLink::eSpherical: + { + //reset to current pos + btVector3 jointVel; + jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); + btQuaternion jointOri; + jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); + pQuatUpdateFun(jointVel, jointOri, false, dt); + pJointPos[0] = jointOri.x(); + pJointPos[1] = jointOri.y(); + pJointPos[2] = jointOri.z(); + pJointPos[3] = jointOri.w(); + break; + } + case btMultibodyLink::ePlanar: + { + pJointPos[0] += dt * getJointVelMultiDof(i)[0]; + + btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); + btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2); + pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt; + pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt; + + break; + } + default: + { + } } } diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index cf965047e..94e9b317f 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -716,6 +716,29 @@ public: btVector3 &top_out, // top part of output vector btVector3 &bottom_out); // bottom part of output vector + void setLinkDynamicType(const int i, int type) + { + if(i == -1) + { + setBaseDynamicType(type); + } + else if (i >= 0 && i < getNumLinks()) + { + m_links[i].setDynamicType(type); + } + } + + bool isLinkStaticOrKinematic(const int i) const + { + if(i == -1) + { + return isBaseStaticOrKinematic(); + } + else + { + return m_links[i].isStaticOrKinematic(); + } + } private: diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.cpp b/src/BulletDynamics/Featherstone/btMultiBodyLink.cpp new file mode 100644 index 000000000..cd508b344 --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.cpp @@ -0,0 +1,31 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btMultiBodyLink.h" +#include "btMultiBodyLinkCollider.h" + +bool btMultibodyLink::isStaticOrKinematic() const +{ + return m_collider && m_collider->isStaticOrKinematicObject(); +} + +void btMultibodyLink::setDynamicType(int dynamicType) +{ + if(m_collider) { + int oldFlags = m_collider->getCollisionFlags(); + oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT); + m_collider->setCollisionFlags(oldFlags | dynamicType); + } +} diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index 01d5583c2..23cd2ba4f 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -295,6 +295,9 @@ struct btMultibodyLink } } } + + bool isStaticOrKinematic() const; + void setDynamicType(int dynamicType); }; #endif //BT_MULTIBODY_LINK_H diff --git a/src/btBulletDynamicsAll.cpp b/src/btBulletDynamicsAll.cpp index a8069e30a..de182fa51 100644 --- a/src/btBulletDynamicsAll.cpp +++ b/src/btBulletDynamicsAll.cpp @@ -30,6 +30,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.cpp" #include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp" +#include "BulletDynamics/Featherstone/btMultiBodyLink.cpp" #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp" #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp" #include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp"