diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics')
23 files changed, 651 insertions, 242 deletions
diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp index 27f76b8425..0f5ed1c2ce 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp @@ -852,7 +852,7 @@ static void setupSpatialGridBatchesMt( memHelper.addChunk((void**)&constraintRowBatchIds, sizeof(int) * numConstraintRows); size_t scratchSize = memHelper.getSizeToAllocate(); // if we need to reallocate - if (scratchMemory->capacity() < scratchSize) + if (static_cast<size_t>(scratchMemory->capacity()) < scratchSize) { // allocate 6.25% extra to avoid repeated reallocs scratchMemory->reserve(scratchSize + scratchSize / 16); diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 4356c12abf..3316403a87 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -47,6 +47,8 @@ struct btContactSolverInfoData btScalar m_erp; //error reduction for non-contact constraints btScalar m_erp2; //error reduction for contact constraints btScalar m_deformable_erp; //error reduction for deformable constraints + btScalar m_deformable_cfm; //constraint force mixing for deformable constraints + btScalar m_deformable_maxErrorReduction; // maxErrorReduction for deformable contact btScalar m_globalCfm; //constraint force mixing for contacts and non-contacts btScalar m_frictionERP; //error reduction for friction constraints btScalar m_frictionCFM; //constraint force mixing for friction constraints @@ -83,7 +85,9 @@ struct btContactSolverInfo : public btContactSolverInfoData m_numIterations = 10; m_erp = btScalar(0.2); m_erp2 = btScalar(0.2); - m_deformable_erp = btScalar(0.1); + m_deformable_erp = btScalar(0.06); + m_deformable_cfm = btScalar(0.01); + m_deformable_maxErrorReduction = btScalar(0.1); m_globalCfm = btScalar(0.); m_frictionERP = btScalar(0.2); //positional friction 'anchors' are disabled by default m_frictionCFM = btScalar(0.); diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index cac5302a73..cac5302a73 100755..100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h index 75ca34e978..75ca34e978 100755..100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h index 943d724cce..7442dd1e6a 100644 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h +++ b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h @@ -356,12 +356,12 @@ public: } } - btVector3 getPushVelocity() + btVector3 getPushVelocity() const { return m_pushVelocity; } - btVector3 getTurnVelocity() + btVector3 getTurnVelocity() const { return m_turnVelocity; } @@ -465,6 +465,12 @@ public: //for kinematic objects, we could also use use: // return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep; } + + btVector3 getPushVelocityInLocalPoint(const btVector3& rel_pos) const + { + //we also calculate lin/ang velocity for kinematic objects + return m_pushVelocity + m_turnVelocity.cross(rel_pos); + } void translate(const btVector3& v) { diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp index a1d5bb9ca8..d7588aedc8 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp @@ -33,8 +33,8 @@ namespace { -const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2) -const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds +const btScalar INITIAL_SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2) +const btScalar INITIAL_SLEEP_TIMEOUT = btScalar(2); // in seconds } // namespace void btMultiBody::spatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame @@ -110,6 +110,9 @@ btMultiBody::btMultiBody(int n_links, m_canSleep(canSleep), m_canWakeup(true), m_sleepTimer(0), + m_sleepEpsilon(INITIAL_SLEEP_EPSILON), + m_sleepTimeout(INITIAL_SLEEP_TIMEOUT), + m_userObjectPointer(0), m_userIndex2(-1), m_userIndex(-1), @@ -125,7 +128,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 +348,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 +677,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 +826,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 +902,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 +961,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 +971,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 +1085,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar // Second 'upward' loop // (part of TreeForwardDynamics in Mirtich) - if (m_fixedBase) + if (isBaseStaticOrKinematic()) { spatAcc[0].setZero(); } @@ -1081,21 +1119,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) { @@ -1374,7 +1414,7 @@ void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionV } } -void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const +void btMultiBody::mulMatrix(const btScalar *pA, const btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const { for (int row = 0; row < rowsA; row++) { @@ -1432,7 +1472,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 +1491,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 +1536,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 +1549,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 +1594,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 +1664,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 +1694,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: + { + for (int j = 0; j < 4; ++j) + { + pJointPos[j] = m_links[i].m_jointPos[j]; + } + break; + } + case btMultibodyLink::ePlanar: { - pJointPos[j] = m_links[i].m_jointPos[j]; + for (int j = 0; j < 3; ++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; + 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 +1786,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 +1849,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 +1877,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: + { + } } } @@ -2015,10 +2107,10 @@ void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep) motion += m_realBuf[i] * m_realBuf[i]; } - if (motion < SLEEP_EPSILON) + if (motion < m_sleepEpsilon) { m_sleepTimer += timestep; - if (m_sleepTimer > SLEEP_TIMEOUT) + if (m_sleepTimer > m_sleepTimeout) { goToSleep(); } @@ -2135,8 +2227,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 +2381,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..345970d261 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); @@ -490,19 +545,30 @@ public: { m_canWakeup = canWakeup; } - bool isAwake() const { return m_awake; } + bool isAwake() const + { + return m_awake; + } void wakeUp(); 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,6 +719,25 @@ 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; + + void setSleepThreshold(btScalar sleepThreshold) + { + m_sleepEpsilon = sleepThreshold; + } + + void setSleepTimeout(btScalar sleepTimeout) + { + this->m_sleepTimeout = sleepTimeout; + } private: @@ -674,7 +759,7 @@ private: } } - void mulMatrix(btScalar * pA, btScalar * pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const; + void mulMatrix(const btScalar *pA, const btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const; private: btMultiBodyLinkCollider *m_baseCollider; //can be NULL @@ -711,6 +796,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; @@ -729,6 +815,8 @@ private: bool m_canSleep; bool m_canWakeup; btScalar m_sleepTimer; + btScalar m_sleepEpsilon; + btScalar m_sleepTimeout; void *m_userObjectPointer; int m_userIndex2; @@ -751,6 +839,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..00d5fd5609 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), @@ -60,7 +61,8 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(btMultiBodySolverConstra btScalar lowerLimit, btScalar upperLimit, bool angConstraint, btScalar relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip, + btScalar damping) { solverConstraint.m_multiBodyA = m_bodyA; solverConstraint.m_multiBodyB = m_bodyB; @@ -347,7 +349,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(btMultiBodySolverConstra { btScalar positionalError = 0.f; - btScalar velocityError = desiredVelocity - rel_vel; // * damping; + btScalar velocityError = (desiredVelocity - rel_vel) * damping; btScalar erp = infoGlobal.m_erp2; diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 5c15f3e851..1aaa07b69e 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; @@ -77,17 +94,21 @@ protected: bool angConstraint = false, btScalar relaxation = 1.f, - bool isFriction = false, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); + bool isFriction = false, btScalar desiredVelocity = 0, btScalar cfmSlip = 0, btScalar damping = 1.0); 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..e7af332eb3 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -137,7 +137,14 @@ void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) btMultiBodyLinkCollider* col = body->getBaseCollider(); if (col && col->getActivationState() == ACTIVE_TAG) { - col->setActivationState(WANTS_DEACTIVATION); + if (body->hasFixedBase()) + { + col->setActivationState(FIXED_BASE_MULTI_BODY); + } else + { + col->setActivationState(WANTS_DEACTIVATION); + } + col->setDeactivationTime(0.f); } for (int b = 0; b < body->getNumLinks(); b++) @@ -592,6 +599,7 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep) if (!isSleeping) { + bod->addSplitV(); int nLinks = bod->getNumLinks(); ///base + num m_links @@ -610,6 +618,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 +876,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..00a7ef3579 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp @@ -23,13 +23,16 @@ 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.), - m_kp(0.2), + m_use_multi_dof_params(false), + m_kd(1., 1., 1.), + m_kp(0.2, 0.2, 0.2), m_erp(1), - m_rhsClamp(SIMD_INFINITY) + m_rhsClamp(SIMD_INFINITY), + m_maxAppliedImpulseMultiDof(maxMotorImpulse, maxMotorImpulse, maxMotorImpulse), + m_damping(1.0, 1.0, 1.0) { m_maxAppliedImpulse = maxMotorImpulse; @@ -139,7 +142,8 @@ btQuaternion relRot = currentQuat.inverse() * desiredQuat; btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; btScalar desiredVelocity = this->m_desiredVelocity[row]; - btScalar velocityError = desiredVelocity - currentVelocity; + double kd = m_use_multi_dof_params ? m_kd[row % 3] : m_kd[0]; + btScalar velocityError = (desiredVelocity - currentVelocity) * kd; btMatrix3x3 frameAworld; frameAworld.setIdentity(); @@ -152,12 +156,16 @@ btQuaternion relRot = currentQuat.inverse() * desiredQuat; case btMultibodyLink::eSpherical: { btVector3 constraintNormalAng = frameAworld.getColumn(row % 3); - posError = m_kp*angleDiff[row % 3]; + double kp = m_use_multi_dof_params ? m_kp[row % 3] : m_kp[0]; + posError = kp*angleDiff[row % 3]; + double max_applied_impulse = m_use_multi_dof_params ? m_maxAppliedImpulseMultiDof[row % 3] : m_maxAppliedImpulse; fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, btVector3(0,0,0), dummy, dummy, posError, infoGlobal, - -m_maxAppliedImpulse, m_maxAppliedImpulse, true); + -max_applied_impulse, max_applied_impulse, true, + 1.0, false, 0, 0, + m_damping[row % 3]); constraintRow.m_orgConstraint = this; constraintRow.m_orgDofIndex = row; break; diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h index 621beab5a4..bdeccc2e24 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h @@ -26,10 +26,13 @@ class btMultiBodySphericalJointMotor : public btMultiBodyConstraint protected: btVector3 m_desiredVelocity; btQuaternion m_desiredPosition; - btScalar m_kd; - btScalar m_kp; + bool m_use_multi_dof_params; + btVector3 m_kd; + btVector3 m_kp; btScalar m_erp; btScalar m_rhsClamp; //maximum error + btVector3 m_maxAppliedImpulseMultiDof; + btVector3 m_damping; public: btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse); @@ -44,16 +47,32 @@ public: btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal); - virtual void setVelocityTarget(const btVector3& velTarget, btScalar kd = 1.f) + virtual void setVelocityTarget(const btVector3& velTarget, btScalar kd = 1.0) + { + m_desiredVelocity = velTarget; + m_kd = btVector3(kd, kd, kd); + m_use_multi_dof_params = false; + } + + virtual void setVelocityTargetMultiDof(const btVector3& velTarget, const btVector3& kd = btVector3(1.0, 1.0, 1.0)) { m_desiredVelocity = velTarget; m_kd = kd; + m_use_multi_dof_params = true; } - virtual void setPositionTarget(const btQuaternion& posTarget, btScalar kp = 1.f) + virtual void setPositionTarget(const btQuaternion& posTarget, btScalar kp =1.f) + { + m_desiredPosition = posTarget; + m_kp = btVector3(kp, kp, kp); + m_use_multi_dof_params = false; + } + + virtual void setPositionTargetMultiDof(const btQuaternion& posTarget, const btVector3& kp = btVector3(1.f, 1.f, 1.f)) { m_desiredPosition = posTarget; m_kp = kp; + m_use_multi_dof_params = true; } virtual void setErp(btScalar erp) @@ -68,6 +87,28 @@ public: { m_rhsClamp = rhsClamp; } + + btScalar getMaxAppliedImpulseMultiDof(int i) const + { + return m_maxAppliedImpulseMultiDof[i]; + } + + void setMaxAppliedImpulseMultiDof(const btVector3& maxImp) + { + m_maxAppliedImpulseMultiDof = maxImp; + m_use_multi_dof_params = true; + } + + btScalar getDamping(int i) const + { + return m_damping[i]; + } + + void setDamping(const btVector3& damping) + { + m_damping = damping; + } + virtual void debugDraw(class btIDebugDraw* drawer) { //todo(erwincoumans) diff --git a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp index 5d864f2757..ed4e0b686d 100644 --- a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp +++ b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp @@ -532,7 +532,7 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) J_transpose = J.transpose(); btMatrixXu& tmp = m_scratchTmp; - + //Minv.printMatrix("Minv="); { { BT_PROFILE("J*Minv"); @@ -543,7 +543,7 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) m_A = tmp * J_transpose; } } - + //J.printMatrix("J"); if (1) { // add cfm to the diagonal of m_A |