diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/Featherstone')
16 files changed, 538 insertions, 215 deletions
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp index a1d5bb9ca8..7cb92fa3b4 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp @@ -125,7 +125,8 @@ btMultiBody::btMultiBody(int n_links, m_posVarCnt(0), m_useRK4(false), m_useGlobalVelocities(false), - m_internalNeedsJointFeedback(false) + m_internalNeedsJointFeedback(false), + m_kinematic_calculate_velocity(false) { m_cachedInertiaTopLeft.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0); m_cachedInertiaTopRight.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0); @@ -344,6 +345,8 @@ void btMultiBody::finalizeMultiDof() { m_deltaV.resize(0); m_deltaV.resize(6 + m_dofCount); + m_splitV.resize(0); + m_splitV.resize(6 + m_dofCount); m_realBuf.resize(6 + m_dofCount + m_dofCount * m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels") m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) m_matrixBuf.resize(m_links.size() + 1); @@ -671,6 +674,30 @@ btScalar *btMultiBody::getJointTorqueMultiDof(int i) return &m_links[i].m_jointTorque[0]; } +bool btMultiBody::hasFixedBase() const +{ + return m_fixedBase || (getBaseCollider() && getBaseCollider()->isStaticObject()); +} + +bool btMultiBody::isBaseStaticOrKinematic() const +{ + return m_fixedBase || (getBaseCollider() && getBaseCollider()->isStaticOrKinematicObject()); +} + +bool btMultiBody::isBaseKinematic() const +{ + return getBaseCollider() && getBaseCollider()->isKinematicObject(); +} + +void btMultiBody::setBaseDynamicType(int dynamicType) +{ + if(getBaseCollider()) { + int oldFlags = getBaseCollider()->getCollisionFlags(); + oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT); + getBaseCollider()->setCollisionFlags(oldFlags | dynamicType); + } +} + inline btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1) //renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross? { btVector3 row0 = btVector3( @@ -796,7 +823,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar //create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel); - if (m_fixedBase) + if (isBaseStaticOrKinematic()) { zeroAccSpatFrc[0].setZero(); } @@ -872,31 +899,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 (isLinkAndAllAncestorsKinematic(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) @@ -909,22 +958,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()); @@ -935,6 +968,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar // (part of TreeForwardDynamics in Mirtich.) for (int i = num_links - 1; i >= 0; --i) { + if(isLinkAndAllAncestorsKinematic(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; @@ -1047,7 +1082,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar // Second 'upward' loop // (part of TreeForwardDynamics in Mirtich) - if (m_fixedBase) + if (isBaseStaticOrKinematic()) { spatAcc[0].setZero(); } @@ -1081,21 +1116,23 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]); - for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) + if(!isLinkAndAllAncestorsKinematic(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); - } - - 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]); + 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]; + 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]; + 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) { @@ -1432,7 +1469,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar // Fill in zero_acc // -- set to force/torque on the base, zero otherwise - if (m_fixedBase) + if (isBaseStaticOrKinematic()) { zeroAccSpatFrc[0].setZero(); } @@ -1451,6 +1488,8 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar // (part of TreeForwardDynamics in Mirtich.) for (int i = num_links - 1; i >= 0; --i) { + if(isLinkAndAllAncestorsKinematic(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; @@ -1494,7 +1533,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar // Second 'upward' loop // (part of TreeForwardDynamics in Mirtich) - if (m_fixedBase) + if (isBaseStaticOrKinematic()) { spatAcc[0].setZero(); } @@ -1507,6 +1546,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(isLinkAndAllAncestorsKinematic(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; @@ -1550,23 +1591,26 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar void btMultiBody::predictPositionsMultiDof(btScalar dt) { int num_links = getNumLinks(); - // step position by adding dt * velocity - //btVector3 v = getBaseVel(); - //m_basePos += dt * v; - // - btScalar *pBasePos; - btScalar *pBaseVel = &m_realBuf[3]; //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) - - // reset to current position - for (int i = 0; i < 3; ++i) - { - m_basePos_interpolate[i] = m_basePos[i]; - } - pBasePos = m_basePos_interpolate; + if(!isBaseKinematic()) + { + // step position by adding dt * velocity + //btVector3 v = getBaseVel(); + //m_basePos += dt * v; + // + btScalar *pBasePos; + btScalar *pBaseVel = &m_realBuf[3]; //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) - pBasePos[0] += dt * pBaseVel[0]; - pBasePos[1] += dt * pBaseVel[1]; - pBasePos[2] += dt * pBaseVel[2]; + // reset to current position + for (int i = 0; i < 3; ++i) + { + m_basePos_interpolate[i] = m_basePos[i]; + } + pBasePos = m_basePos_interpolate; + + pBasePos[0] += dt * pBaseVel[0]; + pBasePos[1] += dt * pBaseVel[1]; + pBasePos[2] += dt * pBaseVel[2]; + } /////////////////////////////// //local functor for quaternion integration (to avoid error prone redundancy) @@ -1617,26 +1661,29 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt) //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); // - btScalar *pBaseQuat; - - // reset to current orientation - for (int i = 0; i < 4; ++i) - { - m_baseQuat_interpolate[i] = m_baseQuat[i]; - } - pBaseQuat = m_baseQuat_interpolate; + if(!isBaseKinematic()) + { + btScalar *pBaseQuat; - btScalar *pBaseOmega = &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) - // - btQuaternion baseQuat; - baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); - btVector3 baseOmega; - baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); - pQuatUpdateFun(baseOmega, baseQuat, true, dt); - pBaseQuat[0] = baseQuat.x(); - pBaseQuat[1] = baseQuat.y(); - pBaseQuat[2] = baseQuat.z(); - pBaseQuat[3] = baseQuat.w(); + // reset to current orientation + for (int i = 0; i < 4; ++i) + { + m_baseQuat_interpolate[i] = m_baseQuat[i]; + } + pBaseQuat = m_baseQuat_interpolate; + + btScalar *pBaseOmega = &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) + // + btQuaternion baseQuat; + baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); + btVector3 baseOmega; + baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); + pQuatUpdateFun(baseOmega, baseQuat, true, dt); + pBaseQuat[0] = baseQuat.x(); + pBaseQuat[1] = baseQuat.y(); + pBaseQuat[2] = baseQuat.z(); + pBaseQuat[3] = baseQuat.w(); + } // Finally we can update m_jointPos for each of the m_links for (int i = 0; i < num_links; ++i) @@ -1644,55 +1691,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) - { - 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) + if (m_links[i].m_collider && m_links[i].m_collider->isStaticOrKinematic()) + { + switch (m_links[i].m_jointType) + { + case btMultibodyLink::ePrismatic: + case btMultibodyLink::eRevolute: { - pJointPos[j] = m_links[i].m_jointPos[j]; + pJointPos[0] = m_links[i].m_jointPos[0]; + break; } - - 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) + case btMultibodyLink::eSpherical: { - pJointPos[j] = m_links[i].m_jointPos[j]; + for (int j = 0; j < 4; ++j) + { + pJointPos[j] = m_links[i].m_jointPos[j]; + } + break; } - 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; + case btMultibodyLink::ePlanar: + { + for (int j = 0; j < 3; ++j) + { + pJointPos[j] = m_links[i].m_jointPos[j]; + } + break; + } + default: + break; } - default: + } + else + { + btScalar *pJointVel = getJointVelMultiDof(i); + + switch (m_links[i].m_jointType) { + 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: + { + } } } @@ -1703,16 +1783,19 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt) void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd) { int num_links = getNumLinks(); - // step position by adding dt * velocity - //btVector3 v = getBaseVel(); - //m_basePos += dt * v; - // - btScalar *pBasePos = (pq ? &pq[4] : m_basePos); - btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) - - pBasePos[0] += dt * pBaseVel[0]; - pBasePos[1] += dt * pBaseVel[1]; - pBasePos[2] += dt * pBaseVel[2]; + if(!isBaseKinematic()) + { + // step position by adding dt * velocity + //btVector3 v = getBaseVel(); + //m_basePos += dt * v; + // + btScalar *pBasePos = (pq ? &pq[4] : m_basePos); + btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) + + pBasePos[0] += dt * pBaseVel[0]; + pBasePos[1] += dt * pBaseVel[1]; + pBasePos[2] += dt * pBaseVel[2]; + } /////////////////////////////// //local functor for quaternion integration (to avoid error prone redundancy) @@ -1763,22 +1846,25 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); // - btScalar *pBaseQuat = pq ? pq : m_baseQuat; - btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) - // - btQuaternion baseQuat; - baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); - btVector3 baseOmega; - baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); - pQuatUpdateFun(baseOmega, baseQuat, true, dt); - pBaseQuat[0] = baseQuat.x(); - pBaseQuat[1] = baseQuat.y(); - pBaseQuat[2] = baseQuat.z(); - pBaseQuat[3] = baseQuat.w(); - - //printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z()); - //printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z()); - //printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w()); + if(!isBaseKinematic()) + { + btScalar *pBaseQuat = pq ? pq : m_baseQuat; + btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) + // + btQuaternion baseQuat; + baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); + btVector3 baseOmega; + baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); + pQuatUpdateFun(baseOmega, baseQuat, true, dt); + pBaseQuat[0] = baseQuat.x(); + pBaseQuat[1] = baseQuat.y(); + pBaseQuat[2] = baseQuat.z(); + pBaseQuat[3] = baseQuat.w(); + + //printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z()); + //printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z()); + //printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w()); + } if (pq) pq += 7; @@ -1788,48 +1874,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].m_collider && m_links[i].m_collider->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: + btScalar *pJointPos; + pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]); + + btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i)); + + switch (m_links[i].m_jointType) { - pJointPos[0] += dt * getJointVelMultiDof(i)[0]; + 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; + 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: - { + break; + } + default: + { + } } } @@ -2135,8 +2224,15 @@ void btMultiBody::updateCollisionObjectInterpolationWorldTransforms(btAlignedObj world_to_local.resize(getNumLinks() + 1); local_origin.resize(getNumLinks() + 1); - world_to_local[0] = getInterpolateWorldToBaseRot(); - local_origin[0] = getInterpolateBasePos(); + if(isBaseKinematic()){ + world_to_local[0] = getWorldToBaseRot(); + local_origin[0] = getBasePos(); + } + else + { + world_to_local[0] = getInterpolateWorldToBaseRot(); + local_origin[0] = getInterpolateBasePos(); + } if (getBaseCollider()) { @@ -2282,3 +2378,81 @@ const char *btMultiBody::serialize(void *dataBuffer, class btSerializer *seriali return btMultiBodyDataName; } + +void btMultiBody::saveKinematicState(btScalar timeStep) +{ + //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities + if (m_kinematic_calculate_velocity && timeStep != btScalar(0.)) + { + btVector3 linearVelocity, angularVelocity; + btTransformUtil::calculateVelocity(getInterpolateBaseWorldTransform(), getBaseWorldTransform(), timeStep, linearVelocity, angularVelocity); + setBaseVel(linearVelocity); + setBaseOmega(angularVelocity); + setInterpolateBaseWorldTransform(getBaseWorldTransform()); + } +} + +void btMultiBody::setLinkDynamicType(const int i, int type) +{ + if (i == -1) + { + setBaseDynamicType(type); + } + else if (i >= 0 && i < getNumLinks()) + { + if (m_links[i].m_collider) + { + m_links[i].m_collider->setDynamicType(type); + } + } +} + +bool btMultiBody::isLinkStaticOrKinematic(const int i) const +{ + if (i == -1) + { + return isBaseStaticOrKinematic(); + } + else + { + if (m_links[i].m_collider) + return m_links[i].m_collider->isStaticOrKinematic(); + } + return false; +} + +bool btMultiBody::isLinkKinematic(const int i) const +{ + if (i == -1) + { + return isBaseKinematic(); + } + else + { + if (m_links[i].m_collider) + return m_links[i].m_collider->isKinematic(); + } + return false; +} + +bool btMultiBody::isLinkAndAllAncestorsStaticOrKinematic(const int i) const +{ + int link = i; + while (link != -1) { + if (!isLinkStaticOrKinematic(link)) + return false; + link = m_links[link].m_parent; + } + return isBaseStaticOrKinematic(); +} + +bool btMultiBody::isLinkAndAllAncestorsKinematic(const int i) const +{ + int link = i; + while (link != -1) { + if (!isLinkKinematic(link)) + return false; + link = m_links[link].m_parent; + } + return isBaseKinematic(); +} diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h index be795633fd..5a3efc9414 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h @@ -210,7 +210,13 @@ public: void setBasePos(const btVector3 &pos) { m_basePos = pos; - m_basePos_interpolate = pos; + if(!isBaseKinematic()) + m_basePos_interpolate = pos; + } + + void setInterpolateBasePos(const btVector3 &pos) + { + m_basePos_interpolate = pos; } void setBaseWorldTransform(const btTransform &tr) @@ -227,17 +233,39 @@ public: return tr; } + void setInterpolateBaseWorldTransform(const btTransform &tr) + { + setInterpolateBasePos(tr.getOrigin()); + setInterpolateWorldToBaseRot(tr.getRotation().inverse()); + } + + btTransform getInterpolateBaseWorldTransform() const + { + btTransform tr; + tr.setOrigin(getInterpolateBasePos()); + tr.setRotation(getInterpolateWorldToBaseRot().inverse()); + return tr; + } + void setBaseVel(const btVector3 &vel) { m_realBuf[3] = vel[0]; m_realBuf[4] = vel[1]; m_realBuf[5] = vel[2]; } + void setWorldToBaseRot(const btQuaternion &rot) { m_baseQuat = rot; //m_baseQuat asumed to ba alias!? - m_baseQuat_interpolate = rot; + if(!isBaseKinematic()) + m_baseQuat_interpolate = rot; } + + void setInterpolateWorldToBaseRot(const btQuaternion &rot) + { + m_baseQuat_interpolate = rot; + } + void setBaseOmega(const btVector3 &omega) { m_realBuf[0] = omega[0]; @@ -245,6 +273,8 @@ public: m_realBuf[2] = omega[2]; } + void saveKinematicState(btScalar timeStep); + // // get/set pos/vel for child m_links (i = 0 to num_links-1) // @@ -278,6 +308,11 @@ public: { return &m_deltaV[0]; } + + const btScalar *getSplitVelocityVector() const + { + return &m_splitV[0]; + } /* btScalar * getVelocityVector() { return &real_buf[0]; @@ -397,6 +432,26 @@ public: m_deltaV[dof] += delta_vee[dof] * multiplier; } } + void applyDeltaSplitVeeMultiDof(const btScalar *delta_vee, btScalar multiplier) + { + for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + { + m_splitV[dof] += delta_vee[dof] * multiplier; + } + } + void addSplitV() + { + applyDeltaVeeMultiDof(&m_splitV[0], 1); + } + void substractSplitV() + { + applyDeltaVeeMultiDof(&m_splitV[0], -1); + + for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + { + m_splitV[dof] = 0.f; + } + } void processDeltaVeeMultiDof2() { applyDeltaVeeMultiDof(&m_deltaV[0], 1); @@ -495,14 +550,22 @@ public: void goToSleep(); void checkMotionAndSleepIfRequired(btScalar timestep); - bool hasFixedBase() const - { - return m_fixedBase; - } + bool hasFixedBase() const; + + bool isBaseKinematic() const; + + bool isBaseStaticOrKinematic() const; + + // set the dynamic type in the base's collision flags. + void setBaseDynamicType(int dynamicType); void setFixedBase(bool fixedBase) { m_fixedBase = fixedBase; + if(m_fixedBase) + setBaseDynamicType(btCollisionObject::CF_STATIC_OBJECT); + else + setBaseDynamicType(btCollisionObject::CF_DYNAMIC_OBJECT); } int getCompanionId() const @@ -653,7 +716,15 @@ public: btVector3 &top_out, // top part of output vector btVector3 &bottom_out); // bottom part of output vector + void setLinkDynamicType(const int i, int type); + bool isLinkStaticOrKinematic(const int i) const; + + bool isLinkKinematic(const int i) const; + + bool isLinkAndAllAncestorsStaticOrKinematic(const int i) const; + + bool isLinkAndAllAncestorsKinematic(const int i) const; private: btMultiBody(const btMultiBody &); // not implemented @@ -711,6 +782,7 @@ private: // offset size array // 0 num_links+1 rot_from_parent // + btAlignedObjectArray<btScalar> m_splitV; btAlignedObjectArray<btScalar> m_deltaV; btAlignedObjectArray<btScalar> m_realBuf; btAlignedObjectArray<btVector3> m_vectorBuf; @@ -751,6 +823,9 @@ private: ///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only bool m_internalNeedsJointFeedback; + + //If enabled, calculate the velocity based on kinematic transform changes. Currently only implemented for the base. + bool m_kinematic_calculate_velocity; }; struct btMultiBodyLinkDoubleData diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index d7ed05ce57..1ba5861145 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -2,11 +2,12 @@ #include "BulletDynamics/Dynamics/btRigidBody.h" #include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro) -btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA, btMultiBody* bodyB, int linkA, int linkB, int numRows, bool isUnilateral) +btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA, btMultiBody* bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type) : m_bodyA(bodyA), m_bodyB(bodyB), m_linkA(linkA), m_linkB(linkB), + m_type(type), m_numRows(numRows), m_jacSizeA(0), m_jacSizeBoth(0), diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 5c15f3e851..4a6007ee3e 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -20,6 +20,21 @@ subject to the following restrictions: #include "LinearMath/btAlignedObjectArray.h" #include "btMultiBody.h" + +//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility +enum btTypedMultiBodyConstraintType +{ + MULTIBODY_CONSTRAINT_LIMIT=3, + MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR, + MULTIBODY_CONSTRAINT_GEAR, + MULTIBODY_CONSTRAINT_POINT_TO_POINT, + MULTIBODY_CONSTRAINT_SLIDER, + MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR, + MULTIBODY_CONSTRAINT_FIXED, + + MAX_MULTIBODY_CONSTRAINT_TYPE, +}; + class btMultiBody; struct btSolverInfo; @@ -46,6 +61,8 @@ protected: int m_linkA; int m_linkB; + int m_type; //btTypedMultiBodyConstraintType + int m_numRows; int m_jacSizeA; int m_jacSizeBoth; @@ -82,12 +99,16 @@ protected: public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btMultiBodyConstraint(btMultiBody * bodyA, btMultiBody * bodyB, int linkA, int linkB, int numRows, bool isUnilateral); + btMultiBodyConstraint(btMultiBody * bodyA, btMultiBody * bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type); virtual ~btMultiBodyConstraint(); void updateJacobianSizes(); void allocateJacobiansMultiDof(); + int getConstraintType() const + { + return m_type; + } //many constraints have setFrameInB/setPivotInB. Will use 'getConstraintType' later. virtual void setFrameInB(const btMatrix3x3& frameInB) {} virtual void setPivotInB(const btVector3& pivotInB) {} diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index cd1bad089e..fef95f0c4e 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -592,6 +592,7 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep) if (!isSleeping) { + bod->addSplitV(); int nLinks = bod->getNumLinks(); ///base + num m_links @@ -610,6 +611,7 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep) m_scratch_world_to_local.resize(nLinks + 1); m_scratch_local_origin.resize(nLinks + 1); bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); + bod->substractSplitV(); } else { @@ -867,6 +869,18 @@ void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer) } } } + +void btMultiBodyDynamicsWorld::saveKinematicState(btScalar timeStep) +{ + btDiscreteDynamicsWorld::saveKinematicState(timeStep); + for(int i = 0; i < m_multiBodies.size(); i++) + { + btMultiBody* body = m_multiBodies[i]; + if(body->isBaseKinematic()) + body->saveKinematicState(timeStep); + } +} + // //void btMultiBodyDynamicsWorld::setSplitIslands(bool split) //{ diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index 9ac46f4b64..d2d76c8b92 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -120,5 +120,7 @@ public: virtual void solveExternalForces(btContactSolverInfo& solverInfo); virtual void solveInternalConstraints(btContactSolverInfo& solverInfo); void buildIslands(); + + virtual void saveKinematicState(btScalar timeStep); }; #endif //BT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp index 5ef9444c2f..df2abbe97a 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp @@ -24,7 +24,7 @@ subject to the following restrictions: #define BTMBFIXEDCONSTRAINT_DIM 6 btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) - : btMultiBodyConstraint(body, 0, link, -1, BTMBFIXEDCONSTRAINT_DIM, false), + : btMultiBodyConstraint(body, 0, link, -1, BTMBFIXEDCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_FIXED), m_rigidBodyA(0), m_rigidBodyB(bodyB), m_pivotInA(pivotInA), @@ -36,7 +36,7 @@ btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int li } btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) - : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBFIXEDCONSTRAINT_DIM, false), + : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBFIXEDCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_FIXED), m_rigidBodyA(0), m_rigidBodyB(0), m_pivotInA(pivotInA), diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp index bf6b811d26..ee02cf9b07 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp @@ -21,7 +21,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) - : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, 1, false), + : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, 1, false, MULTIBODY_CONSTRAINT_GEAR), m_gearRatio(1), m_gearAuxLink(-1), m_erp(0), diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp index 8791ad2868..94b36ac108 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -22,7 +22,7 @@ subject to the following restrictions: btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper) //:btMultiBodyConstraint(body,0,link,-1,2,true), - : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 2, true), + : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 2, true, MULTIBODY_CONSTRAINT_LIMIT), m_lowerBound(lower), m_upperBound(upper) { diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h index 6716ba490f..b810692b4c 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h @@ -42,6 +42,22 @@ public: { //todo(erwincoumans) } + btScalar getLowerBound() const + { + return m_lowerBound; + } + btScalar getUpperBound() const + { + return m_upperBound; + } + void setLowerBound(btScalar lower) + { + m_lowerBound = lower; + } + void setUpperBound(btScalar upper) + { + m_upperBound = upper; + } }; #endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index 5c816c4987..fec9b03213 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -21,7 +21,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse) - : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true), + : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR), m_desiredVelocity(desiredVelocity), m_desiredPosition(0), m_kd(1.), @@ -51,7 +51,7 @@ void btMultiBodyJointMotor::finalizeMultiDof() btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse) //:btMultiBodyConstraint(body,0,link,-1,1,true), - : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true), + : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR), m_desiredVelocity(desiredVelocity), m_desiredPosition(0), m_kd(1.), diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h index 01d5583c2f..5a1429340f 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -295,6 +295,9 @@ struct btMultibodyLink } } } + + + }; #endif //BT_MULTIBODY_LINK_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h index bc909990c2..3dc35a5814 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h @@ -130,6 +130,23 @@ public: return true; } + bool isStaticOrKinematic() const + { + return isStaticOrKinematicObject(); + } + + bool isKinematic() const + { + return isKinematicObject(); + } + + void setDynamicType(int dynamicType) + { + int oldFlags = getCollisionFlags(); + oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT); + setCollisionFlags(oldFlags | dynamicType); + } + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp index 37d3aede37..f51e69deb1 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp @@ -27,7 +27,7 @@ subject to the following restrictions: #endif btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB) - : btMultiBodyConstraint(body, 0, link, -1, BTMBP2PCONSTRAINT_DIM, false), + : btMultiBodyConstraint(body, 0, link, -1, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT), m_rigidBodyA(0), m_rigidBodyB(bodyB), m_pivotInA(pivotInA), @@ -37,7 +37,7 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRi } btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB) - : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBP2PCONSTRAINT_DIM, false), + : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT), m_rigidBodyA(0), m_rigidBodyB(0), m_pivotInA(pivotInA), diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp index e025302ce6..48ec1d5af2 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp @@ -25,7 +25,7 @@ subject to the following restrictions: #define EPSILON 0.000001 btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis) - : btMultiBodyConstraint(body, 0, link, -1, BTMBSLIDERCONSTRAINT_DIM, false), + : btMultiBodyConstraint(body, 0, link, -1, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER), m_rigidBodyA(0), m_rigidBodyB(bodyB), m_pivotInA(pivotInA), @@ -38,7 +38,7 @@ btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int } btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis) - : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false), + : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER), m_rigidBodyA(0), m_rigidBodyB(0), m_pivotInA(pivotInA), diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp index 3e5aa30f28..25ddd539bf 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp @@ -23,7 +23,7 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse) - : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true), + : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true, MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR), m_desiredVelocity(0, 0, 0), m_desiredPosition(0,0,0,1), m_kd(1.), |