diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/Featherstone')
29 files changed, 7086 insertions, 3158 deletions
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp index aea8c683732..9862bd2e228 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -21,37 +21,38 @@ */ - #include "btMultiBody.h" #include "btMultiBodyLink.h" #include "btMultiBodyLinkCollider.h" #include "btMultiBodyJointFeedback.h" #include "LinearMath/btTransformUtil.h" #include "LinearMath/btSerializer.h" -// #define INCLUDE_GYRO_TERM +//#include "Bullet3Common/b3Logging.h" +// #define INCLUDE_GYRO_TERM -///todo: determine if we need these options. If so, make a proper API, otherwise delete those globals -bool gJointFeedbackInWorldSpace = false; -bool gJointFeedbackInJointFrame = false; -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 +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 +} // namespace + +void btMultiBody::spatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame + const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates + const btVector3 &top_in, // top part of input vector + const btVector3 &bottom_in, // bottom part of input vector + btVector3 &top_out, // top part of output vector + btVector3 &bottom_out) // bottom part of output vector +{ + top_out = rotation_matrix * top_in; + bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in; } -namespace { - void SpatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame - const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates - const btVector3 &top_in, // top part of input vector - const btVector3 &bottom_in, // bottom part of input vector - btVector3 &top_out, // top part of output vector - btVector3 &bottom_out) // bottom part of output vector - { - top_out = rotation_matrix * top_in; - bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in; - } +namespace +{ -/* + +#if 0 void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix, const btVector3 &displacement, const btVector3 &top_in, @@ -81,52 +82,65 @@ namespace { top_out = a_top.cross(b_top); bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom); } -*/ -} +#endif +} // namespace // // Implementation of class btMultiBody // btMultiBody::btMultiBody(int n_links, - btScalar mass, - const btVector3 &inertia, - bool fixedBase, - bool canSleep, - bool /*deprecatedUseMultiDof*/) - : - m_baseCollider(0), - m_baseName(0), - m_basePos(0,0,0), - m_baseQuat(0, 0, 0, 1), - m_baseMass(mass), - m_baseInertia(inertia), - - m_fixedBase(fixedBase), - m_awake(true), - m_canSleep(canSleep), - m_sleepTimer(0), - - m_linearDamping(0.04f), - m_angularDamping(0.04f), - m_useGyroTerm(true), - m_maxAppliedImpulse(1000.f), - m_maxCoordinateVelocity(100.f), - m_hasSelfCollision(true), - __posUpdated(false), - m_dofCount(0), - m_posVarCnt(0), - m_useRK4(false), - m_useGlobalVelocities(false), - m_internalNeedsJointFeedback(false) + btScalar mass, + const btVector3 &inertia, + bool fixedBase, + bool canSleep, + bool /*deprecatedUseMultiDof*/) + : m_baseCollider(0), + m_baseName(0), + m_basePos(0, 0, 0), + m_baseQuat(0, 0, 0, 1), + m_basePos_interpolate(0, 0, 0), + m_baseQuat_interpolate(0, 0, 0, 1), + m_baseMass(mass), + m_baseInertia(inertia), + + m_fixedBase(fixedBase), + m_awake(true), + m_canSleep(canSleep), + m_canWakeup(true), + m_sleepTimer(0), + m_userObjectPointer(0), + m_userIndex2(-1), + m_userIndex(-1), + m_companionId(-1), + m_linearDamping(0.04f), + m_angularDamping(0.04f), + m_useGyroTerm(true), + m_maxAppliedImpulse(1000.f), + m_maxCoordinateVelocity(100.f), + m_hasSelfCollision(true), + __posUpdated(false), + m_dofCount(0), + m_posVarCnt(0), + m_useRK4(false), + m_useGlobalVelocities(false), + m_internalNeedsJointFeedback(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); + m_cachedInertiaLowerLeft.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0); + m_cachedInertiaLowerRight.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0); + m_cachedInertiaValid = false; + m_links.resize(n_links); m_matrixBuf.resize(n_links + 1); + m_baseForce.setValue(0, 0, 0); + m_baseTorque.setValue(0, 0, 0); - m_baseForce.setValue(0, 0, 0); - m_baseTorque.setValue(0, 0, 0); + clearConstraintForces(); + clearForcesAndTorques(); } btMultiBody::~btMultiBody() @@ -134,129 +148,125 @@ btMultiBody::~btMultiBody() } void btMultiBody::setupFixed(int i, - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, - const btVector3 &parentComToThisPivotOffset, - const btVector3 &thisPivotToThisComOffset, bool /*deprecatedDisableParentCollision*/) + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, + const btVector3 &parentComToThisPivotOffset, + const btVector3 &thisPivotToThisComOffset, bool /*deprecatedDisableParentCollision*/) { - m_links[i].m_mass = mass; - m_links[i].m_inertiaLocal = inertia; - m_links[i].m_parent = parent; - m_links[i].m_zeroRotParentToThis = rotParentToThis; + m_links[i].m_inertiaLocal = inertia; + m_links[i].m_parent = parent; + m_links[i].setAxisTop(0, 0., 0., 0.); + m_links[i].setAxisBottom(0, btVector3(0, 0, 0)); + m_links[i].m_zeroRotParentToThis = rotParentToThis; m_links[i].m_dVector = thisPivotToThisComOffset; - m_links[i].m_eVector = parentComToThisPivotOffset; + m_links[i].m_eVector = parentComToThisPivotOffset; m_links[i].m_jointType = btMultibodyLink::eFixed; m_links[i].m_dofCount = 0; m_links[i].m_posVarCount = 0; - m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; - + m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + m_links[i].updateCacheMultiDof(); updateLinksDofOffsets(); - } - void btMultiBody::setupPrismatic(int i, - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, - const btVector3 &jointAxis, - const btVector3 &parentComToThisPivotOffset, - const btVector3 &thisPivotToThisComOffset, - bool disableParentCollision) + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, + const btVector3 &jointAxis, + const btVector3 &parentComToThisPivotOffset, + const btVector3 &thisPivotToThisComOffset, + bool disableParentCollision) { m_dofCount += 1; m_posVarCnt += 1; - - m_links[i].m_mass = mass; - m_links[i].m_inertiaLocal = inertia; - m_links[i].m_parent = parent; - m_links[i].m_zeroRotParentToThis = rotParentToThis; - m_links[i].setAxisTop(0, 0., 0., 0.); - m_links[i].setAxisBottom(0, jointAxis); - m_links[i].m_eVector = parentComToThisPivotOffset; + + m_links[i].m_mass = mass; + m_links[i].m_inertiaLocal = inertia; + m_links[i].m_parent = parent; + m_links[i].m_zeroRotParentToThis = rotParentToThis; + m_links[i].setAxisTop(0, 0., 0., 0.); + m_links[i].setAxisBottom(0, jointAxis); + m_links[i].m_eVector = parentComToThisPivotOffset; m_links[i].m_dVector = thisPivotToThisComOffset; - m_links[i].m_cachedRotParentToThis = rotParentToThis; + m_links[i].m_cachedRotParentToThis = rotParentToThis; m_links[i].m_jointType = btMultibodyLink::ePrismatic; m_links[i].m_dofCount = 1; - m_links[i].m_posVarCount = 1; + m_links[i].m_posVarCount = 1; m_links[i].m_jointPos[0] = 0.f; m_links[i].m_jointTorque[0] = 0.f; if (disableParentCollision) - m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; // - + m_links[i].updateCacheMultiDof(); - + updateLinksDofOffsets(); } void btMultiBody::setupRevolute(int i, - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, - const btVector3 &jointAxis, - const btVector3 &parentComToThisPivotOffset, - const btVector3 &thisPivotToThisComOffset, - bool disableParentCollision) + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, + const btVector3 &jointAxis, + const btVector3 &parentComToThisPivotOffset, + const btVector3 &thisPivotToThisComOffset, + bool disableParentCollision) { m_dofCount += 1; m_posVarCnt += 1; - - m_links[i].m_mass = mass; - m_links[i].m_inertiaLocal = inertia; - m_links[i].m_parent = parent; - m_links[i].m_zeroRotParentToThis = rotParentToThis; - m_links[i].setAxisTop(0, jointAxis); - m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset)); - m_links[i].m_dVector = thisPivotToThisComOffset; - m_links[i].m_eVector = parentComToThisPivotOffset; + + m_links[i].m_mass = mass; + m_links[i].m_inertiaLocal = inertia; + m_links[i].m_parent = parent; + m_links[i].m_zeroRotParentToThis = rotParentToThis; + m_links[i].setAxisTop(0, jointAxis); + m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset)); + m_links[i].m_dVector = thisPivotToThisComOffset; + m_links[i].m_eVector = parentComToThisPivotOffset; m_links[i].m_jointType = btMultibodyLink::eRevolute; m_links[i].m_dofCount = 1; - m_links[i].m_posVarCount = 1; + m_links[i].m_posVarCount = 1; m_links[i].m_jointPos[0] = 0.f; m_links[i].m_jointTorque[0] = 0.f; if (disableParentCollision) - m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; - // + m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + // m_links[i].updateCacheMultiDof(); // updateLinksDofOffsets(); } - - void btMultiBody::setupSpherical(int i, - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, - const btVector3 &parentComToThisPivotOffset, - const btVector3 &thisPivotToThisComOffset, - bool disableParentCollision) + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, + const btVector3 &parentComToThisPivotOffset, + const btVector3 &thisPivotToThisComOffset, + bool disableParentCollision) { - m_dofCount += 3; m_posVarCnt += 4; m_links[i].m_mass = mass; - m_links[i].m_inertiaLocal = inertia; - m_links[i].m_parent = parent; - m_links[i].m_zeroRotParentToThis = rotParentToThis; - m_links[i].m_dVector = thisPivotToThisComOffset; - m_links[i].m_eVector = parentComToThisPivotOffset; + m_links[i].m_inertiaLocal = inertia; + m_links[i].m_parent = parent; + m_links[i].m_zeroRotParentToThis = rotParentToThis; + m_links[i].m_dVector = thisPivotToThisComOffset; + m_links[i].m_eVector = parentComToThisPivotOffset; m_links[i].m_jointType = btMultibodyLink::eSpherical; m_links[i].m_dofCount = 3; @@ -267,323 +277,369 @@ void btMultiBody::setupSpherical(int i, m_links[i].setAxisBottom(0, m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset)); m_links[i].setAxisBottom(1, m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset)); m_links[i].setAxisBottom(2, m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset)); - m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f; m_links[i].m_jointPos[3] = 1.f; + m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f; + m_links[i].m_jointPos[3] = 1.f; m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f; - if (disableParentCollision) - m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; // - m_links[i].updateCacheMultiDof(); + m_links[i].updateCacheMultiDof(); // updateLinksDofOffsets(); } void btMultiBody::setupPlanar(int i, - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, - const btVector3 &rotationAxis, - const btVector3 &parentComToThisComOffset, - bool disableParentCollision) + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, + const btVector3 &rotationAxis, + const btVector3 &parentComToThisComOffset, + bool disableParentCollision) { - m_dofCount += 3; m_posVarCnt += 3; m_links[i].m_mass = mass; - m_links[i].m_inertiaLocal = inertia; - m_links[i].m_parent = parent; - m_links[i].m_zeroRotParentToThis = rotParentToThis; + m_links[i].m_inertiaLocal = inertia; + m_links[i].m_parent = parent; + m_links[i].m_zeroRotParentToThis = rotParentToThis; m_links[i].m_dVector.setZero(); - m_links[i].m_eVector = parentComToThisComOffset; + m_links[i].m_eVector = parentComToThisComOffset; // - static btVector3 vecNonParallelToRotAxis(1, 0, 0); - if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999) + btVector3 vecNonParallelToRotAxis(1, 0, 0); + if (rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999) vecNonParallelToRotAxis.setValue(0, 1, 0); // m_links[i].m_jointType = btMultibodyLink::ePlanar; m_links[i].m_dofCount = 3; m_links[i].m_posVarCount = 3; - btVector3 n=rotationAxis.normalized(); - m_links[i].setAxisTop(0, n[0],n[1],n[2]); - m_links[i].setAxisTop(1,0,0,0); - m_links[i].setAxisTop(2,0,0,0); - m_links[i].setAxisBottom(0,0,0,0); + btVector3 n = rotationAxis.normalized(); + m_links[i].setAxisTop(0, n[0], n[1], n[2]); + m_links[i].setAxisTop(1, 0, 0, 0); + m_links[i].setAxisTop(2, 0, 0, 0); + m_links[i].setAxisBottom(0, 0, 0, 0); btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis); - m_links[i].setAxisBottom(1,cr[0],cr[1],cr[2]); + m_links[i].setAxisBottom(1, cr[0], cr[1], cr[2]); cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0)); - m_links[i].setAxisBottom(2,cr[0],cr[1],cr[2]); + m_links[i].setAxisBottom(2, cr[0], cr[1], cr[2]); m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f; m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f; if (disableParentCollision) - m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; - // + m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + // m_links[i].updateCacheMultiDof(); // updateLinksDofOffsets(); + + m_links[i].setAxisBottom(1, m_links[i].getAxisBottom(1).normalized()); + m_links[i].setAxisBottom(2, m_links[i].getAxisBottom(2).normalized()); } void btMultiBody::finalizeMultiDof() { m_deltaV.resize(0); m_deltaV.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_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); + for (int i = 0; i < m_vectorBuf.size(); i++) + { + m_vectorBuf[i].setValue(0, 0, 0); + } updateLinksDofOffsets(); } - -int btMultiBody::getParent(int i) const + +int btMultiBody::getParent(int link_num) const { - return m_links[i].m_parent; + return m_links[link_num].m_parent; } btScalar btMultiBody::getLinkMass(int i) const { - return m_links[i].m_mass; + return m_links[i].m_mass; } -const btVector3 & btMultiBody::getLinkInertia(int i) const +const btVector3 &btMultiBody::getLinkInertia(int i) const { - return m_links[i].m_inertiaLocal; + return m_links[i].m_inertiaLocal; } btScalar btMultiBody::getJointPos(int i) const { - return m_links[i].m_jointPos[0]; + return m_links[i].m_jointPos[0]; } btScalar btMultiBody::getJointVel(int i) const { - return m_realBuf[6 + m_links[i].m_dofOffset]; + return m_realBuf[6 + m_links[i].m_dofOffset]; } -btScalar * btMultiBody::getJointPosMultiDof(int i) +btScalar *btMultiBody::getJointPosMultiDof(int i) { return &m_links[i].m_jointPos[0]; } -btScalar * btMultiBody::getJointVelMultiDof(int i) +btScalar *btMultiBody::getJointVelMultiDof(int i) { return &m_realBuf[6 + m_links[i].m_dofOffset]; } -const btScalar * btMultiBody::getJointPosMultiDof(int i) const +const btScalar *btMultiBody::getJointPosMultiDof(int i) const { return &m_links[i].m_jointPos[0]; } -const btScalar * btMultiBody::getJointVelMultiDof(int i) const +const btScalar *btMultiBody::getJointVelMultiDof(int i) const { return &m_realBuf[6 + m_links[i].m_dofOffset]; } - void btMultiBody::setJointPos(int i, btScalar q) { - m_links[i].m_jointPos[0] = q; - m_links[i].updateCacheMultiDof(); + m_links[i].m_jointPos[0] = q; + m_links[i].updateCacheMultiDof(); +} + + +void btMultiBody::setJointPosMultiDof(int i, const double *q) +{ + for (int pos = 0; pos < m_links[i].m_posVarCount; ++pos) + m_links[i].m_jointPos[pos] = (btScalar)q[pos]; + + m_links[i].updateCacheMultiDof(); } -void btMultiBody::setJointPosMultiDof(int i, btScalar *q) +void btMultiBody::setJointPosMultiDof(int i, const float *q) { - for(int pos = 0; pos < m_links[i].m_posVarCount; ++pos) - m_links[i].m_jointPos[pos] = q[pos]; + for (int pos = 0; pos < m_links[i].m_posVarCount; ++pos) + m_links[i].m_jointPos[pos] = (btScalar)q[pos]; - m_links[i].updateCacheMultiDof(); + m_links[i].updateCacheMultiDof(); } + + void btMultiBody::setJointVel(int i, btScalar qdot) { - m_realBuf[6 + m_links[i].m_dofOffset] = qdot; + m_realBuf[6 + m_links[i].m_dofOffset] = qdot; +} + +void btMultiBody::setJointVelMultiDof(int i, const double *qdot) +{ + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) + m_realBuf[6 + m_links[i].m_dofOffset + dof] = (btScalar)qdot[dof]; } -void btMultiBody::setJointVelMultiDof(int i, btScalar *qdot) +void btMultiBody::setJointVelMultiDof(int i, const float* qdot) { - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - m_realBuf[6 + m_links[i].m_dofOffset + dof] = qdot[dof]; + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) + m_realBuf[6 + m_links[i].m_dofOffset + dof] = (btScalar)qdot[dof]; } -const btVector3 & btMultiBody::getRVector(int i) const +const btVector3 &btMultiBody::getRVector(int i) const { - return m_links[i].m_cachedRVector; + return m_links[i].m_cachedRVector; } -const btQuaternion & btMultiBody::getParentToLocalRot(int i) const +const btQuaternion &btMultiBody::getParentToLocalRot(int i) const { - return m_links[i].m_cachedRotParentToThis; + return m_links[i].m_cachedRotParentToThis; +} + +const btVector3 &btMultiBody::getInterpolateRVector(int i) const +{ + return m_links[i].m_cachedRVector_interpolate; +} + +const btQuaternion &btMultiBody::getInterpolateParentToLocalRot(int i) const +{ + return m_links[i].m_cachedRotParentToThis_interpolate; } btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const { - btVector3 result = local_pos; - while (i != -1) { - // 'result' is in frame i. transform it to frame parent(i) - result += getRVector(i); - result = quatRotate(getParentToLocalRot(i).inverse(),result); - i = getParent(i); - } + btAssert(i >= -1); + btAssert(i < m_links.size()); + if ((i < -1) || (i >= m_links.size())) + { + return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY); + } + + btVector3 result = local_pos; + while (i != -1) + { + // 'result' is in frame i. transform it to frame parent(i) + result += getRVector(i); + result = quatRotate(getParentToLocalRot(i).inverse(), result); + i = getParent(i); + } - // 'result' is now in the base frame. transform it to world frame - result = quatRotate(getWorldToBaseRot().inverse() ,result); - result += getBasePos(); + // 'result' is now in the base frame. transform it to world frame + result = quatRotate(getWorldToBaseRot().inverse(), result); + result += getBasePos(); - return result; + return result; } btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const { - if (i == -1) { - // world to base - return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos())); - } else { - // find position in parent frame, then transform to current frame - return quatRotate(getParentToLocalRot(i),worldPosToLocal(getParent(i), world_pos)) - getRVector(i); - } + btAssert(i >= -1); + btAssert(i < m_links.size()); + if ((i < -1) || (i >= m_links.size())) + { + return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY); + } + + if (i == -1) + { + // world to base + return quatRotate(getWorldToBaseRot(), (world_pos - getBasePos())); + } + else + { + // find position in parent frame, then transform to current frame + return quatRotate(getParentToLocalRot(i), worldPosToLocal(getParent(i), world_pos)) - getRVector(i); + } } btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const { - btVector3 result = local_dir; - while (i != -1) { - result = quatRotate(getParentToLocalRot(i).inverse() , result); - i = getParent(i); - } - result = quatRotate(getWorldToBaseRot().inverse() , result); - return result; + btAssert(i >= -1); + btAssert(i < m_links.size()); + if ((i < -1) || (i >= m_links.size())) + { + return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY); + } + + btVector3 result = local_dir; + while (i != -1) + { + result = quatRotate(getParentToLocalRot(i).inverse(), result); + i = getParent(i); + } + result = quatRotate(getWorldToBaseRot().inverse(), result); + return result; } btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const { - if (i == -1) { - return quatRotate(getWorldToBaseRot(), world_dir); - } else { - return quatRotate(getParentToLocalRot(i) ,worldDirToLocal(getParent(i), world_dir)); - } + btAssert(i >= -1); + btAssert(i < m_links.size()); + if ((i < -1) || (i >= m_links.size())) + { + return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY); + } + + if (i == -1) + { + return quatRotate(getWorldToBaseRot(), world_dir); + } + else + { + return quatRotate(getParentToLocalRot(i), worldDirToLocal(getParent(i), world_dir)); + } } -void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const +btMatrix3x3 btMultiBody::localFrameToWorld(int i, const btMatrix3x3 &local_frame) const { - int num_links = getNumLinks(); - // Calculates the velocities of each link (and the base) in its local frame - omega[0] = quatRotate(m_baseQuat ,getBaseOmega()); - vel[0] = quatRotate(m_baseQuat ,getBaseVel()); - - for (int i = 0; i < num_links; ++i) { - const int parent = m_links[i].m_parent; - - // transform parent vel into this frame, store in omega[i+1], vel[i+1] - SpatialTransform(btMatrix3x3(m_links[i].m_cachedRotParentToThis), m_links[i].m_cachedRVector, - omega[parent+1], vel[parent+1], - omega[i+1], vel[i+1]); - - // now add qidot * shat_i - omega[i+1] += getJointVel(i) * m_links[i].getAxisTop(0); - vel[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0); - } + btMatrix3x3 result = local_frame; + btVector3 frameInWorld0 = localDirToWorld(i, local_frame.getColumn(0)); + btVector3 frameInWorld1 = localDirToWorld(i, local_frame.getColumn(1)); + btVector3 frameInWorld2 = localDirToWorld(i, local_frame.getColumn(2)); + result.setValue(frameInWorld0[0], frameInWorld1[0], frameInWorld2[0], frameInWorld0[1], frameInWorld1[1], frameInWorld2[1], frameInWorld0[2], frameInWorld1[2], frameInWorld2[2]); + return result; } -btScalar btMultiBody::getKineticEnergy() const +void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const { int num_links = getNumLinks(); - // TODO: would be better not to allocate memory here - btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1); - btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1); - compTreeLinkVelocities(&omega[0], &vel[0]); - - // we will do the factor of 0.5 at the end - btScalar result = m_baseMass * vel[0].dot(vel[0]); - result += omega[0].dot(m_baseInertia * omega[0]); - - for (int i = 0; i < num_links; ++i) { - result += m_links[i].m_mass * vel[i+1].dot(vel[i+1]); - result += omega[i+1].dot(m_links[i].m_inertiaLocal * omega[i+1]); - } + // Calculates the velocities of each link (and the base) in its local frame + const btQuaternion& base_rot = getWorldToBaseRot(); + omega[0] = quatRotate(base_rot, getBaseOmega()); + vel[0] = quatRotate(base_rot, getBaseVel()); - return 0.5f * result; -} + for (int i = 0; i < num_links; ++i) + { + const btMultibodyLink& link = getLink(i); + const int parent = link.m_parent; -btVector3 btMultiBody::getAngularMomentum() const -{ - int num_links = getNumLinks(); - // TODO: would be better not to allocate memory here - btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1); - btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1); - btAlignedObjectArray<btQuaternion> rot_from_world;rot_from_world.resize(num_links+1); - compTreeLinkVelocities(&omega[0], &vel[0]); - - rot_from_world[0] = m_baseQuat; - btVector3 result = quatRotate(rot_from_world[0].inverse() , (m_baseInertia * omega[0])); - - for (int i = 0; i < num_links; ++i) { - rot_from_world[i+1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent+1]; - result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertiaLocal * omega[i+1]))); - } + // transform parent vel into this frame, store in omega[i+1], vel[i+1] + spatialTransform(btMatrix3x3(link.m_cachedRotParentToThis), link.m_cachedRVector, + omega[parent + 1], vel[parent + 1], + omega[i + 1], vel[i + 1]); - return result; + // now add qidot * shat_i + const btScalar* jointVel = getJointVelMultiDof(i); + for (int dof = 0; dof < link.m_dofCount; ++dof) + { + omega[i + 1] += jointVel[dof] * link.getAxisTop(dof); + vel[i + 1] += jointVel[dof] * link.getAxisBottom(dof); + } + } } + void btMultiBody::clearConstraintForces() { m_baseConstraintForce.setValue(0, 0, 0); m_baseConstraintTorque.setValue(0, 0, 0); - - for (int i = 0; i < getNumLinks(); ++i) { - m_links[i].m_appliedConstraintForce.setValue(0, 0, 0); - m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0); - } + for (int i = 0; i < getNumLinks(); ++i) + { + m_links[i].m_appliedConstraintForce.setValue(0, 0, 0); + m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0); + } } void btMultiBody::clearForcesAndTorques() { - m_baseForce.setValue(0, 0, 0); - m_baseTorque.setValue(0, 0, 0); + m_baseForce.setValue(0, 0, 0); + m_baseTorque.setValue(0, 0, 0); - - for (int i = 0; i < getNumLinks(); ++i) { - m_links[i].m_appliedForce.setValue(0, 0, 0); - m_links[i].m_appliedTorque.setValue(0, 0, 0); + for (int i = 0; i < getNumLinks(); ++i) + { + m_links[i].m_appliedForce.setValue(0, 0, 0); + m_links[i].m_appliedTorque.setValue(0, 0, 0); m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = m_links[i].m_jointTorque[3] = m_links[i].m_jointTorque[4] = m_links[i].m_jointTorque[5] = 0.f; - } + } } void btMultiBody::clearVelocities() { - for (int i = 0; i < 6 + getNumLinks(); ++i) + for (int i = 0; i < 6 + getNumDofs(); ++i) { m_realBuf[i] = 0.f; } } void btMultiBody::addLinkForce(int i, const btVector3 &f) { - m_links[i].m_appliedForce += f; + m_links[i].m_appliedForce += f; } void btMultiBody::addLinkTorque(int i, const btVector3 &t) { - m_links[i].m_appliedTorque += t; + m_links[i].m_appliedTorque += t; } void btMultiBody::addLinkConstraintForce(int i, const btVector3 &f) { - m_links[i].m_appliedConstraintForce += f; + m_links[i].m_appliedConstraintForce += f; } void btMultiBody::addLinkConstraintTorque(int i, const btVector3 &t) { - m_links[i].m_appliedConstraintTorque += t; + m_links[i].m_appliedConstraintTorque += t; } - - void btMultiBody::addJointTorque(int i, btScalar Q) { - m_links[i].m_jointTorque[0] += Q; + m_links[i].m_jointTorque[0] += Q; } void btMultiBody::addJointTorqueMultiDof(int i, int dof, btScalar Q) @@ -593,70 +649,72 @@ void btMultiBody::addJointTorqueMultiDof(int i, int dof, btScalar Q) void btMultiBody::addJointTorqueMultiDof(int i, const btScalar *Q) { - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) m_links[i].m_jointTorque[dof] = Q[dof]; } -const btVector3 & btMultiBody::getLinkForce(int i) const +const btVector3 &btMultiBody::getLinkForce(int i) const { - return m_links[i].m_appliedForce; + return m_links[i].m_appliedForce; } -const btVector3 & btMultiBody::getLinkTorque(int i) const +const btVector3 &btMultiBody::getLinkTorque(int i) const { - return m_links[i].m_appliedTorque; + return m_links[i].m_appliedTorque; } btScalar btMultiBody::getJointTorque(int i) const { - return m_links[i].m_jointTorque[0]; + return m_links[i].m_jointTorque[0]; } -btScalar * btMultiBody::getJointTorqueMultiDof(int i) +btScalar *btMultiBody::getJointTorqueMultiDof(int i) { - return &m_links[i].m_jointTorque[0]; + return &m_links[i].m_jointTorque[0]; } -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? +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( - v0.x() * v1.x(), - v0.x() * v1.y(), - v0.x() * v1.z()); - btVector3 row1 = btVector3( - v0.y() * v1.x(), - v0.y() * v1.y(), - v0.y() * v1.z()); - btVector3 row2 = btVector3( - v0.z() * v1.x(), - v0.z() * v1.y(), - v0.z() * v1.z()); - - btMatrix3x3 m(row0[0],row0[1],row0[2], - row1[0],row1[1],row1[2], - row2[0],row2[1],row2[2]); - return m; + btVector3 row0 = btVector3( + v0.x() * v1.x(), + v0.x() * v1.y(), + v0.x() * v1.z()); + btVector3 row1 = btVector3( + v0.y() * v1.x(), + v0.y() * v1.y(), + v0.y() * v1.z()); + btVector3 row2 = btVector3( + v0.z() * v1.x(), + v0.z() * v1.y(), + v0.z() * v1.z()); + + btMatrix3x3 m(row0[0], row0[1], row0[2], + row1[0], row1[1], row1[2], + row2[0], row2[1], row2[2]); + return m; } #define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed) // void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, - btAlignedObjectArray<btScalar> &scratch_r, - btAlignedObjectArray<btVector3> &scratch_v, - btAlignedObjectArray<btMatrix3x3> &scratch_m, - bool isConstraintPass) + btAlignedObjectArray<btScalar> &scratch_r, + btAlignedObjectArray<btVector3> &scratch_v, + btAlignedObjectArray<btMatrix3x3> &scratch_m, + bool isConstraintPass, + bool jointFeedbackInWorldSpace, + bool jointFeedbackInJointFrame) { - // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot) - // and the base linear & angular accelerations. + // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot) + // and the base linear & angular accelerations. - // We apply damping forces in this routine as well as any external forces specified by the - // caller (via addBaseForce etc). + // We apply damping forces in this routine as well as any external forces specified by the + // caller (via addBaseForce etc). + + // output should point to an array of 6 + num_links reals. + // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame), + // num_links joint acceleration values. - // output should point to an array of 6 + num_links reals. - // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame), - // num_links joint acceleration values. - // We added support for multi degree of freedom (multi dof) joints. // In addition we also can compute the joint reaction forces. This is performed in a second pass, // so that we can include the effect of the constraint solver forces (computed in the PGS LCP solver) @@ -665,96 +723,96 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar int num_links = getNumLinks(); - const btScalar DAMPING_K1_LINEAR = m_linearDamping; + const btScalar DAMPING_K1_LINEAR = m_linearDamping; const btScalar DAMPING_K2_LINEAR = m_linearDamping; const btScalar DAMPING_K1_ANGULAR = m_angularDamping; - const btScalar DAMPING_K2_ANGULAR= m_angularDamping; + const btScalar DAMPING_K2_ANGULAR = m_angularDamping; - btVector3 base_vel = getBaseVel(); - btVector3 base_omega = getBaseOmega(); + const btVector3 base_vel = getBaseVel(); + const btVector3 base_omega = getBaseOmega(); - // Temporary matrices/vectors -- use scratch space from caller - // so that we don't have to keep reallocating every frame + // Temporary matrices/vectors -- use scratch space from caller + // so that we don't have to keep reallocating every frame - scratch_r.resize(2*m_dofCount + 6); //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount - scratch_v.resize(8*num_links + 6); - scratch_m.resize(4*num_links + 4); + scratch_r.resize(2 * m_dofCount + 7); //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount + scratch_v.resize(8 * num_links + 6); + scratch_m.resize(4 * num_links + 4); //btScalar * r_ptr = &scratch_r[0]; - btScalar * output = &scratch_r[m_dofCount]; // "output" holds the q_double_dot results - btVector3 * v_ptr = &scratch_v[0]; - - // vhat_i (top = angular, bottom = linear part) + btScalar *output = &scratch_r[m_dofCount]; // "output" holds the q_double_dot results + btVector3 *v_ptr = &scratch_v[0]; + + // vhat_i (top = angular, bottom = linear part) btSpatialMotionVector *spatVel = (btSpatialMotionVector *)v_ptr; v_ptr += num_links * 2 + 2; // - // zhat_i^A - btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr; + // zhat_i^A + btSpatialForceVector *zeroAccSpatFrc = (btSpatialForceVector *)v_ptr; v_ptr += num_links * 2 + 2; // - // chat_i (note NOT defined for the base) - btSpatialMotionVector * spatCoriolisAcc = (btSpatialMotionVector *)v_ptr; + // chat_i (note NOT defined for the base) + btSpatialMotionVector *spatCoriolisAcc = (btSpatialMotionVector *)v_ptr; v_ptr += num_links * 2; // - // Ihat_i^A. - btSymmetricSpatialDyad * spatInertia = (btSymmetricSpatialDyad *)&scratch_m[num_links + 1]; + // Ihat_i^A. + btSymmetricSpatialDyad *spatInertia = (btSymmetricSpatialDyad *)&scratch_m[num_links + 1]; - // Cached 3x3 rotation matrices from parent frame to this frame. - btMatrix3x3 * rot_from_parent = &m_matrixBuf[0]; - btMatrix3x3 * rot_from_world = &scratch_m[0]; + // Cached 3x3 rotation matrices from parent frame to this frame. + btMatrix3x3 *rot_from_parent = &m_matrixBuf[0]; + btMatrix3x3 *rot_from_world = &scratch_m[0]; - // hhat_i, ahat_i - // hhat is NOT stored for the base (but ahat is) - btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0); - btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr; + // hhat_i, ahat_i + // hhat is NOT stored for the base (but ahat is) + btSpatialForceVector *h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0); + btSpatialMotionVector *spatAcc = (btSpatialMotionVector *)v_ptr; v_ptr += num_links * 2 + 2; // - // Y_i, invD_i - btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0; - btScalar * Y = &scratch_r[0]; + // Y_i, invD_i + btScalar *invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0; + btScalar *Y = &scratch_r[0]; // - //aux variables - static btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence) - static btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do - static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies - static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel) - static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough - static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors - static btSpatialTransformationMatrix fromParent; //spatial transform from parent to child - static btSymmetricSpatialDyad dyadTemp; //inertia matrix temp - static btSpatialTransformationMatrix fromWorld; + //aux variables + btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence) + btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do + btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies + btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel) + btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough + btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors + btSpatialTransformationMatrix fromParent; //spatial transform from parent to child + btSymmetricSpatialDyad dyadTemp; //inertia matrix temp + btSpatialTransformationMatrix fromWorld; fromWorld.m_trnVec.setZero(); ///////////////// - // ptr to the joint accel part of the output - btScalar * joint_accel = output + 6; + // ptr to the joint accel part of the output + btScalar *joint_accel = output + 6; - // Start of the algorithm proper. - - // First 'upward' loop. - // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. + // Start of the algorithm proper. - rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!? + // First 'upward' loop. + // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. + + rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!? //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 (m_fixedBase) + { zeroAccSpatFrc[0].setZero(); - } - else + } + else { - btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce; - btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque; - //external forces - zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce)); + const btVector3 &baseForce = isConstraintPass ? m_baseConstraintForce : m_baseForce; + const btVector3 &baseTorque = isConstraintPass ? m_baseConstraintTorque : m_baseTorque; + //external forces + zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce)); //adding damping terms (only) - btScalar linDampMult = 1., angDampMult = 1.; - zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().norm()), - linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().norm())); + const btScalar linDampMult = 1., angDampMult = 1.; + zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()), + linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm())); // //p += vhat x Ihat vhat - done in a simpler way @@ -762,67 +820,66 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar zeroAccSpatFrc[0].addAngular(spatVel[0].getAngular().cross(m_baseInertia * spatVel[0].getAngular())); // zeroAccSpatFrc[0].addLinear(m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear())); - } - + } //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs) - spatInertia[0].setMatrix( btMatrix3x3(0,0,0,0,0,0,0,0,0), - // - btMatrix3x3(m_baseMass, 0, 0, - 0, m_baseMass, 0, - 0, 0, m_baseMass), - // - btMatrix3x3(m_baseInertia[0], 0, 0, - 0, m_baseInertia[1], 0, - 0, 0, m_baseInertia[2]) - ); - - rot_from_world[0] = rot_from_parent[0]; + spatInertia[0].setMatrix(btMatrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0), + // + btMatrix3x3(m_baseMass, 0, 0, + 0, m_baseMass, 0, + 0, 0, m_baseMass), + // + btMatrix3x3(m_baseInertia[0], 0, 0, + 0, m_baseInertia[1], 0, + 0, 0, m_baseInertia[2])); + + rot_from_world[0] = rot_from_parent[0]; // - for (int i = 0; i < num_links; ++i) { - const int parent = m_links[i].m_parent; - rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); - rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; + for (int i = 0; i < num_links; ++i) + { + const int parent = m_links[i].m_parent; + rot_from_parent[i + 1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); + rot_from_world[i + 1] = rot_from_parent[i + 1] * rot_from_world[parent + 1]; - fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; - fromWorld.m_rotMat = rot_from_world[i+1]; - fromParent.transform(spatVel[parent+1], spatVel[i+1]); + fromParent.m_rotMat = rot_from_parent[i + 1]; + fromParent.m_trnVec = m_links[i].m_cachedRVector; + fromWorld.m_rotMat = rot_from_world[i + 1]; + fromParent.transform(spatVel[parent + 1], spatVel[i + 1]); // now set vhat_i to its true value by doing - // vhat_i += qidot * shat_i - if(!m_useGlobalVelocities) + // vhat_i += qidot * shat_i + if (!m_useGlobalVelocities) { spatJointVel.setZero(); - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof]; // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint - spatVel[i+1] += spatJointVel; + spatVel[i + 1] += spatJointVel; // // vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint //spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel; - } else { - fromWorld.transformRotationOnly(m_links[i].m_absFrameTotVelocity, spatVel[i+1]); + fromWorld.transformRotationOnly(m_links[i].m_absFrameTotVelocity, spatVel[i + 1]); fromWorld.transformRotationOnly(m_links[i].m_absFrameLocVelocity, spatJointVel); } - // we can now calculate chat_i - spatVel[i+1].cross(spatJointVel, spatCoriolisAcc[i]); + // we can now calculate chat_i + spatVel[i + 1].cross(spatJointVel, spatCoriolisAcc[i]); - // calculate zhat_i^A + // 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; - - zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * linkAppliedTorque), -(rot_from_world[i+1] * linkAppliedForce )); - + //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)); + #if 0 { @@ -840,27 +897,26 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar // //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().norm()), - linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm())); - - // calculate Ihat_i^A + 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())); + + // calculate Ihat_i^A //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs) - spatInertia[i+1].setMatrix( btMatrix3x3(0,0,0,0,0,0,0,0,0), - // - btMatrix3x3(m_links[i].m_mass, 0, 0, - 0, m_links[i].m_mass, 0, - 0, 0, m_links[i].m_mass), - // - 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]) - ); + spatInertia[i + 1].setMatrix(btMatrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0), + // + btMatrix3x3(m_links[i].m_mass, 0, 0, + 0, m_links[i].m_mass, 0, + 0, 0, m_links[i].m_mass), + // + 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())); + 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(); @@ -871,63 +927,65 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar //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()); + //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()); //printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z()); - } - - // 'Downward' loop. - // (part of TreeForwardDynamics in Mirtich.) - for (int i = num_links - 1; i >= 0; --i) + } + + // 'Downward' loop. + // (part of TreeForwardDynamics in Mirtich.) + for (int i = num_links - 1; i >= 0; --i) { const int parent = m_links[i].m_parent; - fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; + fromParent.m_rotMat = rot_from_parent[i + 1]; + fromParent.m_trnVec = m_links[i].m_cachedRVector; - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) { btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; // - hDof = spatInertia[i+1] * m_links[i].m_axes[dof]; + hDof = spatInertia[i + 1] * m_links[i].m_axes[dof]; // - Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof] - - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1]) - - spatCoriolisAcc[i].dot(hDof) - ; + Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof] - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i + 1]) - spatCoriolisAcc[i].dot(hDof); } - - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) { - btScalar *D_row = &D[dof * m_links[i].m_dofCount]; - for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) + btScalar *D_row = &D[dof * m_links[i].m_dofCount]; + for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) { - btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2]; + const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2]; D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2); } } - btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; - switch(m_links[i].m_jointType) + btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset]; + switch (m_links[i].m_jointType) { case btMultibodyLink::ePrismatic: case btMultibodyLink::eRevolute: { - invDi[0] = 1.0f / D[0]; + if (D[0] >= SIMD_EPSILON) + { + invDi[0] = 1.0f / D[0]; + } + else + { + invDi[0] = 0; + } break; } case btMultibodyLink::eSpherical: case btMultibodyLink::ePlanar: { - static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]); - static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse(); + const btMatrix3x3 D3x3(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]); + const btMatrix3x3 invD3x3(D3x3.inverse()); //unroll the loop? - for(int row = 0; row < 3; ++row) + for (int row = 0; row < 3; ++row) { - for(int col = 0; col < 3; ++col) - { + for (int col = 0; col < 3; ++col) + { invDi[row * 3 + col] = invD3x3[row][col]; } } @@ -936,85 +994,82 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar } default: { - } } //determine h*D^{-1} - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) { spatForceVecTemps[dof].setZero(); - for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) - { - btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2]; - // + for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) + { + const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2]; + // spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof]; } } - dyadTemp = spatInertia[i+1]; + dyadTemp = spatInertia[i + 1]; //determine (h*D^{-1}) * h^{T} - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - { - btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; // dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]); } - fromParent.transformInverse(dyadTemp, spatInertia[parent+1], btSpatialTransformationMatrix::Add); - - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + fromParent.transformInverse(dyadTemp, spatInertia[parent + 1], btSpatialTransformationMatrix::Add); + + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) { invD_times_Y[dof] = 0.f; - for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) + for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) { - invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2]; - } + invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2]; + } } - - spatForceVecTemps[0] = zeroAccSpatFrc[i+1] + spatInertia[i+1] * spatCoriolisAcc[i]; - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - { - btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + spatForceVecTemps[0] = zeroAccSpatFrc[i + 1] + spatInertia[i + 1] * spatCoriolisAcc[i]; + + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; // - spatForceVecTemps[0] += hDof * invD_times_Y[dof]; + spatForceVecTemps[0] += hDof * invD_times_Y[dof]; } - + fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]); - - zeroAccSpatFrc[parent+1] += spatForceVecTemps[1]; - } + zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1]; + } - // Second 'upward' loop - // (part of TreeForwardDynamics in Mirtich) + // Second 'upward' loop + // (part of TreeForwardDynamics in Mirtich) - if (m_fixedBase) + if (m_fixedBase) { - spatAcc[0].setZero(); - } - else + spatAcc[0].setZero(); + } + else { - if (num_links > 0) + if (num_links > 0) { + m_cachedInertiaValid = true; m_cachedInertiaTopLeft = spatInertia[0].m_topLeftMat; m_cachedInertiaTopRight = spatInertia[0].m_topRightMat; m_cachedInertiaLowerLeft = spatInertia[0].m_bottomLeftMat; - m_cachedInertiaLowerRight= spatInertia[0].m_topLeftMat.transpose(); + m_cachedInertiaLowerRight = spatInertia[0].m_topLeftMat.transpose(); + } - } - solveImatrix(zeroAccSpatFrc[0], result); spatAcc[0] = -result; - } - - - // now do the loop over the m_links - for (int i = 0; i < num_links; ++i) + } + + // now do the loop over the m_links + for (int i = 0; i < num_links; ++i) { // qdd = D^{-1} * (Y - h^{T}*apar) = (S^{T}*I*S)^{-1} * (tau - S^{T}*I*cor - S^{T}*zeroAccFrc - S^{T}*I*apar) // a = apar + cor + Sqdd @@ -1022,79 +1077,79 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar // qdd = D^{-1} * (Y - h^{T}*(apar+cor)) // a = apar + Sqdd - const int parent = m_links[i].m_parent; - fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; + const int parent = m_links[i].m_parent; + fromParent.m_rotMat = rot_from_parent[i + 1]; + fromParent.m_trnVec = m_links[i].m_cachedRVector; + + fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]); - fromParent.transform(spatAcc[parent+1], spatAcc[i+1]); - - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) { - 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); + 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]; + 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) { m_internalNeedsJointFeedback = true; - btVector3 angularBotVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec; - btVector3 linearTopVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec; + btVector3 angularBotVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_bottomVec; + btVector3 linearTopVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_topVec; - if (gJointFeedbackInJointFrame) + if (jointFeedbackInJointFrame) { //shift the reaction forces to the joint frame //linear (force) component is the same //shift the angular (torque, moment) component using the relative position, m_links[i].m_dVector - angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector); + angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector); } - - if (gJointFeedbackInWorldSpace) + if (jointFeedbackInWorldSpace) { if (isConstraintPass) { - m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec; - m_links[i].m_jointFeedback->m_reactionForces.m_topVec += m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec; - } else + m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec; + m_links[i].m_jointFeedback->m_reactionForces.m_topVec += m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec; + } + else { - m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec; - m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec; + m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec; + m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec; } - } else + } + else { if (isConstraintPass) { - m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec; - m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec; - + m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec; + m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec; } else { - m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec; - m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec; - } - } + m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec; + m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec; + } + } + } } - } - - // transform base accelerations back to the world frame. - btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular(); + // transform base accelerations back to the world frame. + const btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular(); output[0] = omegadot_out[0]; output[1] = omegadot_out[1]; output[2] = omegadot_out[2]; - btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear())); + const btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear())); output[3] = vdot_out[0]; output[4] = vdot_out[1]; output[5] = vdot_out[2]; @@ -1117,26 +1172,25 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar //printf("]\n"); ///////////////// - // Final step: add the accelerations (times dt) to the velocities. + // Final step: add the accelerations (times dt) to the velocities. if (!isConstraintPass) { - if(dt > 0.) - applyDeltaVeeMultiDof(output, dt); - + if (dt > 0.) + applyDeltaVeeMultiDof(output, dt); } ///// //btScalar angularThres = 1; - //btScalar maxAngVel = 0.; + //btScalar maxAngVel = 0.; //bool scaleDown = 1.; //for(int link = 0; link < m_links.size(); ++link) - //{ + //{ // if(spatVel[link+1].getAngular().length() > maxAngVel) // { // maxAngVel = spatVel[link+1].getAngular().length(); // scaleDown = angularThres / spatVel[link+1].getAngular().length(); // break; - // } + // } //} //if(scaleDown != 1.) @@ -1153,128 +1207,173 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar ///// ///////////////////// - if(m_useGlobalVelocities) + if (m_useGlobalVelocities) { - for (int i = 0; i < num_links; ++i) + for (int i = 0; i < num_links; ++i) { const int parent = m_links[i].m_parent; //rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); /// <- done //rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; /// <- done - - fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; - fromWorld.m_rotMat = rot_from_world[i+1]; - - // vhat_i = i_xhat_p(i) * vhat_p(i) - fromParent.transform(spatVel[parent+1], spatVel[i+1]); + + fromParent.m_rotMat = rot_from_parent[i + 1]; + fromParent.m_trnVec = m_links[i].m_cachedRVector; + fromWorld.m_rotMat = rot_from_world[i + 1]; + + // vhat_i = i_xhat_p(i) * vhat_p(i) + fromParent.transform(spatVel[parent + 1], spatVel[i + 1]); //nice alternative below (using operator *) but it generates temps ///////////////////////////////////////////////////////////// // now set vhat_i to its true value by doing - // vhat_i += qidot * shat_i + // vhat_i += qidot * shat_i spatJointVel.setZero(); - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof]; - - // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint - spatVel[i+1] += spatJointVel; + // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint + spatVel[i + 1] += spatJointVel; - fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity); + fromWorld.transformInverseRotationOnly(spatVel[i + 1], m_links[i].m_absFrameTotVelocity); fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity); } } - } - - -void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const +void btMultiBody::solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar result[6]) const { int num_links = getNumLinks(); ///solve I * x = rhs, so the result = invI * rhs - if (num_links == 0) + if (num_links == 0) { // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier - result[0] = rhs_bot[0] / m_baseInertia[0]; - result[1] = rhs_bot[1] / m_baseInertia[1]; - result[2] = rhs_bot[2] / m_baseInertia[2]; - result[3] = rhs_top[0] / m_baseMass; - result[4] = rhs_top[1] / m_baseMass; - result[5] = rhs_top[2] / m_baseMass; - } else + + if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON)) + { + result[0] = rhs_bot[0] / m_baseInertia[0]; + result[1] = rhs_bot[1] / m_baseInertia[1]; + result[2] = rhs_bot[2] / m_baseInertia[2]; + } + else + { + result[0] = 0; + result[1] = 0; + result[2] = 0; + } + if (m_baseMass >= SIMD_EPSILON) + { + result[3] = rhs_top[0] / m_baseMass; + result[4] = rhs_top[1] / m_baseMass; + result[5] = rhs_top[2] / m_baseMass; + } + else + { + result[3] = 0; + result[4] = 0; + result[5] = 0; + } + } + else { + if (!m_cachedInertiaValid) + { + for (int i = 0; i < 6; i++) + { + result[i] = 0.f; + } + return; + } /// Special routine for calculating the inverse of a spatial inertia matrix ///the 6x6 matrix is stored as 4 blocks of 3x3 matrices - btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f; + btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse() * -1.f; btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv; btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse(); tmp = invIupper_right * m_cachedInertiaLowerRight; btMatrix3x3 invI_upper_left = (tmp * Binv); btMatrix3x3 invI_lower_right = (invI_upper_left).transpose(); - tmp = m_cachedInertiaTopLeft * invI_upper_left; - tmp[0][0]-= 1.0; - tmp[1][1]-= 1.0; - tmp[2][2]-= 1.0; + tmp = m_cachedInertiaTopLeft * invI_upper_left; + tmp[0][0] -= 1.0; + tmp[1][1] -= 1.0; + tmp[2][2] -= 1.0; btMatrix3x3 invI_lower_left = (Binv * tmp); //multiply result = invI * rhs { - btVector3 vtop = invI_upper_left*rhs_top; - btVector3 tmp; - tmp = invIupper_right * rhs_bot; - vtop += tmp; - btVector3 vbot = invI_lower_left*rhs_top; - tmp = invI_lower_right * rhs_bot; - vbot += tmp; - result[0] = vtop[0]; - result[1] = vtop[1]; - result[2] = vtop[2]; - result[3] = vbot[0]; - result[4] = vbot[1]; - result[5] = vbot[2]; + btVector3 vtop = invI_upper_left * rhs_top; + btVector3 tmp; + tmp = invIupper_right * rhs_bot; + vtop += tmp; + btVector3 vbot = invI_lower_left * rhs_top; + tmp = invI_lower_right * rhs_bot; + vbot += tmp; + result[0] = vtop[0]; + result[1] = vtop[1]; + result[2] = vtop[2]; + result[3] = vbot[0]; + result[4] = vbot[1]; + result[5] = vbot[2]; } - - } + } } void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const { int num_links = getNumLinks(); ///solve I * x = rhs, so the result = invI * rhs - if (num_links == 0) + if (num_links == 0) { // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier - result.setAngular(rhs.getAngular() / m_baseInertia); - result.setLinear(rhs.getLinear() / m_baseMass); - } else + if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON)) + { + result.setAngular(rhs.getAngular() / m_baseInertia); + } + else + { + result.setAngular(btVector3(0, 0, 0)); + } + if (m_baseMass >= SIMD_EPSILON) + { + result.setLinear(rhs.getLinear() / m_baseMass); + } + else + { + result.setLinear(btVector3(0, 0, 0)); + } + } + else { /// Special routine for calculating the inverse of a spatial inertia matrix ///the 6x6 matrix is stored as 4 blocks of 3x3 matrices - btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f; + if (!m_cachedInertiaValid) + { + result.setLinear(btVector3(0, 0, 0)); + result.setAngular(btVector3(0, 0, 0)); + result.setVector(btVector3(0, 0, 0), btVector3(0, 0, 0)); + return; + } + btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse() * -1.f; btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv; btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse(); tmp = invIupper_right * m_cachedInertiaLowerRight; btMatrix3x3 invI_upper_left = (tmp * Binv); btMatrix3x3 invI_lower_right = (invI_upper_left).transpose(); - tmp = m_cachedInertiaTopLeft * invI_upper_left; - tmp[0][0]-= 1.0; - tmp[1][1]-= 1.0; - tmp[2][2]-= 1.0; + tmp = m_cachedInertiaTopLeft * invI_upper_left; + tmp[0][0] -= 1.0; + tmp[1][1] -= 1.0; + tmp[2][2] -= 1.0; btMatrix3x3 invI_lower_left = (Binv * tmp); //multiply result = invI * rhs { - btVector3 vtop = invI_upper_left*rhs.getLinear(); - btVector3 tmp; - tmp = invIupper_right * rhs.getAngular(); - vtop += tmp; - btVector3 vbot = invI_lower_left*rhs.getLinear(); - tmp = invI_lower_right * rhs.getAngular(); - vbot += tmp; - result.setVector(vtop, vbot); + btVector3 vtop = invI_upper_left * rhs.getLinear(); + btVector3 tmp; + tmp = invIupper_right * rhs.getAngular(); + vtop += tmp; + btVector3 vbot = invI_lower_left * rhs.getLinear(); + tmp = invI_lower_right * rhs.getAngular(); + vbot += tmp; + result.setVector(vtop, vbot); } - - } + } } void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const @@ -1293,155 +1392,152 @@ void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, in } void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, - btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const + btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const { - // Temporary matrices/vectors -- use scratch space from caller - // so that we don't have to keep reallocating every frame + // Temporary matrices/vectors -- use scratch space from caller + // so that we don't have to keep reallocating every frame - - int num_links = getNumLinks(); - scratch_r.resize(m_dofCount); - scratch_v.resize(4*num_links + 4); + int num_links = getNumLinks(); + scratch_r.resize(m_dofCount); + scratch_v.resize(4 * num_links + 4); - btScalar * r_ptr = m_dofCount ? &scratch_r[0] : 0; - btVector3 * v_ptr = &scratch_v[0]; + btScalar *r_ptr = m_dofCount ? &scratch_r[0] : 0; + btVector3 *v_ptr = &scratch_v[0]; - // zhat_i^A (scratch space) - btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr; + // zhat_i^A (scratch space) + btSpatialForceVector *zeroAccSpatFrc = (btSpatialForceVector *)v_ptr; v_ptr += num_links * 2 + 2; - // rot_from_parent (cached from calcAccelerations) - const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0]; + // rot_from_parent (cached from calcAccelerations) + const btMatrix3x3 *rot_from_parent = &m_matrixBuf[0]; - // hhat (cached), accel (scratch) - // hhat is NOT stored for the base (but ahat is) - const btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0); - btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr; + // hhat (cached), accel (scratch) + // hhat is NOT stored for the base (but ahat is) + const btSpatialForceVector *h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0); + btSpatialMotionVector *spatAcc = (btSpatialMotionVector *)v_ptr; v_ptr += num_links * 2 + 2; - // Y_i (scratch), invD_i (cached) - const btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0; - btScalar * Y = r_ptr; + // Y_i (scratch), invD_i (cached) + const btScalar *invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0; + btScalar *Y = r_ptr; //////////////// //aux variables - static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies - static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel) - static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough - static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors - static btSpatialTransformationMatrix fromParent; + btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies + btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel) + btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough + btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors + btSpatialTransformationMatrix fromParent; ///////////////// - // First 'upward' loop. - // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. - + // First 'upward' loop. + // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. + // Fill in zero_acc - // -- set to force/torque on the base, zero otherwise - if (m_fixedBase) + // -- set to force/torque on the base, zero otherwise + if (m_fixedBase) + { + zeroAccSpatFrc[0].setZero(); + } + else { - zeroAccSpatFrc[0].setZero(); - } else - { //test forces fromParent.m_rotMat = rot_from_parent[0]; - fromParent.transformRotationOnly(btSpatialForceVector(-force[0],-force[1],-force[2], -force[3],-force[4],-force[5]), zeroAccSpatFrc[0]); - } - for (int i = 0; i < num_links; ++i) + fromParent.transformRotationOnly(btSpatialForceVector(-force[0], -force[1], -force[2], -force[3], -force[4], -force[5]), zeroAccSpatFrc[0]); + } + for (int i = 0; i < num_links; ++i) { - zeroAccSpatFrc[i+1].setZero(); - } + zeroAccSpatFrc[i + 1].setZero(); + } // 'Downward' loop. - // (part of TreeForwardDynamics in Mirtich.) - for (int i = num_links - 1; i >= 0; --i) + // (part of TreeForwardDynamics in Mirtich.) + for (int i = num_links - 1; i >= 0; --i) { const int parent = m_links[i].m_parent; - fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; + fromParent.m_rotMat = rot_from_parent[i + 1]; + fromParent.m_trnVec = m_links[i].m_cachedRVector; - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) { - Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof] - - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1]) - ; + Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof] - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i + 1]); } btVector3 in_top, in_bottom, out_top, out_bottom; - const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; - - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + const btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset]; + + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) { invD_times_Y[dof] = 0.f; - for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) + for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) { - invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2]; - } + invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2]; + } } - - // Zp += pXi * (Zi + hi*Yi/Di) - spatForceVecTemps[0] = zeroAccSpatFrc[i+1]; - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + // Zp += pXi * (Zi + hi*Yi/Di) + spatForceVecTemps[0] = zeroAccSpatFrc[i + 1]; + + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) { const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; // - spatForceVecTemps[0] += hDof * invD_times_Y[dof]; + spatForceVecTemps[0] += hDof * invD_times_Y[dof]; } - fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]); - - zeroAccSpatFrc[parent+1] += spatForceVecTemps[1]; - } - - // ptr to the joint accel part of the output - btScalar * joint_accel = output + 6; + zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1]; + } + + // ptr to the joint accel part of the output + btScalar *joint_accel = output + 6; - // Second 'upward' loop - // (part of TreeForwardDynamics in Mirtich) + // Second 'upward' loop + // (part of TreeForwardDynamics in Mirtich) - if (m_fixedBase) + if (m_fixedBase) { - spatAcc[0].setZero(); - } - else + spatAcc[0].setZero(); + } + else { solveImatrix(zeroAccSpatFrc[0], result); spatAcc[0] = -result; + } - } - - // now do the loop over the m_links - for (int i = 0; i < num_links; ++i) + // now do the loop over the m_links + for (int i = 0; i < num_links; ++i) { - const int parent = m_links[i].m_parent; - fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; + const int parent = m_links[i].m_parent; + fromParent.m_rotMat = rot_from_parent[i + 1]; + fromParent.m_trnVec = m_links[i].m_cachedRVector; - fromParent.transform(spatAcc[parent+1], spatAcc[i+1]); - - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]); + + 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); + // + Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof); } - const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; - mulMatrix(const_cast<btScalar*>(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]); + const btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset]; + mulMatrix(const_cast<btScalar *>(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) - 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]; + } - // transform base accelerations back to the world frame. - btVector3 omegadot_out; - omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular(); + // transform base accelerations back to the world frame. + btVector3 omegadot_out; + omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular(); output[0] = omegadot_out[0]; output[1] = omegadot_out[1]; output[2] = omegadot_out[2]; - btVector3 vdot_out; - vdot_out = rot_from_parent[0].transpose() * spatAcc[0].getLinear(); + btVector3 vdot_out; + vdot_out = rot_from_parent[0].transpose() * spatAcc[0].getLinear(); output[3] = vdot_out[0]; output[4] = vdot_out[1]; output[5] = vdot_out[2]; @@ -1453,20 +1549,169 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar //printf("]\n"); ///////////////// } +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; + + 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) + struct + { + //"exponential map" based on btTransformUtil::integrateTransform(..) + void operator()(const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt) + { + //baseBody => quat is alias and omega is global coor + //!baseBody => quat is alibi and omega is local coor + + btVector3 axis; + btVector3 angvel; + + if (!baseBody) + angvel = quatRotate(quat, omega); //if quat is not m_baseQuat, it is alibi => ok + else + angvel = omega; + + btScalar fAngle = angvel.length(); + //limit the angular motion + if (fAngle * dt > ANGULAR_MOTION_THRESHOLD) + { + fAngle = btScalar(0.5) * SIMD_HALF_PI / dt; + } + + if (fAngle < btScalar(0.001)) + { + // use Taylor's expansions of sync function + axis = angvel * (btScalar(0.5) * dt - (dt * dt * dt) * (btScalar(0.020833333333)) * fAngle * fAngle); + } + else + { + // sync(fAngle) = sin(c*fAngle)/t + axis = angvel * (btSin(btScalar(0.5) * fAngle * dt) / fAngle); + } + + if (!baseBody) + quat = btQuaternion(axis.x(), axis.y(), axis.z(), btCos(fAngle * dt * btScalar(0.5))) * quat; + else + quat = quat * btQuaternion(-axis.x(), -axis.y(), -axis.z(), btCos(fAngle * dt * btScalar(0.5))); + //equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse(); + + quat.normalize(); + } + } pQuatUpdateFun; + /////////////////////////////// + + //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; - + 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) + { + 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) + { + 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: + { + } + } + + m_links[i].updateInterpolationCacheMultiDof(); + } +} 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; + // 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) - // + 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]; @@ -1476,92 +1721,102 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd struct { //"exponential map" based on btTransformUtil::integrateTransform(..) - void operator() (const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt) + void operator()(const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt) { //baseBody => quat is alias and omega is global coor - //!baseBody => quat is alibi and omega is local coor - + //!baseBody => quat is alibi and omega is local coor + btVector3 axis; btVector3 angvel; - if(!baseBody) - angvel = quatRotate(quat, omega); //if quat is not m_baseQuat, it is alibi => ok + if (!baseBody) + angvel = quatRotate(quat, omega); //if quat is not m_baseQuat, it is alibi => ok else angvel = omega; - - btScalar fAngle = angvel.length(); + + btScalar fAngle = angvel.length(); //limit the angular motion if (fAngle * dt > ANGULAR_MOTION_THRESHOLD) { - fAngle = btScalar(0.5)*SIMD_HALF_PI / dt; + fAngle = btScalar(0.5) * SIMD_HALF_PI / dt; } - if ( fAngle < btScalar(0.001) ) + if (fAngle < btScalar(0.001)) { // use Taylor's expansions of sync function - axis = angvel*( btScalar(0.5)*dt-(dt*dt*dt)*(btScalar(0.020833333333))*fAngle*fAngle ); + axis = angvel * (btScalar(0.5) * dt - (dt * dt * dt) * (btScalar(0.020833333333)) * fAngle * fAngle); } else { // sync(fAngle) = sin(c*fAngle)/t - axis = angvel*( btSin(btScalar(0.5)*fAngle*dt)/fAngle ); + axis = angvel * (btSin(btScalar(0.5) * fAngle * dt) / fAngle); } - - if(!baseBody) - quat = btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat; - else - quat = quat * btQuaternion(-axis.x(),-axis.y(),-axis.z(),btCos( fAngle*dt*btScalar(0.5) )); - //equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse(); - + + if (!baseBody) + quat = btQuaternion(axis.x(), axis.y(), axis.z(), btCos(fAngle * dt * btScalar(0.5))) * quat; + else + quat = quat * btQuaternion(-axis.x(), -axis.y(), -axis.z(), btCos(fAngle * dt * btScalar(0.5))); + //equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse(); + quat.normalize(); } } pQuatUpdateFun; /////////////////////////////// //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) // - static btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); - static btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); + 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) + if (pq) pq += 7; - if(pqd) + if (pqd) pqd += 6; // Finally we can update m_jointPos for each of the m_links - for (int i = 0; i < num_links; ++i) + for (int i = 0; i < num_links; ++i) { - btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]); + btScalar *pJointPos; + pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]); + btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i)); - switch(m_links[i].m_jointType) + switch (m_links[i].m_jointType) { case btMultibodyLink::ePrismatic: case btMultibodyLink::eRevolute: { - btScalar jointVel = pJointVel[0]; + //reset to current pos + btScalar jointVel = pJointVel[0]; pJointPos[0] += dt * jointVel; break; } case btMultibodyLink::eSpherical: { - static btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); - static btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); + //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(); + pJointPos[0] = jointOri.x(); + pJointPos[1] = jointOri.y(); + pJointPos[2] = jointOri.z(); + pJointPos[3] = jointOri.w(); break; } case btMultibodyLink::ePlanar: @@ -1578,122 +1833,135 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd default: { } - } m_links[i].updateCacheMultiDof(pq); - if(pq) + if (pq) pq += m_links[i].m_posVarCount; - if(pqd) + if (pqd) pqd += m_links[i].m_dofCount; - } + } } void btMultiBody::fillConstraintJacobianMultiDof(int link, - const btVector3 &contact_point, - const btVector3 &normal_ang, - const btVector3 &normal_lin, - btScalar *jac, - btAlignedObjectArray<btScalar> &scratch_r, - btAlignedObjectArray<btVector3> &scratch_v, - btAlignedObjectArray<btMatrix3x3> &scratch_m) const -{ - // temporary space + const btVector3 &contact_point, + const btVector3 &normal_ang, + const btVector3 &normal_lin, + btScalar *jac, + btAlignedObjectArray<btScalar> &scratch_r1, + btAlignedObjectArray<btVector3> &scratch_v, + btAlignedObjectArray<btMatrix3x3> &scratch_m) const +{ + // temporary space int num_links = getNumLinks(); int m_dofCount = getNumDofs(); - scratch_v.resize(3*num_links + 3); //(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang - scratch_m.resize(num_links + 1); - - btVector3 * v_ptr = &scratch_v[0]; - btVector3 * p_minus_com_local = v_ptr; v_ptr += num_links + 1; - btVector3 * n_local_lin = v_ptr; v_ptr += num_links + 1; - btVector3 * n_local_ang = v_ptr; v_ptr += num_links + 1; - btAssert(v_ptr - &scratch_v[0] == scratch_v.size()); - - scratch_r.resize(m_dofCount); - btScalar * results = m_dofCount > 0 ? &scratch_r[0] : 0; - - btMatrix3x3 * rot_from_world = &scratch_m[0]; + scratch_v.resize(3 * num_links + 3); //(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang + scratch_m.resize(num_links + 1); + + btVector3 *v_ptr = &scratch_v[0]; + btVector3 *p_minus_com_local = v_ptr; + v_ptr += num_links + 1; + btVector3 *n_local_lin = v_ptr; + v_ptr += num_links + 1; + btVector3 *n_local_ang = v_ptr; + v_ptr += num_links + 1; + btAssert(v_ptr - &scratch_v[0] == scratch_v.size()); + + //scratch_r.resize(m_dofCount); + //btScalar *results = m_dofCount > 0 ? &scratch_r[0] : 0; + + scratch_r1.resize(m_dofCount+num_links); + btScalar * results = m_dofCount > 0 ? &scratch_r1[0] : 0; + btScalar* links = num_links? &scratch_r1[m_dofCount] : 0; + int numLinksChildToRoot=0; + int l = link; + while (l != -1) + { + links[numLinksChildToRoot++]=l; + l = m_links[l].m_parent; + } + + btMatrix3x3 *rot_from_world = &scratch_m[0]; - const btVector3 p_minus_com_world = contact_point - m_basePos; - const btVector3 &normal_lin_world = normal_lin; //convenience + const btVector3 p_minus_com_world = contact_point - m_basePos; + const btVector3 &normal_lin_world = normal_lin; //convenience const btVector3 &normal_ang_world = normal_ang; - rot_from_world[0] = btMatrix3x3(m_baseQuat); - - // omega coeffients first. - btVector3 omega_coeffs_world; - omega_coeffs_world = p_minus_com_world.cross(normal_lin_world); + rot_from_world[0] = btMatrix3x3(m_baseQuat); + + // omega coeffients first. + btVector3 omega_coeffs_world; + omega_coeffs_world = p_minus_com_world.cross(normal_lin_world); jac[0] = omega_coeffs_world[0] + normal_ang_world[0]; jac[1] = omega_coeffs_world[1] + normal_ang_world[1]; jac[2] = omega_coeffs_world[2] + normal_ang_world[2]; - // then v coefficients - jac[3] = normal_lin_world[0]; - jac[4] = normal_lin_world[1]; - jac[5] = normal_lin_world[2]; + // then v coefficients + jac[3] = normal_lin_world[0]; + jac[4] = normal_lin_world[1]; + jac[5] = normal_lin_world[2]; //create link-local versions of p_minus_com and normal p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world; - n_local_lin[0] = rot_from_world[0] * normal_lin_world; + n_local_lin[0] = rot_from_world[0] * normal_lin_world; n_local_ang[0] = rot_from_world[0] * normal_ang_world; - // Set remaining jac values to zero for now. - for (int i = 6; i < 6 + m_dofCount; ++i) + // Set remaining jac values to zero for now. + for (int i = 6; i < 6 + m_dofCount; ++i) { - jac[i] = 0; - } - - // Qdot coefficients, if necessary. - if (num_links > 0 && link > -1) { - - // TODO: speed this up -- don't calculate for m_links we don't need. - // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions, - // which is resulting in repeated work being done...) - - // calculate required normals & positions in the local frames. - for (int i = 0; i < num_links; ++i) { + jac[i] = 0; + } - // transform to local frame - const int parent = m_links[i].m_parent; - const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis); - rot_from_world[i+1] = mtx * rot_from_world[parent+1]; + // Qdot coefficients, if necessary. + if (num_links > 0 && link > -1) + { + // TODO: (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions, + // which is resulting in repeated work being done...) + + // calculate required normals & positions in the local frames. + for (int a = 0; a < numLinksChildToRoot; a++) + { + int i = links[numLinksChildToRoot-1-a]; + // transform to local frame + const int parent = m_links[i].m_parent; + const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis); + rot_from_world[i + 1] = mtx * rot_from_world[parent + 1]; - n_local_lin[i+1] = mtx * n_local_lin[parent+1]; - n_local_ang[i+1] = mtx * n_local_ang[parent+1]; - p_minus_com_local[i+1] = mtx * p_minus_com_local[parent+1] - m_links[i].m_cachedRVector; + n_local_lin[i + 1] = mtx * n_local_lin[parent + 1]; + n_local_ang[i + 1] = mtx * n_local_ang[parent + 1]; + p_minus_com_local[i + 1] = mtx * p_minus_com_local[parent + 1] - m_links[i].m_cachedRVector; // calculate the jacobian entry - switch(m_links[i].m_jointType) + switch (m_links[i].m_jointType) { case btMultibodyLink::eRevolute: { - results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0)); - results[m_links[i].m_dofOffset] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0)); + results[m_links[i].m_dofOffset] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(0)); + results[m_links[i].m_dofOffset] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(0)); break; } case btMultibodyLink::ePrismatic: { - results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(0)); + results[m_links[i].m_dofOffset] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(0)); break; } case btMultibodyLink::eSpherical: { - results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0)); - results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisTop(1).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(1)); - results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(2)); - - results[m_links[i].m_dofOffset + 0] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0)); - results[m_links[i].m_dofOffset + 1] += n_local_ang[i+1].dot(m_links[i].getAxisTop(1)); - results[m_links[i].m_dofOffset + 2] += n_local_ang[i+1].dot(m_links[i].getAxisTop(2)); + results[m_links[i].m_dofOffset + 0] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(0)); + results[m_links[i].m_dofOffset + 1] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(1).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(1)); + results[m_links[i].m_dofOffset + 2] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(2)); + + results[m_links[i].m_dofOffset + 0] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(0)); + results[m_links[i].m_dofOffset + 1] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(1)); + results[m_links[i].m_dofOffset + 2] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(2)); break; } case btMultibodyLink::ePlanar: { - results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0)); - results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(1)); - results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(2)); + results[m_links[i].m_dofOffset + 0] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i + 1])); // + m_links[i].getAxisBottom(0)); + results[m_links[i].m_dofOffset + 1] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(1)); + results[m_links[i].m_dofOffset + 2] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(2)); break; } @@ -1701,246 +1969,318 @@ void btMultiBody::fillConstraintJacobianMultiDof(int link, { } } - - } + } - // Now copy through to output. + // Now copy through to output. //printf("jac[%d] = ", link); - while (link != -1) + while (link != -1) { - for(int dof = 0; dof < m_links[link].m_dofCount; ++dof) + for (int dof = 0; dof < m_links[link].m_dofCount; ++dof) { jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof]; //printf("%.2f\t", jac[6 + m_links[link].m_dofOffset + dof]); } - + link = m_links[link].m_parent; - } + } //printf("]\n"); - } + } } - void btMultiBody::wakeUp() { - m_awake = true; + m_sleepTimer = 0; + m_awake = true; } void btMultiBody::goToSleep() { - m_awake = false; + m_awake = false; } void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep) { extern bool gDisableDeactivation; - if (!m_canSleep || gDisableDeactivation) + if (!m_canSleep || gDisableDeactivation) { m_awake = true; m_sleepTimer = 0; return; } - // motion is computed as omega^2 + v^2 + (sum of squares of joint velocities) - btScalar motion = 0; + + + // motion is computed as omega^2 + v^2 + (sum of squares of joint velocities) + btScalar motion = 0; { - for (int i = 0; i < 6 + m_dofCount; ++i) + for (int i = 0; i < 6 + m_dofCount; ++i) motion += m_realBuf[i] * m_realBuf[i]; } - - if (motion < SLEEP_EPSILON) { - m_sleepTimer += timestep; - if (m_sleepTimer > SLEEP_TIMEOUT) { - goToSleep(); - } - } else { - m_sleepTimer = 0; - if (!m_awake) - wakeUp(); - } + if (motion < SLEEP_EPSILON) + { + m_sleepTimer += timestep; + if (m_sleepTimer > SLEEP_TIMEOUT) + { + goToSleep(); + } + } + else + { + m_sleepTimer = 0; + if (m_canWakeup) + { + if (!m_awake) + wakeUp(); + } + } } - -void btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to_local,btAlignedObjectArray<btVector3>& local_origin) +void btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin) { - int num_links = getNumLinks(); // Cached 3x3 rotation matrices from parent frame to this frame. - btMatrix3x3* rot_from_parent =(btMatrix3x3 *) &m_matrixBuf[0]; + btMatrix3x3 *rot_from_parent = (btMatrix3x3 *)&m_matrixBuf[0]; - rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!? - - for (int i = 0; i < num_links; ++i) + rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!? + + for (int i = 0; i < num_links; ++i) { - rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); + rot_from_parent[i + 1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); } - + int nLinks = getNumLinks(); ///base + num m_links - world_to_local.resize(nLinks+1); - local_origin.resize(nLinks+1); + world_to_local.resize(nLinks + 1); + local_origin.resize(nLinks + 1); world_to_local[0] = getWorldToBaseRot(); local_origin[0] = getBasePos(); - - for (int k=0;k<getNumLinks();k++) + + for (int k = 0; k < getNumLinks(); k++) { const int parent = getParent(k); - world_to_local[k+1] = getParentToLocalRot(k) * world_to_local[parent+1]; - local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , getRVector(k))); + world_to_local[k + 1] = getParentToLocalRot(k) * world_to_local[parent + 1]; + local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getRVector(k))); } - for (int link=0;link<getNumLinks();link++) + for (int link = 0; link < getNumLinks(); link++) { - int index = link+1; + int index = link + 1; btVector3 posr = local_origin[index]; - btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()}; + btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()}; btTransform tr; tr.setIdentity(); tr.setOrigin(posr); - tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); getLink(link).m_cachedWorldTransform = tr; - } - } -void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& world_to_local,btAlignedObjectArray<btVector3>& local_origin) +void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin) { - world_to_local.resize(getNumLinks()+1); - local_origin.resize(getNumLinks()+1); - + world_to_local.resize(getNumLinks() + 1); + local_origin.resize(getNumLinks() + 1); + world_to_local[0] = getWorldToBaseRot(); local_origin[0] = getBasePos(); - + if (getBaseCollider()) { btVector3 posr = local_origin[0]; // float pos[4]={posr.x(),posr.y(),posr.z(),1}; - btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; + btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()}; btTransform tr; tr.setIdentity(); tr.setOrigin(posr); - tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); - + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + getBaseCollider()->setWorldTransform(tr); - + getBaseCollider()->setInterpolationWorldTransform(tr); } - - for (int k=0;k<getNumLinks();k++) + + for (int k = 0; k < getNumLinks(); k++) { const int parent = getParent(k); - world_to_local[k+1] = getParentToLocalRot(k) * world_to_local[parent+1]; - local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , getRVector(k))); + world_to_local[k + 1] = getParentToLocalRot(k) * world_to_local[parent + 1]; + local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getRVector(k))); } - - - for (int m=0;m<getNumLinks();m++) + + for (int m = 0; m < getNumLinks(); m++) { - btMultiBodyLinkCollider* col = getLink(m).m_collider; + btMultiBodyLinkCollider *col = getLink(m).m_collider; if (col) { int link = col->m_link; btAssert(link == m); - - int index = link+1; - + + int index = link + 1; + btVector3 posr = local_origin[index]; // float pos[4]={posr.x(),posr.y(),posr.z(),1}; - btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()}; + btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()}; btTransform tr; tr.setIdentity(); tr.setOrigin(posr); - tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); - + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + col->setWorldTransform(tr); + col->setInterpolationWorldTransform(tr); } } } -int btMultiBody::calculateSerializeBufferSize() const +void btMultiBody::updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin) +{ + world_to_local.resize(getNumLinks() + 1); + local_origin.resize(getNumLinks() + 1); + + world_to_local[0] = getInterpolateWorldToBaseRot(); + local_origin[0] = getInterpolateBasePos(); + + if (getBaseCollider()) + { + btVector3 posr = local_origin[0]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()}; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + + getBaseCollider()->setInterpolationWorldTransform(tr); + } + + for (int k = 0; k < getNumLinks(); k++) + { + const int parent = getParent(k); + world_to_local[k + 1] = getInterpolateParentToLocalRot(k) * world_to_local[parent + 1]; + local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getInterpolateRVector(k))); + } + + for (int m = 0; m < getNumLinks(); m++) + { + btMultiBodyLinkCollider *col = getLink(m).m_collider; + if (col) + { + int link = col->m_link; + btAssert(link == m); + + int index = link + 1; + + btVector3 posr = local_origin[index]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()}; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + + col->setInterpolationWorldTransform(tr); + } + } +} + +int btMultiBody::calculateSerializeBufferSize() const { int sz = sizeof(btMultiBodyData); return sz; } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btMultiBody::serialize(void* dataBuffer, class btSerializer* serializer) const +///fills the dataBuffer and returns the struct name (and 0 on failure) +const char *btMultiBody::serialize(void *dataBuffer, class btSerializer *serializer) const { - btMultiBodyData* mbd = (btMultiBodyData*) dataBuffer; - getBaseWorldTransform().serialize(mbd->m_baseWorldTransform); - mbd->m_baseMass = this->getBaseMass(); - getBaseInertia().serialize(mbd->m_baseInertia); + btMultiBodyData *mbd = (btMultiBodyData *)dataBuffer; + getBasePos().serialize(mbd->m_baseWorldPosition); + getWorldToBaseRot().inverse().serialize(mbd->m_baseWorldOrientation); + getBaseVel().serialize(mbd->m_baseLinearVelocity); + getBaseOmega().serialize(mbd->m_baseAngularVelocity); + + mbd->m_baseMass = this->getBaseMass(); + getBaseInertia().serialize(mbd->m_baseInertia); + { + char *name = (char *)serializer->findNameForPointer(m_baseName); + mbd->m_baseName = (char *)serializer->getUniquePointer(name); + if (mbd->m_baseName) { - char* name = (char*) serializer->findNameForPointer(m_baseName); - mbd->m_baseName = (char*)serializer->getUniquePointer(name); - if (mbd->m_baseName) - { - serializer->serializeName(name); - } + serializer->serializeName(name); } - mbd->m_numLinks = this->getNumLinks(); - if (mbd->m_numLinks) + } + mbd->m_numLinks = this->getNumLinks(); + if (mbd->m_numLinks) + { + int sz = sizeof(btMultiBodyLinkData); + int numElem = mbd->m_numLinks; + btChunk *chunk = serializer->allocate(sz, numElem); + btMultiBodyLinkData *memPtr = (btMultiBodyLinkData *)chunk->m_oldPtr; + for (int i = 0; i < numElem; i++, memPtr++) { - int sz = sizeof(btMultiBodyLinkData); - int numElem = mbd->m_numLinks; - btChunk* chunk = serializer->allocate(sz,numElem); - btMultiBodyLinkData* memPtr = (btMultiBodyLinkData*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + memPtr->m_jointType = getLink(i).m_jointType; + memPtr->m_dofCount = getLink(i).m_dofCount; + memPtr->m_posVarCount = getLink(i).m_posVarCount; + + getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia); + + getLink(i).m_absFrameTotVelocity.m_topVec.serialize(memPtr->m_absFrameTotVelocityTop); + getLink(i).m_absFrameTotVelocity.m_bottomVec.serialize(memPtr->m_absFrameTotVelocityBottom); + getLink(i).m_absFrameLocVelocity.m_topVec.serialize(memPtr->m_absFrameLocVelocityTop); + getLink(i).m_absFrameLocVelocity.m_bottomVec.serialize(memPtr->m_absFrameLocVelocityBottom); + + memPtr->m_linkMass = getLink(i).m_mass; + memPtr->m_parentIndex = getLink(i).m_parent; + memPtr->m_jointDamping = getLink(i).m_jointDamping; + memPtr->m_jointFriction = getLink(i).m_jointFriction; + memPtr->m_jointLowerLimit = getLink(i).m_jointLowerLimit; + memPtr->m_jointUpperLimit = getLink(i).m_jointUpperLimit; + memPtr->m_jointMaxForce = getLink(i).m_jointMaxForce; + memPtr->m_jointMaxVelocity = getLink(i).m_jointMaxVelocity; + + getLink(i).m_eVector.serialize(memPtr->m_parentComToThisPivotOffset); + getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset); + getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis); + btAssert(memPtr->m_dofCount <= 3); + for (int dof = 0; dof < getLink(i).m_dofCount; dof++) { + getLink(i).getAxisBottom(dof).serialize(memPtr->m_jointAxisBottom[dof]); + getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]); - memPtr->m_jointType = getLink(i).m_jointType; - memPtr->m_dofCount = getLink(i).m_dofCount; - memPtr->m_posVarCount = getLink(i).m_posVarCount; - - getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia); - memPtr->m_linkMass = getLink(i).m_mass; - memPtr->m_parentIndex = getLink(i).m_parent; - getLink(i).m_eVector.serialize(memPtr->m_parentComToThisComOffset); - getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset); - getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis); - btAssert(memPtr->m_dofCount<=3); - for (int dof = 0;dof<getLink(i).m_dofCount;dof++) - { - getLink(i).getAxisBottom(dof).serialize(memPtr->m_jointAxisBottom[dof]); - getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]); - - memPtr->m_jointTorque[dof] = getLink(i).m_jointTorque[dof]; - memPtr->m_jointVel[dof] = getJointVelMultiDof(i)[dof]; + memPtr->m_jointTorque[dof] = getLink(i).m_jointTorque[dof]; + memPtr->m_jointVel[dof] = getJointVelMultiDof(i)[dof]; + } + int numPosVar = getLink(i).m_posVarCount; + for (int posvar = 0; posvar < numPosVar; posvar++) + { + memPtr->m_jointPos[posvar] = getLink(i).m_jointPos[posvar]; + } - } - int numPosVar = getLink(i).m_posVarCount; - for (int posvar = 0; posvar < numPosVar;posvar++) + { + char *name = (char *)serializer->findNameForPointer(m_links[i].m_linkName); + memPtr->m_linkName = (char *)serializer->getUniquePointer(name); + if (memPtr->m_linkName) { - memPtr->m_jointPos[posvar] = getLink(i).m_jointPos[posvar]; + serializer->serializeName(name); } - - + } + { + char *name = (char *)serializer->findNameForPointer(m_links[i].m_jointName); + memPtr->m_jointName = (char *)serializer->getUniquePointer(name); + if (memPtr->m_jointName) { - char* name = (char*) serializer->findNameForPointer(m_links[i].m_linkName); - memPtr->m_linkName = (char*)serializer->getUniquePointer(name); - if (memPtr->m_linkName) - { - serializer->serializeName(name); - } + serializer->serializeName(name); } - { - char* name = (char*) serializer->findNameForPointer(m_links[i].m_jointName); - memPtr->m_jointName = (char*)serializer->getUniquePointer(name); - if (memPtr->m_jointName) - { - serializer->serializeName(name); - } - } - memPtr->m_linkCollider = (btCollisionObjectData*)serializer->getUniquePointer(getLink(i).m_collider); - } - serializer->finalizeChunk(chunk,btMultiBodyLinkDataName,BT_ARRAY_CODE,(void*) &m_links[0]); + memPtr->m_linkCollider = (btCollisionObjectData *)serializer->getUniquePointer(getLink(i).m_collider); } - mbd->m_links = mbd->m_numLinks? (btMultiBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0; + serializer->finalizeChunk(chunk, btMultiBodyLinkDataName, BT_ARRAY_CODE, (void *)&m_links[0]); + } + mbd->m_links = mbd->m_numLinks ? (btMultiBodyLinkData *)serializer->getUniquePointer((void *)&m_links[0]) : 0; + + // Fill padding with zeros to appease msan. +#ifdef BT_USE_DOUBLE_PRECISION + memset(mbd->m_padding, 0, sizeof(mbd->m_padding)); +#endif - return btMultiBodyDataName; + return btMultiBodyDataName; } diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h index 8fbe6cda827..f2acfab9b24 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h @@ -21,7 +21,6 @@ */ - #ifndef BT_MULTIBODY_H #define BT_MULTIBODY_H @@ -31,170 +30,193 @@ #include "LinearMath/btMatrix3x3.h" #include "LinearMath/btAlignedObjectArray.h" - ///serialization data, don't change them if you are not familiar with the details of the serialization mechanisms #ifdef BT_USE_DOUBLE_PRECISION - #define btMultiBodyData btMultiBodyDoubleData - #define btMultiBodyDataName "btMultiBodyDoubleData" - #define btMultiBodyLinkData btMultiBodyLinkDoubleData - #define btMultiBodyLinkDataName "btMultiBodyLinkDoubleData" +#define btMultiBodyData btMultiBodyDoubleData +#define btMultiBodyDataName "btMultiBodyDoubleData" +#define btMultiBodyLinkData btMultiBodyLinkDoubleData +#define btMultiBodyLinkDataName "btMultiBodyLinkDoubleData" #else - #define btMultiBodyData btMultiBodyFloatData - #define btMultiBodyDataName "btMultiBodyFloatData" - #define btMultiBodyLinkData btMultiBodyLinkFloatData - #define btMultiBodyLinkDataName "btMultiBodyLinkFloatData" -#endif //BT_USE_DOUBLE_PRECISION +#define btMultiBodyData btMultiBodyFloatData +#define btMultiBodyDataName "btMultiBodyFloatData" +#define btMultiBodyLinkData btMultiBodyLinkFloatData +#define btMultiBodyLinkDataName "btMultiBodyLinkFloatData" +#endif //BT_USE_DOUBLE_PRECISION #include "btMultiBodyLink.h" class btMultiBodyLinkCollider; -ATTRIBUTE_ALIGNED16(class) btMultiBody +ATTRIBUTE_ALIGNED16(class) +btMultiBody { public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - // - // initialization - // - - btMultiBody(int n_links, // NOT including the base - btScalar mass, // mass of base - const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal - bool fixedBase, // whether the base is fixed (true) or can move (false) - bool canSleep, bool deprecatedMultiDof=true); + // + // initialization + // + btMultiBody(int n_links, // NOT including the base + btScalar mass, // mass of base + const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal + bool fixedBase, // whether the base is fixed (true) or can move (false) + bool canSleep, bool deprecatedMultiDof = true); virtual ~btMultiBody(); - + //note: fixed link collision with parent is always disabled - void setupFixed(int linkIndex, - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, - const btVector3 &parentComToThisPivotOffset, - const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision=true); - - + void setupFixed(int i, //linkIndex + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, + const btVector3 &parentComToThisPivotOffset, + const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision = true); + void setupPrismatic(int i, - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, - const btVector3 &jointAxis, - const btVector3 &parentComToThisPivotOffset, - const btVector3 &thisPivotToThisComOffset, - bool disableParentCollision); - - void setupRevolute(int linkIndex, // 0 to num_links-1 - btScalar mass, - const btVector3 &inertia, - int parentIndex, - const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 - const btVector3 &jointAxis, // in my frame - const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame - const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame - bool disableParentCollision=false); - - void setupSpherical(int linkIndex, // 0 to num_links-1 - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 - const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame - const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame - bool disableParentCollision=false); - - void setupPlanar(int i, // 0 to num_links-1 - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 - const btVector3 &rotationAxis, - const btVector3 &parentComToThisComOffset, // vector from parent COM to this COM, in PARENT frame - bool disableParentCollision=false); - - const btMultibodyLink& getLink(int index) const + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, + const btVector3 &jointAxis, + const btVector3 &parentComToThisPivotOffset, + const btVector3 &thisPivotToThisComOffset, + bool disableParentCollision); + + void setupRevolute(int i, // 0 to num_links-1 + btScalar mass, + const btVector3 &inertia, + int parentIndex, + const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 + const btVector3 &jointAxis, // in my frame + const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame + const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame + bool disableParentCollision = false); + + void setupSpherical(int i, // linkIndex, 0 to num_links-1 + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 + const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame + const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame + bool disableParentCollision = false); + + void setupPlanar(int i, // 0 to num_links-1 + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 + const btVector3 &rotationAxis, + const btVector3 &parentComToThisComOffset, // vector from parent COM to this COM, in PARENT frame + bool disableParentCollision = false); + + const btMultibodyLink &getLink(int index) const { return m_links[index]; } - btMultibodyLink& getLink(int index) + btMultibodyLink &getLink(int index) { return m_links[index]; } - - void setBaseCollider(btMultiBodyLinkCollider* collider)//collider can be NULL to disable collision for the base + void setBaseCollider(btMultiBodyLinkCollider * collider) //collider can be NULL to disable collision for the base { m_baseCollider = collider; } - const btMultiBodyLinkCollider* getBaseCollider() const + const btMultiBodyLinkCollider *getBaseCollider() const { return m_baseCollider; } - btMultiBodyLinkCollider* getBaseCollider() + btMultiBodyLinkCollider *getBaseCollider() { return m_baseCollider; } - // - // get parent - // input: link num from 0 to num_links-1 - // output: link num from 0 to num_links-1, OR -1 to mean the base. - // - int getParent(int link_num) const; - - - // - // get number of m_links, masses, moments of inertia - // + const btMultiBodyLinkCollider *getLinkCollider(int index) const + { + if (index >= 0 && index < getNumLinks()) + { + return getLink(index).m_collider; + } + return 0; + } + + btMultiBodyLinkCollider *getLinkCollider(int index) + { + if (index >= 0 && index < getNumLinks()) + { + return getLink(index).m_collider; + } + return 0; + } - int getNumLinks() const { return m_links.size(); } + // + // get parent + // input: link num from 0 to num_links-1 + // output: link num from 0 to num_links-1, OR -1 to mean the base. + // + int getParent(int link_num) const; + + // + // get number of m_links, masses, moments of inertia + // + + int getNumLinks() const { return m_links.size(); } int getNumDofs() const { return m_dofCount; } int getNumPosVars() const { return m_posVarCnt; } - btScalar getBaseMass() const { return m_baseMass; } - const btVector3 & getBaseInertia() const { return m_baseInertia; } - btScalar getLinkMass(int i) const; - const btVector3 & getLinkInertia(int i) const; - - - - // - // change mass (incomplete: can only change base mass and inertia at present) - // + btScalar getBaseMass() const { return m_baseMass; } + const btVector3 &getBaseInertia() const { return m_baseInertia; } + btScalar getLinkMass(int i) const; + const btVector3 &getLinkInertia(int i) const; - void setBaseMass(btScalar mass) { m_baseMass = mass; } - void setBaseInertia(const btVector3 &inertia) { m_baseInertia = inertia; } + // + // change mass (incomplete: can only change base mass and inertia at present) + // + void setBaseMass(btScalar mass) { m_baseMass = mass; } + void setBaseInertia(const btVector3 &inertia) { m_baseInertia = inertia; } - // - // get/set pos/vel/rot/omega for the base link - // + // + // get/set pos/vel/rot/omega for the base link + // - const btVector3 & getBasePos() const { return m_basePos; } // in world frame - const btVector3 getBaseVel() const - { - return btVector3(m_realBuf[3],m_realBuf[4],m_realBuf[5]); - } // in world frame - const btQuaternion & getWorldToBaseRot() const + const btVector3 &getBasePos() const { - return m_baseQuat; - } // rotates world vectors into base frame - btVector3 getBaseOmega() const { return btVector3(m_realBuf[0],m_realBuf[1],m_realBuf[2]); } // in world frame + return m_basePos; + } // in world frame + const btVector3 getBaseVel() const + { + return btVector3(m_realBuf[3], m_realBuf[4], m_realBuf[5]); + } // in world frame + const btQuaternion &getWorldToBaseRot() const + { + return m_baseQuat; + } + + const btVector3 &getInterpolateBasePos() const + { + return m_basePos_interpolate; + } // in world frame + const btQuaternion &getInterpolateWorldToBaseRot() const + { + return m_baseQuat_interpolate; + } + + // rotates world vectors into base frame + btVector3 getBaseOmega() const { return btVector3(m_realBuf[0], m_realBuf[1], m_realBuf[2]); } // in world frame - void setBasePos(const btVector3 &pos) - { - m_basePos = pos; + void setBasePos(const btVector3 &pos) + { + m_basePos = pos; + m_basePos_interpolate = pos; } - void setBaseWorldTransform(const btTransform& tr) + void setBaseWorldTransform(const btTransform &tr) { setBasePos(tr.getOrigin()); setWorldToBaseRot(tr.getRotation().inverse()); - } btTransform getBaseWorldTransform() const @@ -205,186 +227,212 @@ public: return tr; } - void setBaseVel(const btVector3 &vel) - { - - m_realBuf[3]=vel[0]; m_realBuf[4]=vel[1]; m_realBuf[5]=vel[2]; + 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!? + void setWorldToBaseRot(const btQuaternion &rot) + { + m_baseQuat = rot; //m_baseQuat asumed to ba alias!? + m_baseQuat_interpolate = rot; } - void setBaseOmega(const btVector3 &omega) - { - m_realBuf[0]=omega[0]; - m_realBuf[1]=omega[1]; - m_realBuf[2]=omega[2]; + void setBaseOmega(const btVector3 &omega) + { + m_realBuf[0] = omega[0]; + m_realBuf[1] = omega[1]; + m_realBuf[2] = omega[2]; } + // + // get/set pos/vel for child m_links (i = 0 to num_links-1) + // - // - // get/set pos/vel for child m_links (i = 0 to num_links-1) - // - - btScalar getJointPos(int i) const; - btScalar getJointVel(int i) const; - - btScalar * getJointVelMultiDof(int i); - btScalar * getJointPosMultiDof(int i); + btScalar getJointPos(int i) const; + btScalar getJointVel(int i) const; - const btScalar * getJointVelMultiDof(int i) const ; - const btScalar * getJointPosMultiDof(int i) const ; + btScalar *getJointVelMultiDof(int i); + btScalar *getJointPosMultiDof(int i); - void setJointPos(int i, btScalar q); - void setJointVel(int i, btScalar qdot); - void setJointPosMultiDof(int i, btScalar *q); - void setJointVelMultiDof(int i, btScalar *qdot); + const btScalar *getJointVelMultiDof(int i) const; + const btScalar *getJointPosMultiDof(int i) const; + void setJointPos(int i, btScalar q); + void setJointVel(int i, btScalar qdot); + void setJointPosMultiDof(int i, const double *q); + void setJointVelMultiDof(int i, const double *qdot); + void setJointPosMultiDof(int i, const float *q); + void setJointVelMultiDof(int i, const float *qdot); - - // - // direct access to velocities as a vector of 6 + num_links elements. - // (omega first, then v, then joint velocities.) - // - const btScalar * getVelocityVector() const - { - return &m_realBuf[0]; + // + // direct access to velocities as a vector of 6 + num_links elements. + // (omega first, then v, then joint velocities.) + // + const btScalar *getVelocityVector() const + { + return &m_realBuf[0]; } -/* btScalar * getVelocityVector() + + const btScalar *getDeltaVelocityVector() const + { + return &m_deltaV[0]; + } + + const btScalar *getSplitVelocityVector() const + { + return &m_splitV[0]; + } + /* btScalar * getVelocityVector() { return &real_buf[0]; } - */ + */ - // - // get the frames of reference (positions and orientations) of the child m_links - // (i = 0 to num_links-1) - // + // + // get the frames of reference (positions and orientations) of the child m_links + // (i = 0 to num_links-1) + // - const btVector3 & getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords - const btQuaternion & getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i. + const btVector3 &getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords + const btQuaternion &getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i. + const btVector3 &getInterpolateRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords + const btQuaternion &getInterpolateParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i. + // + // transform vectors in local frame of link i to world frame (or vice versa) + // + btVector3 localPosToWorld(int i, const btVector3 &local_pos) const; + btVector3 localDirToWorld(int i, const btVector3 &local_dir) const; + btVector3 worldPosToLocal(int i, const btVector3 &world_pos) const; + btVector3 worldDirToLocal(int i, const btVector3 &world_dir) const; - // - // transform vectors in local frame of link i to world frame (or vice versa) - // - btVector3 localPosToWorld(int i, const btVector3 &vec) const; - btVector3 localDirToWorld(int i, const btVector3 &vec) const; - btVector3 worldPosToLocal(int i, const btVector3 &vec) const; - btVector3 worldDirToLocal(int i, const btVector3 &vec) const; - + // + // transform a frame in local coordinate to a frame in world coordinate + // + btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const; - // - // calculate kinetic energy and angular momentum - // useful for debugging. - // - btScalar getKineticEnergy() const; - btVector3 getAngularMomentum() const; - - - // - // set external forces and torques. Note all external forces/torques are given in the WORLD frame. - // + // + // set external forces and torques. Note all external forces/torques are given in the WORLD frame. + // - void clearForcesAndTorques(); - void clearConstraintForces(); + void clearForcesAndTorques(); + void clearConstraintForces(); void clearVelocities(); - void addBaseForce(const btVector3 &f) - { - m_baseForce += f; + void addBaseForce(const btVector3 &f) + { + m_baseForce += f; } - void addBaseTorque(const btVector3 &t) { m_baseTorque += t; } - void addLinkForce(int i, const btVector3 &f); - void addLinkTorque(int i, const btVector3 &t); + void addBaseTorque(const btVector3 &t) { m_baseTorque += t; } + void addLinkForce(int i, const btVector3 &f); + void addLinkTorque(int i, const btVector3 &t); - void addBaseConstraintForce(const btVector3 &f) - { - m_baseConstraintForce += f; - } - void addBaseConstraintTorque(const btVector3 &t) { m_baseConstraintTorque += t; } - void addLinkConstraintForce(int i, const btVector3 &f); - void addLinkConstraintTorque(int i, const btVector3 &t); - + void addBaseConstraintForce(const btVector3 &f) + { + m_baseConstraintForce += f; + } + void addBaseConstraintTorque(const btVector3 &t) { m_baseConstraintTorque += t; } + void addLinkConstraintForce(int i, const btVector3 &f); + void addLinkConstraintTorque(int i, const btVector3 &t); -void addJointTorque(int i, btScalar Q); + void addJointTorque(int i, btScalar Q); void addJointTorqueMultiDof(int i, int dof, btScalar Q); void addJointTorqueMultiDof(int i, const btScalar *Q); - const btVector3 & getBaseForce() const { return m_baseForce; } - const btVector3 & getBaseTorque() const { return m_baseTorque; } - const btVector3 & getLinkForce(int i) const; - const btVector3 & getLinkTorque(int i) const; - btScalar getJointTorque(int i) const; - btScalar * getJointTorqueMultiDof(int i); - - - // - // dynamics routines. - // - - // timestep the velocities (given the external forces/torques set using addBaseForce etc). - // also sets up caches for calcAccelerationDeltas. - // - // Note: the caller must provide three vectors which are used as - // temporary scratch space. The idea here is to reduce dynamic - // memory allocation: the same scratch vectors can be re-used - // again and again for different Multibodies, instead of each - // btMultiBody allocating (and then deallocating) their own - // individual scratch buffers. This gives a considerable speed - // improvement, at least on Windows (where dynamic memory - // allocation appears to be fairly slow). - // - - - void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, - btAlignedObjectArray<btScalar> &scratch_r, - btAlignedObjectArray<btVector3> &scratch_v, - btAlignedObjectArray<btMatrix3x3> &scratch_m, - bool isConstraintPass=false - ); - -///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead - void stepVelocitiesMultiDof(btScalar dt, - btAlignedObjectArray<btScalar> &scratch_r, - btAlignedObjectArray<btVector3> &scratch_v, - btAlignedObjectArray<btMatrix3x3> &scratch_m, - bool isConstraintPass=false) - { - computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt,scratch_r,scratch_v,scratch_m,isConstraintPass); - } + const btVector3 &getBaseForce() const { return m_baseForce; } + const btVector3 &getBaseTorque() const { return m_baseTorque; } + const btVector3 &getLinkForce(int i) const; + const btVector3 &getLinkTorque(int i) const; + btScalar getJointTorque(int i) const; + btScalar *getJointTorqueMultiDof(int i); + + // + // dynamics routines. + // + + // timestep the velocities (given the external forces/torques set using addBaseForce etc). + // also sets up caches for calcAccelerationDeltas. + // + // Note: the caller must provide three vectors which are used as + // temporary scratch space. The idea here is to reduce dynamic + // memory allocation: the same scratch vectors can be re-used + // again and again for different Multibodies, instead of each + // btMultiBody allocating (and then deallocating) their own + // individual scratch buffers. This gives a considerable speed + // improvement, at least on Windows (where dynamic memory + // allocation appears to be fairly slow). + // - // calcAccelerationDeltasMultiDof - // input: force vector (in same format as jacobian, i.e.: - // 3 torque values, 3 force values, num_links joint torque values) - // output: 3 omegadot values, 3 vdot values, num_links q_double_dot values - // (existing contents of output array are replaced) - // calcAccelerationDeltasMultiDof must have been called first. + void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, + btAlignedObjectArray<btScalar> & scratch_r, + btAlignedObjectArray<btVector3> & scratch_v, + btAlignedObjectArray<btMatrix3x3> & scratch_m, + bool isConstraintPass, + bool jointFeedbackInWorldSpace, + bool jointFeedbackInJointFrame + ); + + ///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead + //void stepVelocitiesMultiDof(btScalar dt, + // btAlignedObjectArray<btScalar> & scratch_r, + // btAlignedObjectArray<btVector3> & scratch_v, + // btAlignedObjectArray<btMatrix3x3> & scratch_m, + // bool isConstraintPass = false) + //{ + // computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass, false, false); + //} + + // calcAccelerationDeltasMultiDof + // input: force vector (in same format as jacobian, i.e.: + // 3 torque values, 3 force values, num_links joint torque values) + // output: 3 omegadot values, 3 vdot values, num_links q_double_dot values + // (existing contents of output array are replaced) + // calcAccelerationDeltasMultiDof must have been called first. void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, - btAlignedObjectArray<btScalar> &scratch_r, - btAlignedObjectArray<btVector3> &scratch_v) const; - - - void applyDeltaVeeMultiDof2(const btScalar * delta_vee, btScalar multiplier) + btAlignedObjectArray<btScalar> &scratch_r, + btAlignedObjectArray<btVector3> &scratch_v) const; + + void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier) { for (int dof = 0; dof < 6 + getNumDofs(); ++dof) - { - m_deltaV[dof] += delta_vee[dof] * multiplier; - } + { + 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); + applyDeltaVeeMultiDof(&m_deltaV[0], 1); for (int dof = 0; dof < 6 + getNumDofs(); ++dof) - { + { m_deltaV[dof] = 0.f; } } - void applyDeltaVeeMultiDof(const btScalar * delta_vee, btScalar multiplier) + void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier) { //for (int dof = 0; dof < 6 + getNumDofs(); ++dof) // printf("%.4f ", delta_vee[dof]*multiplier); @@ -405,65 +453,81 @@ void addJointTorque(int i, btScalar Q); for (int dof = 0; dof < 6 + getNumDofs(); ++dof) { m_realBuf[dof] += delta_vee[dof] * multiplier; - btClamp(m_realBuf[dof],-m_maxCoordinateVelocity,m_maxCoordinateVelocity); + btClamp(m_realBuf[dof], -m_maxCoordinateVelocity, m_maxCoordinateVelocity); } - } + } - - - // timestep the positions (given current velocities). + // timestep the positions (given current velocities). void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0); + + // predict the positions + void predictPositionsMultiDof(btScalar dt); + // + // contacts + // - // - // contacts - // + // This routine fills out a contact constraint jacobian for this body. + // the 'normal' supplied must be -n for body1 or +n for body2 of the contact. + // 'normal' & 'contact_point' are both given in world coordinates. - // This routine fills out a contact constraint jacobian for this body. - // the 'normal' supplied must be -n for body1 or +n for body2 of the contact. - // 'normal' & 'contact_point' are both given in world coordinates. - void fillContactJacobianMultiDof(int link, - const btVector3 &contact_point, - const btVector3 &normal, - btScalar *jac, - btAlignedObjectArray<btScalar> &scratch_r, - btAlignedObjectArray<btVector3> &scratch_v, - btAlignedObjectArray<btMatrix3x3> &scratch_m) const { fillConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); } + const btVector3 &contact_point, + const btVector3 &normal, + btScalar *jac, + btAlignedObjectArray<btScalar> &scratch_r, + btAlignedObjectArray<btVector3> &scratch_v, + btAlignedObjectArray<btMatrix3x3> &scratch_m) const { fillConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); } //a more general version of fillContactJacobianMultiDof which does not assume.. //.. that the constraint in question is contact or, to be more precise, constrains linear velocity only void fillConstraintJacobianMultiDof(int link, - const btVector3 &contact_point, - const btVector3 &normal_ang, - const btVector3 &normal_lin, - btScalar *jac, - btAlignedObjectArray<btScalar> &scratch_r, - btAlignedObjectArray<btVector3> &scratch_v, - btAlignedObjectArray<btMatrix3x3> &scratch_m) const; - + const btVector3 &contact_point, + const btVector3 &normal_ang, + const btVector3 &normal_lin, + btScalar *jac, + btAlignedObjectArray<btScalar> &scratch_r, + btAlignedObjectArray<btVector3> &scratch_v, + btAlignedObjectArray<btMatrix3x3> &scratch_m) const; + + // + // sleeping + // + void setCanSleep(bool canSleep) + { + if (m_canWakeup) + { + m_canSleep = canSleep; + } + } - // - // sleeping - // - void setCanSleep(bool canSleep) + bool getCanSleep() const { - m_canSleep = canSleep; + return m_canSleep; } - bool getCanSleep()const + bool getCanWakeup() const { - return m_canSleep; + return m_canWakeup; } + + void setCanWakeup(bool canWakeup) + { + m_canWakeup = canWakeup; + } + bool isAwake() const { return m_awake; } + void wakeUp(); + void goToSleep(); + void checkMotionAndSleepIfRequired(btScalar timestep); - bool isAwake() const { return m_awake; } - void wakeUp(); - void goToSleep(); - void checkMotionAndSleepIfRequired(btScalar timestep); - bool hasFixedBase() const { - return m_fixedBase; + return m_fixedBase; + } + + void setFixedBase(bool fixedBase) + { + m_fixedBase = fixedBase; } int getCompanionId() const @@ -476,16 +540,16 @@ void addJointTorque(int i, btScalar Q); m_companionId = id; } - void setNumLinks(int numLinks)//careful: when changing the number of m_links, make sure to re-initialize or update existing m_links + void setNumLinks(int numLinks) //careful: when changing the number of m_links, make sure to re-initialize or update existing m_links { m_links.resize(numLinks); } btScalar getLinearDamping() const { - return m_linearDamping; + return m_linearDamping; } - void setLinearDamping( btScalar damp) + void setLinearDamping(btScalar damp) { m_linearDamping = damp; } @@ -493,11 +557,11 @@ void addJointTorque(int i, btScalar Q); { return m_angularDamping; } - void setAngularDamping( btScalar damp) + void setAngularDamping(btScalar damp) { m_angularDamping = damp; } - + bool getUseGyroTerm() const { return m_useGyroTerm; @@ -506,24 +570,24 @@ void addJointTorque(int i, btScalar Q); { m_useGyroTerm = useGyro; } - btScalar getMaxCoordinateVelocity() const + btScalar getMaxCoordinateVelocity() const { - return m_maxCoordinateVelocity ; + return m_maxCoordinateVelocity; } - void setMaxCoordinateVelocity(btScalar maxVel) + void setMaxCoordinateVelocity(btScalar maxVel) { m_maxCoordinateVelocity = maxVel; } - btScalar getMaxAppliedImpulse() const + btScalar getMaxAppliedImpulse() const { return m_maxAppliedImpulse; } - void setMaxAppliedImpulse(btScalar maxImp) + void setMaxAppliedImpulse(btScalar maxImp) { m_maxAppliedImpulse = maxImp; } - void setHasSelfCollision(bool hasSelfCollision) + void setHasSelfCollision(bool hasSelfCollision) { m_hasSelfCollision = hasSelfCollision; } @@ -532,7 +596,6 @@ void addJointTorque(int i, btScalar Q); return m_hasSelfCollision; } - void finalizeMultiDof(); void useRK4Integration(bool use) { m_useRK4 = use; } @@ -548,213 +611,278 @@ void addJointTorque(int i, btScalar Q); { __posUpdated = updated; } - + //internalNeedsJointFeedback is for internal use only bool internalNeedsJointFeedback() const { return m_internalNeedsJointFeedback; } - void forwardKinematics(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m); + void forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to_local, btAlignedObjectArray<btVector3> & local_origin); - void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m); - - virtual int calculateSerializeBufferSize() const; + void compTreeLinkVelocities(btVector3 * omega, btVector3 * vel) const; + + void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin); + void updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin); + + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; + virtual const char *serialize(void *dataBuffer, class btSerializer *serializer) const; - const char* getBaseName() const + const char *getBaseName() const { return m_baseName; } ///memory of setBaseName needs to be manager by user - void setBaseName(const char* name) + void setBaseName(const char *name) { m_baseName = name; } -private: - btMultiBody(const btMultiBody &); // not implemented - void operator=(const btMultiBody &); // not implemented + ///users can point to their objects, userPointer is not used by Bullet + void *getUserPointer() const + { + return m_userObjectPointer; + } - void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const; + int getUserIndex() const + { + return m_userIndex; + } - void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const; - void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const; - - void updateLinksDofOffsets() + int getUserIndex2() const { - int dofOffset = 0, cfgOffset = 0; - for(int bidx = 0; bidx < m_links.size(); ++bidx) - { - m_links[bidx].m_dofOffset = dofOffset; m_links[bidx].m_cfgOffset = cfgOffset; - dofOffset += m_links[bidx].m_dofCount; cfgOffset += m_links[bidx].m_posVarCount; - } + return m_userIndex2; + } + ///users can point to their objects, userPointer is not used by Bullet + void setUserPointer(void *userPointer) + { + m_userObjectPointer = userPointer; } - void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const; - - -private: + ///users can point to their objects, userPointer is not used by Bullet + void setUserIndex(int index) + { + m_userIndex = index; + } - btMultiBodyLinkCollider* m_baseCollider;//can be NULL - const char* m_baseName;//memory needs to be manager by user! + void setUserIndex2(int index) + { + m_userIndex2 = index; + } - btVector3 m_basePos; // position of COM of base (world frame) - btQuaternion m_baseQuat; // rotates world points into base frame + static void spatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame + const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates + const btVector3 &top_in, // top part of input vector + const btVector3 &bottom_in, // bottom part of input vector + btVector3 &top_out, // top part of output vector + btVector3 &bottom_out); // bottom part of output vector - btScalar m_baseMass; // mass of the base - btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal) - btVector3 m_baseForce; // external force applied to base. World frame. - btVector3 m_baseTorque; // external torque applied to base. World frame. - - btVector3 m_baseConstraintForce; // external force applied to base. World frame. - btVector3 m_baseConstraintTorque; // external torque applied to base. World frame. - - btAlignedObjectArray<btMultibodyLink> m_links; // array of m_links, excluding the base. index from 0 to num_links-1. - btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders; - - // - // realBuf: - // offset size array - // 0 6 + num_links v (base_omega; base_vel; joint_vels) MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized] - // 6+num_links num_links D - // - // vectorBuf: - // offset size array - // 0 num_links h_top - // num_links num_links h_bottom - // - // matrixBuf: - // offset size array - // 0 num_links+1 rot_from_parent - // - btAlignedObjectArray<btScalar> m_deltaV; - btAlignedObjectArray<btScalar> m_realBuf; - btAlignedObjectArray<btVector3> m_vectorBuf; - btAlignedObjectArray<btMatrix3x3> m_matrixBuf; +private: + btMultiBody(const btMultiBody &); // not implemented + void operator=(const btMultiBody &); // not implemented + void solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar result[6]) const; + void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const; + + void updateLinksDofOffsets() + { + int dofOffset = 0, cfgOffset = 0; + for (int bidx = 0; bidx < m_links.size(); ++bidx) + { + m_links[bidx].m_dofOffset = dofOffset; + m_links[bidx].m_cfgOffset = cfgOffset; + dofOffset += m_links[bidx].m_dofCount; + cfgOffset += m_links[bidx].m_posVarCount; + } + } + + void mulMatrix(btScalar * pA, btScalar * pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const; + +private: + btMultiBodyLinkCollider *m_baseCollider; //can be NULL + const char *m_baseName; //memory needs to be manager by user! + + btVector3 m_basePos; // position of COM of base (world frame) + btVector3 m_basePos_interpolate; // position of interpolated COM of base (world frame) + btQuaternion m_baseQuat; // rotates world points into base frame + btQuaternion m_baseQuat_interpolate; + + btScalar m_baseMass; // mass of the base + btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal) + + btVector3 m_baseForce; // external force applied to base. World frame. + btVector3 m_baseTorque; // external torque applied to base. World frame. + + btVector3 m_baseConstraintForce; // external force applied to base. World frame. + btVector3 m_baseConstraintTorque; // external torque applied to base. World frame. + + btAlignedObjectArray<btMultibodyLink> m_links; // array of m_links, excluding the base. index from 0 to num_links-1. + + // + // realBuf: + // offset size array + // 0 6 + num_links v (base_omega; base_vel; joint_vels) MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized] + // 6+num_links num_links D + // + // vectorBuf: + // offset size array + // 0 num_links h_top + // num_links num_links h_bottom + // + // matrixBuf: + // 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; + btAlignedObjectArray<btMatrix3x3> m_matrixBuf; btMatrix3x3 m_cachedInertiaTopLeft; btMatrix3x3 m_cachedInertiaTopRight; btMatrix3x3 m_cachedInertiaLowerLeft; btMatrix3x3 m_cachedInertiaLowerRight; + bool m_cachedInertiaValid; - bool m_fixedBase; + bool m_fixedBase; - // Sleep parameters. - bool m_awake; - bool m_canSleep; - btScalar m_sleepTimer; + // Sleep parameters. + bool m_awake; + bool m_canSleep; + bool m_canWakeup; + btScalar m_sleepTimer; + + void *m_userObjectPointer; + int m_userIndex2; + int m_userIndex; + + int m_companionId; + btScalar m_linearDamping; + btScalar m_angularDamping; + bool m_useGyroTerm; + btScalar m_maxAppliedImpulse; + btScalar m_maxCoordinateVelocity; + bool m_hasSelfCollision; + + bool __posUpdated; + int m_dofCount, m_posVarCnt; - int m_companionId; - btScalar m_linearDamping; - btScalar m_angularDamping; - bool m_useGyroTerm; - btScalar m_maxAppliedImpulse; - btScalar m_maxCoordinateVelocity; - bool m_hasSelfCollision; - - bool __posUpdated; - int m_dofCount, m_posVarCnt; bool m_useRK4, m_useGlobalVelocities; - + //for global velocities, see 8.3.2B Proposed resolution in Jakub Stepien PhD Thesis + //https://drive.google.com/file/d/0Bz3vEa19XOYGNWdZWGpMdUdqVmZ5ZVBOaEh4ZnpNaUxxZFNV/view?usp=sharing + ///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only bool m_internalNeedsJointFeedback; }; struct btMultiBodyLinkDoubleData { - btQuaternionDoubleData m_zeroRotParentToThis; - btVector3DoubleData m_parentComToThisComOffset; - btVector3DoubleData m_thisPivotToThisComOffset; - btVector3DoubleData m_jointAxisTop[6]; - btVector3DoubleData m_jointAxisBottom[6]; - - - char *m_linkName; - char *m_jointName; - btCollisionObjectDoubleData *m_linkCollider; - - btVector3DoubleData m_linkInertia; // inertia of the base (in local frame; diagonal) - double m_linkMass; - int m_parentIndex; - int m_jointType; - - - - - int m_dofCount; - int m_posVarCount; - double m_jointPos[7]; - double m_jointVel[6]; - double m_jointTorque[6]; - - - + btQuaternionDoubleData m_zeroRotParentToThis; + btVector3DoubleData m_parentComToThisPivotOffset; + btVector3DoubleData m_thisPivotToThisComOffset; + btVector3DoubleData m_jointAxisTop[6]; + btVector3DoubleData m_jointAxisBottom[6]; + + btVector3DoubleData m_linkInertia; // inertia of the base (in local frame; diagonal) + btVector3DoubleData m_absFrameTotVelocityTop; + btVector3DoubleData m_absFrameTotVelocityBottom; + btVector3DoubleData m_absFrameLocVelocityTop; + btVector3DoubleData m_absFrameLocVelocityBottom; + + double m_linkMass; + int m_parentIndex; + int m_jointType; + + int m_dofCount; + int m_posVarCount; + double m_jointPos[7]; + double m_jointVel[6]; + double m_jointTorque[6]; + + double m_jointDamping; + double m_jointFriction; + double m_jointLowerLimit; + double m_jointUpperLimit; + double m_jointMaxForce; + double m_jointMaxVelocity; + + char *m_linkName; + char *m_jointName; + btCollisionObjectDoubleData *m_linkCollider; + char *m_paddingPtr; }; struct btMultiBodyLinkFloatData { - btQuaternionFloatData m_zeroRotParentToThis; - btVector3FloatData m_parentComToThisComOffset; - btVector3FloatData m_thisPivotToThisComOffset; - btVector3FloatData m_jointAxisTop[6]; - btVector3FloatData m_jointAxisBottom[6]; - - - char *m_linkName; - char *m_jointName; - btCollisionObjectFloatData *m_linkCollider; - - btVector3FloatData m_linkInertia; // inertia of the base (in local frame; diagonal) - int m_dofCount; - float m_linkMass; - int m_parentIndex; - int m_jointType; - - - - float m_jointPos[7]; - float m_jointVel[6]; - float m_jointTorque[6]; - int m_posVarCount; - - + btQuaternionFloatData m_zeroRotParentToThis; + btVector3FloatData m_parentComToThisPivotOffset; + btVector3FloatData m_thisPivotToThisComOffset; + btVector3FloatData m_jointAxisTop[6]; + btVector3FloatData m_jointAxisBottom[6]; + btVector3FloatData m_linkInertia; // inertia of the base (in local frame; diagonal) + btVector3FloatData m_absFrameTotVelocityTop; + btVector3FloatData m_absFrameTotVelocityBottom; + btVector3FloatData m_absFrameLocVelocityTop; + btVector3FloatData m_absFrameLocVelocityBottom; + + int m_dofCount; + float m_linkMass; + int m_parentIndex; + int m_jointType; + + float m_jointPos[7]; + float m_jointVel[6]; + float m_jointTorque[6]; + int m_posVarCount; + float m_jointDamping; + float m_jointFriction; + float m_jointLowerLimit; + float m_jointUpperLimit; + float m_jointMaxForce; + float m_jointMaxVelocity; + + char *m_linkName; + char *m_jointName; + btCollisionObjectFloatData *m_linkCollider; + char *m_paddingPtr; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btMultiBodyDoubleData +struct btMultiBodyDoubleData { - char *m_baseName; - btMultiBodyLinkDoubleData *m_links; - btCollisionObjectDoubleData *m_baseCollider; - - btTransformDoubleData m_baseWorldTransform; - btVector3DoubleData m_baseInertia; // inertia of the base (in local frame; diagonal) - - int m_numLinks; - double m_baseMass; - + btVector3DoubleData m_baseWorldPosition; + btQuaternionDoubleData m_baseWorldOrientation; + btVector3DoubleData m_baseLinearVelocity; + btVector3DoubleData m_baseAngularVelocity; + btVector3DoubleData m_baseInertia; // inertia of the base (in local frame; diagonal) + double m_baseMass; + int m_numLinks; char m_padding[4]; - + + char *m_baseName; + btMultiBodyLinkDoubleData *m_links; + btCollisionObjectDoubleData *m_baseCollider; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btMultiBodyFloatData +struct btMultiBodyFloatData { - char *m_baseName; - btMultiBodyLinkFloatData *m_links; - btCollisionObjectFloatData *m_baseCollider; - btTransformFloatData m_baseWorldTransform; - btVector3FloatData m_baseInertia; // inertia of the base (in local frame; diagonal) - - float m_baseMass; - int m_numLinks; + btVector3FloatData m_baseWorldPosition; + btQuaternionFloatData m_baseWorldOrientation; + btVector3FloatData m_baseLinearVelocity; + btVector3FloatData m_baseAngularVelocity; + + btVector3FloatData m_baseInertia; // inertia of the base (in local frame; diagonal) + float m_baseMass; + int m_numLinks; + + char *m_baseName; + btMultiBodyLinkFloatData *m_links; + btCollisionObjectFloatData *m_baseCollider; }; - - #endif diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index 12997d2e374..1ba5861145b 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -1,32 +1,30 @@ #include "btMultiBodyConstraint.h" #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) - :m_bodyA(bodyA), - m_bodyB(bodyB), - m_linkA(linkA), - m_linkB(linkB), - m_numRows(numRows), - m_jacSizeA(0), - m_jacSizeBoth(0), - m_isUnilateral(isUnilateral), - m_numDofsFinalized(-1), - m_maxAppliedImpulse(100) +#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, 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), + m_isUnilateral(isUnilateral), + m_numDofsFinalized(-1), + m_maxAppliedImpulse(100) { - } void btMultiBodyConstraint::updateJacobianSizes() { - if(m_bodyA) + if (m_bodyA) { m_jacSizeA = (6 + m_bodyA->getNumDofs()); } - if(m_bodyB) + if (m_bodyB) { m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs(); } @@ -38,7 +36,7 @@ void btMultiBodyConstraint::allocateJacobiansMultiDof() { updateJacobianSizes(); - m_posOffset = ((1 + m_jacSizeBoth)*m_numRows); + m_posOffset = ((1 + m_jacSizeBoth) * m_numRows); m_data.resize((2 + m_jacSizeBoth) * m_numRows); } @@ -46,25 +44,25 @@ btMultiBodyConstraint::~btMultiBodyConstraint() { } -void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) +void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) { for (int i = 0; i < ndof; ++i) - data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; + data.m_deltaVelocities[velocityIndex + i] += delta_vee[i] * impulse; } -btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint, - btMultiBodyJacobianData& data, - btScalar* jacOrgA, btScalar* jacOrgB, - const btVector3& contactNormalOnB, - const btVector3& posAworld, const btVector3& posBworld, - btScalar posError, - const btContactSolverInfo& infoGlobal, - btScalar lowerLimit, btScalar upperLimit, - btScalar relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) +btScalar btMultiBodyConstraint::fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, + btMultiBodyJacobianData& data, + btScalar* jacOrgA, btScalar* jacOrgB, + const btVector3& constraintNormalAng, + const btVector3& constraintNormalLin, + const btVector3& posAworld, const btVector3& posBworld, + btScalar posError, + const btContactSolverInfo& infoGlobal, + btScalar lowerLimit, btScalar upperLimit, + bool angConstraint, + btScalar relaxation, + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) { - - solverConstraint.m_multiBodyA = m_bodyA; solverConstraint.m_multiBodyB = m_bodyB; solverConstraint.m_linkA = m_linkA; @@ -79,7 +77,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; - btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary + btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary if (bodyA) rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin(); if (bodyB) @@ -87,121 +85,156 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr if (multiBodyA) { - if (solverConstraint.m_linkA<0) + if (solverConstraint.m_linkA < 0) { rel_pos1 = posAworld - multiBodyA->getBasePos(); - } else + } + else { rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin(); } - const int ndofA = multiBodyA->getNumDofs() + 6; + const int ndofA = multiBodyA->getNumDofs() + 6; solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); - if (solverConstraint.m_deltaVelAindex <0) + if (solverConstraint.m_deltaVelAindex < 0) { solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size(); multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); - data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); - } else + data.m_deltaVelocities.resize(data.m_deltaVelocities.size() + ndofA); + } + else { - btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); + btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA); } //determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom //resize.. solverConstraint.m_jacAindex = data.m_jacobians.size(); - data.m_jacobians.resize(data.m_jacobians.size()+ndofA); + data.m_jacobians.resize(data.m_jacobians.size() + ndofA); //copy/determine - if(jacOrgA) + if (jacOrgA) { - for (int i=0;i<ndofA;i++) - data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i]; + for (int i = 0; i < ndofA; i++) + data.m_jacobians[solverConstraint.m_jacAindex + i] = jacOrgA[i]; } else { - btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex]; - multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); + btScalar* jac1 = &data.m_jacobians[solverConstraint.m_jacAindex]; + //multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m); + multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalAng, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m); } //determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint) //resize.. - data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size() + ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; //determine.. - multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); + multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex], delta, data.scratch_r, data.scratch_v); - btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB); + btVector3 torqueAxis0; + if (angConstraint) + { + torqueAxis0 = constraintNormalAng; + } + else + { + torqueAxis0 = rel_pos1.cross(constraintNormalLin); + } solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = contactNormalOnB; + solverConstraint.m_contactNormal1 = constraintNormalLin; } - else //if(rb0) + else //if(rb0) { - btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB); - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); + btVector3 torqueAxis0; + if (angConstraint) + { + torqueAxis0 = constraintNormalAng; + } + else + { + torqueAxis0 = rel_pos1.cross(constraintNormalLin); + } + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0); solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = contactNormalOnB; + solverConstraint.m_contactNormal1 = constraintNormalLin; } if (multiBodyB) { - if (solverConstraint.m_linkB<0) + if (solverConstraint.m_linkB < 0) { rel_pos2 = posBworld - multiBodyB->getBasePos(); - } else + } + else { rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); } - const int ndofB = multiBodyB->getNumDofs() + 6; + const int ndofB = multiBodyB->getNumDofs() + 6; solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); - if (solverConstraint.m_deltaVelBindex <0) + if (solverConstraint.m_deltaVelBindex < 0) { solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size(); multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); - data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size() + ndofB); } //determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom //resize.. solverConstraint.m_jacBindex = data.m_jacobians.size(); - data.m_jacobians.resize(data.m_jacobians.size()+ndofB); + data.m_jacobians.resize(data.m_jacobians.size() + ndofB); //copy/determine.. - if(jacOrgB) + if (jacOrgB) { - for (int i=0;i<ndofB;i++) - data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i]; + for (int i = 0; i < ndofB; i++) + data.m_jacobians[solverConstraint.m_jacBindex + i] = jacOrgB[i]; } else { - multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); + //multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); + multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalAng, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); } //determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint) //resize.. - data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB); + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size() + ndofB); btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; //determine.. - multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v); + multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex], delta, data.scratch_r, data.scratch_v); - btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); + btVector3 torqueAxis1; + if (angConstraint) + { + torqueAxis1 = constraintNormalAng; + } + else + { + torqueAxis1 = rel_pos2.cross(constraintNormalLin); + } solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - solverConstraint.m_contactNormal2 = -contactNormalOnB; - + solverConstraint.m_contactNormal2 = -constraintNormalLin; } - else //if(rb1) + else //if(rb1) { - btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); + btVector3 torqueAxis1; + if (angConstraint) + { + torqueAxis1 = constraintNormalAng; + } + else + { + torqueAxis1 = rel_pos2.cross(constraintNormalLin); + } + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0); solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - solverConstraint.m_contactNormal2 = -contactNormalOnB; + solverConstraint.m_contactNormal2 = -constraintNormalLin; } { - btVector3 vec; btScalar denom0 = 0.f; btScalar denom1 = 0.f; @@ -209,7 +242,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr btScalar* jacA = 0; btScalar* deltaVelA = 0; btScalar* deltaVelB = 0; - int ndofA = 0; + int ndofA = 0; //determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i]) if (multiBodyA) { @@ -218,15 +251,22 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; for (int i = 0; i < ndofA; ++i) { - btScalar j = jacA[i] ; + btScalar j = jacA[i]; btScalar l = deltaVelA[i]; - denom0 += j*l; + denom0 += j * l; } } - else if(rb0) + else if (rb0) { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec); + vec = (solverConstraint.m_angularComponentA).cross(rel_pos1); + if (angConstraint) + { + denom0 = constraintNormalAng.dot(solverConstraint.m_angularComponentA); + } + else + { + denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec); + } } // if (multiBodyB) @@ -236,134 +276,108 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; for (int i = 0; i < ndofB; ++i) { - btScalar j = jacB[i] ; + btScalar j = jacB[i]; btScalar l = deltaVelB[i]; - denom1 += j*l; + denom1 += j * l; } - } - else if(rb1) + else if (rb1) { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec); + vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2); + if (angConstraint) + { + denom1 = constraintNormalAng.dot(-solverConstraint.m_angularComponentB); + } + else + { + denom1 = rb1->getInvMass() + constraintNormalLin.dot(vec); + } } // - btScalar d = denom0+denom1; - if (d>SIMD_EPSILON) + btScalar d = denom0 + denom1; + if (d > SIMD_EPSILON) { - solverConstraint.m_jacDiagABInv = relaxation/(d); + solverConstraint.m_jacDiagABInv = relaxation / (d); } else { - //disable the constraint row to handle singularity/redundant constraint - solverConstraint.m_jacDiagABInv = 0.f; + //disable the constraint row to handle singularity/redundant constraint + solverConstraint.m_jacDiagABInv = 0.f; } } - //compute rhs and remaining solverConstraint fields - btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop; + btScalar penetration = isFriction ? 0 : posError; btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; + int ndofA = 0; + int ndofB = 0; { - btVector3 vel1,vel2; + btVector3 vel1, vel2; if (multiBodyA) { ndofA = multiBodyA->getNumDofs() + 6; btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA ; ++i) + for (int i = 0; i < ndofA; ++i) rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; } - else if(rb0) + else if (rb0) { - rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); + rel_vel += rb0->getLinearVelocity().dot(solverConstraint.m_contactNormal1); + rel_vel += rb0->getAngularVelocity().dot(solverConstraint.m_relpos1CrossNormal); } if (multiBodyB) { ndofB = multiBodyB->getNumDofs() + 6; btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB ; ++i) + for (int i = 0; i < ndofB; ++i) rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; - } - else if(rb1) + else if (rb1) { - rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); + rel_vel += rb1->getLinearVelocity().dot(solverConstraint.m_contactNormal2); + rel_vel += rb1->getAngularVelocity().dot(solverConstraint.m_relpos2CrossNormal); } - solverConstraint.m_friction = 0.f;//cp.m_combinedFriction; + solverConstraint.m_friction = 0.f; //cp.m_combinedFriction; } - - ///warm starting (or zero if disabled) - /* - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - - if (solverConstraint.m_appliedImpulse) - { - if (multiBodyA) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->applyDeltaVee(deltaV,impulse); - applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); - } else - { - if (rb0) - bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); - } - if (multiBodyB) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - multiBodyB->applyDeltaVee(deltaV,impulse); - applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); - } else - { - if (rb1) - bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); - } - } - } else - */ - solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedPushImpulse = 0.f; { - btScalar positionalError = 0.f; - btScalar velocityError = desiredVelocity - rel_vel;// * damping; - + btScalar velocityError = desiredVelocity - rel_vel; // * damping; btScalar erp = infoGlobal.m_erp2; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + + //split impulse is not implemented yet for btMultiBody* + //if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) { erp = infoGlobal.m_erp; } - positionalError = -penetration * erp/infoGlobal.m_timeStep; + positionalError = -penetration * erp / infoGlobal.m_timeStep; - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + //split impulse is not implemented yet for btMultiBody* + + // if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) { //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; solverConstraint.m_rhsPenetration = 0.f; - - } else - { - //split position and velocity into rhs and m_rhsPenetration - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = penetrationImpulse; } + /*else + { + //split position and velocity into rhs and m_rhsPenetration + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = penetrationImpulse; + } + */ solverConstraint.m_cfm = 0.f; solverConstraint.m_lowerLimit = lowerLimit; @@ -371,5 +385,4 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr } return rel_vel; - } diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 3b32b46e911..4a6007ee3ed 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/extern/bullet2/src/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; @@ -27,152 +42,174 @@ struct btSolverInfo; struct btMultiBodyJacobianData { - btAlignedObjectArray<btScalar> m_jacobians; - btAlignedObjectArray<btScalar> m_deltaVelocitiesUnitImpulse; //holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension - btAlignedObjectArray<btScalar> m_deltaVelocities; //holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI - btAlignedObjectArray<btScalar> scratch_r; - btAlignedObjectArray<btVector3> scratch_v; - btAlignedObjectArray<btMatrix3x3> scratch_m; - btAlignedObjectArray<btSolverBody>* m_solverBodyPool; - int m_fixedBodyId; - + btAlignedObjectArray<btScalar> m_jacobians; + btAlignedObjectArray<btScalar> m_deltaVelocitiesUnitImpulse; //holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension + btAlignedObjectArray<btScalar> m_deltaVelocities; //holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI + btAlignedObjectArray<btScalar> scratch_r; + btAlignedObjectArray<btVector3> scratch_v; + btAlignedObjectArray<btMatrix3x3> scratch_m; + btAlignedObjectArray<btSolverBody>* m_solverBodyPool; + int m_fixedBodyId; }; - -class btMultiBodyConstraint +ATTRIBUTE_ALIGNED16(class) +btMultiBodyConstraint { protected: - - btMultiBody* m_bodyA; - btMultiBody* m_bodyB; - int m_linkA; - int m_linkB; - - int m_numRows; - int m_jacSizeA; - int m_jacSizeBoth; - int m_posOffset; - - bool m_isUnilateral; - int m_numDofsFinalized; - btScalar m_maxAppliedImpulse; - - - // warning: the data block lay out is not consistent for all constraints - // data block laid out as follows: - // cached impulses. (one per row.) - // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc) - // positions. (one per row.) - btAlignedObjectArray<btScalar> m_data; - - void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof); - - btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, - btMultiBodyJacobianData& data, - btScalar* jacOrgA, btScalar* jacOrgB, - const btVector3& contactNormalOnB, - const btVector3& posAworld, const btVector3& posBworld, - btScalar posError, - const btContactSolverInfo& infoGlobal, - btScalar lowerLimit, btScalar upperLimit, - btScalar relaxation = 1.f, - bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0); + btMultiBody* m_bodyA; + btMultiBody* m_bodyB; + int m_linkA; + int m_linkB; + + int m_type; //btTypedMultiBodyConstraintType + + int m_numRows; + int m_jacSizeA; + int m_jacSizeBoth; + int m_posOffset; + + bool m_isUnilateral; + int m_numDofsFinalized; + btScalar m_maxAppliedImpulse; + + // warning: the data block lay out is not consistent for all constraints + // data block laid out as follows: + // cached impulses. (one per row.) + // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc) + // positions. (one per row.) + btAlignedObjectArray<btScalar> m_data; + + void applyDeltaVee(btMultiBodyJacobianData & data, btScalar * delta_vee, btScalar impulse, int velocityIndex, int ndof); + + btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint & solverConstraint, + btMultiBodyJacobianData & data, + btScalar * jacOrgA, btScalar * jacOrgB, + const btVector3& constraintNormalAng, + + const btVector3& constraintNormalLin, + const btVector3& posAworld, const btVector3& posBworld, + btScalar posError, + const btContactSolverInfo& infoGlobal, + btScalar lowerLimit, btScalar upperLimit, + bool angConstraint = false, + + btScalar relaxation = 1.f, + bool isFriction = false, btScalar desiredVelocity = 0, btScalar cfmSlip = 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(); - virtual void finalizeMultiDof()=0; + 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) {} - virtual int getIslandIdA() const =0; - virtual int getIslandIdB() const =0; + virtual void finalizeMultiDof() = 0; - virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal)=0; + virtual int getIslandIdA() const = 0; + virtual int getIslandIdB() const = 0; - int getNumRows() const + virtual void createConstraintRows(btMultiBodyConstraintArray & constraintRows, + btMultiBodyJacobianData & data, + const btContactSolverInfo& infoGlobal) = 0; + + int getNumRows() const { return m_numRows; } - btMultiBody* getMultiBodyA() + btMultiBody* getMultiBodyA() { return m_bodyA; } - btMultiBody* getMultiBodyB() + btMultiBody* getMultiBodyB() { return m_bodyB; } - void internalSetAppliedImpulse(int dof, btScalar appliedImpulse) + int getLinkA() const + { + return m_linkA; + } + int getLinkB() const + { + return m_linkB; + } + void internalSetAppliedImpulse(int dof, btScalar appliedImpulse) { - btAssert(dof>=0); + btAssert(dof >= 0); btAssert(dof < getNumRows()); m_data[dof] = appliedImpulse; } - - btScalar getAppliedImpulse(int dof) + + btScalar getAppliedImpulse(int dof) { - btAssert(dof>=0); + btAssert(dof >= 0); btAssert(dof < getNumRows()); return m_data[dof]; } // current constraint position - // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral - // NOTE: ignored position for friction rows. - btScalar getPosition(int row) const + // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral + // NOTE: ignored position for friction rows. + btScalar getPosition(int row) const { return m_data[m_posOffset + row]; } - void setPosition(int row, btScalar pos) + void setPosition(int row, btScalar pos) { m_data[m_posOffset + row] = pos; } - bool isUnilateral() const { return m_isUnilateral; } // jacobian blocks. - // each of size 6 + num_links. (jacobian2 is null if no body2.) - // format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients. - btScalar* jacobianA(int row) + // each of size 6 + num_links. (jacobian2 is null if no body2.) + // format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients. + btScalar* jacobianA(int row) { return &m_data[m_numRows + row * m_jacSizeBoth]; } - const btScalar* jacobianA(int row) const + const btScalar* jacobianA(int row) const { return &m_data[m_numRows + (row * m_jacSizeBoth)]; } - btScalar* jacobianB(int row) + btScalar* jacobianB(int row) { return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; } - const btScalar* jacobianB(int row) const + const btScalar* jacobianB(int row) const { return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; } - btScalar getMaxAppliedImpulse() const + btScalar getMaxAppliedImpulse() const { return m_maxAppliedImpulse; } - void setMaxAppliedImpulse(btScalar maxImp) + void setMaxAppliedImpulse(btScalar maxImp) { m_maxAppliedImpulse = maxImp; } - virtual void debugDraw(class btIDebugDraw* drawer)=0; + virtual void debugDraw(class btIDebugDraw * drawer) = 0; + virtual void setGearRatio(btScalar ratio) {} + virtual void setGearAuxLink(int gearAuxLink) {} + virtual void setRelativePositionTarget(btScalar relPosTarget) {} + virtual void setErp(btScalar erp) {} }; -#endif //BT_MULTIBODY_CONSTRAINT_H - +#endif //BT_MULTIBODY_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 4f66b20b2c1..2788367431e 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btMultiBodyConstraintSolver.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" #include "btMultiBodyLinkCollider.h" @@ -23,73 +22,197 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" #include "LinearMath/btQuickprof.h" +#include "BulletDynamics/Featherstone/btMultiBodySolverConstraint.h" +#include "LinearMath/btScalar.h" -btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { - btScalar val = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); - - //solve featherstone non-contact constraints + btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + //solve featherstone non-contact constraints + btScalar nonContactResidual = 0; //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size()); - for (int j=0;j<m_multiBodyNonContactConstraints.size();j++) + for (int i = 0; i < infoGlobal.m_numNonContactInnerIterations; ++i) { - btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[j]; - - resolveSingleConstraintRowGeneric(constraint); - if(constraint.m_multiBodyA) - constraint.m_multiBodyA->setPosUpdated(false); - if(constraint.m_multiBodyB) - constraint.m_multiBodyB->setPosUpdated(false); + // reset the nonContactResdual to 0 at start of each inner iteration + nonContactResidual = 0; + for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++) + { + int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j; + + btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index]; + + btScalar residual = resolveSingleConstraintRowGeneric(constraint); + nonContactResidual = btMax(nonContactResidual, residual * residual); + + if (constraint.m_multiBodyA) + constraint.m_multiBodyA->setPosUpdated(false); + if (constraint.m_multiBodyB) + constraint.m_multiBodyB->setPosUpdated(false); + } } + leastSquaredResidual = btMax(leastSquaredResidual, nonContactResidual); //solve featherstone normal contact - for (int j=0;j<m_multiBodyNormalContactConstraints.size();j++) + for (int j0 = 0; j0 < m_multiBodyNormalContactConstraints.size(); j0++) { - btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j]; + int index = j0; //iteration&1? j0 : m_multiBodyNormalContactConstraints.size()-1-j0; + + btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[index]; + btScalar residual = 0.f; + if (iteration < infoGlobal.m_numIterations) - resolveSingleConstraintRowGeneric(constraint); + { + residual = resolveSingleConstraintRowGeneric(constraint); + } + + leastSquaredResidual = btMax(leastSquaredResidual, residual * residual); - if(constraint.m_multiBodyA) + if (constraint.m_multiBodyA) constraint.m_multiBodyA->setPosUpdated(false); - if(constraint.m_multiBodyB) + if (constraint.m_multiBodyB) constraint.m_multiBodyB->setPosUpdated(false); } - + //solve featherstone frictional contact + if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0)) + { + for (int j1 = 0; j1 < this->m_multiBodySpinningFrictionContactConstraints.size(); j1++) + { + if (iteration < infoGlobal.m_numIterations) + { + int index = j1; + + btMultiBodySolverConstraint& frictionConstraint = m_multiBodySpinningFrictionContactConstraints[index]; + btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; + //adjust friction limits here + if (totalImpulse > btScalar(0)) + { + frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse); + frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse; + btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint); + leastSquaredResidual = btMax(leastSquaredResidual, residual * residual); + + if (frictionConstraint.m_multiBodyA) + frictionConstraint.m_multiBodyA->setPosUpdated(false); + if (frictionConstraint.m_multiBodyB) + frictionConstraint.m_multiBodyB->setPosUpdated(false); + } + } + } + + for (int j1 = 0; j1 < this->m_multiBodyTorsionalFrictionContactConstraints.size(); j1++) + { + if (iteration < infoGlobal.m_numIterations) + { + int index = j1; //iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1; + + btMultiBodySolverConstraint& frictionConstraint = m_multiBodyTorsionalFrictionContactConstraints[index]; + btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; + j1++; + int index2 = j1; + btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyTorsionalFrictionContactConstraints[index2]; + //adjust friction limits here + if (totalImpulse > btScalar(0) && frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex) + { + frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse); + frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse; + frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction * totalImpulse); + frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction * totalImpulse; + + btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB); + leastSquaredResidual = btMax(leastSquaredResidual, residual * residual); + + if (frictionConstraint.m_multiBodyA) + frictionConstraint.m_multiBodyA->setPosUpdated(false); + if (frictionConstraint.m_multiBodyB) + frictionConstraint.m_multiBodyB->setPosUpdated(false); + + if (frictionConstraintB.m_multiBodyA) + frictionConstraintB.m_multiBodyA->setPosUpdated(false); + if (frictionConstraintB.m_multiBodyB) + frictionConstraintB.m_multiBodyB->setPosUpdated(false); + } + } + } + + for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++) + { + if (iteration < infoGlobal.m_numIterations) + { + int index = j1; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; + btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index]; - for (int j=0;j<this->m_multiBodyFrictionContactConstraints.size();j++) + btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; + j1++; + int index2 = j1; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; + btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyFrictionContactConstraints[index2]; + btAssert(frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex); + + if (frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex) + { + frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse); + frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse; + frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction * totalImpulse); + frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction * totalImpulse; + btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB); + leastSquaredResidual = btMax(leastSquaredResidual, residual * residual); + + if (frictionConstraintB.m_multiBodyA) + frictionConstraintB.m_multiBodyA->setPosUpdated(false); + if (frictionConstraintB.m_multiBodyB) + frictionConstraintB.m_multiBodyB->setPosUpdated(false); + + if (frictionConstraint.m_multiBodyA) + frictionConstraint.m_multiBodyA->setPosUpdated(false); + if (frictionConstraint.m_multiBodyB) + frictionConstraint.m_multiBodyB->setPosUpdated(false); + } + } + } + } + else { - if (iteration < infoGlobal.m_numIterations) + for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++) { - btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j]; - btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; - //adjust friction limits here - if (totalImpulse>btScalar(0)) + if (iteration < infoGlobal.m_numIterations) { - frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); - frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; - resolveSingleConstraintRowGeneric(frictionConstraint); - - if(frictionConstraint.m_multiBodyA) - frictionConstraint.m_multiBodyA->setPosUpdated(false); - if(frictionConstraint.m_multiBodyB) - frictionConstraint.m_multiBodyB->setPosUpdated(false); + int index = j1; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; + + btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index]; + btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; + //adjust friction limits here + if (totalImpulse > btScalar(0)) + { + frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse); + frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse; + btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint); + leastSquaredResidual = btMax(leastSquaredResidual, residual * residual); + + if (frictionConstraint.m_multiBodyA) + frictionConstraint.m_multiBodyA->setPosUpdated(false); + if (frictionConstraint.m_multiBodyB) + frictionConstraint.m_multiBodyB->setPosUpdated(false); + } } } } - return val; + return leastSquaredResidual; } -btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { m_multiBodyNonContactConstraints.resize(0); m_multiBodyNormalContactConstraints.resize(0); m_multiBodyFrictionContactConstraints.resize(0); + m_multiBodyTorsionalFrictionContactConstraints.resize(0); + m_multiBodySpinningFrictionContactConstraints.resize(0); + m_data.m_jacobians.resize(0); m_data.m_deltaVelocitiesUnitImpulse.resize(0); m_data.m_deltaVelocities.resize(0); - for (int i=0;i<numBodies;i++) + for (int i = 0; i < numBodies; i++) { const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(bodies[i]); if (fcA) @@ -98,63 +221,63 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionOb } } - btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer); + btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); return val; } -void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) +void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) { - for (int i = 0; i < ndof; ++i) - m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; + for (int i = 0; i < ndof; ++i) + m_data.m_deltaVelocities[velocityIndex + i] += delta_vee[i] * impulse; } -void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c) +btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c) { - - btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - btScalar deltaVelADotn=0; - btScalar deltaVelBDotn=0; + btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm; + btScalar deltaVelADotn = 0; + btScalar deltaVelBDotn = 0; btSolverBody* bodyA = 0; btSolverBody* bodyB = 0; - int ndofA=0; - int ndofB=0; + int ndofA = 0; + int ndofB = 0; if (c.m_multiBodyA) { - ndofA = c.m_multiBodyA->getNumDofs() + 6; - for (int i = 0; i < ndofA; ++i) - deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i]; - } else if(c.m_solverBodyIdA >= 0) + ndofA = c.m_multiBodyA->getNumDofs() + 6; + for (int i = 0; i < ndofA; ++i) + deltaVelADotn += m_data.m_jacobians[c.m_jacAindex + i] * m_data.m_deltaVelocities[c.m_deltaVelAindex + i]; + } + else if (c.m_solverBodyIdA >= 0) { bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA]; - deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); + deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); } if (c.m_multiBodyB) { - ndofB = c.m_multiBodyB->getNumDofs() + 6; - for (int i = 0; i < ndofB; ++i) - deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i]; - } else if(c.m_solverBodyIdB >= 0) + ndofB = c.m_multiBodyB->getNumDofs() + 6; + for (int i = 0; i < ndofB; ++i) + deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex + i] * m_data.m_deltaVelocities[c.m_deltaVelBindex + i]; + } + else if (c.m_solverBodyIdB >= 0) { bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB]; - deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); + deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); } - - deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom - deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv; + deltaImpulse -= deltaVelADotn * c.m_jacDiagABInv; //m_jacDiagABInv = 1./denom + deltaImpulse -= deltaVelBDotn * c.m_jacDiagABInv; const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; - + if (sum < c.m_lowerLimit) { - deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; + deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse; c.m_appliedImpulse = c.m_lowerLimit; } - else if (sum > c.m_upperLimit) + else if (sum > c.m_upperLimit) { - deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; + deltaImpulse = c.m_upperLimit - c.m_appliedImpulse; c.m_appliedImpulse = c.m_upperLimit; } else @@ -164,42 +287,219 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult if (c.m_multiBodyA) { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA); #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); -#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - } else if(c.m_solverBodyIdA >= 0) + c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse); +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } + else if (c.m_solverBodyIdA >= 0) { - bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - + bodyA->internalApplyImpulse(c.m_contactNormal1 * bodyA->internalGetInvMass(), c.m_angularComponentA, deltaImpulse); } if (c.m_multiBodyB) { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse, c.m_deltaVelBindex, ndofB); #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); -#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - } else if(c.m_solverBodyIdB >= 0) + c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse); +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } + else if (c.m_solverBodyIdB >= 0) { - bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + bodyB->internalApplyImpulse(c.m_contactNormal2 * bodyB->internalGetInvMass(), c.m_angularComponentB, deltaImpulse); } - + btScalar deltaVel = deltaImpulse / c.m_jacDiagABInv; + return deltaVel; } +btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1, const btMultiBodySolverConstraint& cB) +{ + int ndofA = 0; + int ndofB = 0; + btSolverBody* bodyA = 0; + btSolverBody* bodyB = 0; + btScalar deltaImpulseB = 0.f; + btScalar sumB = 0.f; + { + deltaImpulseB = cB.m_rhs - btScalar(cB.m_appliedImpulse) * cB.m_cfm; + btScalar deltaVelADotn = 0; + btScalar deltaVelBDotn = 0; + if (cB.m_multiBodyA) + { + ndofA = cB.m_multiBodyA->getNumDofs() + 6; + for (int i = 0; i < ndofA; ++i) + deltaVelADotn += m_data.m_jacobians[cB.m_jacAindex + i] * m_data.m_deltaVelocities[cB.m_deltaVelAindex + i]; + } + else if (cB.m_solverBodyIdA >= 0) + { + bodyA = &m_tmpSolverBodyPool[cB.m_solverBodyIdA]; + deltaVelADotn += cB.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cB.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); + } + if (cB.m_multiBodyB) + { + ndofB = cB.m_multiBodyB->getNumDofs() + 6; + for (int i = 0; i < ndofB; ++i) + deltaVelBDotn += m_data.m_jacobians[cB.m_jacBindex + i] * m_data.m_deltaVelocities[cB.m_deltaVelBindex + i]; + } + else if (cB.m_solverBodyIdB >= 0) + { + bodyB = &m_tmpSolverBodyPool[cB.m_solverBodyIdB]; + deltaVelBDotn += cB.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cB.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); + } + + deltaImpulseB -= deltaVelADotn * cB.m_jacDiagABInv; //m_jacDiagABInv = 1./denom + deltaImpulseB -= deltaVelBDotn * cB.m_jacDiagABInv; + sumB = btScalar(cB.m_appliedImpulse) + deltaImpulseB; + } + + btScalar deltaImpulseA = 0.f; + btScalar sumA = 0.f; + const btMultiBodySolverConstraint& cA = cA1; + { + { + deltaImpulseA = cA.m_rhs - btScalar(cA.m_appliedImpulse) * cA.m_cfm; + btScalar deltaVelADotn = 0; + btScalar deltaVelBDotn = 0; + if (cA.m_multiBodyA) + { + ndofA = cA.m_multiBodyA->getNumDofs() + 6; + for (int i = 0; i < ndofA; ++i) + deltaVelADotn += m_data.m_jacobians[cA.m_jacAindex + i] * m_data.m_deltaVelocities[cA.m_deltaVelAindex + i]; + } + else if (cA.m_solverBodyIdA >= 0) + { + bodyA = &m_tmpSolverBodyPool[cA.m_solverBodyIdA]; + deltaVelADotn += cA.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cA.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); + } + + if (cA.m_multiBodyB) + { + ndofB = cA.m_multiBodyB->getNumDofs() + 6; + for (int i = 0; i < ndofB; ++i) + deltaVelBDotn += m_data.m_jacobians[cA.m_jacBindex + i] * m_data.m_deltaVelocities[cA.m_deltaVelBindex + i]; + } + else if (cA.m_solverBodyIdB >= 0) + { + bodyB = &m_tmpSolverBodyPool[cA.m_solverBodyIdB]; + deltaVelBDotn += cA.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cA.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); + } + + deltaImpulseA -= deltaVelADotn * cA.m_jacDiagABInv; //m_jacDiagABInv = 1./denom + deltaImpulseA -= deltaVelBDotn * cA.m_jacDiagABInv; + sumA = btScalar(cA.m_appliedImpulse) + deltaImpulseA; + } + } + + if (sumA * sumA + sumB * sumB >= cA.m_lowerLimit * cB.m_lowerLimit) + { + btScalar angle = btAtan2(sumA, sumB); + btScalar sumAclipped = btFabs(cA.m_lowerLimit * btSin(angle)); + btScalar sumBclipped = btFabs(cB.m_lowerLimit * btCos(angle)); + + if (sumA < -sumAclipped) + { + deltaImpulseA = -sumAclipped - cA.m_appliedImpulse; + cA.m_appliedImpulse = -sumAclipped; + } + else if (sumA > sumAclipped) + { + deltaImpulseA = sumAclipped - cA.m_appliedImpulse; + cA.m_appliedImpulse = sumAclipped; + } + else + { + cA.m_appliedImpulse = sumA; + } + + if (sumB < -sumBclipped) + { + deltaImpulseB = -sumBclipped - cB.m_appliedImpulse; + cB.m_appliedImpulse = -sumBclipped; + } + else if (sumB > sumBclipped) + { + deltaImpulseB = sumBclipped - cB.m_appliedImpulse; + cB.m_appliedImpulse = sumBclipped; + } + else + { + cB.m_appliedImpulse = sumB; + } + //deltaImpulseA = sumAclipped-cA.m_appliedImpulse; + //cA.m_appliedImpulse = sumAclipped; + //deltaImpulseB = sumBclipped-cB.m_appliedImpulse; + //cB.m_appliedImpulse = sumBclipped; + } + else + { + cA.m_appliedImpulse = sumA; + cB.m_appliedImpulse = sumB; + } + + if (cA.m_multiBodyA) + { + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex], deltaImpulseA, cA.m_deltaVelAindex, ndofA); +#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations + //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity + cA.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex], deltaImpulseA); +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } + else if (cA.m_solverBodyIdA >= 0) + { + bodyA->internalApplyImpulse(cA.m_contactNormal1 * bodyA->internalGetInvMass(), cA.m_angularComponentA, deltaImpulseA); + } + if (cA.m_multiBodyB) + { + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex], deltaImpulseA, cA.m_deltaVelBindex, ndofB); +#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations + //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity + cA.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex], deltaImpulseA); +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } + else if (cA.m_solverBodyIdB >= 0) + { + bodyB->internalApplyImpulse(cA.m_contactNormal2 * bodyB->internalGetInvMass(), cA.m_angularComponentB, deltaImpulseA); + } + + if (cB.m_multiBodyA) + { + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex], deltaImpulseB, cB.m_deltaVelAindex, ndofA); +#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations + //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity + cB.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex], deltaImpulseB); +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } + else if (cB.m_solverBodyIdA >= 0) + { + bodyA->internalApplyImpulse(cB.m_contactNormal1 * bodyA->internalGetInvMass(), cB.m_angularComponentA, deltaImpulseB); + } + if (cB.m_multiBodyB) + { + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex], deltaImpulseB, cB.m_deltaVelBindex, ndofB); +#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations + //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity + cB.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex], deltaImpulseB); +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } + else if (cB.m_solverBodyIdB >= 0) + { + bodyB->internalApplyImpulse(cB.m_contactNormal2 * bodyB->internalGetInvMass(), cB.m_angularComponentB, deltaImpulseB); + } + btScalar deltaVel = deltaImpulseA / cA.m_jacDiagABInv + deltaImpulseB / cB.m_jacDiagABInv; + return deltaVel; +} -void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, - const btVector3& contactNormal, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) +void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, const btVector3& contactNormal, const btScalar& appliedImpulse, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation, bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) { - BT_PROFILE("setupMultiBodyContactConstraint"); btVector3 rel_pos1; btVector3 rel_pos2; @@ -217,214 +517,266 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; if (bodyA) - rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); if (bodyB) rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); - relaxation = 1.f; + relaxation = infoGlobal.m_sor; - + btScalar invTimeStep = btScalar(1) / infoGlobal.m_timeStep; + + //cfm = 1 / ( dt * kp + kd ) + //erp = dt * kp / ( dt * kp + kd ) + btScalar cfm; + btScalar erp; + if (isFriction) + { + cfm = infoGlobal.m_frictionCFM; + erp = infoGlobal.m_frictionERP; + } + else + { + cfm = infoGlobal.m_globalCfm; + erp = infoGlobal.m_erp2; + + if ((cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP)) + { + if (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM) + cfm = cp.m_contactCFM; + if (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP) + erp = cp.m_contactERP; + } + else + { + if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING) + { + btScalar denom = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1); + if (denom < SIMD_EPSILON) + { + denom = SIMD_EPSILON; + } + cfm = btScalar(1) / denom; + erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom; + } + } + } + + cfm *= invTimeStep; if (multiBodyA) { - if (solverConstraint.m_linkA<0) + if (solverConstraint.m_linkA < 0) { rel_pos1 = pos1 - multiBodyA->getBasePos(); - } else + } + else { rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin(); } - const int ndofA = multiBodyA->getNumDofs() + 6; + const int ndofA = multiBodyA->getNumDofs() + 6; solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); - if (solverConstraint.m_deltaVelAindex <0) + if (solverConstraint.m_deltaVelAindex < 0) { solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size(); multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); - m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA); - } else + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofA); + } + else { - btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); + btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA); } solverConstraint.m_jacAindex = m_data.m_jacobians.size(); - m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA); - m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA); + m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofA); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofA); btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); - btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex]; + btScalar* jac1 = &m_data.m_jacobians[solverConstraint.m_jacAindex]; multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); + multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex], delta, m_data.scratch_r, m_data.scratch_v); btVector3 torqueAxis0 = rel_pos1.cross(contactNormal); solverConstraint.m_relpos1CrossNormal = torqueAxis0; solverConstraint.m_contactNormal1 = contactNormal; - } else + } + else { btVector3 torqueAxis0 = rel_pos1.cross(contactNormal); solverConstraint.m_relpos1CrossNormal = torqueAxis0; solverConstraint.m_contactNormal1 = contactNormal; - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0); } - - if (multiBodyB) { - if (solverConstraint.m_linkB<0) + if (solverConstraint.m_linkB < 0) { rel_pos2 = pos2 - multiBodyB->getBasePos(); - } else + } + else { rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); } - const int ndofB = multiBodyB->getNumDofs() + 6; + const int ndofB = multiBodyB->getNumDofs() + 6; solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); - if (solverConstraint.m_deltaVelBindex <0) + if (solverConstraint.m_deltaVelBindex < 0) { solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size(); multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); - m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB); + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofB); } solverConstraint.m_jacBindex = m_data.m_jacobians.size(); - m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB); - m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB); + m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofB); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofB); btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); - multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); - - btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); + multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex], &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v); + + btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); solverConstraint.m_relpos2CrossNormal = -torqueAxis1; solverConstraint.m_contactNormal2 = -contactNormal; - - } else + } + else { - btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); + btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); solverConstraint.m_relpos2CrossNormal = -torqueAxis1; solverConstraint.m_contactNormal2 = -contactNormal; - - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); + + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0); } { - btVector3 vec; btScalar denom0 = 0.f; btScalar denom1 = 0.f; btScalar* jacB = 0; btScalar* jacA = 0; - btScalar* lambdaA =0; - btScalar* lambdaB =0; - int ndofA = 0; + btScalar* lambdaA = 0; + btScalar* lambdaB = 0; + int ndofA = 0; if (multiBodyA) { - ndofA = multiBodyA->getNumDofs() + 6; + ndofA = multiBodyA->getNumDofs() + 6; jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; for (int i = 0; i < ndofA; ++i) { - btScalar j = jacA[i] ; - btScalar l =lambdaA[i]; - denom0 += j*l; + btScalar j = jacA[i]; + btScalar l = lambdaA[i]; + denom0 += j * l; } - } else + } + else { if (rb0) { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + vec = (solverConstraint.m_angularComponentA).cross(rel_pos1); denom0 = rb0->getInvMass() + contactNormal.dot(vec); } } if (multiBodyB) { - const int ndofB = multiBodyB->getNumDofs() + 6; + const int ndofB = multiBodyB->getNumDofs() + 6; jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; for (int i = 0; i < ndofB; ++i) { - btScalar j = jacB[i] ; - btScalar l =lambdaB[i]; - denom1 += j*l; + btScalar j = jacB[i]; + btScalar l = lambdaB[i]; + denom1 += j * l; } - - } else + } + else { if (rb1) { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); + vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2); denom1 = rb1->getInvMass() + contactNormal.dot(vec); } } - - - btScalar d = denom0+denom1; - if (d>SIMD_EPSILON) - { - solverConstraint.m_jacDiagABInv = relaxation/(d); - } else - { + btScalar d = denom0 + denom1 + cfm; + if (d > SIMD_EPSILON) + { + solverConstraint.m_jacDiagABInv = relaxation / (d); + } + else + { //disable the constraint row to handle singularity/redundant constraint - solverConstraint.m_jacDiagABInv = 0.f; - } - + solverConstraint.m_jacDiagABInv = 0.f; + } } - //compute rhs and remaining solverConstraint fields - - btScalar restitution = 0.f; - btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop; + btScalar distance = 0; + if (!isFriction) + { + distance = cp.getDistance() + infoGlobal.m_linearSlop; + } + else + { + if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR) + { + distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(contactNormal); + } + } btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; + int ndofA = 0; + int ndofB = 0; { - - btVector3 vel1,vel2; + btVector3 vel1, vel2; if (multiBodyA) { - ndofA = multiBodyA->getNumDofs() + 6; + ndofA = multiBodyA->getNumDofs() + 6; btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA ; ++i) + for (int i = 0; i < ndofA; ++i) rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; - } else + } + else { if (rb0) { - rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); + rel_vel += (rb0->getVelocityInLocalPoint(rel_pos1) + + (rb0->getTotalTorque() * rb0->getInvInertiaTensorWorld() * infoGlobal.m_timeStep).cross(rel_pos1) + + rb0->getTotalForce() * rb0->getInvMass() * infoGlobal.m_timeStep) + .dot(solverConstraint.m_contactNormal1); } } if (multiBodyB) { - ndofB = multiBodyB->getNumDofs() + 6; + ndofB = multiBodyB->getNumDofs() + 6; btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB ; ++i) + for (int i = 0; i < ndofB; ++i) rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; - - } else + } + else { if (rb1) { - rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); + rel_vel += (rb1->getVelocityInLocalPoint(rel_pos2) + + (rb1->getTotalTorque() * rb1->getInvInertiaTensorWorld() * infoGlobal.m_timeStep).cross(rel_pos2) + + rb1->getTotalForce() * rb1->getInvMass() * infoGlobal.m_timeStep) + .dot(solverConstraint.m_contactNormal2); } } solverConstraint.m_friction = cp.m_combinedFriction; - if(!isFriction) + if (!isFriction) { - restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); if (restitution <= btScalar(0.)) { restitution = 0.f; @@ -432,12 +784,70 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol } } + { + btScalar positionalError = 0.f; + btScalar velocityError = restitution - rel_vel; // * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction + if (isFriction) + { + positionalError = -distance * erp / infoGlobal.m_timeStep; + } + else + { + if (distance > 0) + { + positionalError = 0; + velocityError -= distance / infoGlobal.m_timeStep; + } + else + { + positionalError = -distance * erp / infoGlobal.m_timeStep; + } + } + + btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; + + if (!isFriction) + { + // if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + //combine position and velocity into rhs + solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; + solverConstraint.m_rhsPenetration = 0.f; + } + /*else + { + //split position and velocity into rhs and m_rhsPenetration + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = penetrationImpulse; + } + */ + solverConstraint.m_lowerLimit = 0; + solverConstraint.m_upperLimit = 1e10f; + } + else + { + solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; + solverConstraint.m_rhsPenetration = 0.f; + solverConstraint.m_lowerLimit = -solverConstraint.m_friction; + solverConstraint.m_upperLimit = solverConstraint.m_friction; + } - ///warm starting (or zero if disabled) - //disable warmstarting for btMultiBody, it has issues gaining energy (==explosion) - if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv; + } + + if (infoGlobal.m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING) { - solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; + if (btFabs(cp.m_prevRHS) > 1e-5 && cp.m_prevRHS < 2* solverConstraint.m_rhs && solverConstraint.m_rhs < 2*cp.m_prevRHS) + { + solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse / cp.m_prevRHS * solverConstraint.m_rhs * infoGlobal.m_articulatedWarmstartingFactor; + if (solverConstraint.m_appliedImpulse < 0) + solverConstraint.m_appliedImpulse = 0; + } + else + { + solverConstraint.m_appliedImpulse = 0.f; + } if (solverConstraint.m_appliedImpulse) { @@ -445,109 +855,348 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol { btScalar impulse = solverConstraint.m_appliedImpulse; btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse); - - applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); - } else + multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse); + + applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA); + } + else { if (rb0) - bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); + bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse); } if (multiBodyB) { btScalar impulse = solverConstraint.m_appliedImpulse; btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse); - applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); - } else + multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse); + applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB); + } + else { if (rb1) - bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); + bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->getLinearFactor(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse); } } - } else + } + else { solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedPushImpulse = 0.f; } +} - solverConstraint.m_appliedPushImpulse = 0.f; +void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint, + const btVector3& constraintNormal, + btManifoldPoint& cp, + btScalar combinedTorsionalFriction, + const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) +{ + BT_PROFILE("setupMultiBodyRollingFrictionConstraint"); + btVector3 rel_pos1; + btVector3 rel_pos2; + + btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; + btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; + + const btVector3& pos1 = cp.getPositionWorldOnA(); + const btVector3& pos2 = cp.getPositionWorldOnB(); + + btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA]; + btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB]; + + btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; + btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; + if (bodyA) + rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + if (bodyB) + rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); + + relaxation = infoGlobal.m_sor; + + // btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep; + + if (multiBodyA) { + if (solverConstraint.m_linkA < 0) + { + rel_pos1 = pos1 - multiBodyA->getBasePos(); + } + else + { + rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin(); + } + const int ndofA = multiBodyA->getNumDofs() + 6; - btScalar positionalError = 0.f; - btScalar velocityError = restitution - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction + solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); - btScalar erp = infoGlobal.m_erp2; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + if (solverConstraint.m_deltaVelAindex < 0) + { + solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size(); + multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofA); + } + else { - erp = infoGlobal.m_erp; + btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA); } - if (penetration>0) + solverConstraint.m_jacAindex = m_data.m_jacobians.size(); + m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofA); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofA); + btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); + + btScalar* jac1 = &m_data.m_jacobians[solverConstraint.m_jacAindex]; + multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), constraintNormal, btVector3(0, 0, 0), jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex], delta, m_data.scratch_r, m_data.scratch_v); + + btVector3 torqueAxis0 = constraintNormal; + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = btVector3(0, 0, 0); + } + else + { + btVector3 torqueAxis0 = constraintNormal; + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = btVector3(0, 0, 0); + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0); + } + + if (multiBodyB) + { + if (solverConstraint.m_linkB < 0) { - positionalError = 0; - velocityError -= penetration / infoGlobal.m_timeStep; + rel_pos2 = pos2 - multiBodyB->getBasePos(); + } + else + { + rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); + } + + const int ndofB = multiBodyB->getNumDofs() + 6; - } else + solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); + if (solverConstraint.m_deltaVelBindex < 0) { - positionalError = -penetration * erp/infoGlobal.m_timeStep; + solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size(); + multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofB); } - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + solverConstraint.m_jacBindex = m_data.m_jacobians.size(); + + m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofB); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofB); + btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); + + multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -constraintNormal, btVector3(0, 0, 0), &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex], &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v); + + btVector3 torqueAxis1 = -constraintNormal; + solverConstraint.m_relpos2CrossNormal = torqueAxis1; + solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0); + } + else + { + btVector3 torqueAxis1 = -constraintNormal; + solverConstraint.m_relpos2CrossNormal = torqueAxis1; + solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0); + + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0); + } - if(!isFriction) + { + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + btScalar* jacB = 0; + btScalar* jacA = 0; + btScalar* lambdaA = 0; + btScalar* lambdaB = 0; + int ndofA = 0; + if (multiBodyA) { - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + ndofA = multiBodyA->getNumDofs() + 6; + jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; + lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) { - //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; - solverConstraint.m_rhsPenetration = 0.f; - - } else + btScalar j = jacA[i]; + btScalar l = lambdaA[i]; + denom0 += j * l; + } + } + else + { + if (rb0) { - //split position and velocity into rhs and m_rhsPenetration - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = penetrationImpulse; + btVector3 iMJaA = rb0 ? rb0->getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal : btVector3(0, 0, 0); + denom0 = iMJaA.dot(solverConstraint.m_relpos1CrossNormal); + } + } + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumDofs() + 6; + jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; + lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + { + btScalar j = jacB[i]; + btScalar l = lambdaB[i]; + denom1 += j * l; } + } + else + { + if (rb1) + { + btVector3 iMJaB = rb1 ? rb1->getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal : btVector3(0, 0, 0); + denom1 = iMJaB.dot(solverConstraint.m_relpos2CrossNormal); + } + } - solverConstraint.m_lowerLimit = 0; - solverConstraint.m_upperLimit = 1e10f; + btScalar d = denom0 + denom1 + infoGlobal.m_globalCfm; + if (d > SIMD_EPSILON) + { + solverConstraint.m_jacDiagABInv = relaxation / (d); } else { - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = 0.f; - solverConstraint.m_lowerLimit = -solverConstraint.m_friction; - solverConstraint.m_upperLimit = solverConstraint.m_friction; + //disable the constraint row to handle singularity/redundant constraint + solverConstraint.m_jacDiagABInv = 0.f; + } + } + + //compute rhs and remaining solverConstraint fields + + btScalar restitution = 0.f; + btScalar penetration = isFriction ? 0 : cp.getDistance(); + + btScalar rel_vel = 0.f; + int ndofA = 0; + int ndofB = 0; + { + btVector3 vel1, vel2; + if (multiBodyA) + { + ndofA = multiBodyA->getNumDofs() + 6; + btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) + rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; + } + else + { + if (rb0) + { + btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA]; + rel_vel += solverConstraint.m_contactNormal1.dot(rb0 ? solverBodyA->m_linearVelocity + solverBodyA->m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(rb0 ? solverBodyA->m_angularVelocity : btVector3(0, 0, 0)); + } + } + if (multiBodyB) + { + ndofB = multiBodyB->getNumDofs() + 6; + btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; + } + else + { + if (rb1) + { + btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB]; + rel_vel += solverConstraint.m_contactNormal2.dot(rb1 ? solverBodyB->m_linearVelocity + solverBodyB->m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(rb1 ? solverBodyB->m_angularVelocity : btVector3(0, 0, 0)); + } } - solverConstraint.m_cfm = 0.f; //why not use cfmSlip? + solverConstraint.m_friction = combinedTorsionalFriction; + + if (!isFriction) + { + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); + if (restitution <= btScalar(0.)) + { + restitution = 0.f; + } + } } -} + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedPushImpulse = 0.f; + { + btScalar velocityError = 0 - rel_vel; // * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction + btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = 0.f; + solverConstraint.m_lowerLimit = -solverConstraint.m_friction; + solverConstraint.m_upperLimit = solverConstraint.m_friction; -btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) + solverConstraint.m_cfm = infoGlobal.m_globalCfm * solverConstraint.m_jacDiagABInv; + } +} + +btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { BT_PROFILE("addMultiBodyFrictionConstraint"); btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing(); - solverConstraint.m_orgConstraint = 0; - solverConstraint.m_orgDofIndex = -1; - + solverConstraint.m_orgConstraint = 0; + solverConstraint.m_orgDofIndex = -1; + solverConstraint.m_frictionIndex = frictionIndex; bool isFriction = true; const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); - - btMultiBody* mbA = fcA? fcA->m_multiBody : 0; - btMultiBody* mbB = fcB? fcB->m_multiBody : 0; - int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); - int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); + btMultiBody* mbA = fcA ? fcA->m_multiBody : 0; + btMultiBody* mbB = fcB ? fcB->m_multiBody : 0; + + int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep); + int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep); + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_multiBodyA = mbA; + if (mbA) + solverConstraint.m_linkA = fcA->m_link; + + solverConstraint.m_multiBodyB = mbB; + if (mbB) + solverConstraint.m_linkB = fcB->m_link; + + solverConstraint.m_originalContactPoint = &cp; + + setupMultiBodyContactConstraint(solverConstraint, normalAxis, 0, cp, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip); + return solverConstraint; +} + +btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, + btScalar combinedTorsionalFriction, + btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +{ + BT_PROFILE("addMultiBodyRollingFrictionConstraint"); + + bool useTorsionalAndConeFriction = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0)); + + btMultiBodySolverConstraint& solverConstraint = useTorsionalAndConeFriction ? m_multiBodyTorsionalFrictionContactConstraints.expandNonInitializing() : m_multiBodyFrictionContactConstraints.expandNonInitializing(); + solverConstraint.m_orgConstraint = 0; + solverConstraint.m_orgDofIndex = -1; + + solverConstraint.m_frictionIndex = frictionIndex; + bool isFriction = true; + + const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); + const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + + btMultiBody* mbA = fcA ? fcA->m_multiBody : 0; + btMultiBody* mbB = fcB ? fcB->m_multiBody : 0; + + int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep); + int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep); solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; @@ -561,54 +1210,89 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionCo solverConstraint.m_originalContactPoint = &cp; - setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip); + setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip); return solverConstraint; } -void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) +btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodySpinningFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, + btScalar combinedTorsionalFriction, + btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +{ + BT_PROFILE("addMultiBodyRollingFrictionConstraint"); + + btMultiBodySolverConstraint& solverConstraint = m_multiBodySpinningFrictionContactConstraints.expandNonInitializing(); + solverConstraint.m_orgConstraint = 0; + solverConstraint.m_orgDofIndex = -1; + + solverConstraint.m_frictionIndex = frictionIndex; + bool isFriction = true; + + const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); + const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + + btMultiBody* mbA = fcA ? fcA->m_multiBody : 0; + btMultiBody* mbB = fcB ? fcB->m_multiBody : 0; + + int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep); + int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep); + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_multiBodyA = mbA; + if (mbA) + solverConstraint.m_linkA = fcA->m_link; + + solverConstraint.m_multiBodyB = mbB; + if (mbB) + solverConstraint.m_linkB = fcB->m_link; + + solverConstraint.m_originalContactPoint = &cp; + + setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip); + return solverConstraint; +} +void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal) { const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); - btMultiBody* mbA = fcA? fcA->m_multiBody : 0; - btMultiBody* mbB = fcB? fcB->m_multiBody : 0; + btMultiBody* mbA = fcA ? fcA->m_multiBody : 0; + btMultiBody* mbB = fcB ? fcB->m_multiBody : 0; - btCollisionObject* colObj0=0,*colObj1=0; + btCollisionObject *colObj0 = 0, *colObj1 = 0; colObj0 = (btCollisionObject*)manifold->getBody0(); colObj1 = (btCollisionObject*)manifold->getBody1(); - int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); - int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); - -// btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA]; -// btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB]; + int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep); + int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep); + // btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA]; + // btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB]; ///avoid collision response between two static objects -// if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero()))) + // if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero()))) // return; + //only a single rollingFriction per manifold + int rollingFriction = 4; - - for (int j=0;j<manifold->getNumContacts();j++) + for (int j = 0; j < manifold->getNumContacts(); j++) { - btManifoldPoint& cp = manifold->getContactPoint(j); if (cp.getDistance() <= manifold->getContactProcessingThreshold()) { - btScalar relaxation; int frictionIndex = m_multiBodyNormalContactConstraints.size(); btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing(); - // btRigidBody* rb0 = btRigidBody::upcast(colObj0); - // btRigidBody* rb1 = btRigidBody::upcast(colObj1); - solverConstraint.m_orgConstraint = 0; - solverConstraint.m_orgDofIndex = -1; + // btRigidBody* rb0 = btRigidBody::upcast(colObj0); + // btRigidBody* rb1 = btRigidBody::upcast(colObj1); + solverConstraint.m_orgConstraint = 0; + solverConstraint.m_orgDofIndex = -1; solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; solverConstraint.m_multiBodyA = mbA; @@ -622,71 +1306,56 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* solverConstraint.m_originalContactPoint = &cp; bool isFriction = false; - setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction); + setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp.m_appliedImpulse, cp, infoGlobal, relaxation, isFriction); -// const btVector3& pos1 = cp.getPositionWorldOnA(); -// const btVector3& pos2 = cp.getPositionWorldOnB(); + // const btVector3& pos1 = cp.getPositionWorldOnA(); + // const btVector3& pos2 = cp.getPositionWorldOnB(); /////setup the friction constraints #define ENABLE_FRICTION #ifdef ENABLE_FRICTION - solverConstraint.m_frictionIndex = frictionIndex; -#if ROLLING_FRICTION - int rollingFriction=1; - btVector3 angVelA(0,0,0),angVelB(0,0,0); - if (rb0) - angVelA = rb0->getAngularVelocity(); - if (rb1) - angVelB = rb1->getAngularVelocity(); - btVector3 relAngVel = angVelB-angVelA; - - if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) - { - //only a single rollingFriction per manifold - rollingFriction--; - if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold) - { - relAngVel.normalize(); - applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - if (relAngVel.length()>0.001) - addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } else - { - addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - btVector3 axis0,axis1; - btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1); - applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - if (axis0.length()>0.001) - addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - if (axis1.length()>0.001) - addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } - } -#endif //ROLLING_FRICTION + solverConstraint.m_frictionIndex = m_multiBodyFrictionContactConstraints.size(); ///Bullet has several options to set the friction directions - ///By default, each contact has only a single friction direction that is recomputed automatically very frame + ///By default, each contact has only a single friction direction that is recomputed automatically every frame ///based on the relative linear velocity. - ///If the relative velocity it zero, it will automatically compute a friction direction. - + ///If the relative velocity is zero, it will automatically compute a friction direction. + ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS. ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction. /// ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity. /// - ///The user can manually override the friction directions for certain contacts using a contact callback, + ///The user can manually override the friction directions for certain contacts using a contact callback, ///and set the cp.m_lateralFrictionInitialized to true ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) ///this will give a conveyor belt effect /// - if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) - {/* + + btPlaneSpace1(cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2); + cp.m_lateralFrictionDir1.normalize(); + cp.m_lateralFrictionDir2.normalize(); + + if (rollingFriction > 0) + { + if (cp.m_combinedSpinningFriction > 0) + { + addMultiBodySpinningFrictionConstraint(cp.m_normalWorldOnB, manifold, frictionIndex, cp, cp.m_combinedSpinningFriction, colObj0, colObj1, relaxation, infoGlobal); + } + if (cp.m_combinedRollingFriction > 0) + { + applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + + addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal); + addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal); + } + rollingFriction--; + } + if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags & BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED)) + { /* cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) @@ -709,83 +1378,123 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* } else */ { - btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); - - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); + applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); + applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal); } if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) { - cp.m_lateralFrictionInitialized = true; + cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED; } } - - } else + } + else { - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2); - - //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); - //todo: + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM); solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedPushImpulse = 0.f; - } - - -#endif //ENABLE_FRICTION + } +#endif //ENABLE_FRICTION + } + else + { + // Reset quantities related to warmstart as 0. + cp.m_appliedImpulse = 0; + cp.m_prevRHS = 0; } } } -void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal) +void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) { - //btPersistentManifold* manifold = 0; - - for (int i=0;i<numManifolds;i++) + for (int i = 0; i < numManifolds; i++) { - btPersistentManifold* manifold= manifoldPtr[i]; + btPersistentManifold* manifold = manifoldPtr[i]; const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); if (!fcA && !fcB) { //the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case - convertContact(manifold,infoGlobal); - } else + convertContact(manifold, infoGlobal); + } + else { - convertMultiBodyContact(manifold,infoGlobal); + convertMultiBodyContact(manifold, infoGlobal); } } //also convert the multibody constraints, if any - - for (int i=0;i<m_tmpNumMultiBodyConstraints;i++) + for (int i = 0; i < m_tmpNumMultiBodyConstraints; i++) { btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i]; m_data.m_solverBodyPool = &m_tmpSolverBodyPool; m_data.m_fixedBodyId = m_fixedBodyId; - - c->createConstraintRows(m_multiBodyNonContactConstraints,m_data, infoGlobal); + + c->createConstraintRows(m_multiBodyNonContactConstraints, m_data, infoGlobal); } + // Warmstart for noncontact constraints + if (infoGlobal.m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING) + { + for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++) + { + btMultiBodySolverConstraint& solverConstraint = + m_multiBodyNonContactConstraints[i]; + solverConstraint.m_appliedImpulse = + solverConstraint.m_orgConstraint->getAppliedImpulse(solverConstraint.m_orgDofIndex) * + infoGlobal.m_articulatedWarmstartingFactor; + + btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; + btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; + if (solverConstraint.m_appliedImpulse) + { + if (multiBodyA) + { + int ndofA = multiBodyA->getNumDofs() + 6; + btScalar* deltaV = + &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + btScalar impulse = solverConstraint.m_appliedImpulse; + multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse); + applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA); + } + if (multiBodyB) + { + int ndofB = multiBodyB->getNumDofs() + 6; + btScalar* deltaV = + &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + btScalar impulse = solverConstraint.m_appliedImpulse; + multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse); + applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB); + } + } + } + } + else + { + for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++) + { + btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i]; + solverConstraint.m_appliedImpulse = 0; + } + } } - - -btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher) +btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) { - return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); + //printf("btMultiBodyConstraintSolver::solveGroup: numBodies=%d, numConstraints=%d\n", numBodies, numConstraints); + return btSequentialImpulseConstraintSolver::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher); } #if 0 @@ -811,53 +1520,52 @@ static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodyS void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime) { -#if 1 - +#if 1 + //bod->addBaseForce(m_gravity * bod->getBaseMass()); //bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); if (c.m_orgConstraint) { - c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex,c.m_appliedImpulse); + c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex, c.m_appliedImpulse); } - if (c.m_multiBodyA) { - c.m_multiBodyA->setCompanionId(-1); - btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime); - btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime); - if (c.m_linkA<0) + btVector3 force = c.m_contactNormal1 * (c.m_appliedImpulse / deltaTime); + btVector3 torque = c.m_relpos1CrossNormal * (c.m_appliedImpulse / deltaTime); + if (c.m_linkA < 0) { c.m_multiBodyA->addBaseConstraintForce(force); c.m_multiBodyA->addBaseConstraintTorque(torque); - } else + } + else { - c.m_multiBodyA->addLinkConstraintForce(c.m_linkA,force); - //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]); - c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA,torque); + c.m_multiBodyA->addLinkConstraintForce(c.m_linkA, force); + //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]); + c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA, torque); } } - + if (c.m_multiBodyB) { { c.m_multiBodyB->setCompanionId(-1); - btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime); - btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime); - if (c.m_linkB<0) + btVector3 force = c.m_contactNormal2 * (c.m_appliedImpulse / deltaTime); + btVector3 torque = c.m_relpos2CrossNormal * (c.m_appliedImpulse / deltaTime); + if (c.m_linkB < 0) { c.m_multiBodyB->addBaseConstraintForce(force); c.m_multiBodyB->addBaseConstraintTorque(torque); - } else + } + else { { - c.m_multiBodyB->addLinkConstraintForce(c.m_linkB,force); + c.m_multiBodyB->addLinkConstraintForce(c.m_linkB, force); //b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]); - c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB,torque); + c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB, torque); } - } } } @@ -867,82 +1575,65 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv if (c.m_multiBodyA) { - - if(c.m_multiBodyA->isMultiDof()) - { - c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse); - } - else - { - c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse); - } + c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], c.m_appliedImpulse); } - + if (c.m_multiBodyB) { - if(c.m_multiBodyB->isMultiDof()) - { - c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse); - } - else - { - c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse); - } + c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], c.m_appliedImpulse); } #endif - - - } -btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal) +btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) { BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish"); int numPoolConstraints = m_multiBodyNormalContactConstraints.size(); - - //write back the delta v to the multi bodies, either as applied impulse (direct velocity change) + //write back the delta v to the multi bodies, either as applied impulse (direct velocity change) //or as applied force, so we can measure the joint reaction forces easier - for (int i=0;i<numPoolConstraints;i++) + for (int i = 0; i < numPoolConstraints; i++) { btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[i]; - writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep); + writeBackSolverBodyToMultiBody(solverConstraint, infoGlobal.m_timeStep); - writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],infoGlobal.m_timeStep); + writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex], infoGlobal.m_timeStep); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { - writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],infoGlobal.m_timeStep); + writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex + 1], infoGlobal.m_timeStep); } } - - for (int i=0;i<m_multiBodyNonContactConstraints.size();i++) + for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++) { btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i]; - writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep); + writeBackSolverBodyToMultiBody(solverConstraint, infoGlobal.m_timeStep); } - - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { BT_PROFILE("warm starting write back"); - for (int j=0;j<numPoolConstraints;j++) + for (int j = 0; j < numPoolConstraints; j++) { const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j]; - btManifoldPoint* pt = (btManifoldPoint*) solverConstraint.m_originalContactPoint; + btManifoldPoint* pt = (btManifoldPoint*)solverConstraint.m_originalContactPoint; btAssert(pt); pt->m_appliedImpulse = solverConstraint.m_appliedImpulse; + pt->m_prevRHS = solverConstraint.m_rhs; pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse; - + //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { - pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse; + pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex + 1].m_appliedImpulse; + } else + { + pt->m_appliedImpulseLateral2 = 0; } - //do a callback here? } } + #if 0 //multibody joint feedback { @@ -1040,23 +1731,22 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO } } -#endif +#endif #endif - return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal); + return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal); } - -void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher) +void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) { + //printf("solveMultiBodyGroup: numBodies=%d, numConstraints=%d, numManifolds=%d, numMultiBodyConstraints=%d\n", numBodies, numConstraints, numManifolds, numMultiBodyConstraints); + //printf("solveMultiBodyGroup start\n"); m_tmpMultiBodyConstraints = multiBodyConstraints; m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; - - btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); + + btSequentialImpulseConstraintSolver::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher); m_tmpMultiBodyConstraints = 0; m_tmpNumMultiBodyConstraints = 0; - - } diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h index 321ee4231a3..f584360e2b6 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -25,63 +25,77 @@ class btMultiBody; #include "btMultiBodyConstraint.h" - - -ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver +ATTRIBUTE_ALIGNED16(class) +btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver { - protected: + btMultiBodyConstraintArray m_multiBodyNonContactConstraints; - btMultiBodyConstraintArray m_multiBodyNonContactConstraints; + btMultiBodyConstraintArray m_multiBodyNormalContactConstraints; + btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints; + btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints; + btMultiBodyConstraintArray m_multiBodySpinningFrictionContactConstraints; - btMultiBodyConstraintArray m_multiBodyNormalContactConstraints; - btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints; + btMultiBodyJacobianData m_data; - btMultiBodyJacobianData m_data; - //temp storage for multi body constraints for a specific island/group called by 'solveGroup' - btMultiBodyConstraint** m_tmpMultiBodyConstraints; - int m_tmpNumMultiBodyConstraints; - - void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c); - - - void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal); - btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0); - - - void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, - btScalar* jacA,btScalar* jacB, - btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff, - const btContactSolverInfo& infoGlobal); + btMultiBodyConstraint** m_tmpMultiBodyConstraints; + int m_tmpNumMultiBodyConstraints; + + btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c); + + //solve 2 friction directions and clamp against the implicit friction cone + btScalar resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1, const btMultiBodySolverConstraint& cB); + + void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal); + + btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); + + btMultiBodySolverConstraint& addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, + btScalar combinedTorsionalFriction, + btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); + + btMultiBodySolverConstraint& addMultiBodySpinningFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, + btScalar combinedTorsionalFriction, + btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); + + void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint & constraintRow, + btScalar * jacA, btScalar * jacB, + btScalar penetration, btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff, + const btContactSolverInfo& infoGlobal); + + void setupMultiBodyContactConstraint(btMultiBodySolverConstraint & solverConstraint, + const btVector3& contactNormal, + const btScalar& appliedImpulse, + btManifoldPoint& cp, + const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); + + //either rolling or spinning friction + void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint & solverConstraint, + const btVector3& contactNormal, + btManifoldPoint& cp, + btScalar combinedTorsionalFriction, + const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); + + void convertMultiBodyContact(btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal); + virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); + // virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); + void applyDeltaVee(btScalar * deltaV, btScalar impulse, int velocityIndex, int ndof); + void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint & constraint, btScalar deltaTime); - void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, - const btVector3& contactNormal, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); - - void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); - virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); -// virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - - virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - void applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof); - void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& constraint, btScalar deltaTime); public: - BT_DECLARE_ALIGNED_ALLOCATOR(); ///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints) - virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); - virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal); - - virtual void solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); -}; + virtual btScalar solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); + virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal); - - - - -#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H + virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); +}; +#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index a9c0b33b3a3..f599c9ccb6f 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -23,45 +23,49 @@ subject to the following restrictions: #include "LinearMath/btIDebugDraw.h" #include "LinearMath/btSerializer.h" - -void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask) +void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, int group, int mask) { m_multiBodies.push_back(body); - } -void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body) +void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body) { m_multiBodies.remove(body); } -void btMultiBodyDynamicsWorld::calculateSimulationIslands() +void btMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +{ + btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); + predictMultiBodyTransforms(timeStep); + +} +void btMultiBodyDynamicsWorld::calculateSimulationIslands() { BT_PROFILE("calculateSimulationIslands"); - getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); + getSimulationIslandManager()->updateActivationState(getCollisionWorld(), getCollisionWorld()->getDispatcher()); + + { + //merge islands based on speculative contact manifolds too + for (int i = 0; i < this->m_predictiveManifolds.size(); i++) + { + btPersistentManifold* manifold = m_predictiveManifolds[i]; + + const btCollisionObject* colObj0 = manifold->getBody0(); + const btCollisionObject* colObj1 = manifold->getBody1(); + + if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && + ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) + { + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag()); + } + } + } - { - //merge islands based on speculative contact manifolds too - for (int i=0;i<this->m_predictiveManifolds.size();i++) - { - btPersistentManifold* manifold = m_predictiveManifolds[i]; - - const btCollisionObject* colObj0 = manifold->getBody0(); - const btCollisionObject* colObj1 = manifold->getBody1(); - - if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && - ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) - { - getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); - } - } - } - { int i; int numConstraints = int(m_constraints.size()); - for (i=0;i< numConstraints ; i++ ) + for (i = 0; i < numConstraints; i++) { btTypedConstraint* constraint = m_constraints[i]; if (constraint->isEnabled()) @@ -72,23 +76,23 @@ void btMultiBodyDynamicsWorld::calculateSimulationIslands() if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) { - getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag()); } } } } //merge islands linked by Featherstone link colliders - for (int i=0;i<m_multiBodies.size();i++) + for (int i = 0; i < m_multiBodies.size(); i++) { btMultiBody* body = m_multiBodies[i]; { btMultiBodyLinkCollider* prev = body->getBaseCollider(); - for (int b=0;b<body->getNumLinks();b++) + for (int b = 0; b < body->getNumLinks(); b++) { btMultiBodyLinkCollider* cur = body->getLink(b).m_collider; - + if (((cur) && (!(cur)->isStaticOrKinematicObject())) && ((prev) && (!(prev)->isStaticOrKinematicObject()))) { @@ -98,36 +102,31 @@ void btMultiBodyDynamicsWorld::calculateSimulationIslands() } if (cur && !cur->isStaticOrKinematicObject()) prev = cur; - } } } //merge islands linked by multibody constraints { - for (int i=0;i<this->m_multiBodyConstraints.size();i++) + for (int i = 0; i < this->m_multiBodyConstraints.size(); i++) { btMultiBodyConstraint* c = m_multiBodyConstraints[i]; int tagA = c->getIslandIdA(); int tagB = c->getIslandIdB(); - if (tagA>=0 && tagB>=0) + if (tagA >= 0 && tagB >= 0) getSimulationIslandManager()->getUnionFind().unite(tagA, tagB); } } //Store the island id in each body getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); - } - -void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) +void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) { BT_PROFILE("btMultiBodyDynamicsWorld::updateActivationState"); - - - for ( int i=0;i<m_multiBodies.size();i++) + for (int i = 0; i < m_multiBodies.size(); i++) { btMultiBody* body = m_multiBodies[i]; if (body) @@ -138,659 +137,529 @@ void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) btMultiBodyLinkCollider* col = body->getBaseCollider(); if (col && col->getActivationState() == ACTIVE_TAG) { - col->setActivationState( WANTS_DEACTIVATION); + col->setActivationState(WANTS_DEACTIVATION); col->setDeactivationTime(0.f); } - for (int b=0;b<body->getNumLinks();b++) + for (int b = 0; b < body->getNumLinks(); b++) { btMultiBodyLinkCollider* col = body->getLink(b).m_collider; if (col && col->getActivationState() == ACTIVE_TAG) { - col->setActivationState( WANTS_DEACTIVATION); + col->setActivationState(WANTS_DEACTIVATION); col->setDeactivationTime(0.f); } } - } else + } + else { btMultiBodyLinkCollider* col = body->getBaseCollider(); if (col && col->getActivationState() != DISABLE_DEACTIVATION) - col->setActivationState( ACTIVE_TAG ); + col->setActivationState(ACTIVE_TAG); - for (int b=0;b<body->getNumLinks();b++) + for (int b = 0; b < body->getNumLinks(); b++) { btMultiBodyLinkCollider* col = body->getLink(b).m_collider; if (col && col->getActivationState() != DISABLE_DEACTIVATION) - col->setActivationState( ACTIVE_TAG ); + col->setActivationState(ACTIVE_TAG); } } - } } btDiscreteDynamicsWorld::updateActivationState(timeStep); } - -SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs) +void btMultiBodyDynamicsWorld::getAnalyticsData(btAlignedObjectArray<btSolverAnalyticsData>& islandAnalyticsData) const { - int islandId; - - const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); - const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); - islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag(); - return islandId; - + islandAnalyticsData = m_solverMultiBodyIslandCallback->m_islandAnalyticsData; } - -class btSortConstraintOnIslandPredicate2 +btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration) + : btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration), + m_multiBodyConstraintSolver(constraintSolver) { - public: - - bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const - { - int rIslandId0,lIslandId0; - rIslandId0 = btGetConstraintIslandId2(rhs); - lIslandId0 = btGetConstraintIslandId2(lhs); - return lIslandId0 < rIslandId0; - } -}; - - + //split impulse is not yet supported for Featherstone hierarchies + // getSolverInfo().m_splitImpulse = false; + getSolverInfo().m_solverMode |= SOLVER_USE_2_FRICTION_DIRECTIONS; + m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver, dispatcher); +} -SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs) +btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld() { - int islandId; - - int islandTagA = lhs->getIslandIdA(); - int islandTagB = lhs->getIslandIdB(); - islandId= islandTagA>=0?islandTagA:islandTagB; - return islandId; - + delete m_solverMultiBodyIslandCallback; } - -class btSortMultiBodyConstraintOnIslandPredicate +void btMultiBodyDynamicsWorld::setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver) { - public: - - bool operator() ( const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs ) const - { - int rIslandId0,lIslandId0; - rIslandId0 = btGetMultiBodyConstraintIslandId(rhs); - lIslandId0 = btGetMultiBodyConstraintIslandId(lhs); - return lIslandId0 < rIslandId0; - } -}; + m_multiBodyConstraintSolver = solver; + m_solverMultiBodyIslandCallback->setMultiBodyConstraintSolver(solver); + btDiscreteDynamicsWorld::setConstraintSolver(solver); +} -struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback +void btMultiBodyDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) { - btContactSolverInfo* m_solverInfo; - btMultiBodyConstraintSolver* m_solver; - btMultiBodyConstraint** m_multiBodySortedConstraints; - int m_numMultiBodyConstraints; - - btTypedConstraint** m_sortedConstraints; - int m_numConstraints; - btIDebugDraw* m_debugDrawer; - btDispatcher* m_dispatcher; - - btAlignedObjectArray<btCollisionObject*> m_bodies; - btAlignedObjectArray<btPersistentManifold*> m_manifolds; - btAlignedObjectArray<btTypedConstraint*> m_constraints; - btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints; - - - MultiBodyInplaceSolverIslandCallback( btMultiBodyConstraintSolver* solver, - btDispatcher* dispatcher) - :m_solverInfo(NULL), - m_solver(solver), - m_multiBodySortedConstraints(NULL), - m_numConstraints(0), - m_debugDrawer(NULL), - m_dispatcher(dispatcher) + if (solver->getSolverType() == BT_MULTIBODY_SOLVER) { - - } - - MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other) - { - btAssert(0); - (void)other; - return *this; - } - - SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer) - { - btAssert(solverInfo); - m_solverInfo = solverInfo; - - m_multiBodySortedConstraints = sortedMultiBodyConstraints; - m_numMultiBodyConstraints = numMultiBodyConstraints; - m_sortedConstraints = sortedConstraints; - m_numConstraints = numConstraints; - - m_debugDrawer = debugDrawer; - m_bodies.resize (0); - m_manifolds.resize (0); - m_constraints.resize (0); - m_multiBodyConstraints.resize(0); + m_multiBodyConstraintSolver = (btMultiBodyConstraintSolver*)solver; } - - - virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) - { - if (islandId<0) - { - ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id - m_solver->solveMultiBodyGroup( bodies,numBodies,manifolds, numManifolds,m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); - } else - { - //also add all non-contact constraints/joints for this island - btTypedConstraint** startConstraint = 0; - btMultiBodyConstraint** startMultiBodyConstraint = 0; - - int numCurConstraints = 0; - int numCurMultiBodyConstraints = 0; - - int i; - - //find the first constraint for this island - - for (i=0;i<m_numConstraints;i++) - { - if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId) - { - startConstraint = &m_sortedConstraints[i]; - break; - } - } - //count the number of constraints in this island - for (;i<m_numConstraints;i++) - { - if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId) - { - numCurConstraints++; - } - } - - for (i=0;i<m_numMultiBodyConstraints;i++) - { - if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId) - { - - startMultiBodyConstraint = &m_multiBodySortedConstraints[i]; - break; - } - } - //count the number of multi body constraints in this island - for (;i<m_numMultiBodyConstraints;i++) - { - if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId) - { - numCurMultiBodyConstraints++; - } - } - - if (m_solverInfo->m_minimumSolverBatchSize<=1) - { - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); - } else - { - - for (i=0;i<numBodies;i++) - m_bodies.push_back(bodies[i]); - for (i=0;i<numManifolds;i++) - m_manifolds.push_back(manifolds[i]); - for (i=0;i<numCurConstraints;i++) - m_constraints.push_back(startConstraint[i]); - - for (i=0;i<numCurMultiBodyConstraints;i++) - m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]); - - if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize) - { - processConstraints(); - } else - { - //printf("deferred\n"); - } - } - } - } - void processConstraints() - { - - btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0; - btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0; - btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0; - btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0; - - //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size()); - - m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher); - m_bodies.resize(0); - m_manifolds.resize(0); - m_constraints.resize(0); - m_multiBodyConstraints.resize(0); - } - -}; - - - -btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) - :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration), - m_multiBodyConstraintSolver(constraintSolver) -{ - //split impulse is not yet supported for Featherstone hierarchies - getSolverInfo().m_splitImpulse = false; - getSolverInfo().m_solverMode |=SOLVER_USE_2_FRICTION_DIRECTIONS; - m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver,dispatcher); -} - -btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld () -{ - delete m_solverMultiBodyIslandCallback; + btDiscreteDynamicsWorld::setConstraintSolver(solver); } -void btMultiBodyDynamicsWorld::forwardKinematics() +void btMultiBodyDynamicsWorld::forwardKinematics() { - btAlignedObjectArray<btQuaternion> world_to_local; - btAlignedObjectArray<btVector3> local_origin; - - for (int b=0;b<m_multiBodies.size();b++) + for (int b = 0; b < m_multiBodies.size(); b++) { btMultiBody* bod = m_multiBodies[b]; - bod->forwardKinematics(world_to_local,local_origin); + bod->forwardKinematics(m_scratch_world_to_local, m_scratch_local_origin); } } -void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) +void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { - forwardKinematics(); - - btAlignedObjectArray<btScalar> scratch_r; - btAlignedObjectArray<btVector3> scratch_v; - btAlignedObjectArray<btMatrix3x3> scratch_m; - - - BT_PROFILE("solveConstraints"); - - m_sortedConstraints.resize( m_constraints.size()); - int i; - for (i=0;i<getNumConstraints();i++) - { - m_sortedConstraints[i] = m_constraints[i]; - } - m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2()); - btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; - - m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size()); - for (i=0;i<m_multiBodyConstraints.size();i++) - { - m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i]; - } - m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate()); + solveExternalForces(solverInfo); + buildIslands(); + solveInternalConstraints(solverInfo); +} - btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; - +void btMultiBodyDynamicsWorld::buildIslands() +{ + m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback); +} - m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer()); - m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); - +void btMultiBodyDynamicsWorld::solveInternalConstraints(btContactSolverInfo& solverInfo) +{ /// solve all the constraints for this island - m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback); + m_solverMultiBodyIslandCallback->processConstraints(); + m_constraintSolver->allSolved(solverInfo, m_debugDrawer); + { + BT_PROFILE("btMultiBody stepVelocities"); + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + + bool isSleeping = false; + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) + m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) + m_scratch_v.resize(bod->getNumLinks() + 1); + m_scratch_m.resize(bod->getNumLinks() + 1); + + if (bod->internalNeedsJointFeedback()) + { + if (!bod->isUsingRK4Integration()) + { + if (bod->internalNeedsJointFeedback()) + { + bool isConstraintPass = true; + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + } + } + } + } + } + } + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + bod->processDeltaVeeMultiDof2(); + } +} +void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverInfo) +{ + forwardKinematics(); + + BT_PROFILE("solveConstraints"); + + clearMultiBodyConstraintForces(); + + m_sortedConstraints.resize(m_constraints.size()); + int i; + for (i = 0; i < getNumConstraints(); i++) + { + m_sortedConstraints[i] = m_constraints[i]; + } + m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2()); + btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; + + m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size()); + for (i = 0; i < m_multiBodyConstraints.size(); i++) + { + m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i]; + } + m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate()); + + btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; + + m_solverMultiBodyIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer()); + m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); + #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - { - BT_PROFILE("btMultiBody addForce"); - for (int i=0;i<this->m_multiBodies.size();i++) - { - btMultiBody* bod = m_multiBodies[i]; - - bool isSleeping = false; - - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b=0;b<bod->getNumLinks();b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) - isSleeping = true; - } - - if (!isSleeping) - { - //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) - scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd) - scratch_v.resize(bod->getNumLinks()+1); - scratch_m.resize(bod->getNumLinks()+1); - - bod->addBaseForce(m_gravity * bod->getBaseMass()); - - for (int j = 0; j < bod->getNumLinks(); ++j) - { - bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); - } - }//if (!isSleeping) - } - } -#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - - - { - BT_PROFILE("btMultiBody stepVelocities"); - for (int i=0;i<this->m_multiBodies.size();i++) - { - btMultiBody* bod = m_multiBodies[i]; - - bool isSleeping = false; - - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b=0;b<bod->getNumLinks();b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) - isSleeping = true; - } - - if (!isSleeping) - { - //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) - scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd) - scratch_v.resize(bod->getNumLinks()+1); - scratch_m.resize(bod->getNumLinks()+1); - bool doNotUpdatePos = false; - - { - if(!bod->isUsingRK4Integration()) - { - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); - } - else - { - // - int numDofs = bod->getNumDofs() + 6; - int numPosVars = bod->getNumPosVars() + 7; - btAlignedObjectArray<btScalar> scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs); - //convenience - btScalar *pMem = &scratch_r2[0]; - btScalar *scratch_q0 = pMem; pMem += numPosVars; - btScalar *scratch_qx = pMem; pMem += numPosVars; - btScalar *scratch_qd0 = pMem; pMem += numDofs; - btScalar *scratch_qd1 = pMem; pMem += numDofs; - btScalar *scratch_qd2 = pMem; pMem += numDofs; - btScalar *scratch_qd3 = pMem; pMem += numDofs; - btScalar *scratch_qdd0 = pMem; pMem += numDofs; - btScalar *scratch_qdd1 = pMem; pMem += numDofs; - btScalar *scratch_qdd2 = pMem; pMem += numDofs; - btScalar *scratch_qdd3 = pMem; pMem += numDofs; - btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]); - - ///// - //copy q0 to scratch_q0 and qd0 to scratch_qd0 - scratch_q0[0] = bod->getWorldToBaseRot().x(); - scratch_q0[1] = bod->getWorldToBaseRot().y(); - scratch_q0[2] = bod->getWorldToBaseRot().z(); - scratch_q0[3] = bod->getWorldToBaseRot().w(); - scratch_q0[4] = bod->getBasePos().x(); - scratch_q0[5] = bod->getBasePos().y(); - scratch_q0[6] = bod->getBasePos().z(); - // - for(int link = 0; link < bod->getNumLinks(); ++link) - { - for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof) - scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof]; - } - // - for(int dof = 0; dof < numDofs; ++dof) - scratch_qd0[dof] = bod->getVelocityVector()[dof]; - //// - struct - { - btMultiBody *bod; + { + BT_PROFILE("btMultiBody addForce"); + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + + bool isSleeping = false; + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) + m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) + m_scratch_v.resize(bod->getNumLinks() + 1); + m_scratch_m.resize(bod->getNumLinks() + 1); + + bod->addBaseForce(m_gravity * bod->getBaseMass()); + + for (int j = 0; j < bod->getNumLinks(); ++j) + { + bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); + } + } //if (!isSleeping) + } + } +#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY + + { + BT_PROFILE("btMultiBody stepVelocities"); + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + + bool isSleeping = false; + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) + m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) + m_scratch_v.resize(bod->getNumLinks() + 1); + m_scratch_m.resize(bod->getNumLinks() + 1); + bool doNotUpdatePos = false; + bool isConstraintPass = false; + { + if (!bod->isUsingRK4Integration()) + { + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, + m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + } + else + { + // + int numDofs = bod->getNumDofs() + 6; + int numPosVars = bod->getNumPosVars() + 7; + btAlignedObjectArray<btScalar> scratch_r2; + scratch_r2.resize(2 * numPosVars + 8 * numDofs); + //convenience + btScalar* pMem = &scratch_r2[0]; + btScalar* scratch_q0 = pMem; + pMem += numPosVars; + btScalar* scratch_qx = pMem; + pMem += numPosVars; + btScalar* scratch_qd0 = pMem; + pMem += numDofs; + btScalar* scratch_qd1 = pMem; + pMem += numDofs; + btScalar* scratch_qd2 = pMem; + pMem += numDofs; + btScalar* scratch_qd3 = pMem; + pMem += numDofs; + btScalar* scratch_qdd0 = pMem; + pMem += numDofs; + btScalar* scratch_qdd1 = pMem; + pMem += numDofs; + btScalar* scratch_qdd2 = pMem; + pMem += numDofs; + btScalar* scratch_qdd3 = pMem; + pMem += numDofs; + btAssert((pMem - (2 * numPosVars + 8 * numDofs)) == &scratch_r2[0]); + + ///// + //copy q0 to scratch_q0 and qd0 to scratch_qd0 + scratch_q0[0] = bod->getWorldToBaseRot().x(); + scratch_q0[1] = bod->getWorldToBaseRot().y(); + scratch_q0[2] = bod->getWorldToBaseRot().z(); + scratch_q0[3] = bod->getWorldToBaseRot().w(); + scratch_q0[4] = bod->getBasePos().x(); + scratch_q0[5] = bod->getBasePos().y(); + scratch_q0[6] = bod->getBasePos().z(); + // + for (int link = 0; link < bod->getNumLinks(); ++link) + { + for (int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof) + scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof]; + } + // + for (int dof = 0; dof < numDofs; ++dof) + scratch_qd0[dof] = bod->getVelocityVector()[dof]; + //// + struct + { + btMultiBody* bod; btScalar *scratch_qx, *scratch_q0; - - void operator()() - { - for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof) + + void operator()() + { + for (int dof = 0; dof < bod->getNumPosVars() + 7; ++dof) scratch_qx[dof] = scratch_q0[dof]; - } - } pResetQx = {bod, scratch_qx, scratch_q0}; - // - struct - { - void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size) - { - for(int i = 0; i < size; ++i) + } + } pResetQx = {bod, scratch_qx, scratch_q0}; + // + struct + { + void operator()(btScalar dt, const btScalar* pDer, const btScalar* pCurVal, btScalar* pVal, int size) + { + for (int i = 0; i < size; ++i) pVal[i] = pCurVal[i] + dt * pDer[i]; - } - - } pEulerIntegrate; - // - struct + } + + } pEulerIntegrate; + // + struct { - void operator()(btMultiBody *pBody, const btScalar *pData) + void operator()(btMultiBody* pBody, const btScalar* pData) { - btScalar *pVel = const_cast<btScalar*>(pBody->getVelocityVector()); - - for(int i = 0; i < pBody->getNumDofs() + 6; ++i) + btScalar* pVel = const_cast<btScalar*>(pBody->getVelocityVector()); + + for (int i = 0; i < pBody->getNumDofs() + 6; ++i) pVel[i] = pData[i]; - } } pCopyToVelocityVector; - // + // struct - { - void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size) - { - for(int i = 0; i < size; ++i) + { + void operator()(const btScalar* pSrc, btScalar* pDst, int start, int size) + { + for (int i = 0; i < size; ++i) pDst[i] = pSrc[start + i]; - } - } pCopy; - // - - btScalar h = solverInfo.m_timeStep; - #define output &scratch_r[bod->getNumDofs()] - //calc qdd0 from: q0 & qd0 - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m); - pCopy(output, scratch_qdd0, 0, numDofs); - //calc q1 = q0 + h/2 * qd0 - pResetQx(); - bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0); - //calc qd1 = qd0 + h/2 * qdd0 - pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs); - // - //calc qdd1 from: q1 & qd1 - pCopyToVelocityVector(bod, scratch_qd1); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m); - pCopy(output, scratch_qdd1, 0, numDofs); - //calc q2 = q0 + h/2 * qd1 - pResetQx(); - bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1); - //calc qd2 = qd0 + h/2 * qdd1 - pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs); - // - //calc qdd2 from: q2 & qd2 - pCopyToVelocityVector(bod, scratch_qd2); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m); - pCopy(output, scratch_qdd2, 0, numDofs); - //calc q3 = q0 + h * qd2 - pResetQx(); - bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2); - //calc qd3 = qd0 + h * qdd2 - pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs); - // - //calc qdd3 from: q3 & qd3 - pCopyToVelocityVector(bod, scratch_qd3); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m); - pCopy(output, scratch_qdd3, 0, numDofs); - - // - //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3) - //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3) - btAlignedObjectArray<btScalar> delta_q; delta_q.resize(numDofs); - btAlignedObjectArray<btScalar> delta_qd; delta_qd.resize(numDofs); - for(int i = 0; i < numDofs; ++i) - { - delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]); - delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]); - //delta_q[i] = h*scratch_qd0[i]; - //delta_qd[i] = h*scratch_qdd0[i]; - } - // - pCopyToVelocityVector(bod, scratch_qd0); - bod->applyDeltaVeeMultiDof(&delta_qd[0], 1); - // - if(!doNotUpdatePos) - { - btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector()); - pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs(); - - for(int i = 0; i < numDofs; ++i) - pRealBuf[i] = delta_q[i]; - - //bod->stepPositionsMultiDof(1, 0, &delta_q[0]); - bod->setPosUpdated(true); - } - - //ugly hack which resets the cached data to t0 (needed for constraint solver) - { - for(int link = 0; link < bod->getNumLinks(); ++link) - bod->getLink(link).updateCacheMultiDof(); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, scratch_r, scratch_v, scratch_m); - } - - } - } - -#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - bod->clearForcesAndTorques(); -#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - }//if (!isSleeping) - } - } - - clearMultiBodyConstraintForces(); - - m_solverMultiBodyIslandCallback->processConstraints(); - - m_constraintSolver->allSolved(solverInfo, m_debugDrawer); - - { - BT_PROFILE("btMultiBody stepVelocities"); - for (int i=0;i<this->m_multiBodies.size();i++) - { - btMultiBody* bod = m_multiBodies[i]; - - bool isSleeping = false; - - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + } + } pCopy; + // + + btScalar h = solverInfo.m_timeStep; +#define output &m_scratch_r[bod->getNumDofs()] + //calc qdd0 from: q0 & qd0 + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + pCopy(output, scratch_qdd0, 0, numDofs); + //calc q1 = q0 + h/2 * qd0 + pResetQx(); + bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd0); + //calc qd1 = qd0 + h/2 * qdd0 + pEulerIntegrate(btScalar(.5) * h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs); + // + //calc qdd1 from: q1 & qd1 + pCopyToVelocityVector(bod, scratch_qd1); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + pCopy(output, scratch_qdd1, 0, numDofs); + //calc q2 = q0 + h/2 * qd1 + pResetQx(); + bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd1); + //calc qd2 = qd0 + h/2 * qdd1 + pEulerIntegrate(btScalar(.5) * h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs); + // + //calc qdd2 from: q2 & qd2 + pCopyToVelocityVector(bod, scratch_qd2); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + pCopy(output, scratch_qdd2, 0, numDofs); + //calc q3 = q0 + h * qd2 + pResetQx(); + bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2); + //calc qd3 = qd0 + h * qdd2 + pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs); + // + //calc qdd3 from: q3 & qd3 + pCopyToVelocityVector(bod, scratch_qd3); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + pCopy(output, scratch_qdd3, 0, numDofs); + + // + //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3) + //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3) + btAlignedObjectArray<btScalar> delta_q; + delta_q.resize(numDofs); + btAlignedObjectArray<btScalar> delta_qd; + delta_qd.resize(numDofs); + for (int i = 0; i < numDofs; ++i) { - isSleeping = true; + delta_q[i] = h / btScalar(6.) * (scratch_qd0[i] + 2 * scratch_qd1[i] + 2 * scratch_qd2[i] + scratch_qd3[i]); + delta_qd[i] = h / btScalar(6.) * (scratch_qdd0[i] + 2 * scratch_qdd1[i] + 2 * scratch_qdd2[i] + scratch_qdd3[i]); + //delta_q[i] = h*scratch_qd0[i]; + //delta_qd[i] = h*scratch_qdd0[i]; } - for (int b=0;b<bod->getNumLinks();b++) + // + pCopyToVelocityVector(bod, scratch_qd0); + bod->applyDeltaVeeMultiDof(&delta_qd[0], 1); + // + if (!doNotUpdatePos) { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) - isSleeping = true; + btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector()); + pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs(); + + for (int i = 0; i < numDofs; ++i) + pRealBuf[i] = delta_q[i]; + + //bod->stepPositionsMultiDof(1, 0, &delta_q[0]); + bod->setPosUpdated(true); } - - if (!isSleeping) + + //ugly hack which resets the cached data to t0 (needed for constraint solver) { - //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) - scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd) - scratch_v.resize(bod->getNumLinks()+1); - scratch_m.resize(bod->getNumLinks()+1); - - - { - if(!bod->isUsingRK4Integration()) - { - bool isConstraintPass = true; - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m, isConstraintPass); - } - } - } - } - } - - for (int i=0;i<this->m_multiBodies.size();i++) - { - btMultiBody* bod = m_multiBodies[i]; - bod->processDeltaVeeMultiDof2(); - } - + for (int link = 0; link < bod->getNumLinks(); ++link) + bod->getLink(link).updateCacheMultiDof(); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + } + } + } + +#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY + bod->clearForcesAndTorques(); +#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY + } //if (!isSleeping) + } + } } -void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) + +void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) { btDiscreteDynamicsWorld::integrateTransforms(timeStep); + integrateMultiBodyTransforms(timeStep); +} - { +void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep) +{ BT_PROFILE("btMultiBody stepPositions"); //integrate and update the Featherstone hierarchies - btAlignedObjectArray<btQuaternion> world_to_local; - btAlignedObjectArray<btVector3> local_origin; - for (int b=0;b<m_multiBodies.size();b++) + for (int b = 0; b < m_multiBodies.size(); b++) { btMultiBody* bod = m_multiBodies[b]; bool isSleeping = false; if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) { isSleeping = true; - } - for (int b=0;b<bod->getNumLinks();b++) + } + for (int b = 0; b < bod->getNumLinks(); b++) { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) isSleeping = true; } - if (!isSleeping) { + bod->addSplitV(); int nLinks = bod->getNumLinks(); ///base + num m_links - - - { - if(!bod->isPosUpdated()) - bod->stepPositionsMultiDof(timeStep); - else - { - btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector()); - pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs(); + if (!bod->isPosUpdated()) + bod->stepPositionsMultiDof(timeStep); + else + { + btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector()); + pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs(); + + bod->stepPositionsMultiDof(1, 0, pRealBuf); + bod->setPosUpdated(false); + } - bod->stepPositionsMultiDof(1, 0, pRealBuf); - bod->setPosUpdated(false); - } - } - - world_to_local.resize(nLinks+1); - local_origin.resize(nLinks+1); - bod->updateCollisionObjectWorldTransforms(world_to_local,local_origin); - - } else + 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 { bod->clearVelocities(); } } - } } +void btMultiBodyDynamicsWorld::predictMultiBodyTransforms(btScalar timeStep) +{ + BT_PROFILE("btMultiBody stepPositions"); + //integrate and update the Featherstone hierarchies + + for (int b = 0; b < m_multiBodies.size(); b++) + { + btMultiBody* bod = m_multiBodies[b]; + bool isSleeping = false; + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + int nLinks = bod->getNumLinks(); + bod->predictPositionsMultiDof(timeStep); + m_scratch_world_to_local.resize(nLinks + 1); + m_scratch_local_origin.resize(nLinks + 1); + bod->updateCollisionObjectInterpolationWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); + } + else + { + bod->clearVelocities(); + } + } +} - -void btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint) +void btMultiBodyDynamicsWorld::addMultiBodyConstraint(btMultiBodyConstraint* constraint) { m_multiBodyConstraints.push_back(constraint); } -void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint) +void btMultiBodyDynamicsWorld::removeMultiBodyConstraint(btMultiBodyConstraint* constraint) { m_multiBodyConstraints.remove(constraint); } @@ -800,16 +669,17 @@ void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstrain constraint->debugDraw(getDebugDrawer()); } - -void btMultiBodyDynamicsWorld::debugDrawWorld() +void btMultiBodyDynamicsWorld::debugDrawWorld() { BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld"); + btDiscreteDynamicsWorld::debugDrawWorld(); + bool drawConstraints = false; if (getDebugDrawer()) { int mode = getDebugDrawer()->getDebugMode(); - if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) + if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) { drawConstraints = true; } @@ -817,159 +687,148 @@ void btMultiBodyDynamicsWorld::debugDrawWorld() if (drawConstraints) { BT_PROFILE("btMultiBody debugDrawWorld"); - - btAlignedObjectArray<btQuaternion> world_to_local1; - btAlignedObjectArray<btVector3> local_origin1; - for (int c=0;c<m_multiBodyConstraints.size();c++) + for (int c = 0; c < m_multiBodyConstraints.size(); c++) { btMultiBodyConstraint* constraint = m_multiBodyConstraints[c]; debugDrawMultiBodyConstraint(constraint); } - for (int b = 0; b<m_multiBodies.size(); b++) + for (int b = 0; b < m_multiBodies.size(); b++) { btMultiBody* bod = m_multiBodies[b]; - bod->forwardKinematics(world_to_local1,local_origin1); - - getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1); + bod->forwardKinematics(m_scratch_world_to_local1, m_scratch_local_origin1); + if (mode & btIDebugDraw::DBG_DrawFrames) + { + getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1); + } - for (int m = 0; m<bod->getNumLinks(); m++) + for (int m = 0; m < bod->getNumLinks(); m++) { - const btTransform& tr = bod->getLink(m).m_cachedWorldTransform; - - getDebugDrawer()->drawTransform(tr, 0.1); - - //draw the joint axis - if (bod->getLink(m).m_jointType==btMultibodyLink::eRevolute) + if (mode & btIDebugDraw::DBG_DrawFrames) { - btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec); - - btVector4 color(0,0,0,1);//1,1,1); - btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); - btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); - getDebugDrawer()->drawLine(from,to,color); + getDebugDrawer()->drawTransform(tr, 0.1); } - if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed) + //draw the joint axis + if (bod->getLink(m).m_jointType == btMultibodyLink::eRevolute) { - btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec); - - btVector4 color(0,0,0,1);//1,1,1); - btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); - btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); - getDebugDrawer()->drawLine(from,to,color); + btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_topVec) * 0.1; + + btVector4 color(0, 0, 0, 1); //1,1,1); + btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector); + btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector); + getDebugDrawer()->drawLine(from, to, color); } - if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic) + if (bod->getLink(m).m_jointType == btMultibodyLink::eFixed) { - btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec); - - btVector4 color(0,0,0,1);//1,1,1); - btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); - btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); - getDebugDrawer()->drawLine(from,to,color); + btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_bottomVec) * 0.1; + + btVector4 color(0, 0, 0, 1); //1,1,1); + btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector); + btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector); + getDebugDrawer()->drawLine(from, to, color); + } + if (bod->getLink(m).m_jointType == btMultibodyLink::ePrismatic) + { + btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_bottomVec) * 0.1; + + btVector4 color(0, 0, 0, 1); //1,1,1); + btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector); + btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector); + getDebugDrawer()->drawLine(from, to, color); } - } } } } - - btDiscreteDynamicsWorld::debugDrawWorld(); } - - void btMultiBodyDynamicsWorld::applyGravity() { - btDiscreteDynamicsWorld::applyGravity(); + btDiscreteDynamicsWorld::applyGravity(); #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - BT_PROFILE("btMultiBody addGravity"); - for (int i=0;i<this->m_multiBodies.size();i++) - { - btMultiBody* bod = m_multiBodies[i]; + BT_PROFILE("btMultiBody addGravity"); + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; - bool isSleeping = false; + bool isSleeping = false; - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b=0;b<bod->getNumLinks();b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) - isSleeping = true; - } + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } - if (!isSleeping) - { - bod->addBaseForce(m_gravity * bod->getBaseMass()); + if (!isSleeping) + { + bod->addBaseForce(m_gravity * bod->getBaseMass()); - for (int j = 0; j < bod->getNumLinks(); ++j) - { - bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); - } - }//if (!isSleeping) - } -#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY + for (int j = 0; j < bod->getNumLinks(); ++j) + { + bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); + } + } //if (!isSleeping) + } +#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY } void btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces() -{ - for (int i=0;i<this->m_multiBodies.size();i++) - { - btMultiBody* bod = m_multiBodies[i]; - bod->clearConstraintForces(); - } +{ + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + bod->clearConstraintForces(); + } } void btMultiBodyDynamicsWorld::clearMultiBodyForces() { - { - BT_PROFILE("clearMultiBodyForces"); - for (int i=0;i<this->m_multiBodies.size();i++) - { - btMultiBody* bod = m_multiBodies[i]; + { + // BT_PROFILE("clearMultiBodyForces"); + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; - bool isSleeping = false; + bool isSleeping = false; - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b=0;b<bod->getNumLinks();b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) - isSleeping = true; - } + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } - if (!isSleeping) - { - btMultiBody* bod = m_multiBodies[i]; - bod->clearForcesAndTorques(); - } + if (!isSleeping) + { + btMultiBody* bod = m_multiBodies[i]; + bod->clearForcesAndTorques(); + } } } - } void btMultiBodyDynamicsWorld::clearForces() { - btDiscreteDynamicsWorld::clearForces(); + btDiscreteDynamicsWorld::clearForces(); #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY clearMultiBodyForces(); #endif } - - - -void btMultiBodyDynamicsWorld::serialize(btSerializer* serializer) +void btMultiBodyDynamicsWorld::serialize(btSerializer* serializer) { - serializer->startSerialization(); - serializeDynamicsWorldInfo( serializer); + serializeDynamicsWorldInfo(serializer); serializeMultiBodies(serializer); @@ -977,22 +836,41 @@ void btMultiBodyDynamicsWorld::serialize(btSerializer* serializer) serializeCollisionObjects(serializer); + serializeContactManifolds(serializer); + serializer->finishSerialization(); } -void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer) +void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer) { int i; //serialize all collision objects - for (i=0;i<m_multiBodies.size();i++) + for (i = 0; i < m_multiBodies.size(); i++) { btMultiBody* mb = m_multiBodies[i]; { int len = mb->calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(len,1); + btChunk* chunk = serializer->allocate(len, 1); const char* structType = mb->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb); + serializer->finalizeChunk(chunk, structType, BT_MULTIBODY_CODE, mb); } } -}
\ No newline at end of file + //serialize all multibody links (collision objects) + for (i = 0; i < m_collisionObjects.size(); i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + if (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + int len = colObj->calculateSerializeBufferSize(); + btChunk* chunk = serializer->allocate(len, 1); + const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk, structType, BT_MB_LINKCOLLIDER_CODE, colObj); + } + } +} +// +//void btMultiBodyDynamicsWorld::setSplitIslands(bool split) +//{ +// m_islandManager->setSplitIslands(split); +//} diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index 03ef3335c22..9ac46f4b64f 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -17,6 +17,7 @@ subject to the following restrictions: #define BT_MULTIBODY_DYNAMICS_WORLD_H #include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" +#include "BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h" #define BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY @@ -33,67 +34,91 @@ protected: btAlignedObjectArray<btMultiBody*> m_multiBodies; btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints; btAlignedObjectArray<btMultiBodyConstraint*> m_sortedMultiBodyConstraints; - btMultiBodyConstraintSolver* m_multiBodyConstraintSolver; - MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback; - - virtual void calculateSimulationIslands(); - virtual void updateActivationState(btScalar timeStep); - virtual void solveConstraints(btContactSolverInfo& solverInfo); + btMultiBodyConstraintSolver* m_multiBodyConstraintSolver; + MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback; + + //cached data to avoid memory allocations + btAlignedObjectArray<btQuaternion> m_scratch_world_to_local; + btAlignedObjectArray<btVector3> m_scratch_local_origin; + btAlignedObjectArray<btQuaternion> m_scratch_world_to_local1; + btAlignedObjectArray<btVector3> m_scratch_local_origin1; + btAlignedObjectArray<btScalar> m_scratch_r; + btAlignedObjectArray<btVector3> m_scratch_v; + btAlignedObjectArray<btMatrix3x3> m_scratch_m; + + virtual void calculateSimulationIslands(); + virtual void updateActivationState(btScalar timeStep); - virtual void serializeMultiBodies(btSerializer* serializer); - -public: - btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); + virtual void serializeMultiBodies(btSerializer* serializer); - virtual ~btMultiBodyDynamicsWorld (); +public: + btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration); - virtual void addMultiBody(btMultiBody* body, short group= btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter); + virtual ~btMultiBodyDynamicsWorld(); + + virtual void solveConstraints(btContactSolverInfo& solverInfo); + + virtual void addMultiBody(btMultiBody* body, int group = btBroadphaseProxy::DefaultFilter, int mask = btBroadphaseProxy::AllFilter); - virtual void removeMultiBody(btMultiBody* body); + virtual void removeMultiBody(btMultiBody* body); - virtual int getNumMultibodies() const + virtual int getNumMultibodies() const { return m_multiBodies.size(); } - btMultiBody* getMultiBody(int mbIndex) + btMultiBody* getMultiBody(int mbIndex) + { + return m_multiBodies[mbIndex]; + } + + const btMultiBody* getMultiBody(int mbIndex) const { return m_multiBodies[mbIndex]; } - virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint); + virtual void addMultiBodyConstraint(btMultiBodyConstraint* constraint); - virtual int getNumMultiBodyConstraints() const + virtual int getNumMultiBodyConstraints() const { - return m_multiBodyConstraints.size(); + return m_multiBodyConstraints.size(); } - virtual btMultiBodyConstraint* getMultiBodyConstraint( int constraintIndex) + virtual btMultiBodyConstraint* getMultiBodyConstraint(int constraintIndex) { - return m_multiBodyConstraints[constraintIndex]; + return m_multiBodyConstraints[constraintIndex]; } - virtual const btMultiBodyConstraint* getMultiBodyConstraint( int constraintIndex) const + virtual const btMultiBodyConstraint* getMultiBodyConstraint(int constraintIndex) const { - return m_multiBodyConstraints[constraintIndex]; + return m_multiBodyConstraints[constraintIndex]; } - virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint); + virtual void removeMultiBodyConstraint(btMultiBodyConstraint* constraint); - virtual void integrateTransforms(btScalar timeStep); + virtual void integrateTransforms(btScalar timeStep); + void integrateMultiBodyTransforms(btScalar timeStep); + void predictMultiBodyTransforms(btScalar timeStep); + + virtual void predictUnconstraintMotion(btScalar timeStep); + virtual void debugDrawWorld(); - virtual void debugDrawWorld(); + virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint); - virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint); - - void forwardKinematics(); + void forwardKinematics(); virtual void clearForces(); virtual void clearMultiBodyConstraintForces(); virtual void clearMultiBodyForces(); virtual void applyGravity(); - - virtual void serialize(btSerializer* serializer); + virtual void serialize(btSerializer* serializer); + virtual void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver); + virtual void setConstraintSolver(btConstraintSolver* solver); + virtual void getAnalyticsData(btAlignedObjectArray<struct btSolverAnalyticsData>& m_islandAnalyticsData) const; + + virtual void solveExternalForces(btContactSolverInfo& solverInfo); + virtual void solveInternalConstraints(btContactSolverInfo& solverInfo); + void buildIslands(); }; -#endif //BT_MULTIBODY_DYNAMICS_WORLD_H +#endif //BT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp new file mode 100644 index 00000000000..df2abbe97a5 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp @@ -0,0 +1,215 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#include "btMultiBodyFixedConstraint.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" +#include "LinearMath/btIDebugDraw.h" + +#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, MULTIBODY_CONSTRAINT_FIXED), + m_rigidBodyA(0), + m_rigidBodyB(bodyB), + m_pivotInA(pivotInA), + m_pivotInB(pivotInB), + m_frameInA(frameInA), + m_frameInB(frameInB) +{ + m_data.resize(BTMBFIXEDCONSTRAINT_DIM); //at least store the applied impulses +} + +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, MULTIBODY_CONSTRAINT_FIXED), + m_rigidBodyA(0), + m_rigidBodyB(0), + m_pivotInA(pivotInA), + m_pivotInB(pivotInB), + m_frameInA(frameInA), + m_frameInB(frameInB) +{ + m_data.resize(BTMBFIXEDCONSTRAINT_DIM); //at least store the applied impulses +} + +void btMultiBodyFixedConstraint::finalizeMultiDof() +{ + //not implemented yet + btAssert(0); +} + +btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint() +{ +} + +int btMultiBodyFixedConstraint::getIslandIdA() const +{ + if (m_rigidBodyA) + return m_rigidBodyA->getIslandTag(); + + if (m_bodyA) + { + if (m_linkA < 0) + { + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + } + else + { + if (m_bodyA->getLink(m_linkA).m_collider) + return m_bodyA->getLink(m_linkA).m_collider->getIslandTag(); + } + } + return -1; +} + +int btMultiBodyFixedConstraint::getIslandIdB() const +{ + if (m_rigidBodyB) + return m_rigidBodyB->getIslandTag(); + if (m_bodyB) + { + if (m_linkB < 0) + { + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); + if (col) + return col->getIslandTag(); + } + else + { + if (m_bodyB->getLink(m_linkB).m_collider) + return m_bodyB->getLink(m_linkB).m_collider->getIslandTag(); + } + } + return -1; +} + +void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal) +{ + int numDim = BTMBFIXEDCONSTRAINT_DIM; + for (int i = 0; i < numDim; i++) + { + btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); + constraintRow.m_orgConstraint = this; + constraintRow.m_orgDofIndex = i; + constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0); + constraintRow.m_contactNormal1.setValue(0, 0, 0); + constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0); + constraintRow.m_contactNormal2.setValue(0, 0, 0); + constraintRow.m_angularComponentA.setValue(0, 0, 0); + constraintRow.m_angularComponentB.setValue(0, 0, 0); + + constraintRow.m_solverBodyIdA = data.m_fixedBodyId; + constraintRow.m_solverBodyIdB = data.m_fixedBodyId; + + // Convert local points back to world + btVector3 pivotAworld = m_pivotInA; + btMatrix3x3 frameAworld = m_frameInA; + if (m_rigidBodyA) + { + constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId(); + pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA; + frameAworld = frameAworld.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation()); + } + else + { + if (m_bodyA) + { + pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); + frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld); + } + } + btVector3 pivotBworld = m_pivotInB; + btMatrix3x3 frameBworld = m_frameInB; + if (m_rigidBodyB) + { + constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId(); + pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB; + frameBworld = frameBworld.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation()); + } + else + { + if (m_bodyB) + { + pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); + frameBworld = m_bodyB->localFrameToWorld(m_linkB, frameBworld); + } + } + + btMatrix3x3 relRot = frameAworld.inverse() * frameBworld; + btVector3 angleDiff; + btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff); + + btVector3 constraintNormalLin(0, 0, 0); + btVector3 constraintNormalAng(0, 0, 0); + btScalar posError = 0.0; + if (i < 3) + { + constraintNormalLin[i] = 1; + posError = (pivotAworld - pivotBworld).dot(constraintNormalLin); + fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, + constraintNormalLin, pivotAworld, pivotBworld, + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse); + } + else + { //i>=3 + constraintNormalAng = frameAworld.getColumn(i % 3); + posError = angleDiff[i % 3]; + fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, + constraintNormalLin, pivotAworld, pivotBworld, + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse, true); + } + } +} + +void btMultiBodyFixedConstraint::debugDraw(class btIDebugDraw* drawer) +{ + btTransform tr; + tr.setIdentity(); + + if (m_rigidBodyA) + { + btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA; + tr.setOrigin(pivot); + drawer->drawTransform(tr, 0.1); + } + if (m_bodyA) + { + btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); + tr.setOrigin(pivotAworld); + drawer->drawTransform(tr, 0.1); + } + if (m_rigidBodyB) + { + // that ideally should draw the same frame + btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB; + tr.setOrigin(pivot); + drawer->drawTransform(tr, 0.1); + } + if (m_bodyB) + { + btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); + tr.setOrigin(pivotBworld); + drawer->drawTransform(tr, 0.1); + } +} diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h new file mode 100644 index 00000000000..adb1cb47dad --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h @@ -0,0 +1,91 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#ifndef BT_MULTIBODY_FIXED_CONSTRAINT_H +#define BT_MULTIBODY_FIXED_CONSTRAINT_H + +#include "btMultiBodyConstraint.h" + +class btMultiBodyFixedConstraint : public btMultiBodyConstraint +{ +protected: + btRigidBody* m_rigidBodyA; + btRigidBody* m_rigidBodyB; + btVector3 m_pivotInA; + btVector3 m_pivotInB; + btMatrix3x3 m_frameInA; + btMatrix3x3 m_frameInB; + +public: + btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); + btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); + + virtual ~btMultiBodyFixedConstraint(); + + virtual void finalizeMultiDof(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + const btVector3& getPivotInA() const + { + return m_pivotInA; + } + + void setPivotInA(const btVector3& pivotInA) + { + m_pivotInA = pivotInA; + } + + const btVector3& getPivotInB() const + { + return m_pivotInB; + } + + virtual void setPivotInB(const btVector3& pivotInB) + { + m_pivotInB = pivotInB; + } + + const btMatrix3x3& getFrameInA() const + { + return m_frameInA; + } + + void setFrameInA(const btMatrix3x3& frameInA) + { + m_frameInA = frameInA; + } + + const btMatrix3x3& getFrameInB() const + { + return m_frameInB; + } + + virtual void setFrameInB(const btMatrix3x3& frameInB) + { + m_frameInB = frameInB; + } + + virtual void debugDraw(class btIDebugDraw* drawer); +}; + +#endif //BT_MULTIBODY_FIXED_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp new file mode 100644 index 00000000000..ee02cf9b07a --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp @@ -0,0 +1,181 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#include "btMultiBodyGearConstraint.h" +#include "btMultiBody.h" +#include "btMultiBodyLinkCollider.h" +#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, MULTIBODY_CONSTRAINT_GEAR), + m_gearRatio(1), + m_gearAuxLink(-1), + m_erp(0), + m_relativePositionTarget(0) +{ +} + +void btMultiBodyGearConstraint::finalizeMultiDof() +{ + allocateJacobiansMultiDof(); + + m_numDofsFinalized = m_jacSizeBoth; +} + +btMultiBodyGearConstraint::~btMultiBodyGearConstraint() +{ +} + +int btMultiBodyGearConstraint::getIslandIdA() const +{ + if (m_bodyA) + { + if (m_linkA < 0) + { + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + } + else + { + if (m_bodyA->getLink(m_linkA).m_collider) + return m_bodyA->getLink(m_linkA).m_collider->getIslandTag(); + } + } + return -1; +} + +int btMultiBodyGearConstraint::getIslandIdB() const +{ + if (m_bodyB) + { + if (m_linkB < 0) + { + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); + if (col) + return col->getIslandTag(); + } + else + { + if (m_bodyB->getLink(m_linkB).m_collider) + return m_bodyB->getLink(m_linkB).m_collider->getIslandTag(); + } + } + return -1; +} + +void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal) +{ + // only positions need to be updated -- data.m_jacobians and force + // directions were set in the ctor and never change. + + if (m_numDofsFinalized != m_jacSizeBoth) + { + finalizeMultiDof(); + } + + //don't crash + if (m_numDofsFinalized != m_jacSizeBoth) + return; + + if (m_maxAppliedImpulse == 0.f) + return; + + // note: we rely on the fact that data.m_jacobians are + // always initialized to zero by the Constraint ctor + int linkDoF = 0; + unsigned int offsetA = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF); + unsigned int offsetB = 6 + (m_bodyB->getLink(m_linkB).m_dofOffset + linkDoF); + + // row 0: the lower bound + jacobianA(0)[offsetA] = 1; + jacobianB(0)[offsetB] = m_gearRatio; + + btScalar posError = 0; + const btVector3 dummy(0, 0, 0); + + btScalar kp = 1; + btScalar kd = 1; + int numRows = getNumRows(); + + for (int row = 0; row < numRows; row++) + { + btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); + + int dof = 0; + btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof]; + btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; + btScalar auxVel = 0; + + if (m_gearAuxLink >= 0) + { + auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof]; + } + currentVelocity += auxVel; + if (m_erp != 0) + { + btScalar currentPositionA = m_bodyA->getJointPosMultiDof(m_linkA)[dof]; + if (m_gearAuxLink >= 0) + { + currentPositionA -= m_bodyA->getJointPosMultiDof(m_gearAuxLink)[dof]; + } + btScalar currentPositionB = m_gearRatio * m_bodyA->getJointPosMultiDof(m_linkB)[dof]; + btScalar diff = currentPositionB + currentPositionA; + btScalar desiredPositionDiff = this->m_relativePositionTarget; + posError = -m_erp * (desiredPositionDiff - diff); + } + + btScalar desiredRelativeVelocity = auxVel; + + fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse, false, 1, false, desiredRelativeVelocity); + + constraintRow.m_orgConstraint = this; + constraintRow.m_orgDofIndex = row; + { + //expect either prismatic or revolute joint type for now + btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic)); + switch (m_bodyA->getLink(m_linkA).m_jointType) + { + case btMultibodyLink::eRevolute: + { + constraintRow.m_contactNormal1.setZero(); + constraintRow.m_contactNormal2.setZero(); + btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec); + constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld; + constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld; + + break; + } + case btMultibodyLink::ePrismatic: + { + btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec); + constraintRow.m_contactNormal1 = prismaticAxisInWorld; + constraintRow.m_contactNormal2 = -prismaticAxisInWorld; + constraintRow.m_relpos1CrossNormal.setZero(); + constraintRow.m_relpos2CrossNormal.setZero(); + break; + } + default: + { + btAssert(0); + } + }; + } + } +} diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h new file mode 100644 index 00000000000..31888fbc682 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h @@ -0,0 +1,115 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#ifndef BT_MULTIBODY_GEAR_CONSTRAINT_H +#define BT_MULTIBODY_GEAR_CONSTRAINT_H + +#include "btMultiBodyConstraint.h" + +class btMultiBodyGearConstraint : public btMultiBodyConstraint +{ +protected: + btRigidBody* m_rigidBodyA; + btRigidBody* m_rigidBodyB; + btVector3 m_pivotInA; + btVector3 m_pivotInB; + btMatrix3x3 m_frameInA; + btMatrix3x3 m_frameInB; + btScalar m_gearRatio; + int m_gearAuxLink; + btScalar m_erp; + btScalar m_relativePositionTarget; + +public: + //btMultiBodyGearConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); + btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); + + virtual ~btMultiBodyGearConstraint(); + + virtual void finalizeMultiDof(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + const btVector3& getPivotInA() const + { + return m_pivotInA; + } + + void setPivotInA(const btVector3& pivotInA) + { + m_pivotInA = pivotInA; + } + + const btVector3& getPivotInB() const + { + return m_pivotInB; + } + + virtual void setPivotInB(const btVector3& pivotInB) + { + m_pivotInB = pivotInB; + } + + const btMatrix3x3& getFrameInA() const + { + return m_frameInA; + } + + void setFrameInA(const btMatrix3x3& frameInA) + { + m_frameInA = frameInA; + } + + const btMatrix3x3& getFrameInB() const + { + return m_frameInB; + } + + virtual void setFrameInB(const btMatrix3x3& frameInB) + { + m_frameInB = frameInB; + } + + virtual void debugDraw(class btIDebugDraw* drawer) + { + //todo(erwincoumans) + } + + virtual void setGearRatio(btScalar gearRatio) + { + m_gearRatio = gearRatio; + } + virtual void setGearAuxLink(int gearAuxLink) + { + m_gearAuxLink = gearAuxLink; + } + virtual void setRelativePositionTarget(btScalar relPosTarget) + { + m_relativePositionTarget = relPosTarget; + } + virtual void setErp(btScalar erp) + { + m_erp = erp; + } +}; + +#endif //BT_MULTIBODY_GEAR_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h new file mode 100644 index 00000000000..3169b86e616 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h @@ -0,0 +1,247 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H +#define BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H + +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "btMultiBodyConstraintSolver.h" + +SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs) +{ + int islandId; + + const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); + const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); + islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag(); + return islandId; +} +class btSortConstraintOnIslandPredicate2 +{ +public: + bool operator()(const btTypedConstraint* lhs, const btTypedConstraint* rhs) const + { + int rIslandId0, lIslandId0; + rIslandId0 = btGetConstraintIslandId2(rhs); + lIslandId0 = btGetConstraintIslandId2(lhs); + return lIslandId0 < rIslandId0; + } +}; + +SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs) +{ + int islandId; + + int islandTagA = lhs->getIslandIdA(); + int islandTagB = lhs->getIslandIdB(); + islandId = islandTagA >= 0 ? islandTagA : islandTagB; + return islandId; +} + +class btSortMultiBodyConstraintOnIslandPredicate +{ +public: + bool operator()(const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs) const + { + int rIslandId0, lIslandId0; + rIslandId0 = btGetMultiBodyConstraintIslandId(rhs); + lIslandId0 = btGetMultiBodyConstraintIslandId(lhs); + return lIslandId0 < rIslandId0; + } +}; + +struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback +{ + + btContactSolverInfo* m_solverInfo; + btMultiBodyConstraintSolver* m_solver; + btMultiBodyConstraint** m_multiBodySortedConstraints; + int m_numMultiBodyConstraints; + + btTypedConstraint** m_sortedConstraints; + int m_numConstraints; + btIDebugDraw* m_debugDrawer; + btDispatcher* m_dispatcher; + + btAlignedObjectArray<btCollisionObject*> m_bodies; + btAlignedObjectArray<btCollisionObject*> m_softBodies; + btAlignedObjectArray<btPersistentManifold*> m_manifolds; + btAlignedObjectArray<btTypedConstraint*> m_constraints; + btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints; + + btAlignedObjectArray<btSolverAnalyticsData> m_islandAnalyticsData; + + MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver* solver, + btDispatcher* dispatcher) + : m_solverInfo(NULL), + m_solver(solver), + m_multiBodySortedConstraints(NULL), + m_numConstraints(0), + m_debugDrawer(NULL), + m_dispatcher(dispatcher) + { + } + + MultiBodyInplaceSolverIslandCallback& operator=(const MultiBodyInplaceSolverIslandCallback& other) + { + btAssert(0); + (void)other; + return *this; + } + + SIMD_FORCE_INLINE virtual void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer) + { + m_islandAnalyticsData.clear(); + btAssert(solverInfo); + m_solverInfo = solverInfo; + + m_multiBodySortedConstraints = sortedMultiBodyConstraints; + m_numMultiBodyConstraints = numMultiBodyConstraints; + m_sortedConstraints = sortedConstraints; + m_numConstraints = numConstraints; + + m_debugDrawer = debugDrawer; + m_bodies.resize(0); + m_manifolds.resize(0); + m_constraints.resize(0); + m_multiBodyConstraints.resize(0); + } + + void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver) + { + m_solver = solver; + } + + virtual void processIsland(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifolds, int numManifolds, int islandId) + { + if (islandId < 0) + { + ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id + m_solver->solveMultiBodyGroup(bodies, numBodies, manifolds, numManifolds, m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher); + if (m_solverInfo->m_reportSolverAnalytics&1) + { + m_solver->m_analyticsData.m_islandId = islandId; + m_islandAnalyticsData.push_back(m_solver->m_analyticsData); + } + } + else + { + //also add all non-contact constraints/joints for this island + btTypedConstraint** startConstraint = 0; + btMultiBodyConstraint** startMultiBodyConstraint = 0; + + int numCurConstraints = 0; + int numCurMultiBodyConstraints = 0; + + int i; + + //find the first constraint for this island + + for (i = 0; i < m_numConstraints; i++) + { + if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId) + { + startConstraint = &m_sortedConstraints[i]; + break; + } + } + //count the number of constraints in this island + for (; i < m_numConstraints; i++) + { + if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId) + { + numCurConstraints++; + } + } + + for (i = 0; i < m_numMultiBodyConstraints; i++) + { + if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId) + { + startMultiBodyConstraint = &m_multiBodySortedConstraints[i]; + break; + } + } + //count the number of multi body constraints in this island + for (; i < m_numMultiBodyConstraints; i++) + { + if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId) + { + numCurMultiBodyConstraints++; + } + } + + //if (m_solverInfo->m_minimumSolverBatchSize<=1) + //{ + // m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); + //} else + { + for (i = 0; i < numBodies; i++) + { + bool isSoftBodyType = (bodies[i]->getInternalType() & btCollisionObject::CO_SOFT_BODY); + if (!isSoftBodyType) + { + m_bodies.push_back(bodies[i]); + } + else + { + m_softBodies.push_back(bodies[i]); + } + } + for (i = 0; i < numManifolds; i++) + m_manifolds.push_back(manifolds[i]); + for (i = 0; i < numCurConstraints; i++) + m_constraints.push_back(startConstraint[i]); + + for (i = 0; i < numCurMultiBodyConstraints; i++) + m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]); + + if ((m_multiBodyConstraints.size() + m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize) + { + processConstraints(islandId); + } + else + { + //printf("deferred\n"); + } + } + } + } + + virtual void processConstraints(int islandId=-1) + { + btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0; + btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0; + btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0; + btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0; + + //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size()); + + m_solver->solveMultiBodyGroup(bodies, m_bodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher); + if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1)) + { + m_solver->m_analyticsData.m_islandId = islandId; + m_islandAnalyticsData.push_back(m_solver->m_analyticsData); + } + m_bodies.resize(0); + m_softBodies.resize(0); + m_manifolds.resize(0); + m_constraints.resize(0); + m_multiBodyConstraints.resize(0); + } +}; + + +#endif /*BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H */ diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h index 5c2fa8ed5b9..d943019e711 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h @@ -12,8 +12,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #ifndef BT_MULTIBODY_JOINT_FEEDBACK_H #define BT_MULTIBODY_JOINT_FEEDBACK_H @@ -21,7 +19,7 @@ subject to the following restrictions: struct btMultiBodyJointFeedback { - btSpatialForceVector m_reactionForces; + btSpatialForceVector m_reactionForces; }; -#endif //BT_MULTIBODY_JOINT_FEEDBACK_H +#endif //BT_MULTIBODY_JOINT_FEEDBACK_H diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp index 3f05aa4d5fa..94b36ac108c 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -20,21 +20,18 @@ subject to the following restrictions: #include "btMultiBodyLinkCollider.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" - - 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), - m_lowerBound(lower), - m_upperBound(upper) + : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 2, true, MULTIBODY_CONSTRAINT_LIMIT), + m_lowerBound(lower), + m_upperBound(upper) { - } void btMultiBodyJointLimitConstraint::finalizeMultiDof() { // the data.m_jacobians never change, so may as well - // initialize them here + // initialize them here allocateJacobiansMultiDof(); @@ -55,15 +52,18 @@ btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint() int btMultiBodyJointLimitConstraint::getIslandIdA() const { - if(m_bodyA) + if (m_bodyA) { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;i<m_bodyA->getNumLinks();i++) + if (m_linkA < 0) + { + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + } + else { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); + if (m_bodyA->getLink(m_linkA).m_collider) + return m_bodyA->getLink(m_linkA).m_collider->getIslandTag(); } } return -1; @@ -71,82 +71,86 @@ int btMultiBodyJointLimitConstraint::getIslandIdA() const int btMultiBodyJointLimitConstraint::getIslandIdB() const { - if(m_bodyB) + if (m_bodyB) { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - - for (int i=0;i<m_bodyB->getNumLinks();i++) + if (m_linkB < 0) { - col = m_bodyB->getLink(i).m_collider; + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); if (col) return col->getIslandTag(); } + else + { + if (m_bodyB->getLink(m_linkB).m_collider) + return m_bodyB->getLink(m_linkB).m_collider->getIslandTag(); + } } return -1; } - void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal) + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal) { - - // only positions need to be updated -- data.m_jacobians and force - // directions were set in the ctor and never change. + // only positions need to be updated -- data.m_jacobians and force + // directions were set in the ctor and never change. if (m_numDofsFinalized != m_jacSizeBoth) { - finalizeMultiDof(); + finalizeMultiDof(); } + // row 0: the lower bound + setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent - // row 0: the lower bound - setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent + // row 1: the upper bound + setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA)); - // row 1: the upper bound - setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA)); - - for (int row=0;row<getNumRows();row++) + for (int row = 0; row < getNumRows(); row++) { - - btScalar direction = row? -1 : 1; + btScalar penetration = getPosition(row); + + //todo: consider adding some safety threshold here + if (penetration > 0) + { + continue; + } + btScalar direction = row ? -1 : 1; btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); - constraintRow.m_orgConstraint = this; - constraintRow.m_orgDofIndex = row; - + constraintRow.m_orgConstraint = this; + constraintRow.m_orgDofIndex = row; + constraintRow.m_multiBodyA = m_bodyA; constraintRow.m_multiBodyB = m_bodyB; - const btScalar posError = 0; //why assume it's zero? + const btScalar posError = 0; //why assume it's zero? const btVector3 dummy(0, 0, 0); - btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse); + btScalar rel_vel = fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, 0, m_maxAppliedImpulse); { //expect either prismatic or revolute joint type for now - btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic)); + btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic)); switch (m_bodyA->getLink(m_linkA).m_jointType) { case btMultibodyLink::eRevolute: { constraintRow.m_contactNormal1.setZero(); constraintRow.m_contactNormal2.setZero(); - btVector3 revoluteAxisInWorld = direction*quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec); - constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld; - constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld; - + btVector3 revoluteAxisInWorld = direction * quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec); + constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld; + constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld; + break; } case btMultibodyLink::ePrismatic: { - btVector3 prismaticAxisInWorld = direction* quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec); - constraintRow.m_contactNormal1=prismaticAxisInWorld; - constraintRow.m_contactNormal2=-prismaticAxisInWorld; + btVector3 prismaticAxisInWorld = direction * quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec); + constraintRow.m_contactNormal1 = prismaticAxisInWorld; + constraintRow.m_contactNormal2 = -prismaticAxisInWorld; constraintRow.m_relpos1CrossNormal.setZero(); constraintRow.m_relpos2CrossNormal.setZero(); - + break; } default: @@ -154,36 +158,35 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint btAssert(0); } }; - } { - btScalar penetration = getPosition(row); btScalar positionalError = 0.f; - btScalar velocityError = - rel_vel;// * damping; + btScalar velocityError = -rel_vel; // * damping; btScalar erp = infoGlobal.m_erp2; if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) { erp = infoGlobal.m_erp; } - if (penetration>0) + if (penetration > 0) { positionalError = 0; velocityError = -penetration / infoGlobal.m_timeStep; - } else + } + else { - positionalError = -penetration * erp/infoGlobal.m_timeStep; + positionalError = -penetration * erp / infoGlobal.m_timeStep; } - btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv; + btScalar penetrationImpulse = positionalError * constraintRow.m_jacDiagABInv; + btScalar velocityImpulse = velocityError * constraintRow.m_jacDiagABInv; if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) { //combine position and velocity into rhs - constraintRow.m_rhs = penetrationImpulse+velocityImpulse; + constraintRow.m_rhs = penetrationImpulse + velocityImpulse; constraintRow.m_rhsPenetration = 0.f; - - } else + } + else { //split position and velocity into rhs and m_rhsPenetration constraintRow.m_rhs = velocityImpulse; @@ -191,9 +194,4 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint } } } - } - - - - diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h index 55b8d122b97..b810692b4ce 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h @@ -22,11 +22,10 @@ struct btSolverInfo; class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint { protected: + btScalar m_lowerBound; + btScalar m_upperBound; - btScalar m_lowerBound; - btScalar m_upperBound; public: - btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper); virtual ~btMultiBodyJointLimitConstraint(); @@ -36,15 +35,29 @@ public: virtual int getIslandIdB() const; virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal); + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); virtual void debugDraw(class btIDebugDraw* drawer) { //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 - +#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index 062d19accaa..fec9b03213a 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -20,17 +20,18 @@ subject to the following restrictions: #include "btMultiBodyLinkCollider.h" #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), - m_desiredVelocity(desiredVelocity) + : 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.), + m_kp(0), + m_erp(1), + m_rhsClamp(SIMD_INFINITY) { - m_maxAppliedImpulse = maxMotorImpulse; // the data.m_jacobians never change, so may as well - // initialize them here - - + // initialize them here } void btMultiBodyJointMotor::finalizeMultiDof() @@ -50,13 +51,17 @@ 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), - m_desiredVelocity(desiredVelocity) + : 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.), + m_kp(0), + m_erp(1), + m_rhsClamp(SIMD_INFINITY) { btAssert(linkDoF < body->getLink(link).m_dofCount); m_maxAppliedImpulse = maxMotorImpulse; - } btMultiBodyJointMotor::~btMultiBodyJointMotor() { @@ -64,83 +69,108 @@ btMultiBodyJointMotor::~btMultiBodyJointMotor() int btMultiBodyJointMotor::getIslandIdA() const { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;i<m_bodyA->getNumLinks();i++) + if (this->m_linkA < 0) { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + } + else + { + if (m_bodyA->getLink(m_linkA).m_collider) + { + return m_bodyA->getLink(m_linkA).m_collider->getIslandTag(); + } } return -1; } int btMultiBodyJointMotor::getIslandIdB() const { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - - for (int i=0;i<m_bodyB->getNumLinks();i++) + if (m_linkB < 0) { - col = m_bodyB->getLink(i).m_collider; + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); if (col) return col->getIslandTag(); } + else + { + if (m_bodyB->getLink(m_linkB).m_collider) + { + return m_bodyB->getLink(m_linkB).m_collider->getIslandTag(); + } + } return -1; } - void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal) + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal) { - // only positions need to be updated -- data.m_jacobians and force - // directions were set in the ctor and never change. - + // only positions need to be updated -- data.m_jacobians and force + // directions were set in the ctor and never change. + if (m_numDofsFinalized != m_jacSizeBoth) { - finalizeMultiDof(); + finalizeMultiDof(); } //don't crash if (m_numDofsFinalized != m_jacSizeBoth) return; + if (m_maxAppliedImpulse == 0.f) + return; + const btScalar posError = 0; const btVector3 dummy(0, 0, 0); - for (int row=0;row<getNumRows();row++) + for (int row = 0; row < getNumRows(); row++) { btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); + int dof = 0; + btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof]; + btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; + btScalar positionStabiliationTerm = m_erp * (m_desiredPosition - currentPosition) / infoGlobal.m_timeStep; - fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity); + btScalar velocityError = (m_desiredVelocity - currentVelocity); + btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity + m_kd * velocityError; + if (rhs > m_rhsClamp) + { + rhs = m_rhsClamp; + } + if (rhs < -m_rhsClamp) + { + rhs = -m_rhsClamp; + } + + fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse, false, 1, false, rhs); constraintRow.m_orgConstraint = this; constraintRow.m_orgDofIndex = row; { //expect either prismatic or revolute joint type for now - btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic)); + btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic)); switch (m_bodyA->getLink(m_linkA).m_jointType) { case btMultibodyLink::eRevolute: { constraintRow.m_contactNormal1.setZero(); constraintRow.m_contactNormal2.setZero(); - btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec); - constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld; - constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld; - + btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec); + constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld; + constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld; + break; } case btMultibodyLink::ePrismatic: { - btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec); - constraintRow.m_contactNormal1=prismaticAxisInWorld; - constraintRow.m_contactNormal2=-prismaticAxisInWorld; + btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec); + constraintRow.m_contactNormal1 = prismaticAxisInWorld; + constraintRow.m_contactNormal2 = -prismaticAxisInWorld; constraintRow.m_relpos1CrossNormal.setZero(); constraintRow.m_relpos2CrossNormal.setZero(); - + break; } default: @@ -148,10 +178,6 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con btAssert(0); } }; - } - } - } - diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h index 011aadcfa4b..1aca36352e7 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h @@ -24,34 +24,54 @@ struct btSolverInfo; class btMultiBodyJointMotor : public btMultiBodyConstraint { protected: - - - btScalar m_desiredVelocity; + btScalar m_desiredVelocity; + btScalar m_desiredPosition; + btScalar m_kd; + btScalar m_kp; + btScalar m_erp; + btScalar m_rhsClamp; //maximum error public: - btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse); btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse); virtual ~btMultiBodyJointMotor(); - virtual void finalizeMultiDof(); + virtual void finalizeMultiDof(); virtual int getIslandIdA() const; virtual int getIslandIdB() const; virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal); + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 1.f) + { + m_desiredVelocity = velTarget; + m_kd = kd; + } - virtual void setVelocityTarget(btScalar velTarget) - { - m_desiredVelocity = velTarget; - } + virtual void setPositionTarget(btScalar posTarget, btScalar kp = 1.f) + { + m_desiredPosition = posTarget; + m_kp = kp; + } + virtual void setErp(btScalar erp) + { + m_erp = erp; + } + virtual btScalar getErp() const + { + return m_erp; + } + virtual void setRhsClamp(btScalar rhsClamp) + { + m_rhsClamp = rhsClamp; + } virtual void debugDraw(class btIDebugDraw* drawer) { //todo(erwincoumans) } }; -#endif //BT_MULTIBODY_JOINT_MOTOR_H - +#endif //BT_MULTIBODY_JOINT_MOTOR_H diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h index 668e4443904..01d5583c2fd 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -20,9 +20,10 @@ subject to the following restrictions: #include "LinearMath/btVector3.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" -enum btMultiBodyLinkFlags +enum btMultiBodyLinkFlags { - BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1 + BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1, + BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION = 2, }; //both defines are now permanently enabled @@ -35,7 +36,6 @@ enum btMultiBodyLinkFlags //namespace { - #include "LinearMath/btSpatialAlgebra.h" //} @@ -44,27 +44,26 @@ enum btMultiBodyLinkFlags // Link struct // -struct btMultibodyLink +struct btMultibodyLink { - BT_DECLARE_ALIGNED_ALLOCATOR(); - btScalar m_mass; // mass of link - btVector3 m_inertiaLocal; // inertia of link (local frame; diagonal) + btScalar m_mass; // mass of link + btVector3 m_inertiaLocal; // inertia of link (local frame; diagonal) + + int m_parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link. - int m_parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link. + btQuaternion m_zeroRotParentToThis; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant. - btQuaternion m_zeroRotParentToThis; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant. + btVector3 m_dVector; // vector from the inboard joint pos to this link's COM. (local frame.) constant. + //this is set to zero for planar joint (see also m_eVector comment) - btVector3 m_dVector; // vector from the inboard joint pos to this link's COM. (local frame.) constant. - //this is set to zero for planar joint (see also m_eVector comment) - - // m_eVector is constant, but depends on the joint type: - // revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame. + // m_eVector is constant, but depends on the joint type: + // revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame. // planar: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.) // todo: fix the planar so it is consistent with the other joints - - btVector3 m_eVector; + + btVector3 m_eVector; btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity; @@ -78,13 +77,11 @@ struct btMultibodyLink eInvalid }; - - // "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant. - // for prismatic: m_axesTop[0] = zero; - // m_axesBottom[0] = unit vector along the joint axis. - // for revolute: m_axesTop[0] = unit vector along the rotation axis (u); - // m_axesBottom[0] = u cross m_dVector (i.e. COM linear motion due to the rotation at the joint) + // for prismatic: m_axesTop[0] = zero; + // m_axesBottom[0] = unit vector along the joint axis. + // for revolute: m_axesTop[0] = unit vector along the rotation axis (u); + // m_axesBottom[0] = u cross m_dVector (i.e. COM linear motion due to the rotation at the joint) // // for spherical: m_axesTop[0][1][2] (u1,u2,u3) form a 3x3 identity matrix (3 rotation axes) // m_axesBottom[0][1][2] cross u1,u2,u3 (i.e. COM linear motion due to the rotation at the joint) @@ -92,117 +89,149 @@ struct btMultibodyLink // for planar: m_axesTop[0] = unit vector along the rotation axis (u); defines the plane of motion // m_axesTop[1][2] = zero // m_axesBottom[0] = zero - // m_axesBottom[1][2] = unit vectors along the translational axes on that plane + // m_axesBottom[1][2] = unit vectors along the translational axes on that plane btSpatialMotionVector m_axes[6]; void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; } - void setAxisBottom(int dof, const btVector3 &axis) { m_axes[dof].m_bottomVec = axis; } - void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_topVec.setValue(x, y, z); } - void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_bottomVec.setValue(x, y, z); } - const btVector3 & getAxisTop(int dof) const { return m_axes[dof].m_topVec; } - const btVector3 & getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; } + void setAxisBottom(int dof, const btVector3 &axis) + { + m_axes[dof].m_bottomVec = axis; + } + void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) + { + m_axes[dof].m_topVec.setValue(x, y, z); + } + void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) + { + m_axes[dof].m_bottomVec.setValue(x, y, z); + } + const btVector3 &getAxisTop(int dof) const { return m_axes[dof].m_topVec; } + const btVector3 &getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; } int m_dofOffset, m_cfgOffset; - btQuaternion m_cachedRotParentToThis; // rotates vectors in parent frame to vectors in local frame - btVector3 m_cachedRVector; // vector from COM of parent to COM of this link, in local frame. + btQuaternion m_cachedRotParentToThis; // rotates vectors in parent frame to vectors in local frame + btVector3 m_cachedRVector; // vector from COM of parent to COM of this link, in local frame. + + // predicted verstion + btQuaternion m_cachedRotParentToThis_interpolate; // rotates vectors in parent frame to vectors in local frame + btVector3 m_cachedRVector_interpolate; // vector from COM of parent to COM of this link, in local frame. - btVector3 m_appliedForce; // In WORLD frame - btVector3 m_appliedTorque; // In WORLD frame + btVector3 m_appliedForce; // In WORLD frame + btVector3 m_appliedTorque; // In WORLD frame -btVector3 m_appliedConstraintForce; // In WORLD frame - btVector3 m_appliedConstraintTorque; // In WORLD frame + btVector3 m_appliedConstraintForce; // In WORLD frame + btVector3 m_appliedConstraintTorque; // In WORLD frame btScalar m_jointPos[7]; - - //m_jointTorque is the joint torque applied by the user using 'addJointTorque'. - //It gets set to zero after each internal stepSimulation call + btScalar m_jointPos_interpolate[7]; + + //m_jointTorque is the joint torque applied by the user using 'addJointTorque'. + //It gets set to zero after each internal stepSimulation call btScalar m_jointTorque[6]; - - class btMultiBodyLinkCollider* m_collider; + + class btMultiBodyLinkCollider *m_collider; int m_flags; - - - int m_dofCount, m_posVarCount; //redundant but handy - + + int m_dofCount, m_posVarCount; //redundant but handy + eFeatherstoneJointType m_jointType; - - struct btMultiBodyJointFeedback* m_jointFeedback; - btTransform m_cachedWorldTransform;//this cache is updated when calling btMultiBody::forwardKinematics + struct btMultiBodyJointFeedback *m_jointFeedback; - const char* m_linkName;//m_linkName memory needs to be managed by the developer/user! - const char* m_jointName;//m_jointName memory needs to be managed by the developer/user! + btTransform m_cachedWorldTransform; //this cache is updated when calling btMultiBody::forwardKinematics - // ctor: set some sensible defaults + const char *m_linkName; //m_linkName memory needs to be managed by the developer/user! + const char *m_jointName; //m_jointName memory needs to be managed by the developer/user! + const void *m_userPtr; //m_userPtr ptr needs to be managed by the developer/user! + + btScalar m_jointDamping; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping. + btScalar m_jointFriction; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual friction using a velocity motor. + btScalar m_jointLowerLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader. + btScalar m_jointUpperLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader. + btScalar m_jointMaxForce; //todo: implement this internally. It is unused for now, it is set by a URDF loader. + btScalar m_jointMaxVelocity; //todo: implement this internally. It is unused for now, it is set by a URDF loader. + + // ctor: set some sensible defaults btMultibodyLink() - : m_mass(1), - m_parent(-1), - m_zeroRotParentToThis(0, 0, 0, 1), - m_cachedRotParentToThis(0, 0, 0, 1), - m_collider(0), - m_flags(0), - m_dofCount(0), - m_posVarCount(0), - m_jointType(btMultibodyLink::eInvalid), - m_jointFeedback(0), - m_linkName(0), - m_jointName(0) + : m_mass(1), + m_parent(-1), + m_zeroRotParentToThis(0, 0, 0, 1), + m_cachedRotParentToThis(0, 0, 0, 1), + m_cachedRotParentToThis_interpolate(0, 0, 0, 1), + m_collider(0), + m_flags(0), + m_dofCount(0), + m_posVarCount(0), + m_jointType(btMultibodyLink::eInvalid), + m_jointFeedback(0), + m_linkName(0), + m_jointName(0), + m_userPtr(0), + m_jointDamping(0), + m_jointFriction(0), + m_jointLowerLimit(0), + m_jointUpperLimit(0), + m_jointMaxForce(0), + m_jointMaxVelocity(0) { - m_inertiaLocal.setValue(1, 1, 1); setAxisTop(0, 0., 0., 0.); setAxisBottom(0, 1., 0., 0.); m_dVector.setValue(0, 0, 0); m_eVector.setValue(0, 0, 0); m_cachedRVector.setValue(0, 0, 0); - m_appliedForce.setValue( 0, 0, 0); + m_cachedRVector_interpolate.setValue(0, 0, 0); + m_appliedForce.setValue(0, 0, 0); m_appliedTorque.setValue(0, 0, 0); - // + m_appliedConstraintForce.setValue(0, 0, 0); + m_appliedConstraintTorque.setValue(0, 0, 0); + // m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f; - m_jointPos[3] = 1.f; //"quat.w" + m_jointPos[3] = 1.f; //"quat.w" m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f; m_cachedWorldTransform.setIdentity(); } - // routine to update m_cachedRotParentToThis and m_cachedRVector + // routine to update m_cachedRotParentToThis and m_cachedRVector void updateCacheMultiDof(btScalar *pq = 0) { - btScalar *pJointPos = (pq ? pq : &m_jointPos[0]); - - switch(m_jointType) + btScalar *pJointPos = (pq ? pq : &m_jointPos[0]); + btQuaternion& cachedRot = m_cachedRotParentToThis; + btVector3& cachedVector = m_cachedRVector; + switch (m_jointType) { case eRevolute: { - m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); + cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); break; } case ePrismatic: { // m_cachedRotParentToThis never changes, so no need to update - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0); + cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0); break; } case eSpherical: { - m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); + cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); break; } case ePlanar: { - m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis; - m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis,m_eVector); + cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; + cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector); break; } case eFixed: { - m_cachedRotParentToThis = m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); + cachedRot = m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); break; } @@ -212,8 +241,60 @@ btVector3 m_appliedConstraintForce; // In WORLD frame btAssert(0); } } + m_cachedRotParentToThis_interpolate = m_cachedRotParentToThis; + m_cachedRVector_interpolate = m_cachedRVector; } + + void updateInterpolationCacheMultiDof() + { + btScalar *pJointPos = &m_jointPos_interpolate[0]; + + btQuaternion& cachedRot = m_cachedRotParentToThis_interpolate; + btVector3& cachedVector = m_cachedRVector_interpolate; + switch (m_jointType) + { + case eRevolute: + { + cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); + + break; + } + case ePrismatic: + { + // m_cachedRotParentToThis never changes, so no need to update + cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0); + + break; + } + case eSpherical: + { + cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); + + break; + } + case ePlanar: + { + cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; + cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector); + + break; + } + case eFixed: + { + cachedRot = m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); + + break; + } + default: + { + //invalid type + btAssert(0); + } + } + } }; - -#endif //BT_MULTIBODY_LINK_H +#endif //BT_MULTIBODY_LINK_H diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h index 5080ea87454..bc909990c21 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h @@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. @@ -19,21 +19,32 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "btMultiBody.h" +#include "LinearMath/btSerializer.h" + +#ifdef BT_USE_DOUBLE_PRECISION +#define btMultiBodyLinkColliderData btMultiBodyLinkColliderDoubleData +#define btMultiBodyLinkColliderDataName "btMultiBodyLinkColliderDoubleData" +#else +#define btMultiBodyLinkColliderData btMultiBodyLinkColliderFloatData +#define btMultiBodyLinkColliderDataName "btMultiBodyLinkColliderFloatData" +#endif class btMultiBodyLinkCollider : public btCollisionObject { -//protected: + //protected: public: - btMultiBody* m_multiBody; int m_link; + virtual ~btMultiBodyLinkCollider() + { - btMultiBodyLinkCollider (btMultiBody* multiBody,int link) - :m_multiBody(multiBody), - m_link(link) + } + btMultiBodyLinkCollider(btMultiBody* multiBody, int link) + : m_multiBody(multiBody), + m_link(link) { - m_checkCollideWith = true; + m_checkCollideWith = true; //we need to remove the 'CF_STATIC_OBJECT' flag, otherwise links/base doesn't merge islands //this means that some constraints might point to bodies that are not in the islands, causing crashes //if (link>=0 || (multiBody && !multiBody->hasFixedBase())) @@ -49,18 +60,18 @@ public: } static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj) { - if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK) + if (colObj->getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK) return (btMultiBodyLinkCollider*)colObj; return 0; } static const btMultiBodyLinkCollider* upcast(const btCollisionObject* colObj) { - if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK) + if (colObj->getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK) return (btMultiBodyLinkCollider*)colObj; return 0; } - virtual bool checkCollideWithOverride(const btCollisionObject* co) const + virtual bool checkCollideWithOverride(const btCollisionObject* co) const { const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co); if (!other) @@ -71,22 +82,97 @@ public: return false; //check if 'link' has collision disabled - if (m_link>=0) + if (m_link >= 0) { const btMultibodyLink& link = m_multiBody->getLink(this->m_link); - if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.m_parent == other->m_link) - return false; + if (link.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION) + { + int parent_of_this = m_link; + while (1) + { + if (parent_of_this == -1) + break; + parent_of_this = m_multiBody->getLink(parent_of_this).m_parent; + if (parent_of_this == other->m_link) + { + return false; + } + } + } + else if (link.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) + { + if (link.m_parent == other->m_link) + return false; + } } - - if (other->m_link>=0) + + if (other->m_link >= 0) { const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link); - if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.m_parent == this->m_link) - return false; + if (otherLink.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION) + { + int parent_of_other = other->m_link; + while (1) + { + if (parent_of_other == -1) + break; + parent_of_other = m_multiBody->getLink(parent_of_other).m_parent; + if (parent_of_other == this->m_link) + return false; + } + } + else if (otherLink.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) + { + if (otherLink.m_parent == this->m_link) + return false; + } } return true; } + + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; +}; + +// clang-format off + +struct btMultiBodyLinkColliderFloatData +{ + btCollisionObjectFloatData m_colObjData; + btMultiBodyFloatData *m_multiBody; + int m_link; + char m_padding[4]; }; -#endif //BT_FEATHERSTONE_LINK_COLLIDER_H +struct btMultiBodyLinkColliderDoubleData +{ + btCollisionObjectDoubleData m_colObjData; + btMultiBodyDoubleData *m_multiBody; + int m_link; + char m_padding[4]; +}; + +// clang-format on + +SIMD_FORCE_INLINE int btMultiBodyLinkCollider::calculateSerializeBufferSize() const +{ + return sizeof(btMultiBodyLinkColliderData); +} + +SIMD_FORCE_INLINE const char* btMultiBodyLinkCollider::serialize(void* dataBuffer, class btSerializer* serializer) const +{ + btMultiBodyLinkColliderData* dataOut = (btMultiBodyLinkColliderData*)dataBuffer; + btCollisionObject::serialize(&dataOut->m_colObjData, serializer); + + dataOut->m_link = this->m_link; + dataOut->m_multiBody = (btMultiBodyData*)serializer->getUniquePointer(m_multiBody); + + // Fill padding with zeros to appease msan. + memset(dataOut->m_padding, 0, sizeof(dataOut->m_padding)); + + return btMultiBodyLinkColliderDataName; +} +#endif //BT_FEATHERSTONE_LINK_COLLIDER_H diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp new file mode 100644 index 00000000000..f2186a93e9d --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp @@ -0,0 +1,966 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2018 Google Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h" + +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" +#include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h" + +#define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + +static bool interleaveContactAndFriction1 = false; + +struct btJointNode1 +{ + int jointIndex; // pointer to enclosing dxJoint object + int otherBodyIndex; // *other* body this joint is connected to + int nextJointNodeIndex; //-1 for null + int constraintRowIndex; +}; + +// Helper function to compute a delta velocity in the constraint space. +static btScalar computeDeltaVelocityInConstraintSpace( + const btVector3& angularDeltaVelocity, + const btVector3& contactNormal, + btScalar invMass, + const btVector3& angularJacobian, + const btVector3& linearJacobian) +{ + return angularDeltaVelocity.dot(angularJacobian) + contactNormal.dot(linearJacobian) * invMass; +} + +// Faster version of computeDeltaVelocityInConstraintSpace that can be used when contactNormal and linearJacobian are +// identical. +static btScalar computeDeltaVelocityInConstraintSpace( + const btVector3& angularDeltaVelocity, + btScalar invMass, + const btVector3& angularJacobian) +{ + return angularDeltaVelocity.dot(angularJacobian) + invMass; +} + +// Helper function to compute a delta velocity in the constraint space. +static btScalar computeDeltaVelocityInConstraintSpace(const btScalar* deltaVelocity, const btScalar* jacobian, int size) +{ + btScalar result = 0; + for (int i = 0; i < size; ++i) + result += deltaVelocity[i] * jacobian[i]; + + return result; +} + +static btScalar computeConstraintMatrixDiagElementMultiBody( + const btAlignedObjectArray<btSolverBody>& solverBodyPool, + const btMultiBodyJacobianData& data, + const btMultiBodySolverConstraint& constraint) +{ + btScalar ret = 0; + + const btMultiBody* multiBodyA = constraint.m_multiBodyA; + const btMultiBody* multiBodyB = constraint.m_multiBodyB; + + if (multiBodyA) + { + const btScalar* jacA = &data.m_jacobians[constraint.m_jacAindex]; + const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex]; + const int ndofA = multiBodyA->getNumDofs() + 6; + ret += computeDeltaVelocityInConstraintSpace(deltaA, jacA, ndofA); + } + else + { + const int solverBodyIdA = constraint.m_solverBodyIdA; + btAssert(solverBodyIdA != -1); + const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA]; + const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0; + ret += computeDeltaVelocityInConstraintSpace( + constraint.m_relpos1CrossNormal, + invMassA, + constraint.m_angularComponentA); + } + + if (multiBodyB) + { + const btScalar* jacB = &data.m_jacobians[constraint.m_jacBindex]; + const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex]; + const int ndofB = multiBodyB->getNumDofs() + 6; + ret += computeDeltaVelocityInConstraintSpace(deltaB, jacB, ndofB); + } + else + { + const int solverBodyIdB = constraint.m_solverBodyIdB; + btAssert(solverBodyIdB != -1); + const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB]; + const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0; + ret += computeDeltaVelocityInConstraintSpace( + constraint.m_relpos2CrossNormal, + invMassB, + constraint.m_angularComponentB); + } + + return ret; +} + +static btScalar computeConstraintMatrixOffDiagElementMultiBody( + const btAlignedObjectArray<btSolverBody>& solverBodyPool, + const btMultiBodyJacobianData& data, + const btMultiBodySolverConstraint& constraint, + const btMultiBodySolverConstraint& offDiagConstraint) +{ + btScalar offDiagA = btScalar(0); + + const btMultiBody* multiBodyA = constraint.m_multiBodyA; + const btMultiBody* multiBodyB = constraint.m_multiBodyB; + const btMultiBody* offDiagMultiBodyA = offDiagConstraint.m_multiBodyA; + const btMultiBody* offDiagMultiBodyB = offDiagConstraint.m_multiBodyB; + + // Assumed at least one system is multibody + btAssert(multiBodyA || multiBodyB); + btAssert(offDiagMultiBodyA || offDiagMultiBodyB); + + if (offDiagMultiBodyA) + { + const btScalar* offDiagJacA = &data.m_jacobians[offDiagConstraint.m_jacAindex]; + + if (offDiagMultiBodyA == multiBodyA) + { + const int ndofA = multiBodyA->getNumDofs() + 6; + const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex]; + offDiagA += computeDeltaVelocityInConstraintSpace(deltaA, offDiagJacA, ndofA); + } + else if (offDiagMultiBodyA == multiBodyB) + { + const int ndofB = multiBodyB->getNumDofs() + 6; + const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex]; + offDiagA += computeDeltaVelocityInConstraintSpace(deltaB, offDiagJacA, ndofB); + } + } + else + { + const int solverBodyIdA = constraint.m_solverBodyIdA; + const int solverBodyIdB = constraint.m_solverBodyIdB; + + const int offDiagSolverBodyIdA = offDiagConstraint.m_solverBodyIdA; + btAssert(offDiagSolverBodyIdA != -1); + + if (offDiagSolverBodyIdA == solverBodyIdA) + { + btAssert(solverBodyIdA != -1); + const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA]; + const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0; + offDiagA += computeDeltaVelocityInConstraintSpace( + offDiagConstraint.m_relpos1CrossNormal, + offDiagConstraint.m_contactNormal1, + invMassA, constraint.m_angularComponentA, + constraint.m_contactNormal1); + } + else if (offDiagSolverBodyIdA == solverBodyIdB) + { + btAssert(solverBodyIdB != -1); + const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB]; + const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0; + offDiagA += computeDeltaVelocityInConstraintSpace( + offDiagConstraint.m_relpos1CrossNormal, + offDiagConstraint.m_contactNormal1, + invMassB, + constraint.m_angularComponentB, + constraint.m_contactNormal2); + } + } + + if (offDiagMultiBodyB) + { + const btScalar* offDiagJacB = &data.m_jacobians[offDiagConstraint.m_jacBindex]; + + if (offDiagMultiBodyB == multiBodyA) + { + const int ndofA = multiBodyA->getNumDofs() + 6; + const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex]; + offDiagA += computeDeltaVelocityInConstraintSpace(deltaA, offDiagJacB, ndofA); + } + else if (offDiagMultiBodyB == multiBodyB) + { + const int ndofB = multiBodyB->getNumDofs() + 6; + const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex]; + offDiagA += computeDeltaVelocityInConstraintSpace(deltaB, offDiagJacB, ndofB); + } + } + else + { + const int solverBodyIdA = constraint.m_solverBodyIdA; + const int solverBodyIdB = constraint.m_solverBodyIdB; + + const int offDiagSolverBodyIdB = offDiagConstraint.m_solverBodyIdB; + btAssert(offDiagSolverBodyIdB != -1); + + if (offDiagSolverBodyIdB == solverBodyIdA) + { + btAssert(solverBodyIdA != -1); + const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA]; + const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0; + offDiagA += computeDeltaVelocityInConstraintSpace( + offDiagConstraint.m_relpos2CrossNormal, + offDiagConstraint.m_contactNormal2, + invMassA, constraint.m_angularComponentA, + constraint.m_contactNormal1); + } + else if (offDiagSolverBodyIdB == solverBodyIdB) + { + btAssert(solverBodyIdB != -1); + const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB]; + const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0; + offDiagA += computeDeltaVelocityInConstraintSpace( + offDiagConstraint.m_relpos2CrossNormal, + offDiagConstraint.m_contactNormal2, + invMassB, constraint.m_angularComponentB, + constraint.m_contactNormal2); + } + } + + return offDiagA; +} + +void btMultiBodyMLCPConstraintSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) +{ + createMLCPFastRigidBody(infoGlobal); + createMLCPFastMultiBody(infoGlobal); +} + +void btMultiBodyMLCPConstraintSolver::createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal) +{ + int numContactRows = interleaveContactAndFriction1 ? 3 : 1; + + int numConstraintRows = m_allConstraintPtrArray.size(); + + if (numConstraintRows == 0) + return; + + int n = numConstraintRows; + { + BT_PROFILE("init b (rhs)"); + m_b.resize(numConstraintRows); + m_bSplit.resize(numConstraintRows); + m_b.setZero(); + m_bSplit.setZero(); + for (int i = 0; i < numConstraintRows; i++) + { + btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv; + if (!btFuzzyZero(jacDiag)) + { + btScalar rhs = m_allConstraintPtrArray[i]->m_rhs; + btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration; + m_b[i] = rhs / jacDiag; + m_bSplit[i] = rhsPenetration / jacDiag; + } + } + } + + // btScalar* w = 0; + // int nub = 0; + + m_lo.resize(numConstraintRows); + m_hi.resize(numConstraintRows); + + { + BT_PROFILE("init lo/ho"); + + for (int i = 0; i < numConstraintRows; i++) + { + if (0) //m_limitDependencies[i]>=0) + { + m_lo[i] = -BT_INFINITY; + m_hi[i] = BT_INFINITY; + } + else + { + m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit; + m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; + } + } + } + + // + int m = m_allConstraintPtrArray.size(); + + int numBodies = m_tmpSolverBodyPool.size(); + btAlignedObjectArray<int> bodyJointNodeArray; + { + BT_PROFILE("bodyJointNodeArray.resize"); + bodyJointNodeArray.resize(numBodies, -1); + } + btAlignedObjectArray<btJointNode1> jointNodeArray; + { + BT_PROFILE("jointNodeArray.reserve"); + jointNodeArray.reserve(2 * m_allConstraintPtrArray.size()); + } + + btMatrixXu& J3 = m_scratchJ3; + { + BT_PROFILE("J3.resize"); + J3.resize(2 * m, 8); + } + btMatrixXu& JinvM3 = m_scratchJInvM3; + { + BT_PROFILE("JinvM3.resize/setZero"); + + JinvM3.resize(2 * m, 8); + JinvM3.setZero(); + J3.setZero(); + } + int cur = 0; + int rowOffset = 0; + btAlignedObjectArray<int>& ofs = m_scratchOfs; + { + BT_PROFILE("ofs resize"); + ofs.resize(0); + ofs.resizeNoInitialize(m_allConstraintPtrArray.size()); + } + { + BT_PROFILE("Compute J and JinvM"); + int c = 0; + + int numRows = 0; + + for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++) + { + ofs[c] = rowOffset; + int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA; + int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; + btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + + numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows; + if (orgBodyA) + { + { + int slotA = -1; + //find free jointNode slot for sbA + slotA = jointNodeArray.size(); + jointNodeArray.expand(); //NonInitializing(); + int prevSlot = bodyJointNodeArray[sbA]; + bodyJointNodeArray[sbA] = slotA; + jointNodeArray[slotA].nextJointNodeIndex = prevSlot; + jointNodeArray[slotA].jointIndex = c; + jointNodeArray[slotA].constraintRowIndex = i; + jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1; + } + for (int row = 0; row < numRows; row++, cur++) + { + btVector3 normalInvMass = m_allConstraintPtrArray[i + row]->m_contactNormal1 * orgBodyA->getInvMass(); + btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld(); + + for (int r = 0; r < 3; r++) + { + J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal1[r]); + J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal[r]); + JinvM3.setElem(cur, r, normalInvMass[r]); + JinvM3.setElem(cur, r + 4, relPosCrossNormalInvInertia[r]); + } + J3.setElem(cur, 3, 0); + JinvM3.setElem(cur, 3, 0); + J3.setElem(cur, 7, 0); + JinvM3.setElem(cur, 7, 0); + } + } + else + { + cur += numRows; + } + if (orgBodyB) + { + { + int slotB = -1; + //find free jointNode slot for sbA + slotB = jointNodeArray.size(); + jointNodeArray.expand(); //NonInitializing(); + int prevSlot = bodyJointNodeArray[sbB]; + bodyJointNodeArray[sbB] = slotB; + jointNodeArray[slotB].nextJointNodeIndex = prevSlot; + jointNodeArray[slotB].jointIndex = c; + jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1; + jointNodeArray[slotB].constraintRowIndex = i; + } + + for (int row = 0; row < numRows; row++, cur++) + { + btVector3 normalInvMassB = m_allConstraintPtrArray[i + row]->m_contactNormal2 * orgBodyB->getInvMass(); + btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld(); + + for (int r = 0; r < 3; r++) + { + J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal2[r]); + J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal[r]); + JinvM3.setElem(cur, r, normalInvMassB[r]); + JinvM3.setElem(cur, r + 4, relPosInvInertiaB[r]); + } + J3.setElem(cur, 3, 0); + JinvM3.setElem(cur, 3, 0); + J3.setElem(cur, 7, 0); + JinvM3.setElem(cur, 7, 0); + } + } + else + { + cur += numRows; + } + rowOffset += numRows; + } + } + + //compute JinvM = J*invM. + const btScalar* JinvM = JinvM3.getBufferPointer(); + + const btScalar* Jptr = J3.getBufferPointer(); + { + BT_PROFILE("m_A.resize"); + m_A.resize(n, n); + } + + { + BT_PROFILE("m_A.setZero"); + m_A.setZero(); + } + int c = 0; + { + int numRows = 0; + BT_PROFILE("Compute A"); + for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++) + { + int row__ = ofs[c]; + int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA; + int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; + // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + // btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + + numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows; + + const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__; + + { + int startJointNodeA = bodyJointNodeArray[sbA]; + while (startJointNodeA >= 0) + { + int j0 = jointNodeArray[startJointNodeA].jointIndex; + int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex; + if (j0 < c) + { + int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows; + size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8 * numRowsOther : 0; + //printf("%d joint i %d and j0: %d: ",count++,i,j0); + m_A.multiplyAdd2_p8r(JinvMrow, + Jptr + 2 * 8 * (size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]); + } + startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex; + } + } + + { + int startJointNodeB = bodyJointNodeArray[sbB]; + while (startJointNodeB >= 0) + { + int j1 = jointNodeArray[startJointNodeB].jointIndex; + int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex; + + if (j1 < c) + { + int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows; + size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8 * numRowsOther : 0; + m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)numRows, + Jptr + 2 * 8 * (size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]); + } + startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex; + } + } + } + + { + BT_PROFILE("compute diagonal"); + // compute diagonal blocks of m_A + + int row__ = 0; + int numJointRows = m_allConstraintPtrArray.size(); + + int jj = 0; + for (; row__ < numJointRows;) + { + //int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA; + int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB; + // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + + const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows; + + const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__; + const btScalar* Jrow = Jptr + 2 * 8 * (size_t)row__; + m_A.multiply2_p8r(JinvMrow, Jrow, infom, infom, row__, row__); + if (orgBodyB) + { + m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)infom, Jrow + 8 * (size_t)infom, infom, infom, row__, row__); + } + row__ += infom; + jj++; + } + } + } + + if (1) + { + // add cfm to the diagonal of m_A + for (int i = 0; i < m_A.rows(); ++i) + { + m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep); + } + } + + ///fill the upper triangle of the matrix, to make it symmetric + { + BT_PROFILE("fill the upper triangle "); + m_A.copyLowerToUpperTriangle(); + } + + { + BT_PROFILE("resize/init x"); + m_x.resize(numConstraintRows); + m_xSplit.resize(numConstraintRows); + + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + for (int i = 0; i < m_allConstraintPtrArray.size(); i++) + { + const btSolverConstraint& c = *m_allConstraintPtrArray[i]; + m_x[i] = c.m_appliedImpulse; + m_xSplit[i] = c.m_appliedPushImpulse; + } + } + else + { + m_x.setZero(); + m_xSplit.setZero(); + } + } +} + +void btMultiBodyMLCPConstraintSolver::createMLCPFastMultiBody(const btContactSolverInfo& infoGlobal) +{ + const int multiBodyNumConstraints = m_multiBodyAllConstraintPtrArray.size(); + + if (multiBodyNumConstraints == 0) + return; + + // 1. Compute b + { + BT_PROFILE("init b (rhs)"); + + m_multiBodyB.resize(multiBodyNumConstraints); + m_multiBodyB.setZero(); + + for (int i = 0; i < multiBodyNumConstraints; ++i) + { + const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i]; + const btScalar jacDiag = constraint.m_jacDiagABInv; + + if (!btFuzzyZero(jacDiag)) + { + // Note that rhsPenetration is currently always zero because the split impulse hasn't been implemented for multibody yet. + const btScalar rhs = constraint.m_rhs; + m_multiBodyB[i] = rhs / jacDiag; + } + } + } + + // 2. Compute lo and hi + { + BT_PROFILE("init lo/ho"); + + m_multiBodyLo.resize(multiBodyNumConstraints); + m_multiBodyHi.resize(multiBodyNumConstraints); + + for (int i = 0; i < multiBodyNumConstraints; ++i) + { + const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i]; + m_multiBodyLo[i] = constraint.m_lowerLimit; + m_multiBodyHi[i] = constraint.m_upperLimit; + } + } + + // 3. Construct A matrix by using the impulse testing + { + BT_PROFILE("Compute A"); + + { + BT_PROFILE("m_A.resize"); + m_multiBodyA.resize(multiBodyNumConstraints, multiBodyNumConstraints); + } + + for (int i = 0; i < multiBodyNumConstraints; ++i) + { + // Compute the diagonal of A, which is A(i, i) + const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i]; + const btScalar diagA = computeConstraintMatrixDiagElementMultiBody(m_tmpSolverBodyPool, m_data, constraint); + m_multiBodyA.setElem(i, i, diagA); + + // Computes the off-diagonals of A: + // a. The rest of i-th row of A, from A(i, i+1) to A(i, n) + // b. The rest of i-th column of A, from A(i+1, i) to A(n, i) + for (int j = i + 1; j < multiBodyNumConstraints; ++j) + { + const btMultiBodySolverConstraint& offDiagConstraint = *m_multiBodyAllConstraintPtrArray[j]; + const btScalar offDiagA = computeConstraintMatrixOffDiagElementMultiBody(m_tmpSolverBodyPool, m_data, constraint, offDiagConstraint); + + // Set the off-diagonal values of A. Note that A is symmetric. + m_multiBodyA.setElem(i, j, offDiagA); + m_multiBodyA.setElem(j, i, offDiagA); + } + } + } + + // Add CFM to the diagonal of m_A + for (int i = 0; i < m_multiBodyA.rows(); ++i) + { + m_multiBodyA.setElem(i, i, m_multiBodyA(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep); + } + + // 4. Initialize x + { + BT_PROFILE("resize/init x"); + + m_multiBodyX.resize(multiBodyNumConstraints); + + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + for (int i = 0; i < multiBodyNumConstraints; ++i) + { + const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i]; + m_multiBodyX[i] = constraint.m_appliedImpulse; + } + } + else + { + m_multiBodyX.setZero(); + } + } +} + +bool btMultiBodyMLCPConstraintSolver::solveMLCP(const btContactSolverInfo& infoGlobal) +{ + bool result = true; + + if (m_A.rows() != 0) + { + // If using split impulse, we solve 2 separate (M)LCPs + if (infoGlobal.m_splitImpulse) + { + const btMatrixXu Acopy = m_A; + const btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies; + // TODO(JS): Do we really need these copies when solveMLCP takes them as const? + + result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations); + if (result) + result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo, m_hi, limitDependenciesCopy, infoGlobal.m_numIterations); + } + else + { + result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations); + } + } + + if (!result) + return false; + + if (m_multiBodyA.rows() != 0) + { + result = m_solver->solveMLCP(m_multiBodyA, m_multiBodyB, m_multiBodyX, m_multiBodyLo, m_multiBodyHi, m_multiBodyLimitDependencies, infoGlobal.m_numIterations); + } + + return result; +} + +btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlySetup( + btCollisionObject** bodies, + int numBodies, + btPersistentManifold** manifoldPtr, + int numManifolds, + btTypedConstraint** constraints, + int numConstraints, + const btContactSolverInfo& infoGlobal, + btIDebugDraw* debugDrawer) +{ + // 1. Setup for rigid-bodies + btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup( + bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + + // 2. Setup for multi-bodies + // a. Collect all different kinds of constraint as pointers into one array, m_allConstraintPtrArray + // b. Set the index array for frictional contact constraints, m_limitDependencies + { + BT_PROFILE("gather constraint data"); + + int dindex = 0; + + const int numRigidBodyConstraints = m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size(); + const int numMultiBodyConstraints = m_multiBodyNonContactConstraints.size() + m_multiBodyNormalContactConstraints.size() + m_multiBodyFrictionContactConstraints.size(); + + m_allConstraintPtrArray.resize(0); + m_multiBodyAllConstraintPtrArray.resize(0); + + // i. Setup for rigid bodies + + m_limitDependencies.resize(numRigidBodyConstraints); + + for (int i = 0; i < m_tmpSolverNonContactConstraintPool.size(); ++i) + { + m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]); + m_limitDependencies[dindex++] = -1; + } + + int firstContactConstraintOffset = dindex; + + // The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead + if (interleaveContactAndFriction1) + { + for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++) + { + const int numFrictionPerContact = m_tmpSolverContactConstraintPool.size() == m_tmpSolverContactFrictionConstraintPool.size() ? 1 : 2; + + m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]); + m_limitDependencies[dindex++] = -1; + m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact]); + int findex = (m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact].m_frictionIndex * (1 + numFrictionPerContact)); + m_limitDependencies[dindex++] = findex + firstContactConstraintOffset; + if (numFrictionPerContact == 2) + { + m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact + 1]); + m_limitDependencies[dindex++] = findex + firstContactConstraintOffset; + } + } + } + else + { + for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++) + { + m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]); + m_limitDependencies[dindex++] = -1; + } + for (int i = 0; i < m_tmpSolverContactFrictionConstraintPool.size(); i++) + { + m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]); + m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex + firstContactConstraintOffset; + } + } + + if (!m_allConstraintPtrArray.size()) + { + m_A.resize(0, 0); + m_b.resize(0); + m_x.resize(0); + m_lo.resize(0); + m_hi.resize(0); + } + + // ii. Setup for multibodies + + dindex = 0; + + m_multiBodyLimitDependencies.resize(numMultiBodyConstraints); + + for (int i = 0; i < m_multiBodyNonContactConstraints.size(); ++i) + { + m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNonContactConstraints[i]); + m_multiBodyLimitDependencies[dindex++] = -1; + } + + firstContactConstraintOffset = dindex; + + // The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead + if (interleaveContactAndFriction1) + { + for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i) + { + const int numtiBodyNumFrictionPerContact = m_multiBodyNormalContactConstraints.size() == m_multiBodyFrictionContactConstraints.size() ? 1 : 2; + + m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]); + m_multiBodyLimitDependencies[dindex++] = -1; + + btMultiBodySolverConstraint& frictionContactConstraint1 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact]; + m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint1); + + const int findex = (frictionContactConstraint1.m_frictionIndex * (1 + numtiBodyNumFrictionPerContact)) + firstContactConstraintOffset; + + m_multiBodyLimitDependencies[dindex++] = findex; + + if (numtiBodyNumFrictionPerContact == 2) + { + btMultiBodySolverConstraint& frictionContactConstraint2 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact + 1]; + m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint2); + + m_multiBodyLimitDependencies[dindex++] = findex; + } + } + } + else + { + for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i) + { + m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]); + m_multiBodyLimitDependencies[dindex++] = -1; + } + for (int i = 0; i < m_multiBodyFrictionContactConstraints.size(); ++i) + { + m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyFrictionContactConstraints[i]); + m_multiBodyLimitDependencies[dindex++] = m_multiBodyFrictionContactConstraints[i].m_frictionIndex + firstContactConstraintOffset; + } + } + + if (!m_multiBodyAllConstraintPtrArray.size()) + { + m_multiBodyA.resize(0, 0); + m_multiBodyB.resize(0); + m_multiBodyX.resize(0); + m_multiBodyLo.resize(0); + m_multiBodyHi.resize(0); + } + } + + // Construct MLCP terms + { + BT_PROFILE("createMLCPFast"); + createMLCPFast(infoGlobal); + } + + return btScalar(0); +} + +btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) +{ + bool result = true; + { + BT_PROFILE("solveMLCP"); + result = solveMLCP(infoGlobal); + } + + // Fallback to btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations if the solution isn't valid. + if (!result) + { + m_fallback++; + return btMultiBodyConstraintSolver::solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + } + + { + BT_PROFILE("process MLCP results"); + + for (int i = 0; i < m_allConstraintPtrArray.size(); ++i) + { + const btSolverConstraint& c = *m_allConstraintPtrArray[i]; + + const btScalar deltaImpulse = m_x[i] - c.m_appliedImpulse; + c.m_appliedImpulse = m_x[i]; + + int sbA = c.m_solverBodyIdA; + int sbB = c.m_solverBodyIdB; + + btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA]; + btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB]; + + solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); + + if (infoGlobal.m_splitImpulse) + { + const btScalar deltaPushImpulse = m_xSplit[i] - c.m_appliedPushImpulse; + solverBodyA.internalApplyPushImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaPushImpulse); + solverBodyB.internalApplyPushImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaPushImpulse); + c.m_appliedPushImpulse = m_xSplit[i]; + } + } + + for (int i = 0; i < m_multiBodyAllConstraintPtrArray.size(); ++i) + { + btMultiBodySolverConstraint& c = *m_multiBodyAllConstraintPtrArray[i]; + + const btScalar deltaImpulse = m_multiBodyX[i] - c.m_appliedImpulse; + c.m_appliedImpulse = m_multiBodyX[i]; + + btMultiBody* multiBodyA = c.m_multiBodyA; + if (multiBodyA) + { + const int ndofA = multiBodyA->getNumDofs() + 6; + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA); +#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations + //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity + multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse); +#endif // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } + else + { + const int sbA = c.m_solverBodyIdA; + btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA]; + solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + } + + btMultiBody* multiBodyB = c.m_multiBodyB; + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumDofs() + 6; + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse, c.m_deltaVelBindex, ndofB); +#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations + //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity + multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse); +#endif // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } + else + { + const int sbB = c.m_solverBodyIdB; + btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB]; + solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); + } + } + } + + return btScalar(0); +} + +btMultiBodyMLCPConstraintSolver::btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface* solver) + : m_solver(solver), m_fallback(0) +{ + // Do nothing +} + +btMultiBodyMLCPConstraintSolver::~btMultiBodyMLCPConstraintSolver() +{ + // Do nothing +} + +void btMultiBodyMLCPConstraintSolver::setMLCPSolver(btMLCPSolverInterface* solver) +{ + m_solver = solver; +} + +int btMultiBodyMLCPConstraintSolver::getNumFallbacks() const +{ + return m_fallback; +} + +void btMultiBodyMLCPConstraintSolver::setNumFallbacks(int num) +{ + m_fallback = num; +} + +btConstraintSolverType btMultiBodyMLCPConstraintSolver::getSolverType() const +{ + return BT_MLCP_SOLVER; +} diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h new file mode 100644 index 00000000000..77fdb86bb95 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h @@ -0,0 +1,187 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2018 Google Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H +#define BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H + +#include "LinearMath/btMatrixX.h" +#include "LinearMath/btThreads.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" + +class btMLCPSolverInterface; +class btMultiBody; + +class btMultiBodyMLCPConstraintSolver : public btMultiBodyConstraintSolver +{ +protected: + /// \name MLCP Formulation for Rigid Bodies + /// \{ + + /// A matrix in the MLCP formulation + btMatrixXu m_A; + + /// b vector in the MLCP formulation. + btVectorXu m_b; + + /// Constraint impulse, which is an output of MLCP solving. + btVectorXu m_x; + + /// Lower bound of constraint impulse, \c m_x. + btVectorXu m_lo; + + /// Upper bound of constraint impulse, \c m_x. + btVectorXu m_hi; + + /// \} + + /// \name Cache Variables for Split Impulse for Rigid Bodies + /// When using 'split impulse' we solve two separate (M)LCPs + /// \{ + + /// Split impulse Cache vector corresponding to \c m_b. + btVectorXu m_bSplit; + + /// Split impulse cache vector corresponding to \c m_x. + btVectorXu m_xSplit; + + /// \} + + /// \name MLCP Formulation for Multibodies + /// \{ + + /// A matrix in the MLCP formulation + btMatrixXu m_multiBodyA; + + /// b vector in the MLCP formulation. + btVectorXu m_multiBodyB; + + /// Constraint impulse, which is an output of MLCP solving. + btVectorXu m_multiBodyX; + + /// Lower bound of constraint impulse, \c m_x. + btVectorXu m_multiBodyLo; + + /// Upper bound of constraint impulse, \c m_x. + btVectorXu m_multiBodyHi; + + /// \} + + /// Indices of normal contact constraint associated with frictional contact constraint for rigid bodies. + /// + /// This is used by the MLCP solver to update the upper bounds of frictional contact impulse given intermediate + /// normal contact impulse. For example, i-th element represents the index of a normal constraint that is + /// accosiated with i-th frictional contact constraint if i-th constraint is a frictional contact constraint. + /// Otherwise, -1. + btAlignedObjectArray<int> m_limitDependencies; + + /// Indices of normal contact constraint associated with frictional contact constraint for multibodies. + /// + /// This is used by the MLCP solver to update the upper bounds of frictional contact impulse given intermediate + /// normal contact impulse. For example, i-th element represents the index of a normal constraint that is + /// accosiated with i-th frictional contact constraint if i-th constraint is a frictional contact constraint. + /// Otherwise, -1. + btAlignedObjectArray<int> m_multiBodyLimitDependencies; + + /// Array of all the rigid body constraints + btAlignedObjectArray<btSolverConstraint*> m_allConstraintPtrArray; + + /// Array of all the multibody constraints + btAlignedObjectArray<btMultiBodySolverConstraint*> m_multiBodyAllConstraintPtrArray; + + /// MLCP solver + btMLCPSolverInterface* m_solver; + + /// Count of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the MLCP solver fails. + int m_fallback; + + /// \name MLCP Scratch Variables + /// The following scratch variables are not stateful -- contents are cleared prior to each use. + /// They are only cached here to avoid extra memory allocations and deallocations and to ensure + /// that multiple instances of the solver can be run in parallel. + /// + /// \{ + + /// Cache variable for constraint Jacobian matrix. + btMatrixXu m_scratchJ3; + + /// Cache variable for constraint Jacobian times inverse mass matrix. + btMatrixXu m_scratchJInvM3; + + /// Cache variable for offsets. + btAlignedObjectArray<int> m_scratchOfs; + + /// \} + + /// Constructs MLCP terms, which are \c m_A, \c m_b, \c m_lo, and \c m_hi. + virtual void createMLCPFast(const btContactSolverInfo& infoGlobal); + + /// Constructs MLCP terms for constraints of two rigid bodies + void createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal); + + /// Constructs MLCP terms for constraints of two multi-bodies or one rigid body and one multibody + void createMLCPFastMultiBody(const btContactSolverInfo& infoGlobal); + + /// Solves MLCP and returns the success + virtual bool solveMLCP(const btContactSolverInfo& infoGlobal); + + // Documentation inherited + btScalar solveGroupCacheFriendlySetup( + btCollisionObject** bodies, + int numBodies, + btPersistentManifold** manifoldPtr, + int numManifolds, + btTypedConstraint** constraints, + int numConstraints, + const btContactSolverInfo& infoGlobal, + btIDebugDraw* debugDrawer) BT_OVERRIDE; + + // Documentation inherited + btScalar solveGroupCacheFriendlyIterations( + btCollisionObject** bodies, + int numBodies, + btPersistentManifold** manifoldPtr, + int numManifolds, + btTypedConstraint** constraints, + int numConstraints, + const btContactSolverInfo& infoGlobal, + btIDebugDraw* debugDrawer) ; + +public: + BT_DECLARE_ALIGNED_ALLOCATOR() + + /// Constructor + /// + /// \param[in] solver MLCP solver. Assumed it's not null. + /// \param[in] maxLCPSize Maximum size of LCP to solve using MLCP solver. If the MLCP size exceeds this number, sequaltial impulse method will be used. + explicit btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface* solver); + + /// Destructor + virtual ~btMultiBodyMLCPConstraintSolver(); + + /// Sets MLCP solver. Assumed it's not null. + void setMLCPSolver(btMLCPSolverInterface* solver); + + /// Returns the number of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the MLCP + /// solver fails. + int getNumFallbacks() const; + + /// Sets the number of fallbacks. This function may be used to reset the number to zero. + void setNumFallbacks(int num); + + /// Returns the constraint solver type. + virtual btConstraintSolverType getSolverType() const; +}; + +#endif // BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp index 12b21f74603..f51e69deb1c 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp @@ -21,29 +21,29 @@ subject to the following restrictions: #include "LinearMath/btIDebugDraw.h" #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST - #define BTMBP2PCONSTRAINT_DIM 3 +#define BTMBP2PCONSTRAINT_DIM 3 #else - #define BTMBP2PCONSTRAINT_DIM 6 +#define BTMBP2PCONSTRAINT_DIM 6 #endif btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB) - :btMultiBodyConstraint(body,0,link,-1,BTMBP2PCONSTRAINT_DIM,false), - m_rigidBodyA(0), - m_rigidBodyB(bodyB), - m_pivotInA(pivotInA), - m_pivotInB(pivotInB) + : btMultiBodyConstraint(body, 0, link, -1, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT), + m_rigidBodyA(0), + m_rigidBodyB(bodyB), + m_pivotInA(pivotInA), + m_pivotInB(pivotInB) { - m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses + m_data.resize(BTMBP2PCONSTRAINT_DIM); //at least store the applied impulses } btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB) - :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBP2PCONSTRAINT_DIM,false), - m_rigidBodyA(0), - m_rigidBodyB(0), - m_pivotInA(pivotInA), - m_pivotInB(pivotInB) + : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT), + m_rigidBodyA(0), + m_rigidBodyB(0), + m_pivotInA(pivotInA), + m_pivotInB(pivotInB) { - m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses + m_data.resize(BTMBP2PCONSTRAINT_DIM); //at least store the applied impulses } void btMultiBodyPoint2Point::finalizeMultiDof() @@ -56,7 +56,6 @@ btMultiBodyPoint2Point::~btMultiBodyPoint2Point() { } - int btMultiBodyPoint2Point::getIslandIdA() const { if (m_rigidBodyA) @@ -64,13 +63,16 @@ int btMultiBodyPoint2Point::getIslandIdA() const if (m_bodyA) { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;i<m_bodyA->getNumLinks();i++) + if (m_linkA < 0) + { + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + } + else { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); + if (m_bodyA->getLink(m_linkA).m_collider) + return m_bodyA->getLink(m_linkA).m_collider->getIslandTag(); } } return -1; @@ -82,62 +84,58 @@ int btMultiBodyPoint2Point::getIslandIdB() const return m_rigidBodyB->getIslandTag(); if (m_bodyB) { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - - for (int i=0;i<m_bodyB->getNumLinks();i++) + if (m_linkB < 0) { - col = m_bodyB->getLink(i).m_collider; + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); if (col) return col->getIslandTag(); } + else + { + if (m_bodyB->getLink(m_linkB).m_collider) + return m_bodyB->getLink(m_linkB).m_collider->getIslandTag(); + } } return -1; } - - void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal) + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal) { - -// int i=1; -int numDim = BTMBP2PCONSTRAINT_DIM; - for (int i=0;i<numDim;i++) + // int i=1; + int numDim = BTMBP2PCONSTRAINT_DIM; + for (int i = 0; i < numDim; i++) { - btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); - //memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint)); - constraintRow.m_orgConstraint = this; - constraintRow.m_orgDofIndex = i; - constraintRow.m_relpos1CrossNormal.setValue(0,0,0); - constraintRow.m_contactNormal1.setValue(0,0,0); - constraintRow.m_relpos2CrossNormal.setValue(0,0,0); - constraintRow.m_contactNormal2.setValue(0,0,0); - constraintRow.m_angularComponentA.setValue(0,0,0); - constraintRow.m_angularComponentB.setValue(0,0,0); + //memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint)); + constraintRow.m_orgConstraint = this; + constraintRow.m_orgDofIndex = i; + constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0); + constraintRow.m_contactNormal1.setValue(0, 0, 0); + constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0); + constraintRow.m_contactNormal2.setValue(0, 0, 0); + constraintRow.m_angularComponentA.setValue(0, 0, 0); + constraintRow.m_angularComponentB.setValue(0, 0, 0); constraintRow.m_solverBodyIdA = data.m_fixedBodyId; constraintRow.m_solverBodyIdB = data.m_fixedBodyId; - btVector3 contactNormalOnB(0,0,0); + btVector3 contactNormalOnB(0, 0, 0); #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST contactNormalOnB[i] = -1; #else - contactNormalOnB[i%3] = -1; + contactNormalOnB[i % 3] = -1; #endif - - // Convert local points back to world + // Convert local points back to world btVector3 pivotAworld = m_pivotInA; if (m_rigidBodyA) { - constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId(); - pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA; - } else + pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA; + } + else { if (m_bodyA) pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); @@ -146,44 +144,41 @@ int numDim = BTMBP2PCONSTRAINT_DIM; if (m_rigidBodyB) { constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId(); - pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB; - } else + pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB; + } + else { if (m_bodyB) pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); - } - btScalar posError = i < 3 ? (pivotAworld-pivotBworld).dot(contactNormalOnB) : 0; + btScalar posError = i < 3 ? (pivotAworld - pivotBworld).dot(contactNormalOnB) : 0; #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST - - fillMultiBodyConstraint(constraintRow, data, 0, 0, - contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being" - posError, - infoGlobal, - -m_maxAppliedImpulse, m_maxAppliedImpulse - ); - //@todo: support the case of btMultiBody versus btRigidBody, - //see btPoint2PointConstraint::getInfo2NonVirtual + fillMultiBodyConstraint(constraintRow, data, 0, 0, btVector3(0, 0, 0), + contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being" + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse); + //@todo: support the case of btMultiBody versus btRigidBody, + //see btPoint2PointConstraint::getInfo2NonVirtual #else const btVector3 dummy(0, 0, 0); btAssert(m_bodyA->isMultiDof()); btScalar* jac1 = jacobianA(i); - const btVector3 &normalAng = i >= 3 ? contactNormalOnB : dummy; - const btVector3 &normalLin = i < 3 ? contactNormalOnB : dummy; + const btVector3& normalAng = i >= 3 ? contactNormalOnB : dummy; + const btVector3& normalLin = i < 3 ? contactNormalOnB : dummy; m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m); fillMultiBodyConstraint(constraintRow, data, jac1, 0, - dummy, dummy, dummy, //sucks but let it be this way "for the time being" - posError, - infoGlobal, - -m_maxAppliedImpulse, m_maxAppliedImpulse - ); + dummy, dummy, dummy, //sucks but let it be this way "for the time being" + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse); #endif } } diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h index b2e219ac159..ef03a557ec5 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h @@ -22,20 +22,20 @@ subject to the following restrictions: //#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST -class btMultiBodyPoint2Point : public btMultiBodyConstraint +ATTRIBUTE_ALIGNED16(class) +btMultiBodyPoint2Point : public btMultiBodyConstraint { protected: - - btRigidBody* m_rigidBodyA; - btRigidBody* m_rigidBodyB; - btVector3 m_pivotInA; - btVector3 m_pivotInB; - + btRigidBody* m_rigidBodyA; + btRigidBody* m_rigidBodyB; + btVector3 m_pivotInA; + btVector3 m_pivotInB; public: + BT_DECLARE_ALIGNED_ALLOCATOR(); - btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB); - btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB); + btMultiBodyPoint2Point(btMultiBody * body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB); + btMultiBodyPoint2Point(btMultiBody * bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB); virtual ~btMultiBodyPoint2Point(); @@ -44,22 +44,21 @@ public: virtual int getIslandIdA() const; virtual int getIslandIdB() const; - virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal); + virtual void createConstraintRows(btMultiBodyConstraintArray & constraintRows, + btMultiBodyJacobianData & data, + const btContactSolverInfo& infoGlobal); const btVector3& getPivotInB() const { return m_pivotInB; } - void setPivotInB(const btVector3& pivotInB) + virtual void setPivotInB(const btVector3& pivotInB) { m_pivotInB = pivotInB; } - virtual void debugDraw(class btIDebugDraw* drawer); - + virtual void debugDraw(class btIDebugDraw * drawer); }; -#endif //BT_MULTIBODY_POINT2POINT_H +#endif //BT_MULTIBODY_POINT2POINT_H diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp new file mode 100644 index 00000000000..48ec1d5af2b --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp @@ -0,0 +1,234 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#include "btMultiBodySliderConstraint.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" +#include "LinearMath/btIDebugDraw.h" + +#define BTMBSLIDERCONSTRAINT_DIM 5 +#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, MULTIBODY_CONSTRAINT_SLIDER), + m_rigidBodyA(0), + m_rigidBodyB(bodyB), + m_pivotInA(pivotInA), + m_pivotInB(pivotInB), + m_frameInA(frameInA), + m_frameInB(frameInB), + m_jointAxis(jointAxis) +{ + m_data.resize(BTMBSLIDERCONSTRAINT_DIM); //at least store the applied impulses +} + +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, MULTIBODY_CONSTRAINT_SLIDER), + m_rigidBodyA(0), + m_rigidBodyB(0), + m_pivotInA(pivotInA), + m_pivotInB(pivotInB), + m_frameInA(frameInA), + m_frameInB(frameInB), + m_jointAxis(jointAxis) +{ + m_data.resize(BTMBSLIDERCONSTRAINT_DIM); //at least store the applied impulses +} + +void btMultiBodySliderConstraint::finalizeMultiDof() +{ + //not implemented yet + btAssert(0); +} + +btMultiBodySliderConstraint::~btMultiBodySliderConstraint() +{ +} + +int btMultiBodySliderConstraint::getIslandIdA() const +{ + if (m_rigidBodyA) + return m_rigidBodyA->getIslandTag(); + + if (m_bodyA) + { + if (m_linkA < 0) + { + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + } + else + { + if (m_bodyA->getLink(m_linkA).m_collider) + return m_bodyA->getLink(m_linkA).m_collider->getIslandTag(); + } + } + return -1; +} + +int btMultiBodySliderConstraint::getIslandIdB() const +{ + if (m_rigidBodyB) + return m_rigidBodyB->getIslandTag(); + if (m_bodyB) + { + if (m_linkB < 0) + { + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); + if (col) + return col->getIslandTag(); + } + else + { + if (m_bodyB->getLink(m_linkB).m_collider) + return m_bodyB->getLink(m_linkB).m_collider->getIslandTag(); + } + } + return -1; +} +void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal) +{ + // Convert local points back to world + btVector3 pivotAworld = m_pivotInA; + btMatrix3x3 frameAworld = m_frameInA; + btVector3 jointAxis = m_jointAxis; + if (m_rigidBodyA) + { + pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA; + frameAworld = m_frameInA.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation()); + jointAxis = quatRotate(m_rigidBodyA->getOrientation(), m_jointAxis); + } + else if (m_bodyA) + { + pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); + frameAworld = m_bodyA->localFrameToWorld(m_linkA, m_frameInA); + jointAxis = m_bodyA->localDirToWorld(m_linkA, m_jointAxis); + } + btVector3 pivotBworld = m_pivotInB; + btMatrix3x3 frameBworld = m_frameInB; + if (m_rigidBodyB) + { + pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB; + frameBworld = m_frameInB.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation()); + } + else if (m_bodyB) + { + pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); + frameBworld = m_bodyB->localFrameToWorld(m_linkB, m_frameInB); + } + + btVector3 constraintAxis[2]; + for (int i = 0; i < 3; ++i) + { + constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis); + if (constraintAxis[0].safeNorm() > EPSILON) + { + constraintAxis[0] = constraintAxis[0].normalized(); + constraintAxis[1] = jointAxis.cross(constraintAxis[0]); + constraintAxis[1] = constraintAxis[1].normalized(); + break; + } + } + + btMatrix3x3 relRot = frameAworld.inverse() * frameBworld; + btVector3 angleDiff; + btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff); + + int numDim = BTMBSLIDERCONSTRAINT_DIM; + for (int i = 0; i < numDim; i++) + { + btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); + constraintRow.m_orgConstraint = this; + constraintRow.m_orgDofIndex = i; + constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0); + constraintRow.m_contactNormal1.setValue(0, 0, 0); + constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0); + constraintRow.m_contactNormal2.setValue(0, 0, 0); + constraintRow.m_angularComponentA.setValue(0, 0, 0); + constraintRow.m_angularComponentB.setValue(0, 0, 0); + + constraintRow.m_solverBodyIdA = data.m_fixedBodyId; + constraintRow.m_solverBodyIdB = data.m_fixedBodyId; + + if (m_rigidBodyA) + { + constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId(); + } + if (m_rigidBodyB) + { + constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId(); + } + + btVector3 constraintNormalLin(0, 0, 0); + btVector3 constraintNormalAng(0, 0, 0); + btScalar posError = 0.0; + if (i < 2) + { + constraintNormalLin = constraintAxis[i]; + posError = (pivotAworld - pivotBworld).dot(constraintNormalLin); + fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, + constraintNormalLin, pivotAworld, pivotBworld, + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse); + } + else + { //i>=2 + constraintNormalAng = frameAworld.getColumn(i % 3); + posError = angleDiff[i % 3]; + fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, + constraintNormalLin, pivotAworld, pivotBworld, + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse, true); + } + } +} + +void btMultiBodySliderConstraint::debugDraw(class btIDebugDraw* drawer) +{ + btTransform tr; + tr.setIdentity(); + + if (m_rigidBodyA) + { + btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA; + tr.setOrigin(pivot); + drawer->drawTransform(tr, 0.1); + } + if (m_bodyA) + { + btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); + tr.setOrigin(pivotAworld); + drawer->drawTransform(tr, 0.1); + } + if (m_rigidBodyB) + { + // that ideally should draw the same frame + btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB; + tr.setOrigin(pivot); + drawer->drawTransform(tr, 0.1); + } + if (m_bodyB) + { + btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); + tr.setOrigin(pivotBworld); + drawer->drawTransform(tr, 0.1); + } +} diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h new file mode 100644 index 00000000000..b192b6f8f37 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h @@ -0,0 +1,102 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#ifndef BT_MULTIBODY_SLIDER_CONSTRAINT_H +#define BT_MULTIBODY_SLIDER_CONSTRAINT_H + +#include "btMultiBodyConstraint.h" + +class btMultiBodySliderConstraint : public btMultiBodyConstraint +{ +protected: + btRigidBody* m_rigidBodyA; + btRigidBody* m_rigidBodyB; + btVector3 m_pivotInA; + btVector3 m_pivotInB; + btMatrix3x3 m_frameInA; + btMatrix3x3 m_frameInB; + btVector3 m_jointAxis; + +public: + btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis); + btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis); + + virtual ~btMultiBodySliderConstraint(); + + virtual void finalizeMultiDof(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + const btVector3& getPivotInA() const + { + return m_pivotInA; + } + + void setPivotInA(const btVector3& pivotInA) + { + m_pivotInA = pivotInA; + } + + const btVector3& getPivotInB() const + { + return m_pivotInB; + } + + virtual void setPivotInB(const btVector3& pivotInB) + { + m_pivotInB = pivotInB; + } + + const btMatrix3x3& getFrameInA() const + { + return m_frameInA; + } + + void setFrameInA(const btMatrix3x3& frameInA) + { + m_frameInA = frameInA; + } + + const btMatrix3x3& getFrameInB() const + { + return m_frameInB; + } + + virtual void setFrameInB(const btMatrix3x3& frameInB) + { + m_frameInB = frameInB; + } + + const btVector3& getJointAxis() const + { + return m_jointAxis; + } + + void setJointAxis(const btVector3& jointAxis) + { + m_jointAxis = jointAxis; + } + + virtual void debugDraw(class btIDebugDraw* drawer); +}; + +#endif //BT_MULTIBODY_SLIDER_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h index 6fa1550e9e6..deed3e2a123 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h @@ -25,66 +25,66 @@ class btMultiBodyConstraint; #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. -ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint +ATTRIBUTE_ALIGNED16(struct) +btMultiBodySolverConstraint { BT_DECLARE_ALIGNED_ALLOCATOR(); - btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1),m_orgConstraint(0), m_orgDofIndex(-1) - {} - - int m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1 - int m_jacAindex; - int m_deltaVelBindex; - int m_jacBindex; - - btVector3 m_relpos1CrossNormal; - btVector3 m_contactNormal1; - btVector3 m_relpos2CrossNormal; - btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always - - - btVector3 m_angularComponentA; - btVector3 m_angularComponentB; - - mutable btSimdScalar m_appliedPushImpulse; - mutable btSimdScalar m_appliedImpulse; - - btScalar m_friction; - btScalar m_jacDiagABInv; - btScalar m_rhs; - btScalar m_cfm; - - btScalar m_lowerLimit; - btScalar m_upperLimit; - btScalar m_rhsPenetration; - union + btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1), m_orgConstraint(0), m_orgDofIndex(-1) { - void* m_originalContactPoint; - btScalar m_unusedPadding4; + } + + int m_deltaVelAindex; //more generic version of m_relpos1CrossNormal/m_contactNormal1 + int m_jacAindex; + int m_deltaVelBindex; + int m_jacBindex; + + btVector3 m_relpos1CrossNormal; + btVector3 m_contactNormal1; + btVector3 m_relpos2CrossNormal; + btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always + + btVector3 m_angularComponentA; + btVector3 m_angularComponentB; + + mutable btSimdScalar m_appliedPushImpulse; + mutable btSimdScalar m_appliedImpulse; + + btScalar m_friction; + btScalar m_jacDiagABInv; + btScalar m_rhs; + btScalar m_cfm; + + btScalar m_lowerLimit; + btScalar m_upperLimit; + btScalar m_rhsPenetration; + union { + void* m_originalContactPoint; + btScalar m_unusedPadding4; }; - int m_overrideNumSolverIterations; - int m_frictionIndex; + int m_overrideNumSolverIterations; + int m_frictionIndex; int m_solverBodyIdA; btMultiBody* m_multiBodyA; - int m_linkA; - + int m_linkA; + int m_solverBodyIdB; btMultiBody* m_multiBodyB; - int m_linkB; + int m_linkB; //for writing back applied impulses - btMultiBodyConstraint* m_orgConstraint; + btMultiBodyConstraint* m_orgConstraint; int m_orgDofIndex; - enum btSolverConstraintType + enum btSolverConstraintType { BT_SOLVER_CONTACT_1D = 0, BT_SOLVER_FRICTION_1D }; }; -typedef btAlignedObjectArray<btMultiBodySolverConstraint> btMultiBodyConstraintArray; +typedef btAlignedObjectArray<btMultiBodySolverConstraint> btMultiBodyConstraintArray; -#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H +#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp new file mode 100644 index 00000000000..25ddd539bf9 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp @@ -0,0 +1,172 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2018 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#include "btMultiBodySphericalJointMotor.h" +#include "btMultiBody.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "LinearMath/btTransformUtil.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" + +btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse) + : 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_erp(1), + m_rhsClamp(SIMD_INFINITY) +{ + + m_maxAppliedImpulse = maxMotorImpulse; +} + + +void btMultiBodySphericalJointMotor::finalizeMultiDof() +{ + allocateJacobiansMultiDof(); + // note: we rely on the fact that data.m_jacobians are + // always initialized to zero by the Constraint ctor + int linkDoF = 0; + unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF); + + // row 0: the lower bound + // row 0: the lower bound + jacobianA(0)[offset] = 1; + + m_numDofsFinalized = m_jacSizeBoth; +} + + +btMultiBodySphericalJointMotor::~btMultiBodySphericalJointMotor() +{ +} + +int btMultiBodySphericalJointMotor::getIslandIdA() const +{ + if (this->m_linkA < 0) + { + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + } + else + { + if (m_bodyA->getLink(m_linkA).m_collider) + { + return m_bodyA->getLink(m_linkA).m_collider->getIslandTag(); + } + } + return -1; +} + +int btMultiBodySphericalJointMotor::getIslandIdB() const +{ + if (m_linkB < 0) + { + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); + if (col) + return col->getIslandTag(); + } + else + { + if (m_bodyB->getLink(m_linkB).m_collider) + { + return m_bodyB->getLink(m_linkB).m_collider->getIslandTag(); + } + } + return -1; +} + +void btMultiBodySphericalJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal) +{ + // only positions need to be updated -- data.m_jacobians and force + // directions were set in the ctor and never change. + + if (m_numDofsFinalized != m_jacSizeBoth) + { + finalizeMultiDof(); + } + + //don't crash + if (m_numDofsFinalized != m_jacSizeBoth) + return; + + + if (m_maxAppliedImpulse == 0.f) + return; + + const btScalar posError = 0; + const btVector3 dummy(0, 0, 0); + + + btVector3 axis[3] = { btVector3(1, 0, 0), btVector3(0, 1, 0), btVector3(0, 0, 1) }; + + btQuaternion desiredQuat = m_desiredPosition; + btQuaternion currentQuat(m_bodyA->getJointPosMultiDof(m_linkA)[0], + m_bodyA->getJointPosMultiDof(m_linkA)[1], + m_bodyA->getJointPosMultiDof(m_linkA)[2], + m_bodyA->getJointPosMultiDof(m_linkA)[3]); + +btQuaternion relRot = currentQuat.inverse() * desiredQuat; + btVector3 angleDiff; + btGeneric6DofSpring2Constraint::matrixToEulerXYZ(btMatrix3x3(relRot), angleDiff); + + + + for (int row = 0; row < getNumRows(); row++) + { + btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); + + int dof = row; + + btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; + btScalar desiredVelocity = this->m_desiredVelocity[row]; + + btScalar velocityError = desiredVelocity - currentVelocity; + + btMatrix3x3 frameAworld; + frameAworld.setIdentity(); + frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld); + btScalar posError = 0; + { + btAssert(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eSpherical); + switch (m_bodyA->getLink(m_linkA).m_jointType) + { + case btMultibodyLink::eSpherical: + { + btVector3 constraintNormalAng = frameAworld.getColumn(row % 3); + posError = m_kp*angleDiff[row % 3]; + fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, + btVector3(0,0,0), dummy, dummy, + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse, true); + constraintRow.m_orgConstraint = this; + constraintRow.m_orgDofIndex = row; + break; + } + default: + { + btAssert(0); + } + }; + } + } +} diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h new file mode 100644 index 00000000000..621beab5a42 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h @@ -0,0 +1,77 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2018 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#ifndef BT_MULTIBODY_SPHERICAL_JOINT_MOTOR_H +#define BT_MULTIBODY_SPHERICAL_JOINT_MOTOR_H + +#include "btMultiBodyConstraint.h" +struct btSolverInfo; + +class btMultiBodySphericalJointMotor : public btMultiBodyConstraint +{ +protected: + btVector3 m_desiredVelocity; + btQuaternion m_desiredPosition; + btScalar m_kd; + btScalar m_kp; + btScalar m_erp; + btScalar m_rhsClamp; //maximum error + +public: + btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse); + + virtual ~btMultiBodySphericalJointMotor(); + virtual void finalizeMultiDof(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + virtual void setVelocityTarget(const btVector3& velTarget, btScalar kd = 1.f) + { + m_desiredVelocity = velTarget; + m_kd = kd; + } + + virtual void setPositionTarget(const btQuaternion& posTarget, btScalar kp = 1.f) + { + m_desiredPosition = posTarget; + m_kp = kp; + } + + virtual void setErp(btScalar erp) + { + m_erp = erp; + } + virtual btScalar getErp() const + { + return m_erp; + } + virtual void setRhsClamp(btScalar rhsClamp) + { + m_rhsClamp = rhsClamp; + } + virtual void debugDraw(class btIDebugDraw* drawer) + { + //todo(erwincoumans) + } +}; + +#endif //BT_MULTIBODY_SPHERICAL_JOINT_MOTOR_H |