diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/Featherstone')
26 files changed, 4057 insertions, 3973 deletions
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp index 0e85b55728..53fc48d4b9 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp @@ -21,7 +21,6 @@ */ - #include "btMultiBody.h" #include "btMultiBodyLink.h" #include "btMultiBodyLinkCollider.h" @@ -29,28 +28,29 @@ #include "LinearMath/btTransformUtil.h" #include "LinearMath/btSerializer.h" //#include "Bullet3Common/b3Logging.h" -// #define INCLUDE_GYRO_TERM +// #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, @@ -83,60 +83,58 @@ namespace { 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_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; + 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_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(); @@ -147,131 +145,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].setAxisTop(0, 0., 0., 0.); - m_links[i].setAxisBottom(0, btVector3(0,0,0)); - 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; @@ -282,281 +274,297 @@ 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; // btVector3 vecNonParallelToRotAxis(1, 0, 0); - if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999) + 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) - for (int i=0;i<m_vectorBuf.size();i++) + 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) + for (int i = 0; i < m_vectorBuf.size(); i++) { - m_vectorBuf[i].setValue(0,0,0); + m_vectorBuf[i].setValue(0, 0, 0); } updateLinksDofOffsets(); } - + int btMultiBody::getParent(int i) const { - return m_links[i].m_parent; + return m_links[i].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, btScalar *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] = 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 +void btMultiBody::setJointVelMultiDof(int i, const float* qdot) { - return m_links[i].m_cachedRVector; + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) + m_realBuf[6 + m_links[i].m_dofOffset + dof] = (btScalar)qdot[dof]; } -const btQuaternion & btMultiBody::getParentToLocalRot(int i) const +const btVector3 &btMultiBody::getRVector(int i) const { - return m_links[i].m_cachedRotParentToThis; + return m_links[i].m_cachedRVector; +} + +const btQuaternion &btMultiBody::getParentToLocalRot(int i) const +{ + return m_links[i].m_cachedRotParentToThis; } btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const { - btAssert(i>=-1); - btAssert(i<m_links.size()); - if ((i<-1) || (i>=m_links.size())) + btAssert(i >= -1); + btAssert(i < m_links.size()); + if ((i < -1) || (i >= m_links.size())) { - return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY); + 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); - } + 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 { - btAssert(i>=-1); - btAssert(i<m_links.size()); - if ((i<-1) || (i>=m_links.size())) + btAssert(i >= -1); + btAssert(i < m_links.size()); + if ((i < -1) || (i >= m_links.size())) { - return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY); + 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); - } + 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 { - btAssert(i>=-1); - btAssert(i<m_links.size()); - if ((i<-1) || (i>=m_links.size())) + btAssert(i >= -1); + btAssert(i < m_links.size()); + if ((i < -1) || (i >= m_links.size())) { - return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY); + 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 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 { - btAssert(i>=-1); - btAssert(i<m_links.size()); - if ((i<-1) || (i>=m_links.size())) + btAssert(i >= -1); + btAssert(i < m_links.size()); + if ((i < -1) || (i >= m_links.size())) { - return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY); + 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)); - } + if (i == -1) + { + return quatRotate(getWorldToBaseRot(), world_dir); + } + else + { + return quatRotate(getParentToLocalRot(i), worldDirToLocal(getParent(i), world_dir)); + } } btMatrix3x3 btMultiBody::localFrameToWorld(int i, const btMatrix3x3 &local_frame) const { - 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; + 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; } void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) 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) + // 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()); + + for (int i = 0; i < num_links; ++i) { - const int parent = m_links[i].m_parent; + const btMultibodyLink& link = getLink(i); + const int parent = link.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]); + // 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]); - // now add qidot * shat_i - //only supported for revolute/prismatic joints, todo: spherical and planar joints - switch(m_links[i].m_jointType) + // now add qidot * shat_i + const btScalar* jointVel = getJointVelMultiDof(i); + for (int dof = 0; dof < link.m_dofCount; ++dof) { - case btMultibodyLink::ePrismatic: - case btMultibodyLink::eRevolute: - { - btVector3 axisTop = m_links[i].getAxisTop(0); - btVector3 axisBottom = m_links[i].getAxisBottom(0); - btScalar jointVel = getJointVel(i); - omega[i+1] += jointVel * axisTop; - vel[i+1] += jointVel * axisBottom; - break; - } - default: - { - } + omega[i + 1] += jointVel[dof] * link.getAxisTop(dof); + vel[i + 1] += jointVel[dof] * link.getAxisBottom(dof); } } } @@ -564,41 +572,48 @@ void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const btScalar btMultiBody::getKineticEnergy() 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]); - } + // 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]); + } - return 0.5f * result; + return 0.5f * result; } 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]))); - } + // 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]))); + } - return result; + return result; } void btMultiBody::clearConstraintForces() @@ -606,57 +621,55 @@ 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 + getNumDofs(); ++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) @@ -666,70 +679,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? -{ - 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; +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; } #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) @@ -738,96 +753,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; 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 + 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); + 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]; - - // 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; - v_ptr += num_links * 2 + 2; + // 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]; + + // 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 - 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 + //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. + + // First 'upward' loop. + // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. - rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!? + 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 { - 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)); + 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) 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())); + 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 @@ -835,67 +850,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 { @@ -913,27 +927,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().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 + 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(); @@ -944,52 +957,49 @@ 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) + for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++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: { - if (D[0]>=SIMD_EPSILON) + if (D[0] >= SIMD_EPSILON) { invDi[0] = 1.0f / D[0]; - } else + } + else { invDi[0] = 0; } @@ -1002,10 +1012,10 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar 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]; } } @@ -1014,86 +1024,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) - { + 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) - { + 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) - { + 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 @@ -1101,73 +1107,73 @@ 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) { 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); } - 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. + // 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]; @@ -1196,26 +1202,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.) @@ -1232,77 +1237,77 @@ 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, btScalar 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 - - 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; + + 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 + else { if (!m_cachedInertiaValid) { - for (int i=0;i<6;i++) + for (int i = 0; i < 6; i++) { result[i] = 0.f; } @@ -1310,94 +1315,95 @@ void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bo } /// 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 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 + { + 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 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)); + 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 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 @@ -1416,155 +1422,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 - 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; + 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]; + } - // Second 'upward' loop - // (part of TreeForwardDynamics in Mirtich) + // ptr to the joint accel part of the output + btScalar *joint_accel = output + 6; - if (m_fixedBase) + // Second 'upward' loop + // (part of TreeForwardDynamics in Mirtich) + + 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]; @@ -1577,19 +1580,16 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar ///////////////// } - - - 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 *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]; @@ -1599,92 +1599,98 @@ 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) // - btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); - 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 = (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]; + btScalar jointVel = pJointVel[0]; pJointPos[0] += dt * jointVel; break; } case btMultibodyLink::eSpherical: { - btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); - btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); + 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: @@ -1701,122 +1707,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; } @@ -1824,269 +1843,260 @@ 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_sleepTimer = 0; - m_awake = true; + 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 (motion < SLEEP_EPSILON) + { + m_sleepTimer += timestep; + if (m_sleepTimer > SLEEP_TIMEOUT) + { + goToSleep(); + } + } + else + { + m_sleepTimer = 0; 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); - } - - 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); } } } -int btMultiBody::calculateSerializeBufferSize() const +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; - 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); + 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); - - 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_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++) - { - memPtr->m_jointPos[posvar] = getLink(i).m_jointPos[posvar]; - } - - + { + char *name = (char *)serializer->findNameForPointer(m_links[i].m_linkName); + memPtr->m_linkName = (char *)serializer->getUniquePointer(name); + if (memPtr->m_linkName) { - 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) { - char* name = (char*) serializer->findNameForPointer(m_links[i].m_jointName); - memPtr->m_jointName = (char*)serializer->getUniquePointer(name); - if (memPtr->m_jointName) - { - serializer->serializeName(name); - } + 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. + // Fill padding with zeros to appease msan. #ifdef BT_USE_DOUBLE_PRECISION - memset(mbd->m_padding, 0, sizeof(mbd->m_padding)); + memset(mbd->m_padding, 0, sizeof(mbd->m_padding)); #endif - return btMultiBodyDataName; + return btMultiBodyDataName; } diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h index 5cd00e5173..e5c0f1806b 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h @@ -21,7 +21,6 @@ */ - #ifndef BT_MULTIBODY_H #define BT_MULTIBODY_H @@ -31,116 +30,111 @@ #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); - - + 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 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 { 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; } - btMultiBodyLinkCollider* getLinkCollider(int index) + const btMultiBodyLinkCollider *getLinkCollider(int index) const { if (index >= 0 && index < getNumLinks()) { @@ -149,61 +143,65 @@ public: return 0; } - // - // 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 - // + btMultiBodyLinkCollider *getLinkCollider(int index) + { + if (index >= 0 && index < getNumLinks()) + { + return getLink(index).m_collider; + } + return 0; + } + + // + // 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 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; - - + 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) - // + // + // 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; } + 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 - { - 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 + 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 + { + 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 - void setBasePos(const btVector3 &pos) - { - m_basePos = pos; + void setBasePos(const btVector3 &pos) + { + m_basePos = pos; } - void setBaseWorldTransform(const btTransform& tr) + void setBaseWorldTransform(const btTransform &tr) { setBasePos(tr.getOrigin()); setWorldToBaseRot(tr.getRotation().inverse()); - } btTransform getBaseWorldTransform() const @@ -214,190 +212,186 @@ 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!? } - 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); - - const btScalar * getJointVelMultiDof(int i) const ; - const btScalar * getJointPosMultiDof(int i) const ; + btScalar getJointPos(int i) const; + btScalar getJointVel(int i) const; - void setJointPos(int i, btScalar q); - void setJointVel(int i, btScalar qdot); - void setJointPosMultiDof(int i, btScalar *q); - void setJointVelMultiDof(int i, btScalar *qdot); + btScalar *getJointVelMultiDof(int i); + btScalar *getJointPosMultiDof(int i); + 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() + /* 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. + // + // 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 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 &mat) const; - // - // transform a frame in local coordinate to a frame in world coordinate - // - btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const; + // + // calculate kinetic energy and angular momentum + // useful for debugging. + // - // - // calculate kinetic energy and angular momentum - // useful for debugging. - // + btScalar getKineticEnergy() const; + btVector3 getAngularMomentum() const; - 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 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 addJointTorque(int i, btScalar Q); + 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 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 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). - // - - + 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); - } - - // 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. + 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 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); @@ -418,65 +412,61 @@ 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); + // + // 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; - - - // - // sleeping - // - void setCanSleep(bool canSleep) + 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) { m_canSleep = canSleep; } - bool getCanSleep()const + bool getCanSleep() const { return m_canSleep; } - 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; } int getCompanionId() const @@ -489,16 +479,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; } @@ -506,11 +496,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; @@ -519,24 +509,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; } @@ -545,7 +535,6 @@ void addJointTorque(int i, btScalar Q); return m_hasSelfCollision; } - void finalizeMultiDof(); void useRK4Integration(bool use) { m_useRK4 = use; } @@ -561,126 +550,132 @@ 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> & scratch_q, btAlignedObjectArray<btVector3> & scratch_m); + + void compTreeLinkVelocities(btVector3 * omega, btVector3 * vel) const; - void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const; + void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> & scratch_q, btAlignedObjectArray<btVector3> & scratch_m); - void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m); - - virtual int calculateSerializeBufferSize() const; + 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; } ///users can point to their objects, userPointer is not used by Bullet - void* getUserPointer() const + void *getUserPointer() const { return m_userObjectPointer; } - int getUserIndex() const + int getUserIndex() const { return m_userIndex; } - int getUserIndex2() const + int getUserIndex2() const { return m_userIndex2; } ///users can point to their objects, userPointer is not used by Bullet - void setUserPointer(void* userPointer) + void setUserPointer(void *userPointer) { m_userObjectPointer = userPointer; } ///users can point to their objects, userPointer is not used by Bullet - void setUserIndex(int index) + void setUserIndex(int index) { m_userIndex = index; } - void setUserIndex2(int index) + void setUserIndex2(int index) { m_userIndex2 = index; } -private: - btMultiBody(const btMultiBody &); // not implemented - void operator=(const btMultiBody &); // not implemented + 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 + - void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, btScalar result[6]) const; +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) + 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; + 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) - btQuaternion m_baseQuat; // rotates world points into base frame - - 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_deltaV; - btAlignedObjectArray<btScalar> m_realBuf; - btAlignedObjectArray<btVector3> m_vectorBuf; - btAlignedObjectArray<btMatrix3x3> m_matrixBuf; + 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) + btQuaternion m_baseQuat; // rotates world points into base frame + + 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_deltaV; + btAlignedObjectArray<btScalar> m_realBuf; + btAlignedObjectArray<btVector3> m_vectorBuf; + btAlignedObjectArray<btMatrix3x3> m_matrixBuf; btMatrix3x3 m_cachedInertiaTopLeft; btMatrix3x3 m_cachedInertiaTopRight; @@ -688,25 +683,25 @@ private: 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; + btScalar m_sleepTimer; - void* m_userObjectPointer; + 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; - + 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; @@ -720,117 +715,108 @@ private: struct btMultiBodyLinkDoubleData { - 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; - + 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_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; - + 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 { 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; - - + 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 { 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; + 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/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index 9f61874b83..e17ab94d98 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -1,32 +1,29 @@ #include "btMultiBodyConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" -#include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro) +#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) +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) { - } 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 +35,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,298 +43,307 @@ 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& 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) +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; - solverConstraint.m_linkB = m_linkB; - - btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; - btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; - - btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA); - btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB); - - 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 - if (bodyA) - rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin(); - if (bodyB) - rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin(); - - if (multiBodyA) - { - if (solverConstraint.m_linkA<0) - { - rel_pos1 = posAworld - multiBodyA->getBasePos(); - } else - { - rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin(); - } - - const int ndofA = multiBodyA->getNumDofs() + 6; - - solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); - - 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 - { - 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); - //copy/determine - if(jacOrgA) - { - 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, 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 - 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); - - btVector3 torqueAxis0; - if (angConstraint) { - torqueAxis0 = constraintNormalAng; - } - else { - torqueAxis0 = rel_pos1.cross(constraintNormalLin); - - } - solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = constraintNormalLin; - } - else //if(rb0) - { - 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 = constraintNormalLin; - } - - if (multiBodyB) - { - if (solverConstraint.m_linkB<0) - { - rel_pos2 = posBworld - multiBodyB->getBasePos(); - } else - { - rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); - } - - const int ndofB = multiBodyB->getNumDofs() + 6; - - solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); - 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); - } - - //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); - //copy/determine.. - if(jacOrgB) - { - for (int i=0;i<ndofB;i++) - data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i]; - } - else - { - //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); - 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); - - btVector3 torqueAxis1; - if (angConstraint) { - torqueAxis1 = constraintNormalAng; - } - else { - torqueAxis1 = rel_pos2.cross(constraintNormalLin); - } - solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - solverConstraint.m_contactNormal2 = -constraintNormalLin; - } - else //if(rb1) - { - 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 = -constraintNormalLin; - } - { - - btVector3 vec; - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - btScalar* jacB = 0; - btScalar* jacA = 0; - btScalar* deltaVelA = 0; - btScalar* deltaVelB = 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) - { - ndofA = multiBodyA->getNumDofs() + 6; - jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; - deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA; ++i) - { - btScalar j = jacA[i] ; - btScalar l = deltaVelA[i]; - denom0 += j*l; - } - } - else if(rb0) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - if (angConstraint) { + solverConstraint.m_multiBodyA = m_bodyA; + solverConstraint.m_multiBodyB = m_bodyB; + solverConstraint.m_linkA = m_linkA; + solverConstraint.m_linkB = m_linkB; + + btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; + btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; + + btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA); + btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB); + + 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 + if (bodyA) + rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin(); + if (bodyB) + rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin(); + + if (multiBodyA) + { + if (solverConstraint.m_linkA < 0) + { + rel_pos1 = posAworld - multiBodyA->getBasePos(); + } + else + { + rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin(); + } + + const int ndofA = multiBodyA->getNumDofs() + 6; + + solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); + + 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 + { + 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); + //copy/determine + if (jacOrgA) + { + 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, 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 + 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); + + btVector3 torqueAxis0; + if (angConstraint) + { + torqueAxis0 = constraintNormalAng; + } + else + { + torqueAxis0 = rel_pos1.cross(constraintNormalLin); + } + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = constraintNormalLin; + } + else //if(rb0) + { + 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 = constraintNormalLin; + } + + if (multiBodyB) + { + if (solverConstraint.m_linkB < 0) + { + rel_pos2 = posBworld - multiBodyB->getBasePos(); + } + else + { + rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); + } + + const int ndofB = multiBodyB->getNumDofs() + 6; + + solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); + 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); + } + + //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); + //copy/determine.. + if (jacOrgB) + { + for (int i = 0; i < ndofB; i++) + data.m_jacobians[solverConstraint.m_jacBindex + i] = jacOrgB[i]; + } + else + { + //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); + 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); + + btVector3 torqueAxis1; + if (angConstraint) + { + torqueAxis1 = constraintNormalAng; + } + else + { + torqueAxis1 = rel_pos2.cross(constraintNormalLin); + } + solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + solverConstraint.m_contactNormal2 = -constraintNormalLin; + } + else //if(rb1) + { + 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 = -constraintNormalLin; + } + { + btVector3 vec; + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + btScalar* jacB = 0; + btScalar* jacA = 0; + btScalar* deltaVelA = 0; + btScalar* deltaVelB = 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) + { + ndofA = multiBodyA->getNumDofs() + 6; + jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; + deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) + { + btScalar j = jacA[i]; + btScalar l = deltaVelA[i]; + denom0 += j * l; + } + } + else if (rb0) + { + vec = (solverConstraint.m_angularComponentA).cross(rel_pos1); + if (angConstraint) + { denom0 = constraintNormalAng.dot(solverConstraint.m_angularComponentA); - } - else { - denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec); - } - } - // - if (multiBodyB) - { - const int ndofB = multiBodyB->getNumDofs() + 6; - jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; - deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB; ++i) - { - btScalar j = jacB[i] ; - btScalar l = deltaVelB[i]; - denom1 += j*l; - } - - } - else if(rb1) - { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - if (angConstraint) { + } + else + { + denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec); + } + } + // + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumDofs() + 6; + jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; + deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + { + btScalar j = jacB[i]; + btScalar l = deltaVelB[i]; + denom1 += j * l; + } + } + else if (rb1) + { + 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) - { - solverConstraint.m_jacDiagABInv = relaxation/(d); - } - else - { - //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; - - btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; - { - btVector3 vel1,vel2; - if (multiBodyA) - { - ndofA = multiBodyA->getNumDofs() + 6; - btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA ; ++i) - rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; - } - else if(rb0) - { + } + else + { + denom1 = rb1->getInvMass() + constraintNormalLin.dot(vec); + } + } + + // + btScalar d = denom0 + denom1; + if (d > SIMD_EPSILON) + { + solverConstraint.m_jacDiagABInv = relaxation / (d); + } + else + { + //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; + + btScalar rel_vel = 0.f; + int ndofA = 0; + int ndofB = 0; + { + btVector3 vel1, vel2; + if (multiBodyA) + { + ndofA = multiBodyA->getNumDofs() + 6; + btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) + rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; + } + else if (rb0) + { 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) - rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; - - } - else if(rb1) - { + } + if (multiBodyB) + { + ndofB = multiBodyB->getNumDofs() + 6; + btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; + } + else if (rb1) + { rel_vel += rb1->getLinearVelocity().dot(solverConstraint.m_contactNormal2); rel_vel += rb1->getAngularVelocity().dot(solverConstraint.m_relpos2CrossNormal); - } - - solverConstraint.m_friction = 0.f;//cp.m_combinedFriction; - } - - - ///warm starting (or zero if disabled) - /* + } + + 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; @@ -369,38 +375,35 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr } } else */ - - solverConstraint.m_appliedImpulse = 0.f; - solverConstraint.m_appliedPushImpulse = 0.f; - - { - - btScalar positionalError = 0.f; - btScalar velocityError = desiredVelocity - rel_vel;// * damping; - - - btScalar erp = infoGlobal.m_erp2; - + + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedPushImpulse = 0.f; + + { + btScalar positionalError = 0.f; + btScalar velocityError = desiredVelocity - rel_vel; // * damping; + + btScalar erp = infoGlobal.m_erp2; + //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; - - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - + { + erp = infoGlobal.m_erp; + } + + positionalError = -penetration * erp / infoGlobal.m_timeStep; + + btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; + //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_rhsPenetration = 0.f; - - } + // 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 @@ -409,11 +412,10 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr } */ - solverConstraint.m_cfm = 0.f; - solverConstraint.m_lowerLimit = lowerLimit; - solverConstraint.m_upperLimit = upperLimit; - } - - return rel_vel; - + solverConstraint.m_cfm = 0.f; + solverConstraint.m_lowerLimit = lowerLimit; + solverConstraint.m_upperLimit = upperLimit; + } + + return rel_vel; } diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h index a2ae571273..5c15f3e851 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -27,66 +27,62 @@ 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; }; - -ATTRIBUTE_ALIGNED16(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& 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); + 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& 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); virtual ~btMultiBodyConstraint(); void updateJacobianSizes(); @@ -94,27 +90,27 @@ public: //many constraints have setFrameInB/setPivotInB. Will use 'getConstraintType' later. virtual void setFrameInB(const btMatrix3x3& frameInB) {} - virtual void setPivotInB(const btVector3& pivotInB){} + virtual void setPivotInB(const btVector3& pivotInB) {} - virtual void finalizeMultiDof()=0; + virtual void finalizeMultiDof() = 0; - virtual int getIslandIdA() const =0; - virtual int getIslandIdB() const =0; + virtual int getIslandIdA() const = 0; + virtual int getIslandIdB() const = 0; - virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal)=0; + virtual void createConstraintRows(btMultiBodyConstraintArray & constraintRows, + btMultiBodyJacobianData & data, + const btContactSolverInfo& infoGlobal) = 0; - int getNumRows() const + int getNumRows() const { return m_numRows; } - btMultiBody* getMultiBodyA() + btMultiBody* getMultiBodyA() { return m_bodyA; } - btMultiBody* getMultiBodyB() + btMultiBody* getMultiBodyB() { return m_bodyB; } @@ -127,77 +123,72 @@ public: { return m_linkB; } - void internalSetAppliedImpulse(int dof, btScalar appliedImpulse) + 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){} - - + virtual void setRelativePositionTarget(btScalar relPosTarget) {} + virtual void setErp(btScalar erp) {} }; -#endif //BT_MULTIBODY_CONSTRAINT_H - +#endif //BT_MULTIBODY_CONSTRAINT_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index cd84826e1a..e97bd71cc4 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/thirdparty/bullet/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" @@ -24,33 +23,33 @@ subject to the following restrictions: #include "LinearMath/btQuickprof.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 leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); - + btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + //solve featherstone non-contact constraints //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size()); - for (int j=0;j<m_multiBodyNonContactConstraints.size();j++) + for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++) { - int index = iteration&1? j : m_multiBodyNonContactConstraints.size()-1-j; + int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j; btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index]; - + btScalar residual = resolveSingleConstraintRowGeneric(constraint); - leastSquaredResidual = btMax(leastSquaredResidual,residual*residual); + 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 normal contact - for (int j0=0;j0<m_multiBodyNormalContactConstraints.size();j0++) + for (int j0 = 0; j0 < m_multiBodyNormalContactConstraints.size(); j0++) { - int index = j0;//iteration&1? j0 : m_multiBodyNormalContactConstraints.size()-1-j0; + int index = j0; //iteration&1? j0 : m_multiBodyNormalContactConstraints.size()-1-j0; btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[index]; btScalar residual = 0.f; @@ -60,32 +59,32 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl residual = resolveSingleConstraintRowGeneric(constraint); } - leastSquaredResidual = btMax(leastSquaredResidual,residual*residual); - - if(constraint.m_multiBodyA) + leastSquaredResidual = btMax(leastSquaredResidual, residual * residual); + + 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)) + 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_multiBodyTorsionalFrictionContactConstraints.size(); j1++) + 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; + 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; //adjust friction limits here - if (totalImpulse>btScalar(0)) + if (totalImpulse > btScalar(0)) { - frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); - frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; + frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse); + frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse; btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint); - leastSquaredResidual = btMax(leastSquaredResidual , residual*residual); + leastSquaredResidual = btMax(leastSquaredResidual, residual * residual); if (frictionConstraint.m_multiBodyA) frictionConstraint.m_multiBodyA->setPosUpdated(false); @@ -99,29 +98,29 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl { if (iteration < infoGlobal.m_numIterations) { - int index = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; + 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; j1++; - int index2 = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-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; + 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); - + 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) @@ -129,26 +128,24 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl } } } - - } else { - for (int j1 = 0; j1<this->m_multiBodyFrictionContactConstraints.size(); j1++) + 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; + 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)) + if (totalImpulse > btScalar(0)) { - frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); - frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; + frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse); + frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse; btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint); - leastSquaredResidual = btMax(leastSquaredResidual, residual*residual); + leastSquaredResidual = btMax(leastSquaredResidual, residual * residual); if (frictionConstraint.m_multiBodyA) frictionConstraint.m_multiBodyA->setPosUpdated(false); @@ -161,18 +158,18 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl 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_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) @@ -181,21 +178,20 @@ 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; } btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c) { - - btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm; + btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm; btScalar deltaVelADotn = 0; btScalar deltaVelBDotn = 0; btSolverBody* bodyA = 0; @@ -227,9 +223,8 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt 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) @@ -246,7 +241,7 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt { c.m_appliedImpulse = sum; } - + if (c.m_multiBodyA) { applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA); @@ -254,12 +249,11 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt //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 +#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) { @@ -268,54 +262,54 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt //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 +#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; + btScalar deltaVel = deltaImpulse / c.m_jacDiagABInv; return deltaVel; } - -btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1,const btMultiBodySolverConstraint& cB) +btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1, const btMultiBodySolverConstraint& cB) { - int ndofA=0; - int ndofB=0; + 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; + 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) + 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()); + 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) + 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()); + 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; + deltaImpulseB -= deltaVelADotn * cB.m_jacDiagABInv; //m_jacDiagABInv = 1./denom + deltaImpulseB -= deltaVelBDotn * cB.m_jacDiagABInv; sumB = btScalar(cB.m_appliedImpulse) + deltaImpulseB; } @@ -324,45 +318,45 @@ btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const bt const btMultiBodySolverConstraint& cA = cA1; { { - deltaImpulseA = cA.m_rhs-btScalar(cA.m_appliedImpulse)*cA.m_cfm; - btScalar deltaVelADotn=0; - btScalar deltaVelBDotn=0; + 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) + 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()); + 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) + 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()); + 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; + 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) + 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)); + 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; @@ -396,78 +390,77 @@ btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const bt //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); + 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) + 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); - + 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); + 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) + 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); + 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); + 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) + 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); + 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); + 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) + 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); + bodyB->internalApplyImpulse(cB.m_contactNormal2 * bodyB->internalGetInvMass(), cB.m_angularComponentB, deltaImpulseB); } - btScalar deltaVel =deltaImpulseA/cA.m_jacDiagABInv+deltaImpulseB/cB.m_jacDiagABInv; - return deltaVel; + 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, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) { - BT_PROFILE("setupMultiBodyContactConstraint"); btVector3 rel_pos1; btVector3 rel_pos2; @@ -485,44 +478,46 @@ 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 = infoGlobal.m_sor; - - btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep; - - //cfm = 1 / ( dt * kp + kd ) - //erp = dt * kp / ( dt * kp + kd ) - - btScalar cfm; + + 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 + } + 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) || (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_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 ); + btScalar denom = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1); if (denom < SIMD_EPSILON) { denom = SIMD_EPSILON; } - cfm = btScalar(1) / denom; + cfm = btScalar(1) / denom; erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom; } } @@ -532,218 +527,217 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol 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+cfm; - 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 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; + 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); + } + } - btVector3 vel1,vel2; + btScalar rel_vel = 0.f; + int ndofA = 0; + int ndofB = 0; + { + 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) + - (rb0->getTotalTorque()*rb0->getInvInertiaTensorWorld()*infoGlobal.m_timeStep).cross(rel_pos1)+ - rb0->getTotalForce()*rb0->getInvMass()*infoGlobal.m_timeStep).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)+ - (rb1->getTotalTorque()*rb1->getInvInertiaTensorWorld()*infoGlobal.m_timeStep).cross(rel_pos2) + - rb1->getTotalForce()*rb1->getInvMass()*infoGlobal.m_timeStep).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, infoGlobal.m_restitutionVelocityThreshold); + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); if (restitution <= btScalar(0.)) { restitution = 0.f; @@ -751,10 +745,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol } } - ///warm starting (or zero if disabled) //disable warmstarting for btMultiBody, it has issues gaining energy (==explosion) - if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + if (0) //infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) { solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; @@ -764,27 +757,30 @@ 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->applyDeltaVeeMultiDof(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->applyDeltaVeeMultiDof(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; } @@ -792,38 +788,37 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol solverConstraint.m_appliedPushImpulse = 0.f; { - 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 + 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 + positionalError = -distance * erp / infoGlobal.m_timeStep; + } + else { - if (distance>0) + if (distance > 0) { positionalError = 0; velocityError -= distance / infoGlobal.m_timeStep; - - } else + } + else { - positionalError = -distance * erp/infoGlobal.m_timeStep; + positionalError = -distance * 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(!isFriction) + if (!isFriction) { - // if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + // 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 + /*else { //split position and velocity into rhs and m_rhsPenetration solverConstraint.m_rhs = velocityImpulse; @@ -835,309 +830,288 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol } else { - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; solverConstraint.m_rhsPenetration = 0.f; solverConstraint.m_lowerLimit = -solverConstraint.m_friction; solverConstraint.m_upperLimit = solverConstraint.m_friction; } - solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv; - - - + solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv; } - } void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint, - const btVector3& constraintNormal, - btManifoldPoint& cp, - btScalar combinedTorsionalFriction, - const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) + 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; - - solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); - - 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 - { - 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); - 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) - { - rel_pos2 = pos2 - multiBodyB->getBasePos(); - } else - { - rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); - } - - const int ndofB = multiBodyB->getNumDofs() + 6; - - solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); - 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); - } - - 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); - } - - { - - 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) - { - 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; - } - } else - { - if (rb0) - { - btVector3 iMJaA = rb0?rb0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0); + 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; + + solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); + + 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 + { + 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); + 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) + { + rel_pos2 = pos2 - multiBodyB->getBasePos(); + } + else + { + rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); + } + + const int ndofB = multiBodyB->getNumDofs() + 6; + + solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); + 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); + } + + 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); + } + + { + 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) + { + 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; + } + } + else + { + if (rb0) + { + 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); + } + } + 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); - } - } - - - - btScalar d = denom0+denom1+infoGlobal.m_globalCfm; - if (d>SIMD_EPSILON) - { - solverConstraint.m_jacDiagABInv = relaxation/(d); - } else - { - //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) - { + } + } + + btScalar d = denom0 + denom1 + infoGlobal.m_globalCfm; + if (d > SIMD_EPSILON) + { + solverConstraint.m_jacDiagABInv = relaxation / (d); + } + else + { + //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) - { + 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_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; - - solverConstraint.m_cfm = infoGlobal.m_globalCfm*solverConstraint.m_jacDiagABInv; - - - - } - + 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_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; + + solverConstraint.m_cfm = infoGlobal.m_globalCfm * solverConstraint.m_jacDiagABInv; + } } -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) +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) { 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; @@ -1151,95 +1125,92 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionCo solverConstraint.m_originalContactPoint = &cp; - setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip); + setupMultiBodyContactConstraint(solverConstraint, normalAxis, 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) +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; - 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; + 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; + 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) +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; - btCollisionObject* colObj0=0,*colObj1=0; + btMultiBody* mbA = fcA ? fcA->m_multiBody : 0; + btMultiBody* mbB = fcB ? fcB->m_multiBody : 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=1; - - for (int j=0;j<manifold->getNumContacts();j++) - { + //only a single rollingFriction per manifold + int rollingFriction = 1; + 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; @@ -1253,60 +1224,59 @@ 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, 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; + 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 every frame ///based on the relative linear velocity. ///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 /// - btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + 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) - { - addMultiBodyTorsionalFrictionConstraint(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); - - if (cp.m_lateralFrictionDir1.length()>0.001) - addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal); - - if (cp.m_lateralFrictionDir2.length()>0.001) - 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)) - {/* + if (rollingFriction > 0) + { + if (cp.m_combinedSpinningFriction > 0) + { + addMultiBodyTorsionalFrictionConstraint(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); + + if (cp.m_lateralFrictionDir1.length() > 0.001) + addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal); + + if (cp.m_lateralFrictionDir2.length() > 0.001) + 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) @@ -1329,85 +1299,77 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* } else */ { - - - 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, 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, 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_contactPointFlags|=BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED; + 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_frictionCFM); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, 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_frictionCFM); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM); //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); //todo: solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedPushImpulse = 0.f; } - - -#endif //ENABLE_FRICTION +#endif //ENABLE_FRICTION } } } -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); + } } - - -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) { //printf("btMultiBodyConstraintSolver::solveGroup: numBodies=%d, numConstraints=%d\n", numBodies, numConstraints); - return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); + return btSequentialImpulseConstraintSolver::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher); } #if 0 @@ -1431,56 +1393,54 @@ static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodyS } #endif - 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); } - } } } @@ -1490,66 +1450,64 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv if (c.m_multiBodyA) { - c.m_multiBodyA->applyDeltaVeeMultiDof(&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) { - c.m_multiBodyB->applyDeltaVeeMultiDof(&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_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? } + + //do a callback here? } #if 0 //multibody joint feedback @@ -1648,25 +1606,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/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h index 29f484e1d8..f39f2879fc 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -25,80 +25,71 @@ 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_multiBodyNormalContactConstraints; - btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints; - btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints; + 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; + 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,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); - - void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, - btScalar* jacA,btScalar* jacB, - btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff, - const btContactSolverInfo& infoGlobal); - - void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, - const btVector3& contactNormal, - 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); -public: - BT_DECLARE_ALIGNED_ALLOCATOR(); + void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal); - ///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); -}; + 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); + + 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); - - + void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint & constraintRow, + btScalar * jacA, btScalar * jacB, + btScalar penetration, btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff, + const btContactSolverInfo& infoGlobal); + void setupMultiBodyContactConstraint(btMultiBodySolverConstraint & solverConstraint, + const btVector3& contactNormal, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); -#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H + //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); + +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); +}; +#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 9c5f3ad8a9..1557987bc3 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -23,45 +23,43 @@ subject to the following restrictions: #include "LinearMath/btIDebugDraw.h" #include "LinearMath/btSerializer.h" - -void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, int group, int 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::calculateSimulationIslands() { BT_PROFILE("calculateSimulationIslands"); - 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()); - } - } - } - + 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()); + } + } + } + { 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 +70,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 +96,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,119 +131,108 @@ 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) +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(); + 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; - } +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) +SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs) { int islandId; - + int islandTagA = lhs->getIslandIdA(); int islandTagB = lhs->getIslandIdB(); - islandId= islandTagA>=0?islandTagA:islandTagB; + 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; - } +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; - + 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) + 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=(MultiBodyInplaceSolverIslandCallback& other) @@ -260,7 +242,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: return *this; } - SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer) + SIMD_FORCE_INLINE void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer) { btAssert(solverInfo); m_solverInfo = solverInfo; @@ -271,26 +253,27 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: m_numConstraints = numConstraints; m_debugDrawer = debugDrawer; - m_bodies.resize (0); - m_manifolds.resize (0); - m_constraints.resize (0); + 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) + void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver) + { + m_solver = solver; + } + + virtual void processIsland(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifolds, int numManifolds, int islandId) { - if (islandId<0) + 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 + 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 + //also add all non-contact constraints/joints for this island btTypedConstraint** startConstraint = 0; btMultiBodyConstraint** startMultiBodyConstraint = 0; @@ -298,10 +281,10 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: int numCurMultiBodyConstraints = 0; int i; - + //find the first constraint for this island - for (i=0;i<m_numConstraints;i++) + for (i = 0; i < m_numConstraints; i++) { if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId) { @@ -310,7 +293,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: } } //count the number of constraints in this island - for (;i<m_numConstraints;i++) + for (; i < m_numConstraints; i++) { if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId) { @@ -318,17 +301,16 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: } } - for (i=0;i<m_numMultiBodyConstraints;i++) + 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++) + for (; i < m_numMultiBodyConstraints; i++) { if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId) { @@ -341,101 +323,94 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: // m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); //} else { - - for (i=0;i<numBodies;i++) + for (i = 0; i < numBodies; i++) m_bodies.push_back(bodies[i]); - for (i=0;i<numManifolds;i++) + for (i = 0; i < numManifolds; i++) m_manifolds.push_back(manifolds[i]); - for (i=0;i<numCurConstraints;i++) + for (i = 0; i < numCurConstraints; i++) m_constraints.push_back(startConstraint[i]); - - for (i=0;i<numCurMultiBodyConstraints;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) + + if ((m_multiBodyConstraints.size() + m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize) { processConstraints(); - } else + } + else { //printf("deferred\n"); } } } } - void processConstraints() + 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; + 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_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) +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); + // getSolverInfo().m_splitImpulse = false; + getSolverInfo().m_solverMode |= SOLVER_USE_2_FRICTION_DIRECTIONS; + m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver, dispatcher); } -btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld () +btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld() { delete m_solverMultiBodyIslandCallback; } -void btMultiBodyDynamicsWorld::setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver) +void btMultiBodyDynamicsWorld::setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver) { - m_multiBodyConstraintSolver = solver; - m_solverMultiBodyIslandCallback->setMultiBodyConstraintSolver(solver); - btDiscreteDynamicsWorld::setConstraintSolver(solver); + m_multiBodyConstraintSolver = solver; + m_solverMultiBodyIslandCallback->setMultiBodyConstraintSolver(solver); + btDiscreteDynamicsWorld::setConstraintSolver(solver); } -void btMultiBodyDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) +void btMultiBodyDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) { - if (solver->getSolverType()==BT_MULTIBODY_SOLVER) - { - m_multiBodyConstraintSolver = (btMultiBodyConstraintSolver*)solver; - } - btDiscreteDynamicsWorld::setConstraintSolver(solver); + if (solver->getSolverType() == BT_MULTIBODY_SOLVER) + { + m_multiBodyConstraintSolver = (btMultiBodyConstraintSolver*)solver; + } + btDiscreteDynamicsWorld::setConstraintSolver(solver); } -void btMultiBodyDynamicsWorld::forwardKinematics() +void btMultiBodyDynamicsWorld::forwardKinematics() { - - 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(m_scratch_world_to_local,m_scratch_local_origin); + bod->forwardKinematics(m_scratch_world_to_local, m_scratch_local_origin); } } -void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) +void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { forwardKinematics(); - - BT_PROFILE("solveConstraints"); - + clearMultiBodyConstraintForces(); - m_sortedConstraints.resize( m_constraints.size()); - int i; - for (i=0;i<getNumConstraints();i++) + m_sortedConstraints.resize(m_constraints.size()); + int i; + for (i = 0; i < getNumConstraints(); i++) { m_sortedConstraints[i] = m_constraints[i]; } @@ -443,109 +418,120 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size()); - for (i=0;i<m_multiBodyConstraints.size();i++) + 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; - + btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; - m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer()); + 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++) + 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++) + } + 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) { //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); + 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) + for (int j = 0; j < bod->getNumLinks(); ++j) { bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); } - }//if (!isSleeping) + } //if (!isSleeping) } } -#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - +#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY { BT_PROFILE("btMultiBody stepVelocities"); - for (int i=0;i<this->m_multiBodies.size();i++) + 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++) + } + 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) { //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); + 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()) + if (!bod->isUsingRK4Integration()) { - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m); + 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); + 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]); - - ///// + 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(); @@ -555,83 +541,88 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) scratch_q0[5] = bod->getBasePos().y(); scratch_q0[6] = bod->getBasePos().z(); // - for(int link = 0; link < bod->getNumLinks(); ++link) + 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 < 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) + 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) - scratch_qx[dof] = scratch_q0[dof]; - } + btMultiBody* bod; + btScalar *scratch_qx, *scratch_q0; + + 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) - pVal[i] = pCurVal[i] + dt * pDer[i]; - } + 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 - { - void operator()(btMultiBody *pBody, const btScalar *pData) - { - btScalar *pVel = const_cast<btScalar*>(pBody->getVelocityVector()); - - for(int i = 0; i < pBody->getNumDofs() + 6; ++i) - pVel[i] = pData[i]; - - } - } pCopyToVelocityVector; + { + void operator()(btMultiBody* pBody, const btScalar* pData) + { + btScalar* pVel = const_cast<btScalar*>(pBody->getVelocityVector()); + + for (int i = 0; i < pBody->getNumDofs() + 6; ++i) + pVel[i] = pData[i]; + } + } pCopyToVelocityVector; // - struct + struct { - void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size) - { - for(int i = 0; i < size; ++i) - pDst[i] = pSrc[start + 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 &m_scratch_r[bod->getNumDofs()] - //calc qdd0 from: q0 & qd0 - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m); +#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); + 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); + 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); + 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); + 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); + 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); + 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(); @@ -641,156 +632,158 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) // //calc qdd3 from: q3 & qd3 pCopyToVelocityVector(bod, scratch_qd3); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m); + 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) + //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 / 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); + bod->applyDeltaVeeMultiDof(&delta_qd[0], 1); // - if(!doNotUpdatePos) + if (!doNotUpdatePos) { - btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector()); - pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs(); + btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector()); + pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs(); - for(int i = 0; i < numDofs; ++i) + for (int i = 0; i < numDofs; ++i) pRealBuf[i] = delta_q[i]; //bod->stepPositionsMultiDof(1, 0, &delta_q[0]); - bod->setPosUpdated(true); + bod->setPosUpdated(true); } //ugly hack which resets the cached data to t0 (needed for constraint solver) { - for(int link = 0; link < bod->getNumLinks(); ++link) + for (int link = 0; link < bod->getNumLinks(); ++link) bod->getLink(link).updateCacheMultiDof(); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m); + 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) +#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY + } //if (!isSleeping) } } /// 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->isUsingRK4Integration()) - { - bool isConstraintPass = true; - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass); - } + 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->isUsingRK4Integration()) + { + 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++) + for (int i = 0; i < this->m_multiBodies.size(); i++) { btMultiBody* bod = m_multiBodies[i]; bod->processDeltaVeeMultiDof2(); } - } -void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) +void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) { btDiscreteDynamicsWorld::integrateTransforms(timeStep); { BT_PROFILE("btMultiBody stepPositions"); //integrate and update the Featherstone hierarchies - - 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) { int nLinks = bod->getNumLinks(); ///base + num m_links - - + { - if(!bod->isPosUpdated()) + if (!bod->isPosUpdated()) bod->stepPositionsMultiDof(timeStep); else { - btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector()); - pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs(); + btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector()); + pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs(); bod->stepPositionsMultiDof(1, 0, pRealBuf); bod->setPosUpdated(false); } } - - 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); - - } 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); + } + else { bod->clearVelocities(); } @@ -798,14 +791,12 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) } } - - -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); } @@ -815,8 +806,7 @@ void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstrain constraint->debugDraw(getDebugDrawer()); } - -void btMultiBodyDynamicsWorld::debugDrawWorld() +void btMultiBodyDynamicsWorld::debugDrawWorld() { BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld"); @@ -826,7 +816,7 @@ void btMultiBodyDynamicsWorld::debugDrawWorld() if (getDebugDrawer()) { int mode = getDebugDrawer()->getDebugMode(); - if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) + if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) { drawConstraints = true; } @@ -834,160 +824,148 @@ void btMultiBodyDynamicsWorld::debugDrawWorld() if (drawConstraints) { BT_PROFILE("btMultiBody debugDrawWorld"); - - 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(m_scratch_world_to_local1,m_scratch_local_origin1); - - if (mode & btIDebugDraw::DBG_DrawFrames) + 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; - if (mode & btIDebugDraw::DBG_DrawFrames) + if (mode & btIDebugDraw::DBG_DrawFrames) { getDebugDrawer()->drawTransform(tr, 0.1); } - //draw the joint axis - if (bod->getLink(m).m_jointType==btMultibodyLink::eRevolute) + //draw the joint axis + if (bod->getLink(m).m_jointType == btMultibodyLink::eRevolute) { - 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); + 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::eFixed) + if (bod->getLink(m).m_jointType == btMultibodyLink::eFixed) { - 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); + 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) + 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); + 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); } - } } } } - - } - - 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]; - - 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) - { - 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 addGravity"); + 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) + { + 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 } 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]; - - 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) - { - btMultiBody* bod = m_multiBodies[i]; - bod->clearForcesAndTorques(); - } + { + // BT_PROFILE("clearMultiBodyForces"); + 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) + { + 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); @@ -1000,32 +978,31 @@ void btMultiBodyDynamicsWorld::serialize(btSerializer* 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); } } //serialize all multibody links (collision objects) - for (i=0;i<m_collisionObjects.size();i++) + 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); + btChunk* chunk = serializer->allocate(len, 1); const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_MB_LINKCOLLIDER_CODE,colObj); + serializer->finalizeChunk(chunk, structType, BT_MB_LINKCOLLIDER_CODE, colObj); } } - } diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index 2fbf089d81..641238f3bb 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -33,8 +33,8 @@ protected: btAlignedObjectArray<btMultiBody*> m_multiBodies; btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints; btAlignedObjectArray<btMultiBodyConstraint*> m_sortedMultiBodyConstraints; - btMultiBodyConstraintSolver* m_multiBodyConstraintSolver; - MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback; + btMultiBodyConstraintSolver* m_multiBodyConstraintSolver; + MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback; //cached data to avoid memory allocations btAlignedObjectArray<btQuaternion> m_scratch_world_to_local; @@ -45,72 +45,69 @@ protected: btAlignedObjectArray<btVector3> m_scratch_v; btAlignedObjectArray<btMatrix3x3> m_scratch_m; - - virtual void calculateSimulationIslands(); - virtual void updateActivationState(btScalar timeStep); - virtual void solveConstraints(btContactSolverInfo& solverInfo); - - virtual void serializeMultiBodies(btSerializer* serializer); + virtual void calculateSimulationIslands(); + virtual void updateActivationState(btScalar timeStep); + virtual void solveConstraints(btContactSolverInfo& solverInfo); -public: + virtual void serializeMultiBodies(btSerializer* serializer); - btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); +public: + btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration); - virtual ~btMultiBodyDynamicsWorld (); + virtual ~btMultiBodyDynamicsWorld(); - virtual void addMultiBody(btMultiBody* body, int group= btBroadphaseProxy::DefaultFilter, int mask=btBroadphaseProxy::AllFilter); + 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 + 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); + 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 setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver); - virtual void setConstraintSolver(btConstraintSolver* solver); + virtual void serialize(btSerializer* serializer); + virtual void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver); + virtual void setConstraintSolver(btConstraintSolver* solver); }; -#endif //BT_MULTIBODY_DYNAMICS_WORLD_H +#endif //BT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp index af48e94a83..5ef9444c2f 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp @@ -24,27 +24,27 @@ subject to the following restrictions: #define BTMBFIXEDCONSTRAINT_DIM 6 btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) - :btMultiBodyConstraint(body,0,link,-1,BTMBFIXEDCONSTRAINT_DIM,false), - m_rigidBodyA(0), - m_rigidBodyB(bodyB), - m_pivotInA(pivotInA), - m_pivotInB(pivotInB), - m_frameInA(frameInA), - m_frameInB(frameInB) + : btMultiBodyConstraint(body, 0, link, -1, BTMBFIXEDCONSTRAINT_DIM, false), + 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 + 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), - m_rigidBodyA(0), - m_rigidBodyB(0), - m_pivotInA(pivotInA), - m_pivotInB(pivotInB), - m_frameInA(frameInA), - m_frameInB(frameInB) + : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBFIXEDCONSTRAINT_DIM, false), + 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 + m_data.resize(BTMBFIXEDCONSTRAINT_DIM); //at least store the applied impulses } void btMultiBodyFixedConstraint::finalizeMultiDof() @@ -57,7 +57,6 @@ btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint() { } - int btMultiBodyFixedConstraint::getIslandIdA() const { if (m_rigidBodyA) @@ -103,82 +102,83 @@ int btMultiBodyFixedConstraint::getIslandIdB() const void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal) { - int numDim = BTMBFIXEDCONSTRAINT_DIM; - for (int i=0;i<numDim;i++) + 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 - ); - } + 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); + } } } diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h index 036025136e..adb1cb47da 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h @@ -23,16 +23,14 @@ subject to the following restrictions: class btMultiBodyFixedConstraint : public btMultiBodyConstraint { protected: - - btRigidBody* m_rigidBodyA; - btRigidBody* m_rigidBodyB; - btVector3 m_pivotInA; - btVector3 m_pivotInB; - btMatrix3x3 m_frameInA; - btMatrix3x3 m_frameInB; + 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); @@ -44,18 +42,18 @@ public: 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; - } + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + const btVector3& getPivotInA() const + { + return m_pivotInA; + } + + void setPivotInA(const btVector3& pivotInA) + { + m_pivotInA = pivotInA; + } const btVector3& getPivotInB() const { @@ -66,29 +64,28 @@ public: { 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); + 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 +#endif //BT_MULTIBODY_FIXED_CONSTRAINT_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp index 09ddd65cd8..bf6b811d26 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp @@ -21,20 +21,18 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) - :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,1,false), - m_gearRatio(1), - m_gearAuxLink(-1), - m_erp(0), - m_relativePositionTarget(0) + : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, 1, false), + m_gearRatio(1), + m_gearAuxLink(-1), + m_erp(0), + m_relativePositionTarget(0) { - } void btMultiBodyGearConstraint::finalizeMultiDof() { - allocateJacobiansMultiDof(); - + m_numDofsFinalized = m_jacSizeBoth; } @@ -42,7 +40,6 @@ btMultiBodyGearConstraint::~btMultiBodyGearConstraint() { } - int btMultiBodyGearConstraint::getIslandIdA() const { if (m_bodyA) @@ -81,27 +78,25 @@ int btMultiBodyGearConstraint::getIslandIdB() const return -1; } - void btMultiBodyGearConstraint::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) + 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; @@ -114,67 +109,66 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& 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++) + 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]; + 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) + + if (m_gearAuxLink >= 0) { auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof]; } currentVelocity += auxVel; - if (m_erp!=0) + 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 currentPositionB = m_gearRatio * m_bodyA->getJointPosMultiDof(m_linkB)[dof]; + btScalar diff = currentPositionB + currentPositionA; btScalar desiredPositionDiff = this->m_relativePositionTarget; - posError = -m_erp*(desiredPositionDiff - diff); + 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); + + 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)); + 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(); + constraintRow.m_relpos2CrossNormal.setZero(); break; } default: @@ -182,10 +176,6 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& btAssert(0); } }; - } - } - } - diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h index 0115de6241..31888fbc68 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h @@ -23,20 +23,18 @@ subject to the following restrictions: 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; - 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); @@ -48,18 +46,18 @@ public: 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; - } + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + const btVector3& getPivotInA() const + { + return m_pivotInA; + } + + void setPivotInA(const btVector3& pivotInA) + { + m_pivotInA = pivotInA; + } const btVector3& getPivotInB() const { @@ -70,32 +68,32 @@ public: { 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 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; @@ -114,4 +112,4 @@ public: } }; -#endif //BT_MULTIBODY_GEAR_CONSTRAINT_H +#endif //BT_MULTIBODY_GEAR_CONSTRAINT_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h index 5c2fa8ed5b..d943019e71 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h +++ b/thirdparty/bullet/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/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp index 35c929f7ce..8791ad2868 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ b/thirdparty/bullet/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), + 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(); @@ -53,10 +50,8 @@ btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint() { } - int btMultiBodyJointLimitConstraint::getIslandIdA() const { - if (m_bodyA) { if (m_linkA < 0) @@ -93,72 +88,69 @@ int btMultiBodyJointLimitConstraint::getIslandIdB() const 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 penetration = getPosition(row); //todo: consider adding some safety threshold here - if (penetration>0) + if (penetration > 0) { continue; } - btScalar direction = row? -1 : 1; + 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,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: @@ -166,36 +158,35 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint btAssert(0); } }; - } { - 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; @@ -203,9 +194,4 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint } } } - } - - - - diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h index 55b8d122b9..6716ba490f 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h +++ b/thirdparty/bullet/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,13 @@ 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) } - }; -#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H - +#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index 2a70ea97e5..5c816c4987 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -20,22 +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), - m_desiredPosition(0), - m_kd(1.), - m_kp(0), - m_erp(1), - m_rhsClamp(SIMD_INFINITY) + : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true), + 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() @@ -55,18 +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), - m_desiredPosition(0), - m_kd(1.), - m_kp(0), - m_erp(1), - m_rhsClamp(SIMD_INFINITY) + : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true), + 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() { @@ -108,76 +103,74 @@ int btMultiBodyJointMotor::getIslandIdB() const 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) + 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; - - btScalar velocityError = (m_desiredVelocity - currentVelocity); - btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError; - if (rhs>m_rhsClamp) + 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; + + btScalar velocityError = (m_desiredVelocity - currentVelocity); + btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity + m_kd * velocityError; + if (rhs > m_rhsClamp) { - rhs=m_rhsClamp; + rhs = m_rhsClamp; } - if (rhs<-m_rhsClamp) + if (rhs < -m_rhsClamp) { - 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); + + 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: @@ -185,10 +178,6 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con btAssert(0); } }; - } - } - } - diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.h index 4063bed79a..1aca36352e 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.h @@ -24,41 +24,38 @@ struct btSolverInfo; class btMultiBodyJointMotor : public btMultiBodyConstraint { protected: - - btScalar m_desiredVelocity; - btScalar m_desiredPosition; - btScalar m_kd; - btScalar m_kp; - btScalar m_erp; - btScalar m_rhsClamp;//maximum error - + 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, btScalar kd = 1.f) - { - m_desiredVelocity = velTarget; - m_kd = kd; - } + virtual void setPositionTarget(btScalar posTarget, btScalar kp = 1.f) + { + m_desiredPosition = posTarget; + m_kp = kp; + } - virtual void setPositionTarget(btScalar posTarget, btScalar kp = 1.f) - { - m_desiredPosition = posTarget; - m_kp = kp; - } - virtual void setErp(btScalar erp) { m_erp = erp; @@ -77,5 +74,4 @@ public: } }; -#endif //BT_MULTIBODY_JOINT_MOTOR_H - +#endif //BT_MULTIBODY_JOINT_MOTOR_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h index 21c9e7a557..92d41dfac2 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -20,7 +20,7 @@ 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_ALL_PARENT_COLLISION = 2, @@ -36,7 +36,6 @@ enum btMultiBodyLinkFlags //namespace { - #include "LinearMath/btSpatialAlgebra.h" //} @@ -45,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; @@ -79,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) @@ -93,143 +89,141 @@ 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 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) + void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { - m_axes[dof].m_topVec.setValue(x, y, 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); + 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; } + 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. - 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 + + //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 - - 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. - + + struct btMultiBodyJointFeedback *m_jointFeedback; + + btTransform m_cachedWorldTransform; //this cache is updated when calling btMultiBody::forwardKinematics + + 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_userPtr(0), - m_jointDamping(0), - m_jointFriction(0), - m_jointLowerLimit(0), - m_jointUpperLimit(0), - m_jointMaxForce(0), - m_jointMaxVelocity(0) + : 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_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_appliedForce.setValue(0, 0, 0); m_appliedTorque.setValue(0, 0, 0); - m_appliedConstraintForce.setValue(0,0,0); - m_appliedConstraintTorque.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) + switch (m_jointType) { case eRevolute: { - m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); + m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; + m_cachedRVector = 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); + m_cachedRVector = 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); + m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, 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); + 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); break; } case eFixed: { m_cachedRotParentToThis = m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); + m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); break; } @@ -242,5 +236,4 @@ btVector3 m_appliedConstraintForce; // In WORLD frame } }; - -#endif //BT_MULTIBODY_LINK_H +#endif //BT_MULTIBODY_LINK_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h index 7092e62b5a..f91c001f12 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h @@ -29,21 +29,18 @@ subject to the following restrictions: #define btMultiBodyLinkColliderDataName "btMultiBodyLinkColliderFloatData" #endif - class btMultiBodyLinkCollider : public btCollisionObject { -//protected: + //protected: public: - btMultiBody* m_multiBody; int m_link; - - 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())) @@ -59,18 +56,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) @@ -81,47 +78,46 @@ 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_ALL_PARENT_COLLISION) + if (link.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION) { int parent_of_this = m_link; while (1) { - if (parent_of_this==-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) + if (parent_of_this == other->m_link) { return false; } } } - else if (link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) + else if (link.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) { - if ( link.m_parent == other->m_link) + 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_ALL_PARENT_COLLISION) + if (otherLink.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION) { int parent_of_other = other->m_link; while (1) { - if (parent_of_other==-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) + if (parent_of_other == this->m_link) return false; } } - else if (otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) + else if (otherLink.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) { if (otherLink.m_parent == this->m_link) return false; @@ -130,13 +126,13 @@ public: return true; } - virtual int calculateSerializeBufferSize() const; + 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; }; +// clang-format off struct btMultiBodyLinkColliderFloatData { @@ -154,16 +150,18 @@ struct btMultiBodyLinkColliderDoubleData char m_padding[4]; }; -SIMD_FORCE_INLINE int btMultiBodyLinkCollider::calculateSerializeBufferSize() const +// 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 +SIMD_FORCE_INLINE const char* btMultiBodyLinkCollider::serialize(void* dataBuffer, class btSerializer* serializer) const { btMultiBodyLinkColliderData* dataOut = (btMultiBodyLinkColliderData*)dataBuffer; - btCollisionObject::serialize(&dataOut->m_colObjData,serializer); - + btCollisionObject::serialize(&dataOut->m_colObjData, serializer); + dataOut->m_link = this->m_link; dataOut->m_multiBody = (btMultiBodyData*)serializer->getUniquePointer(m_multiBody); @@ -173,5 +171,4 @@ SIMD_FORCE_INLINE const char* btMultiBodyLinkCollider::serialize(void* dataBuffe return btMultiBodyLinkColliderDataName; } -#endif //BT_FEATHERSTONE_LINK_COLLIDER_H - +#endif //BT_FEATHERSTONE_LINK_COLLIDER_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp index 2b59f0b7a6..37d3aede37 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ b/thirdparty/bullet/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), + 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), + 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) @@ -73,7 +72,7 @@ int btMultiBodyPoint2Point::getIslandIdA() const else { if (m_bodyA->getLink(m_linkA).m_collider) - return m_bodyA->getLink(m_linkA).m_collider->getIslandTag(); + return m_bodyA->getLink(m_linkA).m_collider->getIslandTag(); } } return -1; @@ -100,48 +99,43 @@ int btMultiBodyPoint2Point::getIslandIdB() const 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); @@ -150,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, 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 + 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/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h index bf39acc5b9..ef03a557ec 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h @@ -22,22 +22,20 @@ subject to the following restrictions: //#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST -ATTRIBUTE_ALIGNED16(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(); @@ -46,9 +44,9 @@ 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 { @@ -60,9 +58,7 @@ public: 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/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp index 43f26f9833..e025302ce6 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp @@ -25,29 +25,29 @@ subject to the following restrictions: #define EPSILON 0.000001 btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis) - :btMultiBodyConstraint(body,0,link,-1,BTMBSLIDERCONSTRAINT_DIM,false), - m_rigidBodyA(0), - m_rigidBodyB(bodyB), - m_pivotInA(pivotInA), - m_pivotInB(pivotInB), - m_frameInA(frameInA), - m_frameInB(frameInB), - m_jointAxis(jointAxis) + : btMultiBodyConstraint(body, 0, link, -1, BTMBSLIDERCONSTRAINT_DIM, false), + 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 + 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), - m_rigidBodyA(0), - m_rigidBodyB(0), - m_pivotInA(pivotInA), - m_pivotInB(pivotInB), - m_frameInA(frameInA), - m_frameInB(frameInB), - m_jointAxis(jointAxis) + : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false), + 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 + m_data.resize(BTMBSLIDERCONSTRAINT_DIM); //at least store the applied impulses } void btMultiBodySliderConstraint::finalizeMultiDof() @@ -60,7 +60,6 @@ btMultiBodySliderConstraint::~btMultiBodySliderConstraint() { } - int btMultiBodySliderConstraint::getIslandIdA() const { if (m_rigidBodyA) @@ -105,98 +104,100 @@ int btMultiBodySliderConstraint::getIslandIdB() const } 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++) + // 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) { - 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 - ); - } + 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); + } } } diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h index 0a6cf3df12..b192b6f8f3 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h @@ -23,17 +23,15 @@ subject to the following restrictions: 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; + 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); @@ -45,18 +43,18 @@ public: 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; - } + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + const btVector3& getPivotInA() const + { + return m_pivotInA; + } + + void setPivotInA(const btVector3& pivotInA) + { + m_pivotInA = pivotInA; + } const btVector3& getPivotInB() const { @@ -67,39 +65,38 @@ public: { 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); + 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 +#endif //BT_MULTIBODY_SLIDER_CONSTRAINT_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h index 6fa1550e9e..deed3e2a12 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h +++ b/thirdparty/bullet/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/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp new file mode 100644 index 0000000000..3e5aa30f28 --- /dev/null +++ b/thirdparty/bullet/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), + 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/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h new file mode 100644 index 0000000000..621beab5a4 --- /dev/null +++ b/thirdparty/bullet/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 |