diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/Featherstone')
29 files changed, 0 insertions, 10941 deletions
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp deleted file mode 100644 index d7588aedc8..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp +++ /dev/null @@ -1,2461 +0,0 @@ -/* - * PURPOSE: - * Class representing an articulated rigid body. Stores the body's - * current state, allows forces and torques to be set, handles - * timestepping and implements Featherstone's algorithm. - * - * COPYRIGHT: - * Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013 - * Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix) - * Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements - - This software is provided 'as-is', without any express or implied warranty. - In no event will the authors be held liable for any damages arising from the use of this software. - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it freely, - subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. - - */ - -#include "btMultiBody.h" -#include "btMultiBodyLink.h" -#include "btMultiBodyLinkCollider.h" -#include "btMultiBodyJointFeedback.h" -#include "LinearMath/btTransformUtil.h" -#include "LinearMath/btSerializer.h" -//#include "Bullet3Common/b3Logging.h" -// #define INCLUDE_GYRO_TERM - - -namespace -{ -const btScalar INITIAL_SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2) -const btScalar INITIAL_SLEEP_TIMEOUT = btScalar(2); // in seconds -} // namespace - -void btMultiBody::spatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame - const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates - const btVector3 &top_in, // top part of input vector - const btVector3 &bottom_in, // bottom part of input vector - btVector3 &top_out, // top part of output vector - btVector3 &bottom_out) // bottom part of output vector -{ - top_out = rotation_matrix * top_in; - bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in; -} - -namespace -{ - - -#if 0 - void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix, - const btVector3 &displacement, - const btVector3 &top_in, - const btVector3 &bottom_in, - btVector3 &top_out, - btVector3 &bottom_out) - { - top_out = rotation_matrix.transpose() * top_in; - bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in)); - } - - btScalar SpatialDotProduct(const btVector3 &a_top, - const btVector3 &a_bottom, - const btVector3 &b_top, - const btVector3 &b_bottom) - { - return a_bottom.dot(b_top) + a_top.dot(b_bottom); - } - - void SpatialCrossProduct(const btVector3 &a_top, - const btVector3 &a_bottom, - const btVector3 &b_top, - const btVector3 &b_bottom, - btVector3 &top_out, - btVector3 &bottom_out) - { - top_out = a_top.cross(b_top); - bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom); - } -#endif - -} // namespace - -// -// Implementation of class btMultiBody -// - -btMultiBody::btMultiBody(int n_links, - btScalar mass, - const btVector3 &inertia, - bool fixedBase, - bool canSleep, - bool /*deprecatedUseMultiDof*/) - : m_baseCollider(0), - m_baseName(0), - m_basePos(0, 0, 0), - m_baseQuat(0, 0, 0, 1), - m_basePos_interpolate(0, 0, 0), - m_baseQuat_interpolate(0, 0, 0, 1), - m_baseMass(mass), - m_baseInertia(inertia), - - m_fixedBase(fixedBase), - m_awake(true), - m_canSleep(canSleep), - m_canWakeup(true), - m_sleepTimer(0), - m_sleepEpsilon(INITIAL_SLEEP_EPSILON), - m_sleepTimeout(INITIAL_SLEEP_TIMEOUT), - - 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_kinematic_calculate_velocity(false) -{ - m_cachedInertiaTopLeft.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0); - m_cachedInertiaTopRight.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0); - 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); - - clearConstraintForces(); - clearForcesAndTorques(); -} - -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*/) -{ - 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_dVector = thisPivotToThisComOffset; - 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].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) -{ - 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_dVector = thisPivotToThisComOffset; - 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_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].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) -{ - 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_jointType = btMultibodyLink::eRevolute; - m_links[i].m_dofCount = 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].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) -{ - 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_jointType = btMultibodyLink::eSpherical; - m_links[i].m_dofCount = 3; - m_links[i].m_posVarCount = 4; - m_links[i].setAxisTop(0, 1.f, 0.f, 0.f); - m_links[i].setAxisTop(1, 0.f, 1.f, 0.f); - m_links[i].setAxisTop(2, 0.f, 0.f, 1.f); - 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_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].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) -{ - 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_dVector.setZero(); - m_links[i].m_eVector = parentComToThisComOffset; - - // - btVector3 vecNonParallelToRotAxis(1, 0, 0); - if (rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999) - vecNonParallelToRotAxis.setValue(0, 1, 0); - // - - m_links[i].m_jointType = btMultibodyLink::ePlanar; - m_links[i].m_dofCount = 3; - m_links[i].m_posVarCount = 3; - btVector3 n = rotationAxis.normalized(); - m_links[i].setAxisTop(0, n[0], n[1], n[2]); - m_links[i].setAxisTop(1, 0, 0, 0); - m_links[i].setAxisTop(2, 0, 0, 0); - m_links[i].setAxisBottom(0, 0, 0, 0); - btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis); - 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].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].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_splitV.resize(0); - m_splitV.resize(6 + m_dofCount); - m_realBuf.resize(6 + m_dofCount + m_dofCount * m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels") - m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) - m_matrixBuf.resize(m_links.size() + 1); - for (int i = 0; i < m_vectorBuf.size(); i++) - { - m_vectorBuf[i].setValue(0, 0, 0); - } - updateLinksDofOffsets(); -} - -int btMultiBody::getParent(int link_num) const -{ - return m_links[link_num].m_parent; -} - -btScalar btMultiBody::getLinkMass(int i) const -{ - return m_links[i].m_mass; -} - -const btVector3 &btMultiBody::getLinkInertia(int i) const -{ - return m_links[i].m_inertiaLocal; -} - -btScalar btMultiBody::getJointPos(int i) const -{ - return m_links[i].m_jointPos[0]; -} - -btScalar btMultiBody::getJointVel(int i) const -{ - return m_realBuf[6 + m_links[i].m_dofOffset]; -} - -btScalar *btMultiBody::getJointPosMultiDof(int i) -{ - return &m_links[i].m_jointPos[0]; -} - -btScalar *btMultiBody::getJointVelMultiDof(int i) -{ - return &m_realBuf[6 + m_links[i].m_dofOffset]; -} - -const btScalar *btMultiBody::getJointPosMultiDof(int i) const -{ - return &m_links[i].m_jointPos[0]; -} - -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(); -} - - -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, const float *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::setJointVel(int i, btScalar qdot) -{ - m_realBuf[6 + m_links[i].m_dofOffset] = qdot; -} - -void btMultiBody::setJointVelMultiDof(int i, const double *qdot) -{ - for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) - m_realBuf[6 + m_links[i].m_dofOffset + dof] = (btScalar)qdot[dof]; -} - -void btMultiBody::setJointVelMultiDof(int i, const float* qdot) -{ - 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 -{ - return m_links[i].m_cachedRVector; -} - -const btQuaternion &btMultiBody::getParentToLocalRot(int i) const -{ - return m_links[i].m_cachedRotParentToThis; -} - -const btVector3 &btMultiBody::getInterpolateRVector(int i) const -{ - return m_links[i].m_cachedRVector_interpolate; -} - -const btQuaternion &btMultiBody::getInterpolateParentToLocalRot(int i) const -{ - return m_links[i].m_cachedRotParentToThis_interpolate; -} - -btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const -{ - btAssert(i >= -1); - btAssert(i < m_links.size()); - if ((i < -1) || (i >= m_links.size())) - { - return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY); - } - - btVector3 result = local_pos; - while (i != -1) - { - // 'result' is in frame i. transform it to frame parent(i) - result += getRVector(i); - result = quatRotate(getParentToLocalRot(i).inverse(), result); - i = getParent(i); - } - - // 'result' is now in the base frame. transform it to world frame - result = quatRotate(getWorldToBaseRot().inverse(), result); - result += getBasePos(); - - 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())) - { - return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY); - } - - if (i == -1) - { - // world to base - return quatRotate(getWorldToBaseRot(), (world_pos - getBasePos())); - } - else - { - // find position in parent frame, then transform to current frame - return quatRotate(getParentToLocalRot(i), worldPosToLocal(getParent(i), world_pos)) - getRVector(i); - } -} - -btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const -{ - btAssert(i >= -1); - btAssert(i < m_links.size()); - if ((i < -1) || (i >= m_links.size())) - { - return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY); - } - - btVector3 result = local_dir; - while (i != -1) - { - result = quatRotate(getParentToLocalRot(i).inverse(), result); - i = getParent(i); - } - result = quatRotate(getWorldToBaseRot().inverse(), result); - return result; -} - -btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const -{ - btAssert(i >= -1); - btAssert(i < m_links.size()); - if ((i < -1) || (i >= m_links.size())) - { - return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY); - } - - if (i == -1) - { - return quatRotate(getWorldToBaseRot(), world_dir); - } - else - { - return quatRotate(getParentToLocalRot(i), worldDirToLocal(getParent(i), world_dir)); - } -} - -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; -} - -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 - 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 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(link.m_cachedRotParentToThis), link.m_cachedRVector, - omega[parent + 1], vel[parent + 1], - omega[i + 1], vel[i + 1]); - - // now add qidot * shat_i - const btScalar* jointVel = getJointVelMultiDof(i); - for (int dof = 0; dof < link.m_dofCount; ++dof) - { - omega[i + 1] += jointVel[dof] * link.getAxisTop(dof); - vel[i + 1] += jointVel[dof] * link.getAxisBottom(dof); - } - } -} - - -void btMultiBody::clearConstraintForces() -{ - m_baseConstraintForce.setValue(0, 0, 0); - m_baseConstraintTorque.setValue(0, 0, 0); - - for (int i = 0; i < getNumLinks(); ++i) - { - m_links[i].m_appliedConstraintForce.setValue(0, 0, 0); - m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0); - } -} -void btMultiBody::clearForcesAndTorques() -{ - 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); - 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) - { - m_realBuf[i] = 0.f; - } -} -void btMultiBody::addLinkForce(int i, const btVector3 &f) -{ - m_links[i].m_appliedForce += f; -} - -void btMultiBody::addLinkTorque(int i, const btVector3 &t) -{ - m_links[i].m_appliedTorque += t; -} - -void btMultiBody::addLinkConstraintForce(int i, const btVector3 &f) -{ - m_links[i].m_appliedConstraintForce += f; -} - -void btMultiBody::addLinkConstraintTorque(int i, const btVector3 &t) -{ - m_links[i].m_appliedConstraintTorque += t; -} - -void btMultiBody::addJointTorque(int i, btScalar Q) -{ - m_links[i].m_jointTorque[0] += Q; -} - -void btMultiBody::addJointTorqueMultiDof(int i, int dof, btScalar Q) -{ - m_links[i].m_jointTorque[dof] += Q; -} - -void btMultiBody::addJointTorqueMultiDof(int i, const btScalar *Q) -{ - 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 -{ - return m_links[i].m_appliedForce; -} - -const btVector3 &btMultiBody::getLinkTorque(int i) const -{ - return m_links[i].m_appliedTorque; -} - -btScalar btMultiBody::getJointTorque(int i) const -{ - return m_links[i].m_jointTorque[0]; -} - -btScalar *btMultiBody::getJointTorqueMultiDof(int i) -{ - return &m_links[i].m_jointTorque[0]; -} - -bool btMultiBody::hasFixedBase() const -{ - return m_fixedBase || (getBaseCollider() && getBaseCollider()->isStaticObject()); -} - -bool btMultiBody::isBaseStaticOrKinematic() const -{ - return m_fixedBase || (getBaseCollider() && getBaseCollider()->isStaticOrKinematicObject()); -} - -bool btMultiBody::isBaseKinematic() const -{ - return getBaseCollider() && getBaseCollider()->isKinematicObject(); -} - -void btMultiBody::setBaseDynamicType(int dynamicType) -{ - if(getBaseCollider()) { - int oldFlags = getBaseCollider()->getCollisionFlags(); - oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT); - getBaseCollider()->setCollisionFlags(oldFlags | dynamicType); - } -} - -inline btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1) //renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross? -{ - btVector3 row0 = btVector3( - 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, - bool jointFeedbackInWorldSpace, - bool jointFeedbackInJointFrame) -{ - // 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). - - // 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) - - m_internalNeedsJointFeedback = false; - - int num_links = getNumLinks(); - - 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 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 - - 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) - btSpatialMotionVector *spatVel = (btSpatialMotionVector *)v_ptr; - v_ptr += num_links * 2 + 2; - // - // 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; - 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; - // - // 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 - btSpatialTransformationMatrix fromWorld; - fromWorld.m_trnVec.setZero(); - ///////////////// - - // 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. - - 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 (isBaseStaticOrKinematic()) - { - zeroAccSpatFrc[0].setZero(); - } - 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)); - - //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())); - - // - //p += vhat x Ihat vhat - done in a simpler way - if (m_useGyroTerm) - 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]; - - // - 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]); - - // now set vhat_i to its true value by doing - // vhat_i += qidot * shat_i - if (!m_useGlobalVelocities) - { - spatJointVel.setZero(); - - 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; - - // - // 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_absFrameLocVelocity, spatJointVel); - } - - // we can now calculate chat_i - spatVel[i + 1].cross(spatJointVel, spatCoriolisAcc[i]); - - // calculate zhat_i^A - // - if (isLinkAndAllAncestorsKinematic(i)) - { - zeroAccSpatFrc[i].setZero(); - } - else{ - //external forces - btVector3 linkAppliedForce = isConstraintPass ? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce; - btVector3 linkAppliedTorque = isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque; - - zeroAccSpatFrc[i + 1].setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce)); - -#if 0 - { - - b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f", - i+1, - zeroAccSpatFrc[i+1].m_topVec[0], - zeroAccSpatFrc[i+1].m_topVec[1], - zeroAccSpatFrc[i+1].m_topVec[2], - - zeroAccSpatFrc[i+1].m_bottomVec[0], - zeroAccSpatFrc[i+1].m_bottomVec[1], - zeroAccSpatFrc[i+1].m_bottomVec[2]); - } -#endif - // - //adding damping terms (only) - btScalar linDampMult = 1., angDampMult = 1.; - zeroAccSpatFrc[i + 1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i + 1].getAngular().safeNorm()), - linDampMult * m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm())); - //p += vhat x Ihat vhat - done in a simpler way - if (m_useGyroTerm) - zeroAccSpatFrc[i + 1].addAngular(spatVel[i + 1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular())); - // - zeroAccSpatFrc[i + 1].addLinear(m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear())); - // - //btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()); - ////clamp parent's omega - //btScalar parOmegaMod = temp.length(); - //btScalar parOmegaModMax = 1000; - //if(parOmegaMod > parOmegaModMax) - // temp *= parOmegaModMax / parOmegaMod; - //zeroAccSpatFrc[i+1].addLinear(temp); - //printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length()); - //temp = spatCoriolisAcc[i].getLinear(); - //printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length()); - } - - // calculate Ihat_i^A - //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs) - 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])); - - //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("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) - { - if(isLinkAndAllAncestorsKinematic(i)) - continue; - const int parent = m_links[i].m_parent; - fromParent.m_rotMat = rot_from_parent[i + 1]; - fromParent.m_trnVec = m_links[i].m_cachedRVector; - - 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]; - // - 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) - { - btScalar *D_row = &D[dof * m_links[i].m_dofCount]; - 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) - { - case btMultibodyLink::ePrismatic: - case btMultibodyLink::eRevolute: - { - if (D[0] >= SIMD_EPSILON) - { - invDi[0] = 1.0f / D[0]; - } - else - { - invDi[0] = 0; - } - break; - } - case btMultibodyLink::eSpherical: - case btMultibodyLink::ePlanar: - { - const btMatrix3x3 D3x3(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]); - const btMatrix3x3 invD3x3(D3x3.inverse()); - - //unroll the loop? - for (int row = 0; row < 3; ++row) - { - for (int col = 0; col < 3; ++col) - { - invDi[row * 3 + col] = invD3x3[row][col]; - } - } - - break; - } - default: - { - } - } - - //determine h*D^{-1} - 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) - { - 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]; - - //determine (h*D^{-1}) * h^{T} - 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) - { - invD_times_Y[dof] = 0.f; - - 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]; - } - } - - 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]; - } - - fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]); - - zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1]; - } - - // Second 'upward' loop - // (part of TreeForwardDynamics in Mirtich) - - if (isBaseStaticOrKinematic()) - { - spatAcc[0].setZero(); - } - else - { - 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(); - } - - solveImatrix(zeroAccSpatFrc[0], result); - spatAcc[0] = -result; - } - - // 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 - //or - // 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; - - fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]); - - if(!isLinkAndAllAncestorsKinematic(i)) - { - for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) - { - const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; - // - Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof); - } - btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset]; - //D^{-1} * (Y - h^{T}*apar) - mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]); - - spatAcc[i + 1] += spatCoriolisAcc[i]; - - for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) - spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof]; - } - - 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; - - 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); - } - - 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 - { - if (isConstraintPass) - { - 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; - } - } - } - } - - // transform base accelerations back to the world frame. - const btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular(); - output[0] = omegadot_out[0]; - output[1] = omegadot_out[1]; - output[2] = omegadot_out[2]; - - const btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear())); - output[3] = vdot_out[0]; - output[4] = vdot_out[1]; - output[5] = vdot_out[2]; - - ///////////////// - //printf("q = ["); - //printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f ", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z()); - //for(int link = 0; link < getNumLinks(); ++link) - // for(int dof = 0; dof < m_links[link].m_dofCount; ++dof) - // printf("%.6f ", m_links[link].m_jointPos[dof]); - //printf("]\n"); - //// - //printf("qd = ["); - //for(int dof = 0; dof < getNumDofs() + 6; ++dof) - // printf("%.6f ", m_realBuf[dof]); - //printf("]\n"); - //printf("qdd = ["); - //for(int dof = 0; dof < getNumDofs() + 6; ++dof) - // printf("%.6f ", output[dof]); - //printf("]\n"); - ///////////////// - - // Final step: add the accelerations (times dt) to the velocities. - - if (!isConstraintPass) - { - if (dt > 0.) - applyDeltaVeeMultiDof(output, dt); - } - ///// - //btScalar angularThres = 1; - //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.) - //{ - // for(int link = 0; link < m_links.size(); ++link) - // { - // if(m_links[link].m_jointType == btMultibodyLink::eRevolute || m_links[link].m_jointType == btMultibodyLink::eSpherical) - // { - // for(int dof = 0; dof < m_links[link].m_dofCount; ++dof) - // getJointVelMultiDof(link)[dof] *= scaleDown; - // } - // } - //} - ///// - - ///////////////////// - if (m_useGlobalVelocities) - { - 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]); - //nice alternative below (using operator *) but it generates temps - ///////////////////////////////////////////////////////////// - - // now set vhat_i to its true value by doing - // vhat_i += qidot * shat_i - spatJointVel.setZero(); - - 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; - - 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 -{ - int num_links = getNumLinks(); - ///solve I * x = rhs, so the result = invI * rhs - 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; - } - } - else - { - if (!m_cachedInertiaValid) - { - for (int i = 0; i < 6; i++) - { - result[i] = 0.f; - } - return; - } - /// Special routine for calculating the inverse of a spatial inertia matrix - ///the 6x6 matrix is stored as 4 blocks of 3x3 matrices - btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse() * -1.f; - btMatrix3x3 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; - 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]; - } - } -} -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) - { - // 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 - { - /// 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)); - return; - } - btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse() * -1.f; - btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv; - btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse(); - tmp = invIupper_right * m_cachedInertiaLowerRight; - btMatrix3x3 invI_upper_left = (tmp * Binv); - btMatrix3x3 invI_lower_right = (invI_upper_left).transpose(); - tmp = m_cachedInertiaTopLeft * invI_upper_left; - tmp[0][0] -= 1.0; - tmp[1][1] -= 1.0; - tmp[2][2] -= 1.0; - 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); - } - } -} - -void btMultiBody::mulMatrix(const btScalar *pA, const btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const -{ - for (int row = 0; row < rowsA; row++) - { - for (int col = 0; col < colsB; col++) - { - pC[row * colsB + col] = 0.f; - for (int inner = 0; inner < rowsB; inner++) - { - pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB]; - } - } - } -} - -void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, - 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 - - 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]; - - // 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]; - - // 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; - //////////////// - //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; - ///////////////// - - // First 'upward' loop. - // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. - - // Fill in zero_acc - // -- set to force/torque on the base, zero otherwise - if (isBaseStaticOrKinematic()) - { - 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) - { - zeroAccSpatFrc[i + 1].setZero(); - } - - // 'Downward' loop. - // (part of TreeForwardDynamics in Mirtich.) - for (int i = num_links - 1; i >= 0; --i) - { - if(isLinkAndAllAncestorsKinematic(i)) - continue; - const int parent = m_links[i].m_parent; - fromParent.m_rotMat = rot_from_parent[i + 1]; - fromParent.m_trnVec = m_links[i].m_cachedRVector; - - 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]); - } - - 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) - { - invD_times_Y[dof] = 0.f; - - 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]; - } - } - - // 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]; - } - - 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; - - // Second 'upward' loop - // (part of TreeForwardDynamics in Mirtich) - - if (isBaseStaticOrKinematic()) - { - 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) - { - if(isLinkAndAllAncestorsKinematic(i)) - continue; - const int parent = m_links[i].m_parent; - fromParent.m_rotMat = rot_from_parent[i + 1]; - fromParent.m_trnVec = m_links[i].m_cachedRVector; - - 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); - } - - 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]; - } - - // 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(); - output[3] = vdot_out[0]; - output[4] = vdot_out[1]; - output[5] = vdot_out[2]; - - ///////////////// - //printf("delta = ["); - //for(int dof = 0; dof < getNumDofs() + 6; ++dof) - // printf("%.2f ", output[dof]); - //printf("]\n"); - ///////////////// -} -void btMultiBody::predictPositionsMultiDof(btScalar dt) -{ - int num_links = getNumLinks(); - if(!isBaseKinematic()) - { - // step position by adding dt * velocity - //btVector3 v = getBaseVel(); - //m_basePos += dt * v; - // - btScalar *pBasePos; - btScalar *pBaseVel = &m_realBuf[3]; //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) - - // reset to current position - for (int i = 0; i < 3; ++i) - { - m_basePos_interpolate[i] = m_basePos[i]; - } - pBasePos = m_basePos_interpolate; - - pBasePos[0] += dt * pBaseVel[0]; - pBasePos[1] += dt * pBaseVel[1]; - pBasePos[2] += dt * pBaseVel[2]; - } - - /////////////////////////////// - //local functor for quaternion integration (to avoid error prone redundancy) - struct - { - //"exponential map" based on btTransformUtil::integrateTransform(..) - void operator()(const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt) - { - //baseBody => quat is alias and omega is global coor - //!baseBody => quat is alibi and omega is local coor - - btVector3 axis; - btVector3 angvel; - - if (!baseBody) - angvel = quatRotate(quat, omega); //if quat is not m_baseQuat, it is alibi => ok - else - angvel = omega; - - btScalar fAngle = angvel.length(); - //limit the angular motion - if (fAngle * dt > ANGULAR_MOTION_THRESHOLD) - { - fAngle = btScalar(0.5) * SIMD_HALF_PI / dt; - } - - if (fAngle < btScalar(0.001)) - { - // use Taylor's expansions of sync function - axis = angvel * (btScalar(0.5) * dt - (dt * dt * dt) * (btScalar(0.020833333333)) * fAngle * fAngle); - } - else - { - // sync(fAngle) = sin(c*fAngle)/t - axis = angvel * (btSin(btScalar(0.5) * fAngle * dt) / fAngle); - } - - if (!baseBody) - quat = btQuaternion(axis.x(), axis.y(), axis.z(), btCos(fAngle * dt * btScalar(0.5))) * quat; - else - quat = quat * btQuaternion(-axis.x(), -axis.y(), -axis.z(), btCos(fAngle * dt * btScalar(0.5))); - //equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse(); - - quat.normalize(); - } - } pQuatUpdateFun; - /////////////////////////////// - - //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); - // - if(!isBaseKinematic()) - { - btScalar *pBaseQuat; - - // reset to current orientation - for (int i = 0; i < 4; ++i) - { - m_baseQuat_interpolate[i] = m_baseQuat[i]; - } - pBaseQuat = m_baseQuat_interpolate; - - btScalar *pBaseOmega = &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) - // - btQuaternion baseQuat; - baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); - btVector3 baseOmega; - baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); - pQuatUpdateFun(baseOmega, baseQuat, true, dt); - pBaseQuat[0] = baseQuat.x(); - pBaseQuat[1] = baseQuat.y(); - pBaseQuat[2] = baseQuat.z(); - pBaseQuat[3] = baseQuat.w(); - } - - // Finally we can update m_jointPos for each of the m_links - for (int i = 0; i < num_links; ++i) - { - btScalar *pJointPos; - pJointPos = &m_links[i].m_jointPos_interpolate[0]; - - if (m_links[i].m_collider && m_links[i].m_collider->isStaticOrKinematic()) - { - switch (m_links[i].m_jointType) - { - case btMultibodyLink::ePrismatic: - case btMultibodyLink::eRevolute: - { - pJointPos[0] = m_links[i].m_jointPos[0]; - break; - } - case btMultibodyLink::eSpherical: - { - for (int j = 0; j < 4; ++j) - { - pJointPos[j] = m_links[i].m_jointPos[j]; - } - break; - } - case btMultibodyLink::ePlanar: - { - for (int j = 0; j < 3; ++j) - { - pJointPos[j] = m_links[i].m_jointPos[j]; - } - break; - } - default: - break; - } - } - else - { - btScalar *pJointVel = getJointVelMultiDof(i); - - switch (m_links[i].m_jointType) - { - case btMultibodyLink::ePrismatic: - case btMultibodyLink::eRevolute: - { - //reset to current pos - pJointPos[0] = m_links[i].m_jointPos[0]; - btScalar jointVel = pJointVel[0]; - pJointPos[0] += dt * jointVel; - break; - } - case btMultibodyLink::eSpherical: - { - //reset to current pos - - for (int j = 0; j < 4; ++j) - { - pJointPos[j] = m_links[i].m_jointPos[j]; - } - - btVector3 jointVel; - jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); - btQuaternion jointOri; - jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); - pQuatUpdateFun(jointVel, jointOri, false, dt); - pJointPos[0] = jointOri.x(); - pJointPos[1] = jointOri.y(); - pJointPos[2] = jointOri.z(); - pJointPos[3] = jointOri.w(); - break; - } - case btMultibodyLink::ePlanar: - { - for (int j = 0; j < 3; ++j) - { - pJointPos[j] = m_links[i].m_jointPos[j]; - } - pJointPos[0] += dt * getJointVelMultiDof(i)[0]; - - btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); - btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2); - pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt; - pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt; - break; - } - default: - { - } - } - } - - m_links[i].updateInterpolationCacheMultiDof(); - } -} - -void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd) -{ - int num_links = getNumLinks(); - if(!isBaseKinematic()) - { - // step position by adding dt * velocity - //btVector3 v = getBaseVel(); - //m_basePos += dt * v; - // - btScalar *pBasePos = (pq ? &pq[4] : m_basePos); - btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) - - pBasePos[0] += dt * pBaseVel[0]; - pBasePos[1] += dt * pBaseVel[1]; - pBasePos[2] += dt * pBaseVel[2]; - } - - /////////////////////////////// - //local functor for quaternion integration (to avoid error prone redundancy) - struct - { - //"exponential map" based on btTransformUtil::integrateTransform(..) - void operator()(const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt) - { - //baseBody => quat is alias and omega is global coor - //!baseBody => quat is alibi and omega is local coor - - btVector3 axis; - btVector3 angvel; - - if (!baseBody) - angvel = quatRotate(quat, omega); //if quat is not m_baseQuat, it is alibi => ok - else - angvel = omega; - - btScalar fAngle = angvel.length(); - //limit the angular motion - if (fAngle * dt > ANGULAR_MOTION_THRESHOLD) - { - fAngle = btScalar(0.5) * SIMD_HALF_PI / dt; - } - - if (fAngle < btScalar(0.001)) - { - // use Taylor's expansions of sync function - axis = angvel * (btScalar(0.5) * dt - (dt * dt * dt) * (btScalar(0.020833333333)) * fAngle * fAngle); - } - else - { - // sync(fAngle) = sin(c*fAngle)/t - axis = angvel * (btSin(btScalar(0.5) * fAngle * dt) / fAngle); - } - - if (!baseBody) - quat = btQuaternion(axis.x(), axis.y(), axis.z(), btCos(fAngle * dt * btScalar(0.5))) * quat; - else - quat = quat * btQuaternion(-axis.x(), -axis.y(), -axis.z(), btCos(fAngle * dt * btScalar(0.5))); - //equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse(); - - quat.normalize(); - } - } pQuatUpdateFun; - /////////////////////////////// - - //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); - // - if(!isBaseKinematic()) - { - btScalar *pBaseQuat = pq ? pq : m_baseQuat; - btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) - // - btQuaternion baseQuat; - baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); - btVector3 baseOmega; - baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); - pQuatUpdateFun(baseOmega, baseQuat, true, dt); - pBaseQuat[0] = baseQuat.x(); - pBaseQuat[1] = baseQuat.y(); - pBaseQuat[2] = baseQuat.z(); - pBaseQuat[3] = baseQuat.w(); - - //printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z()); - //printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z()); - //printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w()); - } - - if (pq) - pq += 7; - if (pqd) - pqd += 6; - - // Finally we can update m_jointPos for each of the m_links - for (int i = 0; i < num_links; ++i) - { - if (!(m_links[i].m_collider && m_links[i].m_collider->isStaticOrKinematic())) - { - btScalar *pJointPos; - pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]); - - btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i)); - - switch (m_links[i].m_jointType) - { - case btMultibodyLink::ePrismatic: - case btMultibodyLink::eRevolute: - { - //reset to current pos - btScalar jointVel = pJointVel[0]; - pJointPos[0] += dt * jointVel; - break; - } - case btMultibodyLink::eSpherical: - { - //reset to current pos - btVector3 jointVel; - jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); - btQuaternion jointOri; - jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); - pQuatUpdateFun(jointVel, jointOri, false, dt); - pJointPos[0] = jointOri.x(); - pJointPos[1] = jointOri.y(); - pJointPos[2] = jointOri.z(); - pJointPos[3] = jointOri.w(); - break; - } - case btMultibodyLink::ePlanar: - { - pJointPos[0] += dt * getJointVelMultiDof(i)[0]; - - btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); - btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2); - pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt; - pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt; - - break; - } - default: - { - } - } - } - - m_links[i].updateCacheMultiDof(pq); - - if (pq) - pq += m_links[i].m_posVarCount; - 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_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; - - 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 &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); - 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]; - - //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_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) - { - jac[i] = 0; - } - - // 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; - - // calculate the jacobian entry - 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)); - break; - } - case btMultibodyLink::ePrismatic: - { - 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)); - - 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)); - - break; - } - default: - { - } - } - } - - // Now copy through to output. - //printf("jac[%d] = ", link); - while (link != -1) - { - 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; -} - -void btMultiBody::goToSleep() -{ - m_awake = false; -} - -void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep) -{ - extern bool 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; - { - for (int i = 0; i < 6 + m_dofCount; ++i) - motion += m_realBuf[i] * m_realBuf[i]; - } - - if (motion < m_sleepEpsilon) - { - m_sleepTimer += timestep; - if (m_sleepTimer > m_sleepTimeout) - { - goToSleep(); - } - } - else - { - m_sleepTimer = 0; - if (m_canWakeup) - { - if (!m_awake) - wakeUp(); - } - } -} - -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]; - - 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); - } - - int nLinks = getNumLinks(); - ///base + num m_links - 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++) - { - 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))); - } - - for (int link = 0; link < getNumLinks(); link++) - { - 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()}; - btTransform tr; - tr.setIdentity(); - tr.setOrigin(posr); - 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) -{ - 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()}; - btTransform tr; - tr.setIdentity(); - tr.setOrigin(posr); - tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); - - getBaseCollider()->setWorldTransform(tr); - getBaseCollider()->setInterpolationWorldTransform(tr); - } - - 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))); - } - - for (int m = 0; m < getNumLinks(); m++) - { - btMultiBodyLinkCollider *col = getLink(m).m_collider; - if (col) - { - int link = col->m_link; - btAssert(link == m); - - int index = link + 1; - - btVector3 posr = local_origin[index]; - // float pos[4]={posr.x(),posr.y(),posr.z(),1}; - btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()}; - btTransform tr; - tr.setIdentity(); - tr.setOrigin(posr); - tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); - - col->setWorldTransform(tr); - col->setInterpolationWorldTransform(tr); - } - } -} - -void btMultiBody::updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin) -{ - world_to_local.resize(getNumLinks() + 1); - local_origin.resize(getNumLinks() + 1); - - if(isBaseKinematic()){ - world_to_local[0] = getWorldToBaseRot(); - local_origin[0] = getBasePos(); - } - else - { - world_to_local[0] = getInterpolateWorldToBaseRot(); - local_origin[0] = getInterpolateBasePos(); - } - - if (getBaseCollider()) - { - btVector3 posr = local_origin[0]; - // float pos[4]={posr.x(),posr.y(),posr.z(),1}; - btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()}; - btTransform tr; - tr.setIdentity(); - tr.setOrigin(posr); - tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); - - getBaseCollider()->setInterpolationWorldTransform(tr); - } - - for (int k = 0; k < getNumLinks(); k++) - { - const int parent = getParent(k); - world_to_local[k + 1] = getInterpolateParentToLocalRot(k) * world_to_local[parent + 1]; - local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getInterpolateRVector(k))); - } - - for (int m = 0; m < getNumLinks(); m++) - { - btMultiBodyLinkCollider *col = getLink(m).m_collider; - if (col) - { - int link = col->m_link; - btAssert(link == m); - - int index = link + 1; - - btVector3 posr = local_origin[index]; - // float pos[4]={posr.x(),posr.y(),posr.z(),1}; - btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()}; - btTransform tr; - tr.setIdentity(); - tr.setOrigin(posr); - tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); - - col->setInterpolationWorldTransform(tr); - } - } -} - -int btMultiBody::calculateSerializeBufferSize() const -{ - int sz = sizeof(btMultiBodyData); - return sz; -} - -///fills the dataBuffer and returns the struct name (and 0 on failure) -const char *btMultiBody::serialize(void *dataBuffer, class btSerializer *serializer) const -{ - 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) - { - serializer->serializeName(name); - } - } - 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++) - { - 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]; - } - 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) - { - serializer->serializeName(name); - } - } - { - char *name = (char *)serializer->findNameForPointer(m_links[i].m_jointName); - memPtr->m_jointName = (char *)serializer->getUniquePointer(name); - if (memPtr->m_jointName) - { - serializer->serializeName(name); - } - } - memPtr->m_linkCollider = (btCollisionObjectData *)serializer->getUniquePointer(getLink(i).m_collider); - } - serializer->finalizeChunk(chunk, btMultiBodyLinkDataName, BT_ARRAY_CODE, (void *)&m_links[0]); - } - mbd->m_links = mbd->m_numLinks ? (btMultiBodyLinkData *)serializer->getUniquePointer((void *)&m_links[0]) : 0; - - // Fill padding with zeros to appease msan. -#ifdef BT_USE_DOUBLE_PRECISION - memset(mbd->m_padding, 0, sizeof(mbd->m_padding)); -#endif - - return btMultiBodyDataName; -} - -void btMultiBody::saveKinematicState(btScalar timeStep) -{ - //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities - if (m_kinematic_calculate_velocity && timeStep != btScalar(0.)) - { - btVector3 linearVelocity, angularVelocity; - btTransformUtil::calculateVelocity(getInterpolateBaseWorldTransform(), getBaseWorldTransform(), timeStep, linearVelocity, angularVelocity); - setBaseVel(linearVelocity); - setBaseOmega(angularVelocity); - setInterpolateBaseWorldTransform(getBaseWorldTransform()); - } -} - -void btMultiBody::setLinkDynamicType(const int i, int type) -{ - if (i == -1) - { - setBaseDynamicType(type); - } - else if (i >= 0 && i < getNumLinks()) - { - if (m_links[i].m_collider) - { - m_links[i].m_collider->setDynamicType(type); - } - } -} - -bool btMultiBody::isLinkStaticOrKinematic(const int i) const -{ - if (i == -1) - { - return isBaseStaticOrKinematic(); - } - else - { - if (m_links[i].m_collider) - return m_links[i].m_collider->isStaticOrKinematic(); - } - return false; -} - -bool btMultiBody::isLinkKinematic(const int i) const -{ - if (i == -1) - { - return isBaseKinematic(); - } - else - { - if (m_links[i].m_collider) - return m_links[i].m_collider->isKinematic(); - } - return false; -} - -bool btMultiBody::isLinkAndAllAncestorsStaticOrKinematic(const int i) const -{ - int link = i; - while (link != -1) { - if (!isLinkStaticOrKinematic(link)) - return false; - link = m_links[link].m_parent; - } - return isBaseStaticOrKinematic(); -} - -bool btMultiBody::isLinkAndAllAncestorsKinematic(const int i) const -{ - int link = i; - while (link != -1) { - if (!isLinkKinematic(link)) - return false; - link = m_links[link].m_parent; - } - return isBaseKinematic(); -} diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h deleted file mode 100644 index 345970d261..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h +++ /dev/null @@ -1,953 +0,0 @@ -/* - * PURPOSE: - * Class representing an articulated rigid body. Stores the body's - * current state, allows forces and torques to be set, handles - * timestepping and implements Featherstone's algorithm. - * - * COPYRIGHT: - * Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013 - * Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix) - * Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements - - This software is provided 'as-is', without any express or implied warranty. - In no event will the authors be held liable for any damages arising from the use of this software. - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it freely, - subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. - - */ - -#ifndef BT_MULTIBODY_H -#define BT_MULTIBODY_H - -#include "LinearMath/btScalar.h" -#include "LinearMath/btVector3.h" -#include "LinearMath/btQuaternion.h" -#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" -#else -#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 -{ -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); - - virtual ~btMultiBody(); - - //note: fixed link collision with parent is always disabled - void setupFixed(int i, //linkIndex - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, - const btVector3 &parentComToThisPivotOffset, - const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision = true); - - void setupPrismatic(int i, - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, - const btVector3 &jointAxis, - const btVector3 &parentComToThisPivotOffset, - const btVector3 &thisPivotToThisComOffset, - bool disableParentCollision); - - void setupRevolute(int i, // 0 to num_links-1 - btScalar mass, - const btVector3 &inertia, - int parentIndex, - const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 - const btVector3 &jointAxis, // in my frame - const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame - const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame - bool disableParentCollision = false); - - void setupSpherical(int i, // linkIndex, 0 to num_links-1 - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 - const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame - const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame - bool disableParentCollision = false); - - void setupPlanar(int i, // 0 to num_links-1 - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 - const btVector3 &rotationAxis, - const btVector3 &parentComToThisComOffset, // vector from parent COM to this COM, in PARENT frame - bool disableParentCollision = false); - - const btMultibodyLink &getLink(int index) const - { - return m_links[index]; - } - - btMultibodyLink &getLink(int index) - { - return m_links[index]; - } - - void setBaseCollider(btMultiBodyLinkCollider * collider) //collider can be NULL to disable collision for the base - { - m_baseCollider = collider; - } - const btMultiBodyLinkCollider *getBaseCollider() const - { - return m_baseCollider; - } - btMultiBodyLinkCollider *getBaseCollider() - { - return m_baseCollider; - } - - const btMultiBodyLinkCollider *getLinkCollider(int index) const - { - if (index >= 0 && index < getNumLinks()) - { - return getLink(index).m_collider; - } - return 0; - } - - btMultiBodyLinkCollider *getLinkCollider(int index) - { - if (index >= 0 && index < getNumLinks()) - { - return getLink(index).m_collider; - } - return 0; - } - - // - // get parent - // input: link num from 0 to num_links-1 - // output: link num from 0 to num_links-1, OR -1 to mean the base. - // - int getParent(int link_num) const; - - // - // get number of m_links, masses, moments of inertia - // - - int getNumLinks() const { return m_links.size(); } - int getNumDofs() const { return m_dofCount; } - int getNumPosVars() const { return m_posVarCnt; } - btScalar getBaseMass() const { return m_baseMass; } - const btVector3 &getBaseInertia() const { return m_baseInertia; } - btScalar getLinkMass(int i) const; - const btVector3 &getLinkInertia(int i) const; - - // - // change mass (incomplete: can only change base mass and inertia at present) - // - - 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 - // - - 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; - } - - const btVector3 &getInterpolateBasePos() const - { - return m_basePos_interpolate; - } // in world frame - const btQuaternion &getInterpolateWorldToBaseRot() const - { - return m_baseQuat_interpolate; - } - - // rotates world vectors into base frame - btVector3 getBaseOmega() const { return btVector3(m_realBuf[0], m_realBuf[1], m_realBuf[2]); } // in world frame - - void setBasePos(const btVector3 &pos) - { - m_basePos = pos; - if(!isBaseKinematic()) - m_basePos_interpolate = pos; - } - - void setInterpolateBasePos(const btVector3 &pos) - { - m_basePos_interpolate = pos; - } - - void setBaseWorldTransform(const btTransform &tr) - { - setBasePos(tr.getOrigin()); - setWorldToBaseRot(tr.getRotation().inverse()); - } - - btTransform getBaseWorldTransform() const - { - btTransform tr; - tr.setOrigin(getBasePos()); - tr.setRotation(getWorldToBaseRot().inverse()); - return tr; - } - - void setInterpolateBaseWorldTransform(const btTransform &tr) - { - setInterpolateBasePos(tr.getOrigin()); - setInterpolateWorldToBaseRot(tr.getRotation().inverse()); - } - - btTransform getInterpolateBaseWorldTransform() const - { - btTransform tr; - tr.setOrigin(getInterpolateBasePos()); - tr.setRotation(getInterpolateWorldToBaseRot().inverse()); - return tr; - } - - void setBaseVel(const btVector3 &vel) - { - m_realBuf[3] = vel[0]; - m_realBuf[4] = vel[1]; - m_realBuf[5] = vel[2]; - } - - void setWorldToBaseRot(const btQuaternion &rot) - { - m_baseQuat = rot; //m_baseQuat asumed to ba alias!? - if(!isBaseKinematic()) - m_baseQuat_interpolate = rot; - } - - void setInterpolateWorldToBaseRot(const btQuaternion &rot) - { - m_baseQuat_interpolate = rot; - } - - void setBaseOmega(const btVector3 &omega) - { - m_realBuf[0] = omega[0]; - m_realBuf[1] = omega[1]; - m_realBuf[2] = omega[2]; - } - - void saveKinematicState(btScalar timeStep); - - // - // 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; - - 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]; - } - - const btScalar *getDeltaVelocityVector() const - { - return &m_deltaV[0]; - } - - const btScalar *getSplitVelocityVector() const - { - return &m_splitV[0]; - } - /* btScalar * getVelocityVector() - { - return &real_buf[0]; - } - */ - - // - // get the frames of reference (positions and orientations) of the child m_links - // (i = 0 to num_links-1) - // - - const btVector3 &getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords - const btQuaternion &getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i. - const btVector3 &getInterpolateRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords - const btQuaternion &getInterpolateParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i. - - // - // transform vectors in local frame of link i to world frame (or vice versa) - // - btVector3 localPosToWorld(int i, const btVector3 &local_pos) const; - btVector3 localDirToWorld(int i, const btVector3 &local_dir) const; - btVector3 worldPosToLocal(int i, const btVector3 &world_pos) const; - btVector3 worldDirToLocal(int i, const btVector3 &world_dir) const; - - // - // transform a frame in local coordinate to a frame in world coordinate - // - btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const; - - - // - // set external forces and torques. Note all external forces/torques are given in the WORLD frame. - // - - 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 addJointTorqueMultiDof(int i, int dof, btScalar Q); - void addJointTorqueMultiDof(int i, const btScalar *Q); - - const btVector3 &getBaseForce() const { return m_baseForce; } - const btVector3 &getBaseTorque() const { return m_baseTorque; } - const btVector3 &getLinkForce(int i) const; - const btVector3 &getLinkTorque(int i) const; - btScalar getJointTorque(int i) const; - btScalar *getJointTorqueMultiDof(int i); - - // - // dynamics routines. - // - - // timestep the velocities (given the external forces/torques set using addBaseForce etc). - // also sets up caches for calcAccelerationDeltas. - // - // Note: the caller must provide three vectors which are used as - // temporary scratch space. The idea here is to reduce dynamic - // memory allocation: the same scratch vectors can be re-used - // again and again for different Multibodies, instead of each - // btMultiBody allocating (and then deallocating) their own - // individual scratch buffers. This gives a considerable speed - // improvement, at least on Windows (where dynamic memory - // allocation appears to be fairly slow). - // - - void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, - btAlignedObjectArray<btScalar> & scratch_r, - btAlignedObjectArray<btVector3> & scratch_v, - btAlignedObjectArray<btMatrix3x3> & scratch_m, - bool isConstraintPass, - 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) - { - for (int dof = 0; dof < 6 + getNumDofs(); ++dof) - { - m_deltaV[dof] += delta_vee[dof] * multiplier; - } - } - void applyDeltaSplitVeeMultiDof(const btScalar *delta_vee, btScalar multiplier) - { - for (int dof = 0; dof < 6 + getNumDofs(); ++dof) - { - m_splitV[dof] += delta_vee[dof] * multiplier; - } - } - void addSplitV() - { - applyDeltaVeeMultiDof(&m_splitV[0], 1); - } - void substractSplitV() - { - applyDeltaVeeMultiDof(&m_splitV[0], -1); - - for (int dof = 0; dof < 6 + getNumDofs(); ++dof) - { - m_splitV[dof] = 0.f; - } - } - void processDeltaVeeMultiDof2() - { - applyDeltaVeeMultiDof(&m_deltaV[0], 1); - - for (int dof = 0; dof < 6 + getNumDofs(); ++dof) - { - m_deltaV[dof] = 0.f; - } - } - - void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier) - { - //for (int dof = 0; dof < 6 + getNumDofs(); ++dof) - // printf("%.4f ", delta_vee[dof]*multiplier); - //printf("\n"); - - //btScalar sum = 0; - //for (int dof = 0; dof < 6 + getNumDofs(); ++dof) - //{ - // sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier; - //} - //btScalar l = btSqrt(sum); - - //if (l>m_maxAppliedImpulse) - //{ - // multiplier *= m_maxAppliedImpulse/l; - //} - - for (int dof = 0; dof < 6 + getNumDofs(); ++dof) - { - m_realBuf[dof] += delta_vee[dof] * multiplier; - btClamp(m_realBuf[dof], -m_maxCoordinateVelocity, m_maxCoordinateVelocity); - } - } - - // timestep the positions (given current velocities). - void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0); - - // predict the positions - void predictPositionsMultiDof(btScalar dt); - - // - // 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. - - 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); } - - //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) - { - if (m_canWakeup) - { - m_canSleep = canSleep; - } - } - - bool getCanSleep() const - { - return m_canSleep; - } - - bool getCanWakeup() const - { - return m_canWakeup; - } - - void setCanWakeup(bool canWakeup) - { - m_canWakeup = canWakeup; - } - bool isAwake() const - { - return m_awake; - } - void wakeUp(); - void goToSleep(); - void checkMotionAndSleepIfRequired(btScalar timestep); - - bool hasFixedBase() const; - - bool isBaseKinematic() const; - - bool isBaseStaticOrKinematic() const; - - // set the dynamic type in the base's collision flags. - void setBaseDynamicType(int dynamicType); - - void setFixedBase(bool fixedBase) - { - m_fixedBase = fixedBase; - if(m_fixedBase) - setBaseDynamicType(btCollisionObject::CF_STATIC_OBJECT); - else - setBaseDynamicType(btCollisionObject::CF_DYNAMIC_OBJECT); - } - - int getCompanionId() const - { - return m_companionId; - } - void setCompanionId(int id) - { - //printf("for %p setCompanionId(%d)\n",this, id); - 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 - { - m_links.resize(numLinks); - } - - btScalar getLinearDamping() const - { - return m_linearDamping; - } - void setLinearDamping(btScalar damp) - { - m_linearDamping = damp; - } - btScalar getAngularDamping() const - { - return m_angularDamping; - } - void setAngularDamping(btScalar damp) - { - m_angularDamping = damp; - } - - bool getUseGyroTerm() const - { - return m_useGyroTerm; - } - void setUseGyroTerm(bool useGyro) - { - m_useGyroTerm = useGyro; - } - btScalar getMaxCoordinateVelocity() const - { - return m_maxCoordinateVelocity; - } - void setMaxCoordinateVelocity(btScalar maxVel) - { - m_maxCoordinateVelocity = maxVel; - } - - btScalar getMaxAppliedImpulse() const - { - return m_maxAppliedImpulse; - } - void setMaxAppliedImpulse(btScalar maxImp) - { - m_maxAppliedImpulse = maxImp; - } - void setHasSelfCollision(bool hasSelfCollision) - { - m_hasSelfCollision = hasSelfCollision; - } - bool hasSelfCollision() const - { - return m_hasSelfCollision; - } - - void finalizeMultiDof(); - - void useRK4Integration(bool use) { m_useRK4 = use; } - bool isUsingRK4Integration() const { return m_useRK4; } - void useGlobalVelocities(bool use) { m_useGlobalVelocities = use; } - bool isUsingGlobalVelocities() const { return m_useGlobalVelocities; } - - bool isPosUpdated() const - { - return __posUpdated; - } - void setPosUpdated(bool updated) - { - __posUpdated = updated; - } - - //internalNeedsJointFeedback is for internal use only - bool internalNeedsJointFeedback() const - { - return m_internalNeedsJointFeedback; - } - void forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to_local, btAlignedObjectArray<btVector3> & local_origin); - - void compTreeLinkVelocities(btVector3 * omega, btVector3 * vel) const; - - void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin); - void updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin); - - virtual int calculateSerializeBufferSize() const; - - ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char *serialize(void *dataBuffer, class btSerializer *serializer) const; - - const char *getBaseName() const - { - return m_baseName; - } - ///memory of setBaseName needs to be manager by user - void setBaseName(const char *name) - { - m_baseName = name; - } - - ///users can point to their objects, userPointer is not used by Bullet - void *getUserPointer() const - { - return m_userObjectPointer; - } - - int getUserIndex() const - { - return m_userIndex; - } - - int getUserIndex2() const - { - return m_userIndex2; - } - ///users can point to their objects, userPointer is not used by Bullet - void setUserPointer(void *userPointer) - { - m_userObjectPointer = userPointer; - } - - ///users can point to their objects, userPointer is not used by Bullet - void setUserIndex(int index) - { - m_userIndex = index; - } - - void setUserIndex2(int index) - { - m_userIndex2 = index; - } - - 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 setLinkDynamicType(const int i, int type); - - bool isLinkStaticOrKinematic(const int i) const; - - bool isLinkKinematic(const int i) const; - - bool isLinkAndAllAncestorsStaticOrKinematic(const int i) const; - - bool isLinkAndAllAncestorsKinematic(const int i) const; - - void setSleepThreshold(btScalar sleepThreshold) - { - m_sleepEpsilon = sleepThreshold; - } - - void setSleepTimeout(btScalar sleepTimeout) - { - this->m_sleepTimeout = sleepTimeout; - } - - -private: - btMultiBody(const btMultiBody &); // not implemented - void operator=(const btMultiBody &); // not implemented - - void solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar result[6]) const; - void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const; - - void updateLinksDofOffsets() - { - int dofOffset = 0, cfgOffset = 0; - for (int bidx = 0; bidx < m_links.size(); ++bidx) - { - m_links[bidx].m_dofOffset = dofOffset; - m_links[bidx].m_cfgOffset = cfgOffset; - dofOffset += m_links[bidx].m_dofCount; - cfgOffset += m_links[bidx].m_posVarCount; - } - } - - void mulMatrix(const btScalar *pA, const btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const; - -private: - btMultiBodyLinkCollider *m_baseCollider; //can be NULL - const char *m_baseName; //memory needs to be manager by user! - - btVector3 m_basePos; // position of COM of base (world frame) - btVector3 m_basePos_interpolate; // position of interpolated COM of base (world frame) - btQuaternion m_baseQuat; // rotates world points into base frame - btQuaternion m_baseQuat_interpolate; - - btScalar m_baseMass; // mass of the base - btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal) - - btVector3 m_baseForce; // external force applied to base. World frame. - btVector3 m_baseTorque; // external torque applied to base. World frame. - - btVector3 m_baseConstraintForce; // external force applied to base. World frame. - btVector3 m_baseConstraintTorque; // external torque applied to base. World frame. - - btAlignedObjectArray<btMultibodyLink> m_links; // array of m_links, excluding the base. index from 0 to num_links-1. - - // - // realBuf: - // offset size array - // 0 6 + num_links v (base_omega; base_vel; joint_vels) MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized] - // 6+num_links num_links D - // - // vectorBuf: - // offset size array - // 0 num_links h_top - // num_links num_links h_bottom - // - // matrixBuf: - // offset size array - // 0 num_links+1 rot_from_parent - // - btAlignedObjectArray<btScalar> m_splitV; - btAlignedObjectArray<btScalar> m_deltaV; - btAlignedObjectArray<btScalar> m_realBuf; - btAlignedObjectArray<btVector3> m_vectorBuf; - btAlignedObjectArray<btMatrix3x3> m_matrixBuf; - - btMatrix3x3 m_cachedInertiaTopLeft; - btMatrix3x3 m_cachedInertiaTopRight; - btMatrix3x3 m_cachedInertiaLowerLeft; - btMatrix3x3 m_cachedInertiaLowerRight; - bool m_cachedInertiaValid; - - bool m_fixedBase; - - // Sleep parameters. - bool m_awake; - bool m_canSleep; - bool m_canWakeup; - btScalar m_sleepTimer; - btScalar m_sleepEpsilon; - btScalar m_sleepTimeout; - - void *m_userObjectPointer; - int m_userIndex2; - int m_userIndex; - - int m_companionId; - btScalar m_linearDamping; - btScalar m_angularDamping; - bool m_useGyroTerm; - btScalar m_maxAppliedImpulse; - btScalar m_maxCoordinateVelocity; - bool m_hasSelfCollision; - - bool __posUpdated; - int m_dofCount, m_posVarCnt; - - bool m_useRK4, m_useGlobalVelocities; - //for global velocities, see 8.3.2B Proposed resolution in Jakub Stepien PhD Thesis - //https://drive.google.com/file/d/0Bz3vEa19XOYGNWdZWGpMdUdqVmZ5ZVBOaEh4ZnpNaUxxZFNV/view?usp=sharing - - ///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only - bool m_internalNeedsJointFeedback; - - //If enabled, calculate the velocity based on kinematic transform changes. Currently only implemented for the base. - bool m_kinematic_calculate_velocity; -}; - -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; -}; - -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; -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -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; -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -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; -}; - -#endif diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp deleted file mode 100644 index 00d5fd5609..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ /dev/null @@ -1,389 +0,0 @@ -#include "btMultiBodyConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro) - -btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA, btMultiBody* bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type) - : m_bodyA(bodyA), - m_bodyB(bodyB), - m_linkA(linkA), - m_linkB(linkB), - m_type(type), - m_numRows(numRows), - m_jacSizeA(0), - m_jacSizeBoth(0), - m_isUnilateral(isUnilateral), - m_numDofsFinalized(-1), - m_maxAppliedImpulse(100) -{ -} - -void btMultiBodyConstraint::updateJacobianSizes() -{ - if (m_bodyA) - { - m_jacSizeA = (6 + m_bodyA->getNumDofs()); - } - - if (m_bodyB) - { - m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs(); - } - else - m_jacSizeBoth = m_jacSizeA; -} - -void btMultiBodyConstraint::allocateJacobiansMultiDof() -{ - updateJacobianSizes(); - - m_posOffset = ((1 + m_jacSizeBoth) * m_numRows); - m_data.resize((2 + m_jacSizeBoth) * m_numRows); -} - -btMultiBodyConstraint::~btMultiBodyConstraint() -{ -} - -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; -} - -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 damping) -{ - 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) - { - 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) - { - 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) - { - rel_vel += rb1->getLinearVelocity().dot(solverConstraint.m_contactNormal2); - rel_vel += rb1->getAngularVelocity().dot(solverConstraint.m_relpos2CrossNormal); - } - - solverConstraint.m_friction = 0.f; //cp.m_combinedFriction; - } - - solverConstraint.m_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; - - //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; - } - /*else - { - //split position and velocity into rhs and m_rhsPenetration - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = penetrationImpulse; - } - */ - - solverConstraint.m_cfm = 0.f; - solverConstraint.m_lowerLimit = lowerLimit; - solverConstraint.m_upperLimit = upperLimit; - } - - return rel_vel; -} diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h deleted file mode 100644 index 1aaa07b69e..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ /dev/null @@ -1,215 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MULTIBODY_CONSTRAINT_H -#define BT_MULTIBODY_CONSTRAINT_H - -#include "LinearMath/btScalar.h" -#include "LinearMath/btAlignedObjectArray.h" -#include "btMultiBody.h" - - -//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility -enum btTypedMultiBodyConstraintType -{ - MULTIBODY_CONSTRAINT_LIMIT=3, - MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR, - MULTIBODY_CONSTRAINT_GEAR, - MULTIBODY_CONSTRAINT_POINT_TO_POINT, - MULTIBODY_CONSTRAINT_SLIDER, - MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR, - MULTIBODY_CONSTRAINT_FIXED, - - MAX_MULTIBODY_CONSTRAINT_TYPE, -}; - -class btMultiBody; -struct btSolverInfo; - -#include "btMultiBodySolverConstraint.h" - -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; -}; - -ATTRIBUTE_ALIGNED16(class) -btMultiBodyConstraint -{ -protected: - btMultiBody* m_bodyA; - btMultiBody* m_bodyB; - int m_linkA; - int m_linkB; - - int m_type; //btTypedMultiBodyConstraintType - - int m_numRows; - int m_jacSizeA; - int m_jacSizeBoth; - int m_posOffset; - - bool m_isUnilateral; - int m_numDofsFinalized; - btScalar m_maxAppliedImpulse; - - // warning: the data block lay out is not consistent for all constraints - // data block laid out as follows: - // cached impulses. (one per row.) - // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc) - // positions. (one per row.) - btAlignedObjectArray<btScalar> m_data; - - void applyDeltaVee(btMultiBodyJacobianData & data, btScalar * delta_vee, btScalar impulse, int velocityIndex, int ndof); - - btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint & solverConstraint, - btMultiBodyJacobianData & data, - btScalar * jacOrgA, btScalar * jacOrgB, - const btVector3& constraintNormalAng, - - const btVector3& constraintNormalLin, - const btVector3& posAworld, const btVector3& posBworld, - btScalar posError, - const btContactSolverInfo& infoGlobal, - btScalar lowerLimit, btScalar upperLimit, - bool angConstraint = false, - - btScalar relaxation = 1.f, - bool isFriction = false, btScalar desiredVelocity = 0, btScalar cfmSlip = 0, btScalar damping = 1.0); - -public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btMultiBodyConstraint(btMultiBody * bodyA, btMultiBody * bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type); - virtual ~btMultiBodyConstraint(); - - void updateJacobianSizes(); - void allocateJacobiansMultiDof(); - - int getConstraintType() const - { - return m_type; - } - //many constraints have setFrameInB/setPivotInB. Will use 'getConstraintType' later. - virtual void setFrameInB(const btMatrix3x3& frameInB) {} - virtual void setPivotInB(const btVector3& pivotInB) {} - - virtual void finalizeMultiDof() = 0; - - virtual int getIslandIdA() const = 0; - virtual int getIslandIdB() const = 0; - - virtual void createConstraintRows(btMultiBodyConstraintArray & constraintRows, - btMultiBodyJacobianData & data, - const btContactSolverInfo& infoGlobal) = 0; - - int getNumRows() const - { - return m_numRows; - } - - btMultiBody* getMultiBodyA() - { - return m_bodyA; - } - btMultiBody* getMultiBodyB() - { - return m_bodyB; - } - - int getLinkA() const - { - return m_linkA; - } - int getLinkB() const - { - return m_linkB; - } - void internalSetAppliedImpulse(int dof, btScalar appliedImpulse) - { - btAssert(dof >= 0); - btAssert(dof < getNumRows()); - m_data[dof] = appliedImpulse; - } - - btScalar getAppliedImpulse(int dof) - { - 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 - { - return m_data[m_posOffset + row]; - } - - 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) - { - return &m_data[m_numRows + row * m_jacSizeBoth]; - } - const btScalar* jacobianA(int row) const - { - return &m_data[m_numRows + (row * m_jacSizeBoth)]; - } - btScalar* jacobianB(int row) - { - return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; - } - const btScalar* jacobianB(int row) const - { - return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; - } - - btScalar getMaxAppliedImpulse() const - { - return m_maxAppliedImpulse; - } - void setMaxAppliedImpulse(btScalar maxImp) - { - m_maxAppliedImpulse = maxImp; - } - - virtual void debugDraw(class btIDebugDraw * drawer) = 0; - - virtual void setGearRatio(btScalar ratio) {} - virtual void setGearAuxLink(int gearAuxLink) {} - virtual void setRelativePositionTarget(btScalar relPosTarget) {} - virtual void setErp(btScalar erp) {} -}; - -#endif //BT_MULTIBODY_CONSTRAINT_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp deleted file mode 100644 index 2788367431..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ /dev/null @@ -1,1752 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btMultiBodyConstraintSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -#include "btMultiBodyLinkCollider.h" - -#include "BulletDynamics/ConstraintSolver/btSolverBody.h" -#include "btMultiBodyConstraint.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" - -#include "LinearMath/btQuickprof.h" -#include "BulletDynamics/Featherstone/btMultiBodySolverConstraint.h" -#include "LinearMath/btScalar.h" - -btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) -{ - btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); - - //solve featherstone non-contact constraints - btScalar nonContactResidual = 0; - //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size()); - for (int i = 0; i < infoGlobal.m_numNonContactInnerIterations; ++i) - { - // reset the nonContactResdual to 0 at start of each inner iteration - nonContactResidual = 0; - for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++) - { - int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j; - - btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index]; - - btScalar residual = resolveSingleConstraintRowGeneric(constraint); - nonContactResidual = btMax(nonContactResidual, residual * residual); - - if (constraint.m_multiBodyA) - constraint.m_multiBodyA->setPosUpdated(false); - if (constraint.m_multiBodyB) - constraint.m_multiBodyB->setPosUpdated(false); - } - } - leastSquaredResidual = btMax(leastSquaredResidual, nonContactResidual); - - //solve featherstone normal contact - for (int j0 = 0; j0 < m_multiBodyNormalContactConstraints.size(); j0++) - { - int index = j0; //iteration&1? j0 : m_multiBodyNormalContactConstraints.size()-1-j0; - - btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[index]; - btScalar residual = 0.f; - - if (iteration < infoGlobal.m_numIterations) - { - residual = resolveSingleConstraintRowGeneric(constraint); - } - - leastSquaredResidual = btMax(leastSquaredResidual, residual * residual); - - if (constraint.m_multiBodyA) - constraint.m_multiBodyA->setPosUpdated(false); - if (constraint.m_multiBodyB) - constraint.m_multiBodyB->setPosUpdated(false); - } - - //solve featherstone frictional contact - if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0)) - { - for (int j1 = 0; j1 < this->m_multiBodySpinningFrictionContactConstraints.size(); j1++) - { - if (iteration < infoGlobal.m_numIterations) - { - int index = j1; - - btMultiBodySolverConstraint& frictionConstraint = m_multiBodySpinningFrictionContactConstraints[index]; - btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; - //adjust friction limits here - if (totalImpulse > btScalar(0)) - { - frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse); - frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse; - btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint); - leastSquaredResidual = btMax(leastSquaredResidual, residual * residual); - - if (frictionConstraint.m_multiBodyA) - frictionConstraint.m_multiBodyA->setPosUpdated(false); - if (frictionConstraint.m_multiBodyB) - frictionConstraint.m_multiBodyB->setPosUpdated(false); - } - } - } - - for (int j1 = 0; j1 < this->m_multiBodyTorsionalFrictionContactConstraints.size(); j1++) - { - if (iteration < infoGlobal.m_numIterations) - { - int index = j1; //iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1; - - btMultiBodySolverConstraint& frictionConstraint = m_multiBodyTorsionalFrictionContactConstraints[index]; - btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; - j1++; - int index2 = j1; - btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyTorsionalFrictionContactConstraints[index2]; - //adjust friction limits here - if (totalImpulse > btScalar(0) && frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex) - { - frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse); - frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse; - frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction * totalImpulse); - frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction * totalImpulse; - - btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB); - leastSquaredResidual = btMax(leastSquaredResidual, residual * residual); - - if (frictionConstraint.m_multiBodyA) - frictionConstraint.m_multiBodyA->setPosUpdated(false); - if (frictionConstraint.m_multiBodyB) - frictionConstraint.m_multiBodyB->setPosUpdated(false); - - if (frictionConstraintB.m_multiBodyA) - frictionConstraintB.m_multiBodyA->setPosUpdated(false); - if (frictionConstraintB.m_multiBodyB) - frictionConstraintB.m_multiBodyB->setPosUpdated(false); - } - } - } - - for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++) - { - if (iteration < infoGlobal.m_numIterations) - { - int index = j1; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; - btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index]; - - btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; - j1++; - int index2 = j1; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; - btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyFrictionContactConstraints[index2]; - btAssert(frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex); - - if (frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex) - { - frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse); - frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse; - frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction * totalImpulse); - frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction * totalImpulse; - btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB); - leastSquaredResidual = btMax(leastSquaredResidual, residual * residual); - - if (frictionConstraintB.m_multiBodyA) - frictionConstraintB.m_multiBodyA->setPosUpdated(false); - if (frictionConstraintB.m_multiBodyB) - frictionConstraintB.m_multiBodyB->setPosUpdated(false); - - if (frictionConstraint.m_multiBodyA) - frictionConstraint.m_multiBodyA->setPosUpdated(false); - if (frictionConstraint.m_multiBodyB) - frictionConstraint.m_multiBodyB->setPosUpdated(false); - } - } - } - } - else - { - for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++) - { - if (iteration < infoGlobal.m_numIterations) - { - int index = j1; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; - - btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index]; - btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; - //adjust friction limits here - if (totalImpulse > btScalar(0)) - { - frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse); - frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse; - btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint); - leastSquaredResidual = btMax(leastSquaredResidual, residual * residual); - - if (frictionConstraint.m_multiBodyA) - frictionConstraint.m_multiBodyA->setPosUpdated(false); - if (frictionConstraint.m_multiBodyB) - frictionConstraint.m_multiBodyB->setPosUpdated(false); - } - } - } - } - return leastSquaredResidual; -} - -btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) -{ - m_multiBodyNonContactConstraints.resize(0); - m_multiBodyNormalContactConstraints.resize(0); - m_multiBodyFrictionContactConstraints.resize(0); - m_multiBodyTorsionalFrictionContactConstraints.resize(0); - m_multiBodySpinningFrictionContactConstraints.resize(0); - - m_data.m_jacobians.resize(0); - m_data.m_deltaVelocitiesUnitImpulse.resize(0); - m_data.m_deltaVelocities.resize(0); - - for (int i = 0; i < numBodies; i++) - { - const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(bodies[i]); - if (fcA) - { - fcA->m_multiBody->setCompanionId(-1); - } - } - - 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) -{ - 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 deltaVelADotn = 0; - btScalar deltaVelBDotn = 0; - btSolverBody* bodyA = 0; - btSolverBody* bodyB = 0; - int ndofA = 0; - int ndofB = 0; - - if (c.m_multiBodyA) - { - ndofA = c.m_multiBodyA->getNumDofs() + 6; - for (int i = 0; i < ndofA; ++i) - deltaVelADotn += m_data.m_jacobians[c.m_jacAindex + i] * m_data.m_deltaVelocities[c.m_deltaVelAindex + i]; - } - else if (c.m_solverBodyIdA >= 0) - { - bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA]; - deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); - } - - if (c.m_multiBodyB) - { - ndofB = c.m_multiBodyB->getNumDofs() + 6; - for (int i = 0; i < ndofB; ++i) - deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex + i] * m_data.m_deltaVelocities[c.m_deltaVelBindex + i]; - } - else if (c.m_solverBodyIdB >= 0) - { - bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB]; - 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; - const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; - - if (sum < c.m_lowerLimit) - { - deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse; - c.m_appliedImpulse = c.m_lowerLimit; - } - else if (sum > c.m_upperLimit) - { - deltaImpulse = c.m_upperLimit - c.m_appliedImpulse; - c.m_appliedImpulse = c.m_upperLimit; - } - else - { - c.m_appliedImpulse = sum; - } - - if (c.m_multiBodyA) - { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA); -#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations - //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse); -#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - } - else if (c.m_solverBodyIdA >= 0) - { - bodyA->internalApplyImpulse(c.m_contactNormal1 * bodyA->internalGetInvMass(), c.m_angularComponentA, deltaImpulse); - } - if (c.m_multiBodyB) - { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse, c.m_deltaVelBindex, ndofB); -#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations - //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse); -#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - } - else if (c.m_solverBodyIdB >= 0) - { - bodyB->internalApplyImpulse(c.m_contactNormal2 * bodyB->internalGetInvMass(), c.m_angularComponentB, deltaImpulse); - } - btScalar deltaVel = deltaImpulse / c.m_jacDiagABInv; - return deltaVel; -} - -btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1, const btMultiBodySolverConstraint& cB) -{ - int ndofA = 0; - int ndofB = 0; - btSolverBody* bodyA = 0; - btSolverBody* bodyB = 0; - btScalar deltaImpulseB = 0.f; - btScalar sumB = 0.f; - { - deltaImpulseB = cB.m_rhs - btScalar(cB.m_appliedImpulse) * cB.m_cfm; - btScalar deltaVelADotn = 0; - btScalar deltaVelBDotn = 0; - if (cB.m_multiBodyA) - { - ndofA = cB.m_multiBodyA->getNumDofs() + 6; - for (int i = 0; i < ndofA; ++i) - deltaVelADotn += m_data.m_jacobians[cB.m_jacAindex + i] * m_data.m_deltaVelocities[cB.m_deltaVelAindex + i]; - } - else if (cB.m_solverBodyIdA >= 0) - { - bodyA = &m_tmpSolverBodyPool[cB.m_solverBodyIdA]; - deltaVelADotn += cB.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cB.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); - } - - if (cB.m_multiBodyB) - { - ndofB = cB.m_multiBodyB->getNumDofs() + 6; - for (int i = 0; i < ndofB; ++i) - deltaVelBDotn += m_data.m_jacobians[cB.m_jacBindex + i] * m_data.m_deltaVelocities[cB.m_deltaVelBindex + i]; - } - else if (cB.m_solverBodyIdB >= 0) - { - bodyB = &m_tmpSolverBodyPool[cB.m_solverBodyIdB]; - deltaVelBDotn += cB.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cB.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); - } - - deltaImpulseB -= deltaVelADotn * cB.m_jacDiagABInv; //m_jacDiagABInv = 1./denom - deltaImpulseB -= deltaVelBDotn * cB.m_jacDiagABInv; - sumB = btScalar(cB.m_appliedImpulse) + deltaImpulseB; - } - - btScalar deltaImpulseA = 0.f; - btScalar sumA = 0.f; - const btMultiBodySolverConstraint& cA = cA1; - { - { - deltaImpulseA = cA.m_rhs - btScalar(cA.m_appliedImpulse) * cA.m_cfm; - btScalar deltaVelADotn = 0; - btScalar deltaVelBDotn = 0; - if (cA.m_multiBodyA) - { - ndofA = cA.m_multiBodyA->getNumDofs() + 6; - for (int i = 0; i < ndofA; ++i) - deltaVelADotn += m_data.m_jacobians[cA.m_jacAindex + i] * m_data.m_deltaVelocities[cA.m_deltaVelAindex + i]; - } - else if (cA.m_solverBodyIdA >= 0) - { - bodyA = &m_tmpSolverBodyPool[cA.m_solverBodyIdA]; - deltaVelADotn += cA.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cA.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); - } - - if (cA.m_multiBodyB) - { - ndofB = cA.m_multiBodyB->getNumDofs() + 6; - for (int i = 0; i < ndofB; ++i) - deltaVelBDotn += m_data.m_jacobians[cA.m_jacBindex + i] * m_data.m_deltaVelocities[cA.m_deltaVelBindex + i]; - } - else if (cA.m_solverBodyIdB >= 0) - { - bodyB = &m_tmpSolverBodyPool[cA.m_solverBodyIdB]; - deltaVelBDotn += cA.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cA.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); - } - - deltaImpulseA -= deltaVelADotn * cA.m_jacDiagABInv; //m_jacDiagABInv = 1./denom - deltaImpulseA -= deltaVelBDotn * cA.m_jacDiagABInv; - sumA = btScalar(cA.m_appliedImpulse) + deltaImpulseA; - } - } - - if (sumA * sumA + sumB * sumB >= cA.m_lowerLimit * cB.m_lowerLimit) - { - btScalar angle = btAtan2(sumA, sumB); - btScalar sumAclipped = btFabs(cA.m_lowerLimit * btSin(angle)); - btScalar sumBclipped = btFabs(cB.m_lowerLimit * btCos(angle)); - - if (sumA < -sumAclipped) - { - deltaImpulseA = -sumAclipped - cA.m_appliedImpulse; - cA.m_appliedImpulse = -sumAclipped; - } - else if (sumA > sumAclipped) - { - deltaImpulseA = sumAclipped - cA.m_appliedImpulse; - cA.m_appliedImpulse = sumAclipped; - } - else - { - cA.m_appliedImpulse = sumA; - } - - if (sumB < -sumBclipped) - { - deltaImpulseB = -sumBclipped - cB.m_appliedImpulse; - cB.m_appliedImpulse = -sumBclipped; - } - else if (sumB > sumBclipped) - { - deltaImpulseB = sumBclipped - cB.m_appliedImpulse; - cB.m_appliedImpulse = sumBclipped; - } - else - { - cB.m_appliedImpulse = sumB; - } - //deltaImpulseA = sumAclipped-cA.m_appliedImpulse; - //cA.m_appliedImpulse = sumAclipped; - //deltaImpulseB = sumBclipped-cB.m_appliedImpulse; - //cB.m_appliedImpulse = sumBclipped; - } - else - { - cA.m_appliedImpulse = sumA; - cB.m_appliedImpulse = sumB; - } - - if (cA.m_multiBodyA) - { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex], deltaImpulseA, cA.m_deltaVelAindex, ndofA); -#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations - //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - cA.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex], deltaImpulseA); -#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - } - else if (cA.m_solverBodyIdA >= 0) - { - bodyA->internalApplyImpulse(cA.m_contactNormal1 * bodyA->internalGetInvMass(), cA.m_angularComponentA, deltaImpulseA); - } - if (cA.m_multiBodyB) - { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex], deltaImpulseA, cA.m_deltaVelBindex, ndofB); -#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations - //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - cA.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex], deltaImpulseA); -#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - } - else if (cA.m_solverBodyIdB >= 0) - { - bodyB->internalApplyImpulse(cA.m_contactNormal2 * bodyB->internalGetInvMass(), cA.m_angularComponentB, deltaImpulseA); - } - - if (cB.m_multiBodyA) - { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex], deltaImpulseB, cB.m_deltaVelAindex, ndofA); -#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations - //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - cB.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex], deltaImpulseB); -#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - } - else if (cB.m_solverBodyIdA >= 0) - { - bodyA->internalApplyImpulse(cB.m_contactNormal1 * bodyA->internalGetInvMass(), cB.m_angularComponentA, deltaImpulseB); - } - if (cB.m_multiBodyB) - { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex], deltaImpulseB, cB.m_deltaVelBindex, ndofB); -#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations - //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - cB.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex], deltaImpulseB); -#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - } - else if (cB.m_solverBodyIdB >= 0) - { - bodyB->internalApplyImpulse(cB.m_contactNormal2 * bodyB->internalGetInvMass(), cB.m_angularComponentB, deltaImpulseB); - } - - btScalar deltaVel = deltaImpulseA / cA.m_jacDiagABInv + deltaImpulseB / cB.m_jacDiagABInv; - return deltaVel; -} - -void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, const btVector3& contactNormal, const btScalar& appliedImpulse, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation, bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) -{ - BT_PROFILE("setupMultiBodyContactConstraint"); - btVector3 rel_pos1; - btVector3 rel_pos2; - - 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; - - //cfm = 1 / ( dt * kp + kd ) - //erp = dt * kp / ( dt * kp + kd ) - - btScalar cfm; - btScalar erp; - if (isFriction) - { - cfm = infoGlobal.m_frictionCFM; - erp = infoGlobal.m_frictionERP; - } - else - { - cfm = infoGlobal.m_globalCfm; - erp = infoGlobal.m_erp2; - - if ((cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP)) - { - if (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM) - cfm = cp.m_contactCFM; - if (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP) - erp = cp.m_contactERP; - } - else - { - if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING) - { - btScalar denom = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1); - if (denom < SIMD_EPSILON) - { - denom = SIMD_EPSILON; - } - cfm = btScalar(1) / denom; - erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom; - } - } - } - - cfm *= invTimeStep; - - if (multiBodyA) - { - if (solverConstraint.m_linkA < 0) - { - 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->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); - - btVector3 torqueAxis0 = rel_pos1.cross(contactNormal); - solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = contactNormal; - } - 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); - } - - 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->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); - solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - solverConstraint.m_contactNormal2 = -contactNormal; - } - else - { - 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); - } - - { - 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; - 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) - { - vec = (solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + contactNormal.dot(vec); - } - } - 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) - { - 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 - { - //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 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; - { - 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) - { - 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; - 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 += (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) - { - restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); - if (restitution <= btScalar(0.)) - { - restitution = 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 - if (isFriction) - { - positionalError = -distance * erp / infoGlobal.m_timeStep; - } - else - { - if (distance > 0) - { - positionalError = 0; - velocityError -= distance / infoGlobal.m_timeStep; - } - else - { - positionalError = -distance * erp / infoGlobal.m_timeStep; - } - } - - btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; - - if (!isFriction) - { - // if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; - solverConstraint.m_rhsPenetration = 0.f; - } - /*else - { - //split position and velocity into rhs and m_rhsPenetration - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = penetrationImpulse; - } - */ - solverConstraint.m_lowerLimit = 0; - solverConstraint.m_upperLimit = 1e10f; - } - else - { - solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; - solverConstraint.m_rhsPenetration = 0.f; - solverConstraint.m_lowerLimit = -solverConstraint.m_friction; - solverConstraint.m_upperLimit = solverConstraint.m_friction; - } - - solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv; - } - - if (infoGlobal.m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING) - { - if (btFabs(cp.m_prevRHS) > 1e-5 && cp.m_prevRHS < 2* solverConstraint.m_rhs && solverConstraint.m_rhs < 2*cp.m_prevRHS) - { - solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse / cp.m_prevRHS * solverConstraint.m_rhs * infoGlobal.m_articulatedWarmstartingFactor; - if (solverConstraint.m_appliedImpulse < 0) - solverConstraint.m_appliedImpulse = 0; - } - else - { - solverConstraint.m_appliedImpulse = 0.f; - } - - if (solverConstraint.m_appliedImpulse) - { - if (multiBodyA) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse); - - applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA); - } - else - { - if (rb0) - bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse); - } - if (multiBodyB) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse); - applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB); - } - else - { - if (rb1) - bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->getLinearFactor(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse); - } - } - } - else - { - solverConstraint.m_appliedImpulse = 0.f; - solverConstraint.m_appliedPushImpulse = 0.f; - } -} - -void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint, - const btVector3& constraintNormal, - btManifoldPoint& cp, - btScalar combinedTorsionalFriction, - const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) -{ - BT_PROFILE("setupMultiBodyRollingFrictionConstraint"); - btVector3 rel_pos1; - btVector3 rel_pos2; - - btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; - btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; - - const btVector3& pos1 = cp.getPositionWorldOnA(); - const btVector3& pos2 = cp.getPositionWorldOnB(); - - btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA]; - btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB]; - - btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; - btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; - - if (bodyA) - rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); - if (bodyB) - rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); - - relaxation = infoGlobal.m_sor; - - // btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep; - - if (multiBodyA) - { - if (solverConstraint.m_linkA < 0) - { - rel_pos1 = pos1 - multiBodyA->getBasePos(); - } - else - { - rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin(); - } - const int ndofA = multiBodyA->getNumDofs() + 6; - - 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); - 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) - { - btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA]; - rel_vel += solverConstraint.m_contactNormal1.dot(rb0 ? solverBodyA->m_linearVelocity + solverBodyA->m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(rb0 ? solverBodyA->m_angularVelocity : btVector3(0, 0, 0)); - } - } - if (multiBodyB) - { - ndofB = multiBodyB->getNumDofs() + 6; - btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB; ++i) - rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; - } - else - { - if (rb1) - { - btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB]; - rel_vel += solverConstraint.m_contactNormal2.dot(rb1 ? solverBodyB->m_linearVelocity + solverBodyB->m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(rb1 ? solverBodyB->m_angularVelocity : btVector3(0, 0, 0)); - } - } - - solverConstraint.m_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, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) -{ - BT_PROFILE("addMultiBodyFrictionConstraint"); - btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing(); - solverConstraint.m_orgConstraint = 0; - solverConstraint.m_orgDofIndex = -1; - - solverConstraint.m_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; - - setupMultiBodyContactConstraint(solverConstraint, normalAxis, 0, cp, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip); - return solverConstraint; -} - -btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, - btScalar combinedTorsionalFriction, - btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) -{ - BT_PROFILE("addMultiBodyRollingFrictionConstraint"); - - bool useTorsionalAndConeFriction = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0)); - - btMultiBodySolverConstraint& solverConstraint = useTorsionalAndConeFriction ? m_multiBodyTorsionalFrictionContactConstraints.expandNonInitializing() : m_multiBodyFrictionContactConstraints.expandNonInitializing(); - solverConstraint.m_orgConstraint = 0; - solverConstraint.m_orgDofIndex = -1; - - solverConstraint.m_frictionIndex = frictionIndex; - bool isFriction = true; - - const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); - const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); - - btMultiBody* mbA = fcA ? fcA->m_multiBody : 0; - btMultiBody* mbB = fcB ? fcB->m_multiBody : 0; - - int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep); - int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep); - - solverConstraint.m_solverBodyIdA = solverBodyIdA; - solverConstraint.m_solverBodyIdB = solverBodyIdB; - 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; -} - -btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodySpinningFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, - btScalar combinedTorsionalFriction, - btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) -{ - BT_PROFILE("addMultiBodyRollingFrictionConstraint"); - - btMultiBodySolverConstraint& solverConstraint = m_multiBodySpinningFrictionContactConstraints.expandNonInitializing(); - solverConstraint.m_orgConstraint = 0; - solverConstraint.m_orgDofIndex = -1; - - solverConstraint.m_frictionIndex = frictionIndex; - bool isFriction = true; - - const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); - const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); - - btMultiBody* mbA = fcA ? fcA->m_multiBody : 0; - btMultiBody* mbB = fcB ? fcB->m_multiBody : 0; - - int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep); - int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep); - - solverConstraint.m_solverBodyIdA = solverBodyIdA; - solverConstraint.m_solverBodyIdB = solverBodyIdB; - solverConstraint.m_multiBodyA = mbA; - if (mbA) - solverConstraint.m_linkA = fcA->m_link; - - solverConstraint.m_multiBodyB = mbB; - if (mbB) - solverConstraint.m_linkB = fcB->m_link; - - solverConstraint.m_originalContactPoint = &cp; - - setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip); - return solverConstraint; -} -void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal) -{ - const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); - const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); - - btMultiBody* mbA = fcA ? fcA->m_multiBody : 0; - btMultiBody* mbB = fcB ? fcB->m_multiBody : 0; - - 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]; - - ///avoid collision response between two static objects - // if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero()))) - // return; - - //only a single rollingFriction per manifold - int rollingFriction = 4; - - for (int j = 0; j < manifold->getNumContacts(); j++) - { - 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; - 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; - - bool isFriction = false; - setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp.m_appliedImpulse, cp, infoGlobal, relaxation, isFriction); - - // const btVector3& pos1 = cp.getPositionWorldOnA(); - // const btVector3& pos2 = cp.getPositionWorldOnB(); - - /////setup the friction constraints -#define ENABLE_FRICTION -#ifdef ENABLE_FRICTION - solverConstraint.m_frictionIndex = m_multiBodyFrictionContactConstraints.size(); - - ///Bullet has several options to set the friction directions - ///By default, each contact has only a single friction direction that is recomputed automatically 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, - ///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); - cp.m_lateralFrictionDir1.normalize(); - cp.m_lateralFrictionDir2.normalize(); - - if (rollingFriction > 0) - { - if (cp.m_combinedSpinningFriction > 0) - { - addMultiBodySpinningFrictionConstraint(cp.m_normalWorldOnB, manifold, frictionIndex, cp, cp.m_combinedSpinningFriction, colObj0, colObj1, relaxation, infoGlobal); - } - if (cp.m_combinedRollingFriction > 0) - { - applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - - addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal); - addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal); - } - rollingFriction--; - } - if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags & BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED)) - { /* - cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; - btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); - if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) - { - cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel); - if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); - cp.m_lateralFrictionDir2.normalize();//?? - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } - - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } else - */ - { - applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal); - - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal); - } - - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) - { - cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED; - } - } - } - else - { - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM); - - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM); - solverConstraint.m_appliedImpulse = 0.f; - solverConstraint.m_appliedPushImpulse = 0.f; - } - -#endif //ENABLE_FRICTION - } - else - { - // Reset quantities related to warmstart as 0. - cp.m_appliedImpulse = 0; - cp.m_prevRHS = 0; - } - } -} - -void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) -{ - for (int i = 0; i < numManifolds; 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 - { - convertMultiBodyContact(manifold, infoGlobal); - } - } - - //also convert the multibody constraints, if any - - 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); - } - - // Warmstart for noncontact constraints - if (infoGlobal.m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING) - { - for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++) - { - btMultiBodySolverConstraint& solverConstraint = - m_multiBodyNonContactConstraints[i]; - solverConstraint.m_appliedImpulse = - solverConstraint.m_orgConstraint->getAppliedImpulse(solverConstraint.m_orgDofIndex) * - infoGlobal.m_articulatedWarmstartingFactor; - - btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; - btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; - if (solverConstraint.m_appliedImpulse) - { - if (multiBodyA) - { - int ndofA = multiBodyA->getNumDofs() + 6; - btScalar* deltaV = - &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - btScalar impulse = solverConstraint.m_appliedImpulse; - multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse); - applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA); - } - if (multiBodyB) - { - int ndofB = multiBodyB->getNumDofs() + 6; - btScalar* deltaV = - &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - btScalar impulse = solverConstraint.m_appliedImpulse; - multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse); - applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB); - } - } - } - } - else - { - for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++) - { - btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i]; - solverConstraint.m_appliedImpulse = 0; - } - } -} - -btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) -{ - //printf("btMultiBodyConstraintSolver::solveGroup: numBodies=%d, numConstraints=%d\n", numBodies, numConstraints); - return btSequentialImpulseConstraintSolver::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher); -} - -#if 0 -static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodySolverConstraint& solverConstraint, int jacIndex, btMultiBody* mb, btScalar appliedImpulse) -{ - if (appliedImpulse!=0 && mb->internalNeedsJointFeedback()) - { - //todo: get rid of those temporary memory allocations for the joint feedback - btAlignedObjectArray<btScalar> forceVector; - int numDofsPlusBase = 6+mb->getNumDofs(); - forceVector.resize(numDofsPlusBase); - for (int i=0;i<numDofsPlusBase;i++) - { - forceVector[i] = data.m_jacobians[jacIndex+i]*appliedImpulse; - } - btAlignedObjectArray<btScalar> output; - output.resize(numDofsPlusBase); - bool applyJointFeedback = true; - mb->calcAccelerationDeltasMultiDof(&forceVector[0],&output[0],data.scratch_r,data.scratch_v,applyJointFeedback); - } -} -#endif - -void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime) -{ -#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); - } - - 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) - { - c.m_multiBodyA->addBaseConstraintForce(force); - c.m_multiBodyA->addBaseConstraintTorque(torque); - } - 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); - } - } - - 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) - { - c.m_multiBodyB->addBaseConstraintForce(force); - c.m_multiBodyB->addBaseConstraintTorque(torque); - } - else - { - { - 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); - } - } - } - } -#endif - -#ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - - if (c.m_multiBodyA) - { - 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); - } -#endif -} - -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) - //or as applied force, so we can measure the joint reaction forces easier - for (int i = 0; i < numPoolConstraints; i++) - { - btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[i]; - writeBackSolverBodyToMultiBody(solverConstraint, 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); - } - } - - for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++) - { - btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i]; - writeBackSolverBodyToMultiBody(solverConstraint, infoGlobal.m_timeStep); - } - - - { - BT_PROFILE("warm starting write back"); - for (int j = 0; j < numPoolConstraints; j++) - { - const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j]; - btManifoldPoint* pt = (btManifoldPoint*)solverConstraint.m_originalContactPoint; - btAssert(pt); - pt->m_appliedImpulse = solverConstraint.m_appliedImpulse; - pt->m_prevRHS = solverConstraint.m_rhs; - pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse; - - //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex + 1].m_appliedImpulse; - } else - { - pt->m_appliedImpulseLateral2 = 0; - } - } - } - -#if 0 - //multibody joint feedback - { - BT_PROFILE("multi body joint feedback"); - for (int j=0;j<numPoolConstraints;j++) - { - const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j]; - - //apply the joint feedback into all links of the btMultiBody - //todo: double-check the signs of the applied impulse - - if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof()) - { - applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep)); - } - if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof()) - { - applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep)); - } -#if 0 - if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA->isMultiDof()) - { - applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex], - m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacAindex, - m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA, - m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep)); - - } - if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB->isMultiDof()) - { - applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex], - m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacBindex, - m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB, - m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep)); - } - - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA->isMultiDof()) - { - applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1], - m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacAindex, - m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA, - m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep)); - } - - if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB->isMultiDof()) - { - applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1], - m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacBindex, - m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB, - m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep)); - } - } -#endif - } - - for (int i=0;i<m_multiBodyNonContactConstraints.size();i++) - { - const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i]; - if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof()) - { - applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep)); - } - if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof()) - { - applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep)); - } - } - } - - numPoolConstraints = m_multiBodyNonContactConstraints.size(); - -#if 0 - //@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint - for (int i=0;i<numPoolConstraints;i++) - { - const btMultiBodySolverConstraint& c = m_multiBodyNonContactConstraints[i]; - - btTypedConstraint* constr = (btTypedConstraint*)c.m_originalContactPoint; - btJointFeedback* fb = constr->getJointFeedback(); - if (fb) - { - fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep; - fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep; - fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; - fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */ - - } - - constr->internalSetAppliedImpulse(c.m_appliedImpulse); - if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold()) - { - constr->setEnabled(false); - } - - } -#endif -#endif - - 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) -{ - //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); - - m_tmpMultiBodyConstraints = 0; - m_tmpNumMultiBodyConstraints = 0; -} diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h deleted file mode 100644 index f584360e2b..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ /dev/null @@ -1,101 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MULTIBODY_CONSTRAINT_SOLVER_H -#define BT_MULTIBODY_CONSTRAINT_SOLVER_H - -#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" -#include "btMultiBodySolverConstraint.h" - -#define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - -class btMultiBody; - -#include "btMultiBodyConstraint.h" - -ATTRIBUTE_ALIGNED16(class) -btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver -{ -protected: - btMultiBodyConstraintArray m_multiBodyNonContactConstraints; - - btMultiBodyConstraintArray m_multiBodyNormalContactConstraints; - btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints; - btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints; - btMultiBodyConstraintArray m_multiBodySpinningFrictionContactConstraints; - - btMultiBodyJacobianData m_data; - - //temp storage for multi body constraints for a specific island/group called by 'solveGroup' - btMultiBodyConstraint** m_tmpMultiBodyConstraints; - int m_tmpNumMultiBodyConstraints; - - btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c); - - //solve 2 friction directions and clamp against the implicit friction cone - btScalar resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1, const btMultiBodySolverConstraint& cB); - - void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal); - - btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); - - btMultiBodySolverConstraint& addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, - btScalar combinedTorsionalFriction, - btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); - - btMultiBodySolverConstraint& addMultiBodySpinningFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, - btScalar combinedTorsionalFriction, - btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); - - void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint & constraintRow, - btScalar * jacA, btScalar * jacB, - btScalar penetration, btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff, - const btContactSolverInfo& infoGlobal); - - void setupMultiBodyContactConstraint(btMultiBodySolverConstraint & solverConstraint, - const btVector3& contactNormal, - const btScalar& appliedImpulse, - btManifoldPoint& cp, - const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); - - //either rolling or spinning friction - void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint & solverConstraint, - const btVector3& contactNormal, - btManifoldPoint& cp, - btScalar combinedTorsionalFriction, - const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); - - void convertMultiBodyContact(btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal); - virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); - // virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); - void applyDeltaVee(btScalar * deltaV, btScalar impulse, int velocityIndex, int ndof); - void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint & constraint, btScalar deltaTime); - -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 deleted file mode 100644 index e7af332eb3..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ /dev/null @@ -1,895 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btMultiBodyDynamicsWorld.h" -#include "btMultiBodyConstraintSolver.h" -#include "btMultiBody.h" -#include "btMultiBodyLinkCollider.h" -#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" -#include "LinearMath/btQuickprof.h" -#include "btMultiBodyConstraint.h" -#include "LinearMath/btIDebugDraw.h" -#include "LinearMath/btSerializer.h" - -void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, int group, int mask) -{ - m_multiBodies.push_back(body); -} - -void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body) -{ - m_multiBodies.remove(body); -} - -void btMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) -{ - btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); - predictMultiBodyTransforms(timeStep); - -} -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()); - } - } - } - - { - int i; - int numConstraints = int(m_constraints.size()); - for (i = 0; i < numConstraints; i++) - { - btTypedConstraint* constraint = m_constraints[i]; - if (constraint->isEnabled()) - { - const btRigidBody* colObj0 = &constraint->getRigidBodyA(); - const btRigidBody* colObj1 = &constraint->getRigidBodyB(); - - if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && - ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) - { - getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag()); - } - } - } - } - - //merge islands linked by Featherstone link colliders - 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++) - { - btMultiBodyLinkCollider* cur = body->getLink(b).m_collider; - - if (((cur) && (!(cur)->isStaticOrKinematicObject())) && - ((prev) && (!(prev)->isStaticOrKinematicObject()))) - { - int tagPrev = prev->getIslandTag(); - int tagCur = cur->getIslandTag(); - getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur); - } - if (cur && !cur->isStaticOrKinematicObject()) - prev = cur; - } - } - } - - //merge islands linked by multibody constraints - { - 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) - getSimulationIslandManager()->getUnionFind().unite(tagA, tagB); - } - } - - //Store the island id in each body - getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); -} - -void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) -{ - BT_PROFILE("btMultiBodyDynamicsWorld::updateActivationState"); - - for (int i = 0; i < m_multiBodies.size(); i++) - { - btMultiBody* body = m_multiBodies[i]; - if (body) - { - body->checkMotionAndSleepIfRequired(timeStep); - if (!body->isAwake()) - { - btMultiBodyLinkCollider* col = body->getBaseCollider(); - if (col && col->getActivationState() == ACTIVE_TAG) - { - if (body->hasFixedBase()) - { - col->setActivationState(FIXED_BASE_MULTI_BODY); - } else - { - col->setActivationState(WANTS_DEACTIVATION); - } - - col->setDeactivationTime(0.f); - } - for (int b = 0; b < body->getNumLinks(); b++) - { - btMultiBodyLinkCollider* col = body->getLink(b).m_collider; - if (col && col->getActivationState() == ACTIVE_TAG) - { - col->setActivationState(WANTS_DEACTIVATION); - col->setDeactivationTime(0.f); - } - } - } - else - { - btMultiBodyLinkCollider* col = body->getBaseCollider(); - if (col && col->getActivationState() != DISABLE_DEACTIVATION) - col->setActivationState(ACTIVE_TAG); - - 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); - } - } - } - } - - btDiscreteDynamicsWorld::updateActivationState(timeStep); -} - -void btMultiBodyDynamicsWorld::getAnalyticsData(btAlignedObjectArray<btSolverAnalyticsData>& islandAnalyticsData) const -{ - islandAnalyticsData = m_solverMultiBodyIslandCallback->m_islandAnalyticsData; -} - -btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration) - : btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration), - m_multiBodyConstraintSolver(constraintSolver) -{ - //split impulse is not yet supported for Featherstone hierarchies - // getSolverInfo().m_splitImpulse = false; - getSolverInfo().m_solverMode |= SOLVER_USE_2_FRICTION_DIRECTIONS; - m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver, dispatcher); -} - -btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld() -{ - delete m_solverMultiBodyIslandCallback; -} - -void btMultiBodyDynamicsWorld::setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver) -{ - m_multiBodyConstraintSolver = solver; - m_solverMultiBodyIslandCallback->setMultiBodyConstraintSolver(solver); - btDiscreteDynamicsWorld::setConstraintSolver(solver); -} - -void btMultiBodyDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) -{ - if (solver->getSolverType() == BT_MULTIBODY_SOLVER) - { - m_multiBodyConstraintSolver = (btMultiBodyConstraintSolver*)solver; - } - btDiscreteDynamicsWorld::setConstraintSolver(solver); -} - -void btMultiBodyDynamicsWorld::forwardKinematics() -{ - 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); - } -} -void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) -{ - solveExternalForces(solverInfo); - buildIslands(); - solveInternalConstraints(solverInfo); -} - -void btMultiBodyDynamicsWorld::buildIslands() -{ - m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback); -} - -void btMultiBodyDynamicsWorld::solveInternalConstraints(btContactSolverInfo& solverInfo) -{ - /// solve all the constraints for this island - m_solverMultiBodyIslandCallback->processConstraints(); - m_constraintSolver->allSolved(solverInfo, m_debugDrawer); - { - BT_PROFILE("btMultiBody stepVelocities"); - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - - bool isSleeping = false; - - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b = 0; b < bod->getNumLinks(); b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) - isSleeping = true; - } - - if (!isSleeping) - { - //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) - m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) - m_scratch_v.resize(bod->getNumLinks() + 1); - m_scratch_m.resize(bod->getNumLinks() + 1); - - if (bod->internalNeedsJointFeedback()) - { - if (!bod->isUsingRK4Integration()) - { - if (bod->internalNeedsJointFeedback()) - { - bool isConstraintPass = true; - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass, - getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - } - } - } - } - } - } - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - bod->processDeltaVeeMultiDof2(); - } -} - -void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverInfo) -{ - forwardKinematics(); - - BT_PROFILE("solveConstraints"); - - clearMultiBodyConstraintForces(); - - m_sortedConstraints.resize(m_constraints.size()); - int i; - for (i = 0; i < getNumConstraints(); i++) - { - m_sortedConstraints[i] = m_constraints[i]; - } - m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2()); - btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; - - m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size()); - for (i = 0; i < m_multiBodyConstraints.size(); i++) - { - m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i]; - } - m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate()); - - btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; - - m_solverMultiBodyIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer()); - m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); - -#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - { - BT_PROFILE("btMultiBody addForce"); - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - - bool isSleeping = false; - - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b = 0; b < bod->getNumLinks(); b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) - isSleeping = true; - } - - if (!isSleeping) - { - //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) - m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) - m_scratch_v.resize(bod->getNumLinks() + 1); - m_scratch_m.resize(bod->getNumLinks() + 1); - - bod->addBaseForce(m_gravity * bod->getBaseMass()); - - for (int j = 0; j < bod->getNumLinks(); ++j) - { - bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); - } - } //if (!isSleeping) - } - } -#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - - { - BT_PROFILE("btMultiBody stepVelocities"); - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - - bool isSleeping = false; - - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b = 0; b < bod->getNumLinks(); b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) - isSleeping = true; - } - - if (!isSleeping) - { - //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) - m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) - m_scratch_v.resize(bod->getNumLinks() + 1); - m_scratch_m.resize(bod->getNumLinks() + 1); - bool doNotUpdatePos = false; - bool isConstraintPass = false; - { - if (!bod->isUsingRK4Integration()) - { - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, - m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass, - getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - } - else - { - // - int numDofs = bod->getNumDofs() + 6; - int numPosVars = bod->getNumPosVars() + 7; - btAlignedObjectArray<btScalar> scratch_r2; - scratch_r2.resize(2 * numPosVars + 8 * numDofs); - //convenience - btScalar* pMem = &scratch_r2[0]; - btScalar* scratch_q0 = pMem; - pMem += numPosVars; - btScalar* scratch_qx = pMem; - pMem += numPosVars; - btScalar* scratch_qd0 = pMem; - pMem += numDofs; - btScalar* scratch_qd1 = pMem; - pMem += numDofs; - btScalar* scratch_qd2 = pMem; - pMem += numDofs; - btScalar* scratch_qd3 = pMem; - pMem += numDofs; - btScalar* scratch_qdd0 = pMem; - pMem += numDofs; - btScalar* scratch_qdd1 = pMem; - pMem += numDofs; - btScalar* scratch_qdd2 = pMem; - pMem += numDofs; - btScalar* scratch_qdd3 = pMem; - pMem += numDofs; - btAssert((pMem - (2 * numPosVars + 8 * numDofs)) == &scratch_r2[0]); - - ///// - //copy q0 to scratch_q0 and qd0 to scratch_qd0 - scratch_q0[0] = bod->getWorldToBaseRot().x(); - scratch_q0[1] = bod->getWorldToBaseRot().y(); - scratch_q0[2] = bod->getWorldToBaseRot().z(); - scratch_q0[3] = bod->getWorldToBaseRot().w(); - scratch_q0[4] = bod->getBasePos().x(); - scratch_q0[5] = bod->getBasePos().y(); - scratch_q0[6] = bod->getBasePos().z(); - // - for (int link = 0; link < bod->getNumLinks(); ++link) - { - for (int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof) - scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof]; - } - // - for (int dof = 0; dof < numDofs; ++dof) - scratch_qd0[dof] = bod->getVelocityVector()[dof]; - //// - struct - { - btMultiBody* bod; - btScalar *scratch_qx, *scratch_q0; - - void operator()() - { - for (int dof = 0; dof < bod->getNumPosVars() + 7; ++dof) - 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]; - } - - } 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; - // - struct - { - 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, - isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - pCopy(output, scratch_qdd0, 0, numDofs); - //calc q1 = q0 + h/2 * qd0 - pResetQx(); - bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd0); - //calc qd1 = qd0 + h/2 * qdd0 - pEulerIntegrate(btScalar(.5) * h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs); - // - //calc qdd1 from: q1 & qd1 - pCopyToVelocityVector(bod, scratch_qd1); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, - isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - pCopy(output, scratch_qdd1, 0, numDofs); - //calc q2 = q0 + h/2 * qd1 - pResetQx(); - bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd1); - //calc qd2 = qd0 + h/2 * qdd1 - pEulerIntegrate(btScalar(.5) * h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs); - // - //calc qdd2 from: q2 & qd2 - pCopyToVelocityVector(bod, scratch_qd2); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, - isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - pCopy(output, scratch_qdd2, 0, numDofs); - //calc q3 = q0 + h * qd2 - pResetQx(); - bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2); - //calc qd3 = qd0 + h * qdd2 - pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs); - // - //calc qdd3 from: q3 & qd3 - pCopyToVelocityVector(bod, scratch_qd3); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, - isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - pCopy(output, scratch_qdd3, 0, numDofs); - - // - //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3) - //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3) - btAlignedObjectArray<btScalar> delta_q; - delta_q.resize(numDofs); - btAlignedObjectArray<btScalar> delta_qd; - delta_qd.resize(numDofs); - for (int i = 0; i < numDofs; ++i) - { - delta_q[i] = h / btScalar(6.) * (scratch_qd0[i] + 2 * scratch_qd1[i] + 2 * scratch_qd2[i] + scratch_qd3[i]); - delta_qd[i] = h / btScalar(6.) * (scratch_qdd0[i] + 2 * scratch_qdd1[i] + 2 * scratch_qdd2[i] + scratch_qdd3[i]); - //delta_q[i] = h*scratch_qd0[i]; - //delta_qd[i] = h*scratch_qdd0[i]; - } - // - pCopyToVelocityVector(bod, scratch_qd0); - bod->applyDeltaVeeMultiDof(&delta_qd[0], 1); - // - if (!doNotUpdatePos) - { - btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector()); - pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs(); - - for (int i = 0; i < numDofs; ++i) - pRealBuf[i] = delta_q[i]; - - //bod->stepPositionsMultiDof(1, 0, &delta_q[0]); - bod->setPosUpdated(true); - } - - //ugly hack which resets the cached data to t0 (needed for constraint solver) - { - for (int link = 0; link < bod->getNumLinks(); ++link) - bod->getLink(link).updateCacheMultiDof(); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m, - isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - } - } - } - -#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - bod->clearForcesAndTorques(); -#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - } //if (!isSleeping) - } - } -} - - -void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) -{ - btDiscreteDynamicsWorld::integrateTransforms(timeStep); - integrateMultiBodyTransforms(timeStep); -} - -void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep) -{ - BT_PROFILE("btMultiBody stepPositions"); - //integrate and update the Featherstone hierarchies - - for (int b = 0; b < m_multiBodies.size(); b++) - { - btMultiBody* bod = m_multiBodies[b]; - bool isSleeping = false; - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b = 0; b < bod->getNumLinks(); b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) - isSleeping = true; - } - - if (!isSleeping) - { - bod->addSplitV(); - int nLinks = bod->getNumLinks(); - - ///base + num m_links - if (!bod->isPosUpdated()) - bod->stepPositionsMultiDof(timeStep); - else - { - btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector()); - pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs(); - - 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); - bod->substractSplitV(); - } - else - { - bod->clearVelocities(); - } - } -} - -void btMultiBodyDynamicsWorld::predictMultiBodyTransforms(btScalar timeStep) -{ - BT_PROFILE("btMultiBody stepPositions"); - //integrate and update the Featherstone hierarchies - - for (int b = 0; b < m_multiBodies.size(); b++) - { - btMultiBody* bod = m_multiBodies[b]; - bool isSleeping = false; - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b = 0; b < bod->getNumLinks(); b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) - isSleeping = true; - } - - if (!isSleeping) - { - int nLinks = bod->getNumLinks(); - bod->predictPositionsMultiDof(timeStep); - m_scratch_world_to_local.resize(nLinks + 1); - m_scratch_local_origin.resize(nLinks + 1); - bod->updateCollisionObjectInterpolationWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); - } - else - { - bod->clearVelocities(); - } - } -} - -void btMultiBodyDynamicsWorld::addMultiBodyConstraint(btMultiBodyConstraint* constraint) -{ - m_multiBodyConstraints.push_back(constraint); -} - -void btMultiBodyDynamicsWorld::removeMultiBodyConstraint(btMultiBodyConstraint* constraint) -{ - m_multiBodyConstraints.remove(constraint); -} - -void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint) -{ - constraint->debugDraw(getDebugDrawer()); -} - -void btMultiBodyDynamicsWorld::debugDrawWorld() -{ - BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld"); - - btDiscreteDynamicsWorld::debugDrawWorld(); - - bool drawConstraints = false; - if (getDebugDrawer()) - { - int mode = getDebugDrawer()->getDebugMode(); - if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) - { - drawConstraints = true; - } - - if (drawConstraints) - { - BT_PROFILE("btMultiBody debugDrawWorld"); - - 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++) - { - btMultiBody* bod = m_multiBodies[b]; - 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++) - { - const btTransform& tr = bod->getLink(m).m_cachedWorldTransform; - if (mode & btIDebugDraw::DBG_DrawFrames) - { - getDebugDrawer()->drawTransform(tr, 0.1); - } - //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); - } - 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); - } - 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); - } - } - } - } - } -} - -void btMultiBodyDynamicsWorld::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 -} - -void btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces() -{ - 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(); - } - } - } -} -void btMultiBodyDynamicsWorld::clearForces() -{ - btDiscreteDynamicsWorld::clearForces(); - -#ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - clearMultiBodyForces(); -#endif -} - -void btMultiBodyDynamicsWorld::serialize(btSerializer* serializer) -{ - serializer->startSerialization(); - - serializeDynamicsWorldInfo(serializer); - - serializeMultiBodies(serializer); - - serializeRigidBodies(serializer); - - serializeCollisionObjects(serializer); - - serializeContactManifolds(serializer); - - serializer->finishSerialization(); -} - -void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer) -{ - int i; - //serialize all collision objects - for (i = 0; i < m_multiBodies.size(); i++) - { - btMultiBody* mb = m_multiBodies[i]; - { - int len = mb->calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(len, 1); - const char* structType = mb->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk, structType, BT_MULTIBODY_CODE, mb); - } - } - - //serialize all multibody links (collision objects) - for (i = 0; i < m_collisionObjects.size(); i++) - { - btCollisionObject* colObj = m_collisionObjects[i]; - if (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - int len = colObj->calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(len, 1); - const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk, structType, BT_MB_LINKCOLLIDER_CODE, colObj); - } - } -} - -void btMultiBodyDynamicsWorld::saveKinematicState(btScalar timeStep) -{ - btDiscreteDynamicsWorld::saveKinematicState(timeStep); - for(int i = 0; i < m_multiBodies.size(); i++) - { - btMultiBody* body = m_multiBodies[i]; - if(body->isBaseKinematic()) - body->saveKinematicState(timeStep); - } -} - -// -//void btMultiBodyDynamicsWorld::setSplitIslands(bool split) -//{ -// m_islandManager->setSplitIslands(split); -//} diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h deleted file mode 100644 index d2d76c8b92..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ /dev/null @@ -1,126 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MULTIBODY_DYNAMICS_WORLD_H -#define BT_MULTIBODY_DYNAMICS_WORLD_H - -#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" -#include "BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h" - -#define BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - -class btMultiBody; -class btMultiBodyConstraint; -class btMultiBodyConstraintSolver; -struct MultiBodyInplaceSolverIslandCallback; - -///The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet -///This implementation is still preliminary/experimental. -class btMultiBodyDynamicsWorld : public btDiscreteDynamicsWorld -{ -protected: - btAlignedObjectArray<btMultiBody*> m_multiBodies; - btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints; - btAlignedObjectArray<btMultiBodyConstraint*> m_sortedMultiBodyConstraints; - btMultiBodyConstraintSolver* m_multiBodyConstraintSolver; - MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback; - - //cached data to avoid memory allocations - btAlignedObjectArray<btQuaternion> m_scratch_world_to_local; - btAlignedObjectArray<btVector3> m_scratch_local_origin; - btAlignedObjectArray<btQuaternion> m_scratch_world_to_local1; - btAlignedObjectArray<btVector3> m_scratch_local_origin1; - btAlignedObjectArray<btScalar> m_scratch_r; - btAlignedObjectArray<btVector3> m_scratch_v; - btAlignedObjectArray<btMatrix3x3> m_scratch_m; - - virtual void calculateSimulationIslands(); - virtual void updateActivationState(btScalar timeStep); - - - virtual void serializeMultiBodies(btSerializer* serializer); - -public: - btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration); - - virtual ~btMultiBodyDynamicsWorld(); - - virtual void solveConstraints(btContactSolverInfo& solverInfo); - - virtual void addMultiBody(btMultiBody* body, int group = btBroadphaseProxy::DefaultFilter, int mask = btBroadphaseProxy::AllFilter); - - virtual void removeMultiBody(btMultiBody* body); - - virtual int getNumMultibodies() const - { - return m_multiBodies.size(); - } - - btMultiBody* getMultiBody(int mbIndex) - { - return m_multiBodies[mbIndex]; - } - - const btMultiBody* getMultiBody(int mbIndex) const - { - return m_multiBodies[mbIndex]; - } - - virtual void addMultiBodyConstraint(btMultiBodyConstraint* constraint); - - virtual int getNumMultiBodyConstraints() const - { - return m_multiBodyConstraints.size(); - } - - virtual btMultiBodyConstraint* getMultiBodyConstraint(int constraintIndex) - { - return m_multiBodyConstraints[constraintIndex]; - } - - virtual const btMultiBodyConstraint* getMultiBodyConstraint(int constraintIndex) const - { - return m_multiBodyConstraints[constraintIndex]; - } - - virtual void removeMultiBodyConstraint(btMultiBodyConstraint* constraint); - - virtual void integrateTransforms(btScalar timeStep); - void integrateMultiBodyTransforms(btScalar timeStep); - void predictMultiBodyTransforms(btScalar timeStep); - - virtual void predictUnconstraintMotion(btScalar timeStep); - virtual void debugDrawWorld(); - - virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint); - - 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 getAnalyticsData(btAlignedObjectArray<struct btSolverAnalyticsData>& m_islandAnalyticsData) const; - - virtual void solveExternalForces(btContactSolverInfo& solverInfo); - virtual void solveInternalConstraints(btContactSolverInfo& solverInfo); - void buildIslands(); - - virtual void saveKinematicState(btScalar timeStep); -}; -#endif //BT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp deleted file mode 100644 index df2abbe97a..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp +++ /dev/null @@ -1,215 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///This file was written by Erwin Coumans - -#include "btMultiBodyFixedConstraint.h" -#include "btMultiBodyLinkCollider.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" -#include "LinearMath/btIDebugDraw.h" - -#define BTMBFIXEDCONSTRAINT_DIM 6 - -btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) - : btMultiBodyConstraint(body, 0, link, -1, BTMBFIXEDCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_FIXED), - m_rigidBodyA(0), - m_rigidBodyB(bodyB), - m_pivotInA(pivotInA), - m_pivotInB(pivotInB), - m_frameInA(frameInA), - m_frameInB(frameInB) -{ - m_data.resize(BTMBFIXEDCONSTRAINT_DIM); //at least store the applied impulses -} - -btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) - : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBFIXEDCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_FIXED), - m_rigidBodyA(0), - m_rigidBodyB(0), - m_pivotInA(pivotInA), - m_pivotInB(pivotInB), - m_frameInA(frameInA), - m_frameInB(frameInB) -{ - m_data.resize(BTMBFIXEDCONSTRAINT_DIM); //at least store the applied impulses -} - -void btMultiBodyFixedConstraint::finalizeMultiDof() -{ - //not implemented yet - btAssert(0); -} - -btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint() -{ -} - -int btMultiBodyFixedConstraint::getIslandIdA() const -{ - if (m_rigidBodyA) - return m_rigidBodyA->getIslandTag(); - - if (m_bodyA) - { - if (m_linkA < 0) - { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - } - else - { - if (m_bodyA->getLink(m_linkA).m_collider) - return m_bodyA->getLink(m_linkA).m_collider->getIslandTag(); - } - } - return -1; -} - -int btMultiBodyFixedConstraint::getIslandIdB() const -{ - if (m_rigidBodyB) - return m_rigidBodyB->getIslandTag(); - if (m_bodyB) - { - if (m_linkB < 0) - { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - } - else - { - if (m_bodyB->getLink(m_linkB).m_collider) - return m_bodyB->getLink(m_linkB).m_collider->getIslandTag(); - } - } - return -1; -} - -void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal) -{ - int numDim = BTMBFIXEDCONSTRAINT_DIM; - for (int i = 0; i < numDim; i++) - { - btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); - constraintRow.m_orgConstraint = this; - constraintRow.m_orgDofIndex = i; - constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0); - constraintRow.m_contactNormal1.setValue(0, 0, 0); - constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0); - constraintRow.m_contactNormal2.setValue(0, 0, 0); - constraintRow.m_angularComponentA.setValue(0, 0, 0); - constraintRow.m_angularComponentB.setValue(0, 0, 0); - - constraintRow.m_solverBodyIdA = data.m_fixedBodyId; - constraintRow.m_solverBodyIdB = data.m_fixedBodyId; - - // Convert local points back to world - btVector3 pivotAworld = m_pivotInA; - btMatrix3x3 frameAworld = m_frameInA; - if (m_rigidBodyA) - { - constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId(); - pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA; - frameAworld = frameAworld.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation()); - } - else - { - if (m_bodyA) - { - pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); - frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld); - } - } - btVector3 pivotBworld = m_pivotInB; - btMatrix3x3 frameBworld = m_frameInB; - if (m_rigidBodyB) - { - constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId(); - pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB; - frameBworld = frameBworld.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation()); - } - else - { - if (m_bodyB) - { - pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); - frameBworld = m_bodyB->localFrameToWorld(m_linkB, frameBworld); - } - } - - btMatrix3x3 relRot = frameAworld.inverse() * frameBworld; - btVector3 angleDiff; - btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff); - - btVector3 constraintNormalLin(0, 0, 0); - btVector3 constraintNormalAng(0, 0, 0); - btScalar posError = 0.0; - if (i < 3) - { - constraintNormalLin[i] = 1; - posError = (pivotAworld - pivotBworld).dot(constraintNormalLin); - fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, - constraintNormalLin, pivotAworld, pivotBworld, - posError, - infoGlobal, - -m_maxAppliedImpulse, m_maxAppliedImpulse); - } - else - { //i>=3 - constraintNormalAng = frameAworld.getColumn(i % 3); - posError = angleDiff[i % 3]; - fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, - constraintNormalLin, pivotAworld, pivotBworld, - posError, - infoGlobal, - -m_maxAppliedImpulse, m_maxAppliedImpulse, true); - } - } -} - -void btMultiBodyFixedConstraint::debugDraw(class btIDebugDraw* drawer) -{ - btTransform tr; - tr.setIdentity(); - - if (m_rigidBodyA) - { - btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA; - tr.setOrigin(pivot); - drawer->drawTransform(tr, 0.1); - } - if (m_bodyA) - { - btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); - tr.setOrigin(pivotAworld); - drawer->drawTransform(tr, 0.1); - } - if (m_rigidBodyB) - { - // that ideally should draw the same frame - btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB; - tr.setOrigin(pivot); - drawer->drawTransform(tr, 0.1); - } - if (m_bodyB) - { - btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); - tr.setOrigin(pivotBworld); - drawer->drawTransform(tr, 0.1); - } -} diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h deleted file mode 100644 index adb1cb47da..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h +++ /dev/null @@ -1,91 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///This file was written by Erwin Coumans - -#ifndef BT_MULTIBODY_FIXED_CONSTRAINT_H -#define BT_MULTIBODY_FIXED_CONSTRAINT_H - -#include "btMultiBodyConstraint.h" - -class btMultiBodyFixedConstraint : public btMultiBodyConstraint -{ -protected: - btRigidBody* m_rigidBodyA; - btRigidBody* m_rigidBodyB; - btVector3 m_pivotInA; - btVector3 m_pivotInB; - btMatrix3x3 m_frameInA; - btMatrix3x3 m_frameInB; - -public: - btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); - btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); - - virtual ~btMultiBodyFixedConstraint(); - - virtual void finalizeMultiDof(); - - virtual int getIslandIdA() const; - virtual int getIslandIdB() const; - - virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal); - - const btVector3& getPivotInA() const - { - return m_pivotInA; - } - - void setPivotInA(const btVector3& pivotInA) - { - m_pivotInA = pivotInA; - } - - const btVector3& getPivotInB() const - { - return m_pivotInB; - } - - virtual void setPivotInB(const btVector3& pivotInB) - { - m_pivotInB = pivotInB; - } - - const btMatrix3x3& getFrameInA() const - { - return m_frameInA; - } - - void setFrameInA(const btMatrix3x3& frameInA) - { - m_frameInA = frameInA; - } - - const btMatrix3x3& getFrameInB() const - { - return m_frameInB; - } - - virtual void setFrameInB(const btMatrix3x3& frameInB) - { - m_frameInB = frameInB; - } - - virtual void debugDraw(class btIDebugDraw* drawer); -}; - -#endif //BT_MULTIBODY_FIXED_CONSTRAINT_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp deleted file mode 100644 index ee02cf9b07..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp +++ /dev/null @@ -1,181 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///This file was written by Erwin Coumans - -#include "btMultiBodyGearConstraint.h" -#include "btMultiBody.h" -#include "btMultiBodyLinkCollider.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" - -btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) - : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, 1, false, MULTIBODY_CONSTRAINT_GEAR), - m_gearRatio(1), - m_gearAuxLink(-1), - m_erp(0), - m_relativePositionTarget(0) -{ -} - -void btMultiBodyGearConstraint::finalizeMultiDof() -{ - allocateJacobiansMultiDof(); - - m_numDofsFinalized = m_jacSizeBoth; -} - -btMultiBodyGearConstraint::~btMultiBodyGearConstraint() -{ -} - -int btMultiBodyGearConstraint::getIslandIdA() const -{ - if (m_bodyA) - { - if (m_linkA < 0) - { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - } - else - { - if (m_bodyA->getLink(m_linkA).m_collider) - return m_bodyA->getLink(m_linkA).m_collider->getIslandTag(); - } - } - return -1; -} - -int btMultiBodyGearConstraint::getIslandIdB() const -{ - if (m_bodyB) - { - if (m_linkB < 0) - { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - } - else - { - if (m_bodyB->getLink(m_linkB).m_collider) - return m_bodyB->getLink(m_linkB).m_collider->getIslandTag(); - } - } - return -1; -} - -void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal) -{ - // only positions need to be updated -- data.m_jacobians and force - // directions were set in the ctor and never change. - - if (m_numDofsFinalized != m_jacSizeBoth) - { - finalizeMultiDof(); - } - - //don't crash - if (m_numDofsFinalized != m_jacSizeBoth) - return; - - if (m_maxAppliedImpulse == 0.f) - return; - - // note: we rely on the fact that data.m_jacobians are - // always initialized to zero by the Constraint ctor - int linkDoF = 0; - unsigned int offsetA = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF); - unsigned int offsetB = 6 + (m_bodyB->getLink(m_linkB).m_dofOffset + linkDoF); - - // row 0: the lower bound - jacobianA(0)[offsetA] = 1; - jacobianB(0)[offsetB] = m_gearRatio; - - btScalar posError = 0; - const btVector3 dummy(0, 0, 0); - - btScalar kp = 1; - btScalar kd = 1; - int numRows = getNumRows(); - - for (int row = 0; row < numRows; row++) - { - btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); - - int dof = 0; - btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof]; - btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; - btScalar auxVel = 0; - - if (m_gearAuxLink >= 0) - { - auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof]; - } - currentVelocity += auxVel; - if (m_erp != 0) - { - btScalar currentPositionA = m_bodyA->getJointPosMultiDof(m_linkA)[dof]; - if (m_gearAuxLink >= 0) - { - currentPositionA -= m_bodyA->getJointPosMultiDof(m_gearAuxLink)[dof]; - } - btScalar currentPositionB = m_gearRatio * m_bodyA->getJointPosMultiDof(m_linkB)[dof]; - btScalar diff = currentPositionB + currentPositionA; - btScalar desiredPositionDiff = this->m_relativePositionTarget; - posError = -m_erp * (desiredPositionDiff - diff); - } - - btScalar desiredRelativeVelocity = auxVel; - - fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse, false, 1, false, desiredRelativeVelocity); - - constraintRow.m_orgConstraint = this; - constraintRow.m_orgDofIndex = row; - { - //expect either prismatic or revolute joint type for now - btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic)); - switch (m_bodyA->getLink(m_linkA).m_jointType) - { - case btMultibodyLink::eRevolute: - { - constraintRow.m_contactNormal1.setZero(); - constraintRow.m_contactNormal2.setZero(); - btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec); - constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld; - constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld; - - break; - } - case btMultibodyLink::ePrismatic: - { - btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec); - constraintRow.m_contactNormal1 = prismaticAxisInWorld; - constraintRow.m_contactNormal2 = -prismaticAxisInWorld; - constraintRow.m_relpos1CrossNormal.setZero(); - constraintRow.m_relpos2CrossNormal.setZero(); - break; - } - default: - { - btAssert(0); - } - }; - } - } -} diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h deleted file mode 100644 index 31888fbc68..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h +++ /dev/null @@ -1,115 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///This file was written by Erwin Coumans - -#ifndef BT_MULTIBODY_GEAR_CONSTRAINT_H -#define BT_MULTIBODY_GEAR_CONSTRAINT_H - -#include "btMultiBodyConstraint.h" - -class btMultiBodyGearConstraint : public btMultiBodyConstraint -{ -protected: - btRigidBody* m_rigidBodyA; - btRigidBody* m_rigidBodyB; - btVector3 m_pivotInA; - btVector3 m_pivotInB; - btMatrix3x3 m_frameInA; - btMatrix3x3 m_frameInB; - btScalar m_gearRatio; - int m_gearAuxLink; - btScalar m_erp; - btScalar m_relativePositionTarget; - -public: - //btMultiBodyGearConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); - btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); - - virtual ~btMultiBodyGearConstraint(); - - virtual void finalizeMultiDof(); - - virtual int getIslandIdA() const; - virtual int getIslandIdB() const; - - virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal); - - const btVector3& getPivotInA() const - { - return m_pivotInA; - } - - void setPivotInA(const btVector3& pivotInA) - { - m_pivotInA = pivotInA; - } - - const btVector3& getPivotInB() const - { - return m_pivotInB; - } - - virtual void setPivotInB(const btVector3& pivotInB) - { - m_pivotInB = pivotInB; - } - - const btMatrix3x3& getFrameInA() const - { - return m_frameInA; - } - - void setFrameInA(const btMatrix3x3& frameInA) - { - m_frameInA = frameInA; - } - - const btMatrix3x3& getFrameInB() const - { - return m_frameInB; - } - - virtual void setFrameInB(const btMatrix3x3& frameInB) - { - m_frameInB = frameInB; - } - - virtual void debugDraw(class btIDebugDraw* drawer) - { - //todo(erwincoumans) - } - - virtual void setGearRatio(btScalar gearRatio) - { - m_gearRatio = gearRatio; - } - virtual void setGearAuxLink(int gearAuxLink) - { - m_gearAuxLink = gearAuxLink; - } - virtual void setRelativePositionTarget(btScalar relPosTarget) - { - m_relativePositionTarget = relPosTarget; - } - virtual void setErp(btScalar erp) - { - m_erp = erp; - } -}; - -#endif //BT_MULTIBODY_GEAR_CONSTRAINT_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h deleted file mode 100644 index 3169b86e61..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h +++ /dev/null @@ -1,247 +0,0 @@ -/* - Bullet Continuous Collision Detection and Physics Library - Copyright (c) 2019 Google Inc. http://bulletphysics.org - This software is provided 'as-is', without any express or implied warranty. - In no event will the authors be held liable for any damages arising from the use of this software. - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it freely, - subject to the following restrictions: - 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. - */ - -#ifndef BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H -#define BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H - -#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" -#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" -#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" -#include "btMultiBodyConstraintSolver.h" - -SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs) -{ - int islandId; - - const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); - const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); - islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag(); - return islandId; -} -class btSortConstraintOnIslandPredicate2 -{ -public: - bool operator()(const btTypedConstraint* lhs, const btTypedConstraint* rhs) const - { - int rIslandId0, lIslandId0; - rIslandId0 = btGetConstraintIslandId2(rhs); - lIslandId0 = btGetConstraintIslandId2(lhs); - return lIslandId0 < rIslandId0; - } -}; - -SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs) -{ - int islandId; - - int islandTagA = lhs->getIslandIdA(); - int islandTagB = lhs->getIslandIdB(); - islandId = islandTagA >= 0 ? islandTagA : islandTagB; - return islandId; -} - -class btSortMultiBodyConstraintOnIslandPredicate -{ -public: - bool operator()(const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs) const - { - int rIslandId0, lIslandId0; - rIslandId0 = btGetMultiBodyConstraintIslandId(rhs); - lIslandId0 = btGetMultiBodyConstraintIslandId(lhs); - return lIslandId0 < rIslandId0; - } -}; - -struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback -{ - - btContactSolverInfo* m_solverInfo; - btMultiBodyConstraintSolver* m_solver; - btMultiBodyConstraint** m_multiBodySortedConstraints; - int m_numMultiBodyConstraints; - - btTypedConstraint** m_sortedConstraints; - int m_numConstraints; - btIDebugDraw* m_debugDrawer; - btDispatcher* m_dispatcher; - - btAlignedObjectArray<btCollisionObject*> m_bodies; - btAlignedObjectArray<btCollisionObject*> m_softBodies; - btAlignedObjectArray<btPersistentManifold*> m_manifolds; - btAlignedObjectArray<btTypedConstraint*> m_constraints; - btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints; - - btAlignedObjectArray<btSolverAnalyticsData> m_islandAnalyticsData; - - MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver* solver, - btDispatcher* dispatcher) - : m_solverInfo(NULL), - m_solver(solver), - m_multiBodySortedConstraints(NULL), - m_numConstraints(0), - m_debugDrawer(NULL), - m_dispatcher(dispatcher) - { - } - - MultiBodyInplaceSolverIslandCallback& operator=(const MultiBodyInplaceSolverIslandCallback& other) - { - btAssert(0); - (void)other; - return *this; - } - - SIMD_FORCE_INLINE virtual void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer) - { - m_islandAnalyticsData.clear(); - btAssert(solverInfo); - m_solverInfo = solverInfo; - - m_multiBodySortedConstraints = sortedMultiBodyConstraints; - m_numMultiBodyConstraints = numMultiBodyConstraints; - m_sortedConstraints = sortedConstraints; - m_numConstraints = numConstraints; - - m_debugDrawer = debugDrawer; - m_bodies.resize(0); - m_manifolds.resize(0); - m_constraints.resize(0); - m_multiBodyConstraints.resize(0); - } - - void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver) - { - m_solver = solver; - } - - virtual void processIsland(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifolds, int numManifolds, int islandId) - { - if (islandId < 0) - { - ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id - m_solver->solveMultiBodyGroup(bodies, numBodies, manifolds, numManifolds, m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher); - if (m_solverInfo->m_reportSolverAnalytics&1) - { - m_solver->m_analyticsData.m_islandId = islandId; - m_islandAnalyticsData.push_back(m_solver->m_analyticsData); - } - } - else - { - //also add all non-contact constraints/joints for this island - btTypedConstraint** startConstraint = 0; - btMultiBodyConstraint** startMultiBodyConstraint = 0; - - int numCurConstraints = 0; - int numCurMultiBodyConstraints = 0; - - int i; - - //find the first constraint for this island - - for (i = 0; i < m_numConstraints; i++) - { - if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId) - { - startConstraint = &m_sortedConstraints[i]; - break; - } - } - //count the number of constraints in this island - for (; i < m_numConstraints; i++) - { - if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId) - { - numCurConstraints++; - } - } - - for (i = 0; i < m_numMultiBodyConstraints; i++) - { - if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId) - { - startMultiBodyConstraint = &m_multiBodySortedConstraints[i]; - break; - } - } - //count the number of multi body constraints in this island - for (; i < m_numMultiBodyConstraints; i++) - { - if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId) - { - numCurMultiBodyConstraints++; - } - } - - //if (m_solverInfo->m_minimumSolverBatchSize<=1) - //{ - // m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); - //} else - { - for (i = 0; i < numBodies; i++) - { - bool isSoftBodyType = (bodies[i]->getInternalType() & btCollisionObject::CO_SOFT_BODY); - if (!isSoftBodyType) - { - m_bodies.push_back(bodies[i]); - } - else - { - m_softBodies.push_back(bodies[i]); - } - } - for (i = 0; i < numManifolds; i++) - m_manifolds.push_back(manifolds[i]); - for (i = 0; i < numCurConstraints; i++) - m_constraints.push_back(startConstraint[i]); - - for (i = 0; i < numCurMultiBodyConstraints; i++) - m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]); - - if ((m_multiBodyConstraints.size() + m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize) - { - processConstraints(islandId); - } - else - { - //printf("deferred\n"); - } - } - } - } - - virtual void processConstraints(int islandId=-1) - { - btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0; - btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0; - btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0; - btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0; - - //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size()); - - m_solver->solveMultiBodyGroup(bodies, m_bodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher); - if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1)) - { - m_solver->m_analyticsData.m_islandId = islandId; - m_islandAnalyticsData.push_back(m_solver->m_analyticsData); - } - m_bodies.resize(0); - m_softBodies.resize(0); - m_manifolds.resize(0); - m_constraints.resize(0); - m_multiBodyConstraints.resize(0); - } -}; - - -#endif /*BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H */ diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h deleted file mode 100644 index d943019e71..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h +++ /dev/null @@ -1,25 +0,0 @@ -/* -Copyright (c) 2015 Google Inc. - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MULTIBODY_JOINT_FEEDBACK_H -#define BT_MULTIBODY_JOINT_FEEDBACK_H - -#include "LinearMath/btSpatialAlgebra.h" - -struct btMultiBodyJointFeedback -{ - btSpatialForceVector m_reactionForces; -}; - -#endif //BT_MULTIBODY_JOINT_FEEDBACK_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp deleted file mode 100644 index 94b36ac108..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ /dev/null @@ -1,197 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///This file was written by Erwin Coumans - -#include "btMultiBodyJointLimitConstraint.h" -#include "btMultiBody.h" -#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, MULTIBODY_CONSTRAINT_LIMIT), - m_lowerBound(lower), - m_upperBound(upper) -{ -} - -void btMultiBodyJointLimitConstraint::finalizeMultiDof() -{ - // the data.m_jacobians never change, so may as well - // initialize them here - - allocateJacobiansMultiDof(); - - unsigned int offset = 6 + m_bodyA->getLink(m_linkA).m_dofOffset; - - // row 0: the lower bound - jacobianA(0)[offset] = 1; - // row 1: the upper bound - //jacobianA(1)[offset] = -1; - jacobianB(1)[offset] = -1; - - m_numDofsFinalized = m_jacSizeBoth; -} - -btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint() -{ -} - -int btMultiBodyJointLimitConstraint::getIslandIdA() const -{ - if (m_bodyA) - { - if (m_linkA < 0) - { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - } - else - { - if (m_bodyA->getLink(m_linkA).m_collider) - return m_bodyA->getLink(m_linkA).m_collider->getIslandTag(); - } - } - return -1; -} - -int btMultiBodyJointLimitConstraint::getIslandIdB() const -{ - if (m_bodyB) - { - if (m_linkB < 0) - { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - } - else - { - if (m_bodyB->getLink(m_linkB).m_collider) - return m_bodyB->getLink(m_linkB).m_collider->getIslandTag(); - } - } - return -1; -} - -void btMultiBodyJointLimitConstraint::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(); - } - - // 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)); - - for (int row = 0; row < getNumRows(); row++) - { - btScalar penetration = getPosition(row); - - //todo: consider adding some safety threshold here - if (penetration > 0) - { - continue; - } - btScalar direction = row ? -1 : 1; - - btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); - constraintRow.m_orgConstraint = this; - constraintRow.m_orgDofIndex = row; - - constraintRow.m_multiBodyA = m_bodyA; - constraintRow.m_multiBodyB = m_bodyB; - 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); - - { - //expect either prismatic or revolute joint type for now - btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic)); - switch (m_bodyA->getLink(m_linkA).m_jointType) - { - case btMultibodyLink::eRevolute: - { - constraintRow.m_contactNormal1.setZero(); - constraintRow.m_contactNormal2.setZero(); - btVector3 revoluteAxisInWorld = 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; - constraintRow.m_relpos1CrossNormal.setZero(); - constraintRow.m_relpos2CrossNormal.setZero(); - - break; - } - default: - { - btAssert(0); - } - }; - } - - { - btScalar positionalError = 0.f; - 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) - { - positionalError = 0; - velocityError = -penetration / infoGlobal.m_timeStep; - } - else - { - positionalError = -penetration * erp / infoGlobal.m_timeStep; - } - - 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_rhsPenetration = 0.f; - } - else - { - //split position and velocity into rhs and m_rhsPenetration - constraintRow.m_rhs = velocityImpulse; - constraintRow.m_rhsPenetration = penetrationImpulse; - } - } - } -} diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h deleted file mode 100644 index b810692b4c..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h +++ /dev/null @@ -1,63 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H -#define BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H - -#include "btMultiBodyConstraint.h" -struct btSolverInfo; - -class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint -{ -protected: - btScalar m_lowerBound; - btScalar m_upperBound; - -public: - btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper); - virtual ~btMultiBodyJointLimitConstraint(); - - virtual void finalizeMultiDof(); - - virtual int getIslandIdA() const; - virtual int getIslandIdB() const; - - virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal); - - virtual void debugDraw(class btIDebugDraw* drawer) - { - //todo(erwincoumans) - } - btScalar getLowerBound() const - { - return m_lowerBound; - } - btScalar getUpperBound() const - { - return m_upperBound; - } - void setLowerBound(btScalar lower) - { - m_lowerBound = lower; - } - void setUpperBound(btScalar upper) - { - m_upperBound = upper; - } -}; - -#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp deleted file mode 100644 index fec9b03213..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ /dev/null @@ -1,183 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///This file was written by Erwin Coumans - -#include "btMultiBodyJointMotor.h" -#include "btMultiBody.h" -#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, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR), - m_desiredVelocity(desiredVelocity), - m_desiredPosition(0), - m_kd(1.), - m_kp(0), - m_erp(1), - m_rhsClamp(SIMD_INFINITY) -{ - m_maxAppliedImpulse = maxMotorImpulse; - // the data.m_jacobians never change, so may as well - // initialize them here -} - -void btMultiBodyJointMotor::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; -} - -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, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR), - m_desiredVelocity(desiredVelocity), - m_desiredPosition(0), - m_kd(1.), - m_kp(0), - m_erp(1), - m_rhsClamp(SIMD_INFINITY) -{ - btAssert(linkDoF < body->getLink(link).m_dofCount); - - m_maxAppliedImpulse = maxMotorImpulse; -} -btMultiBodyJointMotor::~btMultiBodyJointMotor() -{ -} - -int btMultiBodyJointMotor::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 btMultiBodyJointMotor::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 btMultiBodyJointMotor::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); - - 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) - { - rhs = m_rhsClamp; - } - if (rhs < -m_rhsClamp) - { - rhs = -m_rhsClamp; - } - - fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse, false, 1, false, rhs); - constraintRow.m_orgConstraint = this; - constraintRow.m_orgDofIndex = row; - { - //expect either prismatic or revolute joint type for now - btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic)); - switch (m_bodyA->getLink(m_linkA).m_jointType) - { - case btMultibodyLink::eRevolute: - { - constraintRow.m_contactNormal1.setZero(); - constraintRow.m_contactNormal2.setZero(); - btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec); - constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld; - constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld; - - break; - } - case btMultibodyLink::ePrismatic: - { - btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec); - constraintRow.m_contactNormal1 = prismaticAxisInWorld; - constraintRow.m_contactNormal2 = -prismaticAxisInWorld; - constraintRow.m_relpos1CrossNormal.setZero(); - constraintRow.m_relpos2CrossNormal.setZero(); - - break; - } - default: - { - btAssert(0); - } - }; - } - } -} diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.h deleted file mode 100644 index 1aca36352e..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.h +++ /dev/null @@ -1,77 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///This file was written by Erwin Coumans - -#ifndef BT_MULTIBODY_JOINT_MOTOR_H -#define BT_MULTIBODY_JOINT_MOTOR_H - -#include "btMultiBodyConstraint.h" -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 - -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 int getIslandIdA() const; - virtual int getIslandIdB() const; - - virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal); - - 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 setErp(btScalar erp) - { - m_erp = erp; - } - virtual btScalar getErp() const - { - return m_erp; - } - virtual void setRhsClamp(btScalar rhsClamp) - { - m_rhsClamp = rhsClamp; - } - virtual void debugDraw(class btIDebugDraw* drawer) - { - //todo(erwincoumans) - } -}; - -#endif //BT_MULTIBODY_JOINT_MOTOR_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h deleted file mode 100644 index 5a1429340f..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h +++ /dev/null @@ -1,303 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MULTIBODY_LINK_H -#define BT_MULTIBODY_LINK_H - -#include "LinearMath/btQuaternion.h" -#include "LinearMath/btVector3.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" - -enum btMultiBodyLinkFlags -{ - BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1, - BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION = 2, -}; - -//both defines are now permanently enabled -#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS -#define TEST_SPATIAL_ALGEBRA_LAYER - -// -// Various spatial helper functions -// - -//namespace { - -#include "LinearMath/btSpatialAlgebra.h" - -//} - -// -// Link struct -// - -struct btMultibodyLink -{ - BT_DECLARE_ALIGNED_ALLOCATOR(); - - 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. - - 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) - - // 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; - - btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity; - - enum eFeatherstoneJointType - { - eRevolute = 0, - ePrismatic = 1, - eSpherical = 2, - ePlanar = 3, - eFixed = 4, - 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 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) - // - // 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 - btSpatialMotionVector m_axes[6]; - void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; } - void setAxisBottom(int dof, const btVector3 &axis) - { - m_axes[dof].m_bottomVec = axis; - } - void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) - { - m_axes[dof].m_topVec.setValue(x, y, z); - } - void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) - { - m_axes[dof].m_bottomVec.setValue(x, y, z); - } - const btVector3 &getAxisTop(int dof) const { return m_axes[dof].m_topVec; } - const btVector3 &getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; } - - 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. - - // predicted verstion - btQuaternion m_cachedRotParentToThis_interpolate; // rotates vectors in parent frame to vectors in local frame - btVector3 m_cachedRVector_interpolate; // vector from COM of parent to COM of this link, in local frame. - - btVector3 m_appliedForce; // In WORLD frame - btVector3 m_appliedTorque; // In WORLD frame - - btVector3 m_appliedConstraintForce; // In WORLD frame - btVector3 m_appliedConstraintTorque; // In WORLD frame - - btScalar m_jointPos[7]; - btScalar m_jointPos_interpolate[7]; - - //m_jointTorque is the joint torque applied by the user using 'addJointTorque'. - //It gets set to zero after each internal stepSimulation call - btScalar m_jointTorque[6]; - - class btMultiBodyLinkCollider *m_collider; - int m_flags; - - 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. - - // 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_cachedRotParentToThis_interpolate(0, 0, 0, 1), - m_collider(0), - m_flags(0), - m_dofCount(0), - m_posVarCount(0), - m_jointType(btMultibodyLink::eInvalid), - m_jointFeedback(0), - m_linkName(0), - m_jointName(0), - m_userPtr(0), - m_jointDamping(0), - m_jointFriction(0), - m_jointLowerLimit(0), - m_jointUpperLimit(0), - m_jointMaxForce(0), - m_jointMaxVelocity(0) - { - m_inertiaLocal.setValue(1, 1, 1); - setAxisTop(0, 0., 0., 0.); - setAxisBottom(0, 1., 0., 0.); - m_dVector.setValue(0, 0, 0); - m_eVector.setValue(0, 0, 0); - m_cachedRVector.setValue(0, 0, 0); - m_cachedRVector_interpolate.setValue(0, 0, 0); - m_appliedForce.setValue(0, 0, 0); - m_appliedTorque.setValue(0, 0, 0); - m_appliedConstraintForce.setValue(0, 0, 0); - m_appliedConstraintTorque.setValue(0, 0, 0); - // - m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f; - m_jointPos[3] = 1.f; //"quat.w" - m_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 - void updateCacheMultiDof(btScalar *pq = 0) - { - btScalar *pJointPos = (pq ? pq : &m_jointPos[0]); - btQuaternion& cachedRot = m_cachedRotParentToThis; - btVector3& cachedVector = m_cachedRVector; - switch (m_jointType) - { - case eRevolute: - { - cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; - cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); - - break; - } - case ePrismatic: - { - // m_cachedRotParentToThis never changes, so no need to update - cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0); - - break; - } - case eSpherical: - { - cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; - cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); - - break; - } - case ePlanar: - { - cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; - cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector); - - break; - } - case eFixed: - { - cachedRot = m_zeroRotParentToThis; - cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); - - break; - } - default: - { - //invalid type - btAssert(0); - } - } - m_cachedRotParentToThis_interpolate = m_cachedRotParentToThis; - m_cachedRVector_interpolate = m_cachedRVector; - } - - void updateInterpolationCacheMultiDof() - { - btScalar *pJointPos = &m_jointPos_interpolate[0]; - - btQuaternion& cachedRot = m_cachedRotParentToThis_interpolate; - btVector3& cachedVector = m_cachedRVector_interpolate; - switch (m_jointType) - { - case eRevolute: - { - cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; - cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); - - break; - } - case ePrismatic: - { - // m_cachedRotParentToThis never changes, so no need to update - cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0); - - break; - } - case eSpherical: - { - cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; - cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); - - break; - } - case ePlanar: - { - cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; - cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector); - - break; - } - case eFixed: - { - cachedRot = m_zeroRotParentToThis; - cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); - - break; - } - default: - { - //invalid type - btAssert(0); - } - } - } - - - -}; - -#endif //BT_MULTIBODY_LINK_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h deleted file mode 100644 index 3dc35a5814..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h +++ /dev/null @@ -1,195 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_FEATHERSTONE_LINK_COLLIDER_H -#define BT_FEATHERSTONE_LINK_COLLIDER_H - -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" - -#include "btMultiBody.h" -#include "LinearMath/btSerializer.h" - -#ifdef BT_USE_DOUBLE_PRECISION -#define btMultiBodyLinkColliderData btMultiBodyLinkColliderDoubleData -#define btMultiBodyLinkColliderDataName "btMultiBodyLinkColliderDoubleData" -#else -#define btMultiBodyLinkColliderData btMultiBodyLinkColliderFloatData -#define btMultiBodyLinkColliderDataName "btMultiBodyLinkColliderFloatData" -#endif - -class btMultiBodyLinkCollider : public btCollisionObject -{ - //protected: -public: - btMultiBody* m_multiBody; - int m_link; - - virtual ~btMultiBodyLinkCollider() - { - - } - btMultiBodyLinkCollider(btMultiBody* multiBody, int link) - : m_multiBody(multiBody), - m_link(link) - { - 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())) - { - m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT); - } - // else - //{ - // m_collisionFlags |= (btCollisionObject::CF_STATIC_OBJECT); - //} - - m_internalType = CO_FEATHERSTONE_LINK; - } - static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj) - { - 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) - return (btMultiBodyLinkCollider*)colObj; - return 0; - } - - virtual bool checkCollideWithOverride(const btCollisionObject* co) const - { - const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co); - if (!other) - return true; - if (other->m_multiBody != this->m_multiBody) - return true; - if (!m_multiBody->hasSelfCollision()) - return false; - - //check if 'link' has collision disabled - if (m_link >= 0) - { - const btMultibodyLink& link = m_multiBody->getLink(this->m_link); - if (link.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION) - { - int parent_of_this = m_link; - while (1) - { - if (parent_of_this == -1) - break; - parent_of_this = m_multiBody->getLink(parent_of_this).m_parent; - if (parent_of_this == other->m_link) - { - return false; - } - } - } - else if (link.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) - { - if (link.m_parent == other->m_link) - return false; - } - } - - if (other->m_link >= 0) - { - const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link); - if (otherLink.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION) - { - int parent_of_other = other->m_link; - while (1) - { - if (parent_of_other == -1) - break; - parent_of_other = m_multiBody->getLink(parent_of_other).m_parent; - if (parent_of_other == this->m_link) - return false; - } - } - else if (otherLink.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) - { - if (otherLink.m_parent == this->m_link) - return false; - } - } - return true; - } - - bool isStaticOrKinematic() const - { - return isStaticOrKinematicObject(); - } - - bool isKinematic() const - { - return isKinematicObject(); - } - - void setDynamicType(int dynamicType) - { - int oldFlags = getCollisionFlags(); - oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT); - setCollisionFlags(oldFlags | dynamicType); - } - - virtual int calculateSerializeBufferSize() const; - - ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; -}; - -// clang-format off - -struct btMultiBodyLinkColliderFloatData -{ - btCollisionObjectFloatData m_colObjData; - btMultiBodyFloatData *m_multiBody; - int m_link; - char m_padding[4]; -}; - -struct btMultiBodyLinkColliderDoubleData -{ - btCollisionObjectDoubleData m_colObjData; - btMultiBodyDoubleData *m_multiBody; - int m_link; - char m_padding[4]; -}; - -// clang-format on - -SIMD_FORCE_INLINE int btMultiBodyLinkCollider::calculateSerializeBufferSize() const -{ - return sizeof(btMultiBodyLinkColliderData); -} - -SIMD_FORCE_INLINE const char* btMultiBodyLinkCollider::serialize(void* dataBuffer, class btSerializer* serializer) const -{ - btMultiBodyLinkColliderData* dataOut = (btMultiBodyLinkColliderData*)dataBuffer; - btCollisionObject::serialize(&dataOut->m_colObjData, serializer); - - dataOut->m_link = this->m_link; - dataOut->m_multiBody = (btMultiBodyData*)serializer->getUniquePointer(m_multiBody); - - // Fill padding with zeros to appease msan. - memset(dataOut->m_padding, 0, sizeof(dataOut->m_padding)); - - return btMultiBodyLinkColliderDataName; -} - -#endif //BT_FEATHERSTONE_LINK_COLLIDER_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp deleted file mode 100644 index f2186a93e9..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp +++ /dev/null @@ -1,966 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2018 Google Inc. http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h" - -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" -#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" -#include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h" - -#define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - -static bool interleaveContactAndFriction1 = false; - -struct btJointNode1 -{ - int jointIndex; // pointer to enclosing dxJoint object - int otherBodyIndex; // *other* body this joint is connected to - int nextJointNodeIndex; //-1 for null - int constraintRowIndex; -}; - -// Helper function to compute a delta velocity in the constraint space. -static btScalar computeDeltaVelocityInConstraintSpace( - const btVector3& angularDeltaVelocity, - const btVector3& contactNormal, - btScalar invMass, - const btVector3& angularJacobian, - const btVector3& linearJacobian) -{ - return angularDeltaVelocity.dot(angularJacobian) + contactNormal.dot(linearJacobian) * invMass; -} - -// Faster version of computeDeltaVelocityInConstraintSpace that can be used when contactNormal and linearJacobian are -// identical. -static btScalar computeDeltaVelocityInConstraintSpace( - const btVector3& angularDeltaVelocity, - btScalar invMass, - const btVector3& angularJacobian) -{ - return angularDeltaVelocity.dot(angularJacobian) + invMass; -} - -// Helper function to compute a delta velocity in the constraint space. -static btScalar computeDeltaVelocityInConstraintSpace(const btScalar* deltaVelocity, const btScalar* jacobian, int size) -{ - btScalar result = 0; - for (int i = 0; i < size; ++i) - result += deltaVelocity[i] * jacobian[i]; - - return result; -} - -static btScalar computeConstraintMatrixDiagElementMultiBody( - const btAlignedObjectArray<btSolverBody>& solverBodyPool, - const btMultiBodyJacobianData& data, - const btMultiBodySolverConstraint& constraint) -{ - btScalar ret = 0; - - const btMultiBody* multiBodyA = constraint.m_multiBodyA; - const btMultiBody* multiBodyB = constraint.m_multiBodyB; - - if (multiBodyA) - { - const btScalar* jacA = &data.m_jacobians[constraint.m_jacAindex]; - const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex]; - const int ndofA = multiBodyA->getNumDofs() + 6; - ret += computeDeltaVelocityInConstraintSpace(deltaA, jacA, ndofA); - } - else - { - const int solverBodyIdA = constraint.m_solverBodyIdA; - btAssert(solverBodyIdA != -1); - const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA]; - const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0; - ret += computeDeltaVelocityInConstraintSpace( - constraint.m_relpos1CrossNormal, - invMassA, - constraint.m_angularComponentA); - } - - if (multiBodyB) - { - const btScalar* jacB = &data.m_jacobians[constraint.m_jacBindex]; - const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex]; - const int ndofB = multiBodyB->getNumDofs() + 6; - ret += computeDeltaVelocityInConstraintSpace(deltaB, jacB, ndofB); - } - else - { - const int solverBodyIdB = constraint.m_solverBodyIdB; - btAssert(solverBodyIdB != -1); - const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB]; - const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0; - ret += computeDeltaVelocityInConstraintSpace( - constraint.m_relpos2CrossNormal, - invMassB, - constraint.m_angularComponentB); - } - - return ret; -} - -static btScalar computeConstraintMatrixOffDiagElementMultiBody( - const btAlignedObjectArray<btSolverBody>& solverBodyPool, - const btMultiBodyJacobianData& data, - const btMultiBodySolverConstraint& constraint, - const btMultiBodySolverConstraint& offDiagConstraint) -{ - btScalar offDiagA = btScalar(0); - - const btMultiBody* multiBodyA = constraint.m_multiBodyA; - const btMultiBody* multiBodyB = constraint.m_multiBodyB; - const btMultiBody* offDiagMultiBodyA = offDiagConstraint.m_multiBodyA; - const btMultiBody* offDiagMultiBodyB = offDiagConstraint.m_multiBodyB; - - // Assumed at least one system is multibody - btAssert(multiBodyA || multiBodyB); - btAssert(offDiagMultiBodyA || offDiagMultiBodyB); - - if (offDiagMultiBodyA) - { - const btScalar* offDiagJacA = &data.m_jacobians[offDiagConstraint.m_jacAindex]; - - if (offDiagMultiBodyA == multiBodyA) - { - const int ndofA = multiBodyA->getNumDofs() + 6; - const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex]; - offDiagA += computeDeltaVelocityInConstraintSpace(deltaA, offDiagJacA, ndofA); - } - else if (offDiagMultiBodyA == multiBodyB) - { - const int ndofB = multiBodyB->getNumDofs() + 6; - const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex]; - offDiagA += computeDeltaVelocityInConstraintSpace(deltaB, offDiagJacA, ndofB); - } - } - else - { - const int solverBodyIdA = constraint.m_solverBodyIdA; - const int solverBodyIdB = constraint.m_solverBodyIdB; - - const int offDiagSolverBodyIdA = offDiagConstraint.m_solverBodyIdA; - btAssert(offDiagSolverBodyIdA != -1); - - if (offDiagSolverBodyIdA == solverBodyIdA) - { - btAssert(solverBodyIdA != -1); - const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA]; - const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0; - offDiagA += computeDeltaVelocityInConstraintSpace( - offDiagConstraint.m_relpos1CrossNormal, - offDiagConstraint.m_contactNormal1, - invMassA, constraint.m_angularComponentA, - constraint.m_contactNormal1); - } - else if (offDiagSolverBodyIdA == solverBodyIdB) - { - btAssert(solverBodyIdB != -1); - const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB]; - const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0; - offDiagA += computeDeltaVelocityInConstraintSpace( - offDiagConstraint.m_relpos1CrossNormal, - offDiagConstraint.m_contactNormal1, - invMassB, - constraint.m_angularComponentB, - constraint.m_contactNormal2); - } - } - - if (offDiagMultiBodyB) - { - const btScalar* offDiagJacB = &data.m_jacobians[offDiagConstraint.m_jacBindex]; - - if (offDiagMultiBodyB == multiBodyA) - { - const int ndofA = multiBodyA->getNumDofs() + 6; - const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex]; - offDiagA += computeDeltaVelocityInConstraintSpace(deltaA, offDiagJacB, ndofA); - } - else if (offDiagMultiBodyB == multiBodyB) - { - const int ndofB = multiBodyB->getNumDofs() + 6; - const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex]; - offDiagA += computeDeltaVelocityInConstraintSpace(deltaB, offDiagJacB, ndofB); - } - } - else - { - const int solverBodyIdA = constraint.m_solverBodyIdA; - const int solverBodyIdB = constraint.m_solverBodyIdB; - - const int offDiagSolverBodyIdB = offDiagConstraint.m_solverBodyIdB; - btAssert(offDiagSolverBodyIdB != -1); - - if (offDiagSolverBodyIdB == solverBodyIdA) - { - btAssert(solverBodyIdA != -1); - const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA]; - const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0; - offDiagA += computeDeltaVelocityInConstraintSpace( - offDiagConstraint.m_relpos2CrossNormal, - offDiagConstraint.m_contactNormal2, - invMassA, constraint.m_angularComponentA, - constraint.m_contactNormal1); - } - else if (offDiagSolverBodyIdB == solverBodyIdB) - { - btAssert(solverBodyIdB != -1); - const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB]; - const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0; - offDiagA += computeDeltaVelocityInConstraintSpace( - offDiagConstraint.m_relpos2CrossNormal, - offDiagConstraint.m_contactNormal2, - invMassB, constraint.m_angularComponentB, - constraint.m_contactNormal2); - } - } - - return offDiagA; -} - -void btMultiBodyMLCPConstraintSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) -{ - createMLCPFastRigidBody(infoGlobal); - createMLCPFastMultiBody(infoGlobal); -} - -void btMultiBodyMLCPConstraintSolver::createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal) -{ - int numContactRows = interleaveContactAndFriction1 ? 3 : 1; - - int numConstraintRows = m_allConstraintPtrArray.size(); - - if (numConstraintRows == 0) - return; - - int n = numConstraintRows; - { - BT_PROFILE("init b (rhs)"); - m_b.resize(numConstraintRows); - m_bSplit.resize(numConstraintRows); - m_b.setZero(); - m_bSplit.setZero(); - for (int i = 0; i < numConstraintRows; i++) - { - btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv; - if (!btFuzzyZero(jacDiag)) - { - btScalar rhs = m_allConstraintPtrArray[i]->m_rhs; - btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration; - m_b[i] = rhs / jacDiag; - m_bSplit[i] = rhsPenetration / jacDiag; - } - } - } - - // btScalar* w = 0; - // int nub = 0; - - m_lo.resize(numConstraintRows); - m_hi.resize(numConstraintRows); - - { - BT_PROFILE("init lo/ho"); - - for (int i = 0; i < numConstraintRows; i++) - { - if (0) //m_limitDependencies[i]>=0) - { - m_lo[i] = -BT_INFINITY; - m_hi[i] = BT_INFINITY; - } - else - { - m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit; - m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; - } - } - } - - // - int m = m_allConstraintPtrArray.size(); - - int numBodies = m_tmpSolverBodyPool.size(); - btAlignedObjectArray<int> bodyJointNodeArray; - { - BT_PROFILE("bodyJointNodeArray.resize"); - bodyJointNodeArray.resize(numBodies, -1); - } - btAlignedObjectArray<btJointNode1> jointNodeArray; - { - BT_PROFILE("jointNodeArray.reserve"); - jointNodeArray.reserve(2 * m_allConstraintPtrArray.size()); - } - - btMatrixXu& J3 = m_scratchJ3; - { - BT_PROFILE("J3.resize"); - J3.resize(2 * m, 8); - } - btMatrixXu& JinvM3 = m_scratchJInvM3; - { - BT_PROFILE("JinvM3.resize/setZero"); - - JinvM3.resize(2 * m, 8); - JinvM3.setZero(); - J3.setZero(); - } - int cur = 0; - int rowOffset = 0; - btAlignedObjectArray<int>& ofs = m_scratchOfs; - { - BT_PROFILE("ofs resize"); - ofs.resize(0); - ofs.resizeNoInitialize(m_allConstraintPtrArray.size()); - } - { - BT_PROFILE("Compute J and JinvM"); - int c = 0; - - int numRows = 0; - - for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++) - { - ofs[c] = rowOffset; - int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA; - int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; - btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; - btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; - - numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows; - if (orgBodyA) - { - { - int slotA = -1; - //find free jointNode slot for sbA - slotA = jointNodeArray.size(); - jointNodeArray.expand(); //NonInitializing(); - int prevSlot = bodyJointNodeArray[sbA]; - bodyJointNodeArray[sbA] = slotA; - jointNodeArray[slotA].nextJointNodeIndex = prevSlot; - jointNodeArray[slotA].jointIndex = c; - jointNodeArray[slotA].constraintRowIndex = i; - jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1; - } - for (int row = 0; row < numRows; row++, cur++) - { - btVector3 normalInvMass = m_allConstraintPtrArray[i + row]->m_contactNormal1 * orgBodyA->getInvMass(); - btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld(); - - for (int r = 0; r < 3; r++) - { - J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal1[r]); - J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal[r]); - JinvM3.setElem(cur, r, normalInvMass[r]); - JinvM3.setElem(cur, r + 4, relPosCrossNormalInvInertia[r]); - } - J3.setElem(cur, 3, 0); - JinvM3.setElem(cur, 3, 0); - J3.setElem(cur, 7, 0); - JinvM3.setElem(cur, 7, 0); - } - } - else - { - cur += numRows; - } - if (orgBodyB) - { - { - int slotB = -1; - //find free jointNode slot for sbA - slotB = jointNodeArray.size(); - jointNodeArray.expand(); //NonInitializing(); - int prevSlot = bodyJointNodeArray[sbB]; - bodyJointNodeArray[sbB] = slotB; - jointNodeArray[slotB].nextJointNodeIndex = prevSlot; - jointNodeArray[slotB].jointIndex = c; - jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1; - jointNodeArray[slotB].constraintRowIndex = i; - } - - for (int row = 0; row < numRows; row++, cur++) - { - btVector3 normalInvMassB = m_allConstraintPtrArray[i + row]->m_contactNormal2 * orgBodyB->getInvMass(); - btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld(); - - for (int r = 0; r < 3; r++) - { - J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal2[r]); - J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal[r]); - JinvM3.setElem(cur, r, normalInvMassB[r]); - JinvM3.setElem(cur, r + 4, relPosInvInertiaB[r]); - } - J3.setElem(cur, 3, 0); - JinvM3.setElem(cur, 3, 0); - J3.setElem(cur, 7, 0); - JinvM3.setElem(cur, 7, 0); - } - } - else - { - cur += numRows; - } - rowOffset += numRows; - } - } - - //compute JinvM = J*invM. - const btScalar* JinvM = JinvM3.getBufferPointer(); - - const btScalar* Jptr = J3.getBufferPointer(); - { - BT_PROFILE("m_A.resize"); - m_A.resize(n, n); - } - - { - BT_PROFILE("m_A.setZero"); - m_A.setZero(); - } - int c = 0; - { - int numRows = 0; - BT_PROFILE("Compute A"); - for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++) - { - int row__ = ofs[c]; - int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA; - int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; - // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; - // btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; - - numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows; - - const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__; - - { - int startJointNodeA = bodyJointNodeArray[sbA]; - while (startJointNodeA >= 0) - { - int j0 = jointNodeArray[startJointNodeA].jointIndex; - int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex; - if (j0 < c) - { - int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows; - size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8 * numRowsOther : 0; - //printf("%d joint i %d and j0: %d: ",count++,i,j0); - m_A.multiplyAdd2_p8r(JinvMrow, - Jptr + 2 * 8 * (size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]); - } - startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex; - } - } - - { - int startJointNodeB = bodyJointNodeArray[sbB]; - while (startJointNodeB >= 0) - { - int j1 = jointNodeArray[startJointNodeB].jointIndex; - int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex; - - if (j1 < c) - { - int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows; - size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8 * numRowsOther : 0; - m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)numRows, - Jptr + 2 * 8 * (size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]); - } - startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex; - } - } - } - - { - BT_PROFILE("compute diagonal"); - // compute diagonal blocks of m_A - - int row__ = 0; - int numJointRows = m_allConstraintPtrArray.size(); - - int jj = 0; - for (; row__ < numJointRows;) - { - //int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA; - int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB; - // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; - btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; - - const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows; - - const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__; - const btScalar* Jrow = Jptr + 2 * 8 * (size_t)row__; - m_A.multiply2_p8r(JinvMrow, Jrow, infom, infom, row__, row__); - if (orgBodyB) - { - m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)infom, Jrow + 8 * (size_t)infom, infom, infom, row__, row__); - } - row__ += infom; - jj++; - } - } - } - - if (1) - { - // add cfm to the diagonal of m_A - for (int i = 0; i < m_A.rows(); ++i) - { - m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep); - } - } - - ///fill the upper triangle of the matrix, to make it symmetric - { - BT_PROFILE("fill the upper triangle "); - m_A.copyLowerToUpperTriangle(); - } - - { - BT_PROFILE("resize/init x"); - m_x.resize(numConstraintRows); - m_xSplit.resize(numConstraintRows); - - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - for (int i = 0; i < m_allConstraintPtrArray.size(); i++) - { - const btSolverConstraint& c = *m_allConstraintPtrArray[i]; - m_x[i] = c.m_appliedImpulse; - m_xSplit[i] = c.m_appliedPushImpulse; - } - } - else - { - m_x.setZero(); - m_xSplit.setZero(); - } - } -} - -void btMultiBodyMLCPConstraintSolver::createMLCPFastMultiBody(const btContactSolverInfo& infoGlobal) -{ - const int multiBodyNumConstraints = m_multiBodyAllConstraintPtrArray.size(); - - if (multiBodyNumConstraints == 0) - return; - - // 1. Compute b - { - BT_PROFILE("init b (rhs)"); - - m_multiBodyB.resize(multiBodyNumConstraints); - m_multiBodyB.setZero(); - - for (int i = 0; i < multiBodyNumConstraints; ++i) - { - const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i]; - const btScalar jacDiag = constraint.m_jacDiagABInv; - - if (!btFuzzyZero(jacDiag)) - { - // Note that rhsPenetration is currently always zero because the split impulse hasn't been implemented for multibody yet. - const btScalar rhs = constraint.m_rhs; - m_multiBodyB[i] = rhs / jacDiag; - } - } - } - - // 2. Compute lo and hi - { - BT_PROFILE("init lo/ho"); - - m_multiBodyLo.resize(multiBodyNumConstraints); - m_multiBodyHi.resize(multiBodyNumConstraints); - - for (int i = 0; i < multiBodyNumConstraints; ++i) - { - const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i]; - m_multiBodyLo[i] = constraint.m_lowerLimit; - m_multiBodyHi[i] = constraint.m_upperLimit; - } - } - - // 3. Construct A matrix by using the impulse testing - { - BT_PROFILE("Compute A"); - - { - BT_PROFILE("m_A.resize"); - m_multiBodyA.resize(multiBodyNumConstraints, multiBodyNumConstraints); - } - - for (int i = 0; i < multiBodyNumConstraints; ++i) - { - // Compute the diagonal of A, which is A(i, i) - const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i]; - const btScalar diagA = computeConstraintMatrixDiagElementMultiBody(m_tmpSolverBodyPool, m_data, constraint); - m_multiBodyA.setElem(i, i, diagA); - - // Computes the off-diagonals of A: - // a. The rest of i-th row of A, from A(i, i+1) to A(i, n) - // b. The rest of i-th column of A, from A(i+1, i) to A(n, i) - for (int j = i + 1; j < multiBodyNumConstraints; ++j) - { - const btMultiBodySolverConstraint& offDiagConstraint = *m_multiBodyAllConstraintPtrArray[j]; - const btScalar offDiagA = computeConstraintMatrixOffDiagElementMultiBody(m_tmpSolverBodyPool, m_data, constraint, offDiagConstraint); - - // Set the off-diagonal values of A. Note that A is symmetric. - m_multiBodyA.setElem(i, j, offDiagA); - m_multiBodyA.setElem(j, i, offDiagA); - } - } - } - - // Add CFM to the diagonal of m_A - for (int i = 0; i < m_multiBodyA.rows(); ++i) - { - m_multiBodyA.setElem(i, i, m_multiBodyA(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep); - } - - // 4. Initialize x - { - BT_PROFILE("resize/init x"); - - m_multiBodyX.resize(multiBodyNumConstraints); - - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - for (int i = 0; i < multiBodyNumConstraints; ++i) - { - const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i]; - m_multiBodyX[i] = constraint.m_appliedImpulse; - } - } - else - { - m_multiBodyX.setZero(); - } - } -} - -bool btMultiBodyMLCPConstraintSolver::solveMLCP(const btContactSolverInfo& infoGlobal) -{ - bool result = true; - - if (m_A.rows() != 0) - { - // If using split impulse, we solve 2 separate (M)LCPs - if (infoGlobal.m_splitImpulse) - { - const btMatrixXu Acopy = m_A; - const btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies; - // TODO(JS): Do we really need these copies when solveMLCP takes them as const? - - result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations); - if (result) - result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo, m_hi, limitDependenciesCopy, infoGlobal.m_numIterations); - } - else - { - result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations); - } - } - - if (!result) - return false; - - if (m_multiBodyA.rows() != 0) - { - result = m_solver->solveMLCP(m_multiBodyA, m_multiBodyB, m_multiBodyX, m_multiBodyLo, m_multiBodyHi, m_multiBodyLimitDependencies, infoGlobal.m_numIterations); - } - - return result; -} - -btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlySetup( - btCollisionObject** bodies, - int numBodies, - btPersistentManifold** manifoldPtr, - int numManifolds, - btTypedConstraint** constraints, - int numConstraints, - const btContactSolverInfo& infoGlobal, - btIDebugDraw* debugDrawer) -{ - // 1. Setup for rigid-bodies - btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup( - bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); - - // 2. Setup for multi-bodies - // a. Collect all different kinds of constraint as pointers into one array, m_allConstraintPtrArray - // b. Set the index array for frictional contact constraints, m_limitDependencies - { - BT_PROFILE("gather constraint data"); - - int dindex = 0; - - const int numRigidBodyConstraints = m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size(); - const int numMultiBodyConstraints = m_multiBodyNonContactConstraints.size() + m_multiBodyNormalContactConstraints.size() + m_multiBodyFrictionContactConstraints.size(); - - m_allConstraintPtrArray.resize(0); - m_multiBodyAllConstraintPtrArray.resize(0); - - // i. Setup for rigid bodies - - m_limitDependencies.resize(numRigidBodyConstraints); - - for (int i = 0; i < m_tmpSolverNonContactConstraintPool.size(); ++i) - { - m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]); - m_limitDependencies[dindex++] = -1; - } - - int firstContactConstraintOffset = dindex; - - // The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead - if (interleaveContactAndFriction1) - { - for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++) - { - const int numFrictionPerContact = m_tmpSolverContactConstraintPool.size() == m_tmpSolverContactFrictionConstraintPool.size() ? 1 : 2; - - m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]); - m_limitDependencies[dindex++] = -1; - m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact]); - int findex = (m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact].m_frictionIndex * (1 + numFrictionPerContact)); - m_limitDependencies[dindex++] = findex + firstContactConstraintOffset; - if (numFrictionPerContact == 2) - { - m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact + 1]); - m_limitDependencies[dindex++] = findex + firstContactConstraintOffset; - } - } - } - else - { - for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++) - { - m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]); - m_limitDependencies[dindex++] = -1; - } - for (int i = 0; i < m_tmpSolverContactFrictionConstraintPool.size(); i++) - { - m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]); - m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex + firstContactConstraintOffset; - } - } - - if (!m_allConstraintPtrArray.size()) - { - m_A.resize(0, 0); - m_b.resize(0); - m_x.resize(0); - m_lo.resize(0); - m_hi.resize(0); - } - - // ii. Setup for multibodies - - dindex = 0; - - m_multiBodyLimitDependencies.resize(numMultiBodyConstraints); - - for (int i = 0; i < m_multiBodyNonContactConstraints.size(); ++i) - { - m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNonContactConstraints[i]); - m_multiBodyLimitDependencies[dindex++] = -1; - } - - firstContactConstraintOffset = dindex; - - // The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead - if (interleaveContactAndFriction1) - { - for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i) - { - const int numtiBodyNumFrictionPerContact = m_multiBodyNormalContactConstraints.size() == m_multiBodyFrictionContactConstraints.size() ? 1 : 2; - - m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]); - m_multiBodyLimitDependencies[dindex++] = -1; - - btMultiBodySolverConstraint& frictionContactConstraint1 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact]; - m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint1); - - const int findex = (frictionContactConstraint1.m_frictionIndex * (1 + numtiBodyNumFrictionPerContact)) + firstContactConstraintOffset; - - m_multiBodyLimitDependencies[dindex++] = findex; - - if (numtiBodyNumFrictionPerContact == 2) - { - btMultiBodySolverConstraint& frictionContactConstraint2 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact + 1]; - m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint2); - - m_multiBodyLimitDependencies[dindex++] = findex; - } - } - } - else - { - for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i) - { - m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]); - m_multiBodyLimitDependencies[dindex++] = -1; - } - for (int i = 0; i < m_multiBodyFrictionContactConstraints.size(); ++i) - { - m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyFrictionContactConstraints[i]); - m_multiBodyLimitDependencies[dindex++] = m_multiBodyFrictionContactConstraints[i].m_frictionIndex + firstContactConstraintOffset; - } - } - - if (!m_multiBodyAllConstraintPtrArray.size()) - { - m_multiBodyA.resize(0, 0); - m_multiBodyB.resize(0); - m_multiBodyX.resize(0); - m_multiBodyLo.resize(0); - m_multiBodyHi.resize(0); - } - } - - // Construct MLCP terms - { - BT_PROFILE("createMLCPFast"); - createMLCPFast(infoGlobal); - } - - return btScalar(0); -} - -btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) -{ - bool result = true; - { - BT_PROFILE("solveMLCP"); - result = solveMLCP(infoGlobal); - } - - // Fallback to btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations if the solution isn't valid. - if (!result) - { - m_fallback++; - return btMultiBodyConstraintSolver::solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); - } - - { - BT_PROFILE("process MLCP results"); - - for (int i = 0; i < m_allConstraintPtrArray.size(); ++i) - { - const btSolverConstraint& c = *m_allConstraintPtrArray[i]; - - const btScalar deltaImpulse = m_x[i] - c.m_appliedImpulse; - c.m_appliedImpulse = m_x[i]; - - int sbA = c.m_solverBodyIdA; - int sbB = c.m_solverBodyIdB; - - btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA]; - btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB]; - - solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); - solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); - - if (infoGlobal.m_splitImpulse) - { - const btScalar deltaPushImpulse = m_xSplit[i] - c.m_appliedPushImpulse; - solverBodyA.internalApplyPushImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaPushImpulse); - solverBodyB.internalApplyPushImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaPushImpulse); - c.m_appliedPushImpulse = m_xSplit[i]; - } - } - - for (int i = 0; i < m_multiBodyAllConstraintPtrArray.size(); ++i) - { - btMultiBodySolverConstraint& c = *m_multiBodyAllConstraintPtrArray[i]; - - const btScalar deltaImpulse = m_multiBodyX[i] - c.m_appliedImpulse; - c.m_appliedImpulse = m_multiBodyX[i]; - - btMultiBody* multiBodyA = c.m_multiBodyA; - if (multiBodyA) - { - const int ndofA = multiBodyA->getNumDofs() + 6; - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA); -#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations - //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse); -#endif // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - } - else - { - const int sbA = c.m_solverBodyIdA; - btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA]; - solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); - } - - btMultiBody* multiBodyB = c.m_multiBodyB; - if (multiBodyB) - { - const int ndofB = multiBodyB->getNumDofs() + 6; - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse, c.m_deltaVelBindex, ndofB); -#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations - //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse); -#endif // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - } - else - { - const int sbB = c.m_solverBodyIdB; - btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB]; - solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); - } - } - } - - return btScalar(0); -} - -btMultiBodyMLCPConstraintSolver::btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface* solver) - : m_solver(solver), m_fallback(0) -{ - // Do nothing -} - -btMultiBodyMLCPConstraintSolver::~btMultiBodyMLCPConstraintSolver() -{ - // Do nothing -} - -void btMultiBodyMLCPConstraintSolver::setMLCPSolver(btMLCPSolverInterface* solver) -{ - m_solver = solver; -} - -int btMultiBodyMLCPConstraintSolver::getNumFallbacks() const -{ - return m_fallback; -} - -void btMultiBodyMLCPConstraintSolver::setNumFallbacks(int num) -{ - m_fallback = num; -} - -btConstraintSolverType btMultiBodyMLCPConstraintSolver::getSolverType() const -{ - return BT_MLCP_SOLVER; -} diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h deleted file mode 100644 index 77fdb86bb9..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h +++ /dev/null @@ -1,187 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2018 Google Inc. http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H -#define BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H - -#include "LinearMath/btMatrixX.h" -#include "LinearMath/btThreads.h" -#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" - -class btMLCPSolverInterface; -class btMultiBody; - -class btMultiBodyMLCPConstraintSolver : public btMultiBodyConstraintSolver -{ -protected: - /// \name MLCP Formulation for Rigid Bodies - /// \{ - - /// A matrix in the MLCP formulation - btMatrixXu m_A; - - /// b vector in the MLCP formulation. - btVectorXu m_b; - - /// Constraint impulse, which is an output of MLCP solving. - btVectorXu m_x; - - /// Lower bound of constraint impulse, \c m_x. - btVectorXu m_lo; - - /// Upper bound of constraint impulse, \c m_x. - btVectorXu m_hi; - - /// \} - - /// \name Cache Variables for Split Impulse for Rigid Bodies - /// When using 'split impulse' we solve two separate (M)LCPs - /// \{ - - /// Split impulse Cache vector corresponding to \c m_b. - btVectorXu m_bSplit; - - /// Split impulse cache vector corresponding to \c m_x. - btVectorXu m_xSplit; - - /// \} - - /// \name MLCP Formulation for Multibodies - /// \{ - - /// A matrix in the MLCP formulation - btMatrixXu m_multiBodyA; - - /// b vector in the MLCP formulation. - btVectorXu m_multiBodyB; - - /// Constraint impulse, which is an output of MLCP solving. - btVectorXu m_multiBodyX; - - /// Lower bound of constraint impulse, \c m_x. - btVectorXu m_multiBodyLo; - - /// Upper bound of constraint impulse, \c m_x. - btVectorXu m_multiBodyHi; - - /// \} - - /// Indices of normal contact constraint associated with frictional contact constraint for rigid bodies. - /// - /// This is used by the MLCP solver to update the upper bounds of frictional contact impulse given intermediate - /// normal contact impulse. For example, i-th element represents the index of a normal constraint that is - /// accosiated with i-th frictional contact constraint if i-th constraint is a frictional contact constraint. - /// Otherwise, -1. - btAlignedObjectArray<int> m_limitDependencies; - - /// Indices of normal contact constraint associated with frictional contact constraint for multibodies. - /// - /// This is used by the MLCP solver to update the upper bounds of frictional contact impulse given intermediate - /// normal contact impulse. For example, i-th element represents the index of a normal constraint that is - /// accosiated with i-th frictional contact constraint if i-th constraint is a frictional contact constraint. - /// Otherwise, -1. - btAlignedObjectArray<int> m_multiBodyLimitDependencies; - - /// Array of all the rigid body constraints - btAlignedObjectArray<btSolverConstraint*> m_allConstraintPtrArray; - - /// Array of all the multibody constraints - btAlignedObjectArray<btMultiBodySolverConstraint*> m_multiBodyAllConstraintPtrArray; - - /// MLCP solver - btMLCPSolverInterface* m_solver; - - /// Count of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the MLCP solver fails. - int m_fallback; - - /// \name MLCP Scratch Variables - /// The following scratch variables are not stateful -- contents are cleared prior to each use. - /// They are only cached here to avoid extra memory allocations and deallocations and to ensure - /// that multiple instances of the solver can be run in parallel. - /// - /// \{ - - /// Cache variable for constraint Jacobian matrix. - btMatrixXu m_scratchJ3; - - /// Cache variable for constraint Jacobian times inverse mass matrix. - btMatrixXu m_scratchJInvM3; - - /// Cache variable for offsets. - btAlignedObjectArray<int> m_scratchOfs; - - /// \} - - /// Constructs MLCP terms, which are \c m_A, \c m_b, \c m_lo, and \c m_hi. - virtual void createMLCPFast(const btContactSolverInfo& infoGlobal); - - /// Constructs MLCP terms for constraints of two rigid bodies - void createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal); - - /// Constructs MLCP terms for constraints of two multi-bodies or one rigid body and one multibody - void createMLCPFastMultiBody(const btContactSolverInfo& infoGlobal); - - /// Solves MLCP and returns the success - virtual bool solveMLCP(const btContactSolverInfo& infoGlobal); - - // Documentation inherited - btScalar solveGroupCacheFriendlySetup( - btCollisionObject** bodies, - int numBodies, - btPersistentManifold** manifoldPtr, - int numManifolds, - btTypedConstraint** constraints, - int numConstraints, - const btContactSolverInfo& infoGlobal, - btIDebugDraw* debugDrawer) BT_OVERRIDE; - - // Documentation inherited - btScalar solveGroupCacheFriendlyIterations( - btCollisionObject** bodies, - int numBodies, - btPersistentManifold** manifoldPtr, - int numManifolds, - btTypedConstraint** constraints, - int numConstraints, - const btContactSolverInfo& infoGlobal, - btIDebugDraw* debugDrawer) ; - -public: - BT_DECLARE_ALIGNED_ALLOCATOR() - - /// Constructor - /// - /// \param[in] solver MLCP solver. Assumed it's not null. - /// \param[in] maxLCPSize Maximum size of LCP to solve using MLCP solver. If the MLCP size exceeds this number, sequaltial impulse method will be used. - explicit btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface* solver); - - /// Destructor - virtual ~btMultiBodyMLCPConstraintSolver(); - - /// Sets MLCP solver. Assumed it's not null. - void setMLCPSolver(btMLCPSolverInterface* solver); - - /// Returns the number of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the MLCP - /// solver fails. - int getNumFallbacks() const; - - /// Sets the number of fallbacks. This function may be used to reset the number to zero. - void setNumFallbacks(int num); - - /// Returns the constraint solver type. - virtual btConstraintSolverType getSolverType() const; -}; - -#endif // BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp deleted file mode 100644 index f51e69deb1..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ /dev/null @@ -1,216 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///This file was written by Erwin Coumans - -#include "btMultiBodyPoint2Point.h" -#include "btMultiBodyLinkCollider.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btIDebugDraw.h" - -#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST -#define BTMBP2PCONSTRAINT_DIM 3 -#else -#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, MULTIBODY_CONSTRAINT_POINT_TO_POINT), - m_rigidBodyA(0), - m_rigidBodyB(bodyB), - m_pivotInA(pivotInA), - m_pivotInB(pivotInB) -{ - m_data.resize(BTMBP2PCONSTRAINT_DIM); //at least store the applied impulses -} - -btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB) - : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT), - m_rigidBodyA(0), - m_rigidBodyB(0), - m_pivotInA(pivotInA), - m_pivotInB(pivotInB) -{ - m_data.resize(BTMBP2PCONSTRAINT_DIM); //at least store the applied impulses -} - -void btMultiBodyPoint2Point::finalizeMultiDof() -{ - //not implemented yet - btAssert(0); -} - -btMultiBodyPoint2Point::~btMultiBodyPoint2Point() -{ -} - -int btMultiBodyPoint2Point::getIslandIdA() const -{ - if (m_rigidBodyA) - return m_rigidBodyA->getIslandTag(); - - if (m_bodyA) - { - if (m_linkA < 0) - { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - } - else - { - if (m_bodyA->getLink(m_linkA).m_collider) - return m_bodyA->getLink(m_linkA).m_collider->getIslandTag(); - } - } - return -1; -} - -int btMultiBodyPoint2Point::getIslandIdB() const -{ - if (m_rigidBodyB) - return m_rigidBodyB->getIslandTag(); - if (m_bodyB) - { - if (m_linkB < 0) - { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - } - else - { - if (m_bodyB->getLink(m_linkB).m_collider) - return m_bodyB->getLink(m_linkB).m_collider->getIslandTag(); - } - } - return -1; -} - -void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal) -{ - // 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); - - constraintRow.m_solverBodyIdA = data.m_fixedBodyId; - constraintRow.m_solverBodyIdB = data.m_fixedBodyId; - - btVector3 contactNormalOnB(0, 0, 0); -#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST - contactNormalOnB[i] = -1; -#else - contactNormalOnB[i % 3] = -1; -#endif - - // 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 - { - if (m_bodyA) - pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); - } - btVector3 pivotBworld = m_pivotInB; - if (m_rigidBodyB) - { - constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId(); - 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; - -#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 -#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; - - 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); -#endif - } -} - -void btMultiBodyPoint2Point::debugDraw(class btIDebugDraw* drawer) -{ - btTransform tr; - tr.setIdentity(); - - if (m_rigidBodyA) - { - btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA; - tr.setOrigin(pivot); - drawer->drawTransform(tr, 0.1); - } - if (m_bodyA) - { - btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); - tr.setOrigin(pivotAworld); - drawer->drawTransform(tr, 0.1); - } - if (m_rigidBodyB) - { - // that ideally should draw the same frame - btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB; - tr.setOrigin(pivot); - drawer->drawTransform(tr, 0.1); - } - if (m_bodyB) - { - btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); - tr.setOrigin(pivotBworld); - drawer->drawTransform(tr, 0.1); - } -} diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h deleted file mode 100644 index ef03a557ec..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h +++ /dev/null @@ -1,64 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///This file was written by Erwin Coumans - -#ifndef BT_MULTIBODY_POINT2POINT_H -#define BT_MULTIBODY_POINT2POINT_H - -#include "btMultiBodyConstraint.h" - -//#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST - -ATTRIBUTE_ALIGNED16(class) -btMultiBodyPoint2Point : public btMultiBodyConstraint -{ -protected: - 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); - - virtual ~btMultiBodyPoint2Point(); - - virtual void finalizeMultiDof(); - - virtual int getIslandIdA() const; - virtual int getIslandIdB() const; - - virtual void createConstraintRows(btMultiBodyConstraintArray & constraintRows, - btMultiBodyJacobianData & data, - const btContactSolverInfo& infoGlobal); - - const btVector3& getPivotInB() const - { - return m_pivotInB; - } - - virtual void setPivotInB(const btVector3& pivotInB) - { - m_pivotInB = pivotInB; - } - - virtual void debugDraw(class btIDebugDraw * drawer); -}; - -#endif //BT_MULTIBODY_POINT2POINT_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp deleted file mode 100644 index 48ec1d5af2..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp +++ /dev/null @@ -1,234 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///This file was written by Erwin Coumans - -#include "btMultiBodySliderConstraint.h" -#include "btMultiBodyLinkCollider.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" -#include "LinearMath/btIDebugDraw.h" - -#define BTMBSLIDERCONSTRAINT_DIM 5 -#define EPSILON 0.000001 - -btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis) - : btMultiBodyConstraint(body, 0, link, -1, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER), - m_rigidBodyA(0), - m_rigidBodyB(bodyB), - m_pivotInA(pivotInA), - m_pivotInB(pivotInB), - m_frameInA(frameInA), - m_frameInB(frameInB), - m_jointAxis(jointAxis) -{ - m_data.resize(BTMBSLIDERCONSTRAINT_DIM); //at least store the applied impulses -} - -btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis) - : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER), - m_rigidBodyA(0), - m_rigidBodyB(0), - m_pivotInA(pivotInA), - m_pivotInB(pivotInB), - m_frameInA(frameInA), - m_frameInB(frameInB), - m_jointAxis(jointAxis) -{ - m_data.resize(BTMBSLIDERCONSTRAINT_DIM); //at least store the applied impulses -} - -void btMultiBodySliderConstraint::finalizeMultiDof() -{ - //not implemented yet - btAssert(0); -} - -btMultiBodySliderConstraint::~btMultiBodySliderConstraint() -{ -} - -int btMultiBodySliderConstraint::getIslandIdA() const -{ - if (m_rigidBodyA) - return m_rigidBodyA->getIslandTag(); - - if (m_bodyA) - { - if (m_linkA < 0) - { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - } - else - { - if (m_bodyA->getLink(m_linkA).m_collider) - return m_bodyA->getLink(m_linkA).m_collider->getIslandTag(); - } - } - return -1; -} - -int btMultiBodySliderConstraint::getIslandIdB() const -{ - if (m_rigidBodyB) - return m_rigidBodyB->getIslandTag(); - if (m_bodyB) - { - if (m_linkB < 0) - { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - } - else - { - if (m_bodyB->getLink(m_linkB).m_collider) - return m_bodyB->getLink(m_linkB).m_collider->getIslandTag(); - } - } - return -1; -} -void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal) -{ - // Convert local points back to world - btVector3 pivotAworld = m_pivotInA; - btMatrix3x3 frameAworld = m_frameInA; - btVector3 jointAxis = m_jointAxis; - if (m_rigidBodyA) - { - pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA; - frameAworld = m_frameInA.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation()); - jointAxis = quatRotate(m_rigidBodyA->getOrientation(), m_jointAxis); - } - else if (m_bodyA) - { - pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); - frameAworld = m_bodyA->localFrameToWorld(m_linkA, m_frameInA); - jointAxis = m_bodyA->localDirToWorld(m_linkA, m_jointAxis); - } - btVector3 pivotBworld = m_pivotInB; - btMatrix3x3 frameBworld = m_frameInB; - if (m_rigidBodyB) - { - pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB; - frameBworld = m_frameInB.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation()); - } - else if (m_bodyB) - { - pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); - frameBworld = m_bodyB->localFrameToWorld(m_linkB, m_frameInB); - } - - btVector3 constraintAxis[2]; - for (int i = 0; i < 3; ++i) - { - constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis); - if (constraintAxis[0].safeNorm() > EPSILON) - { - constraintAxis[0] = constraintAxis[0].normalized(); - constraintAxis[1] = jointAxis.cross(constraintAxis[0]); - constraintAxis[1] = constraintAxis[1].normalized(); - break; - } - } - - btMatrix3x3 relRot = frameAworld.inverse() * frameBworld; - btVector3 angleDiff; - btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff); - - int numDim = BTMBSLIDERCONSTRAINT_DIM; - for (int i = 0; i < numDim; i++) - { - btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); - constraintRow.m_orgConstraint = this; - constraintRow.m_orgDofIndex = i; - constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0); - constraintRow.m_contactNormal1.setValue(0, 0, 0); - constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0); - constraintRow.m_contactNormal2.setValue(0, 0, 0); - constraintRow.m_angularComponentA.setValue(0, 0, 0); - constraintRow.m_angularComponentB.setValue(0, 0, 0); - - constraintRow.m_solverBodyIdA = data.m_fixedBodyId; - constraintRow.m_solverBodyIdB = data.m_fixedBodyId; - - if (m_rigidBodyA) - { - constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId(); - } - if (m_rigidBodyB) - { - constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId(); - } - - btVector3 constraintNormalLin(0, 0, 0); - btVector3 constraintNormalAng(0, 0, 0); - btScalar posError = 0.0; - if (i < 2) - { - constraintNormalLin = constraintAxis[i]; - posError = (pivotAworld - pivotBworld).dot(constraintNormalLin); - fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, - constraintNormalLin, pivotAworld, pivotBworld, - posError, - infoGlobal, - -m_maxAppliedImpulse, m_maxAppliedImpulse); - } - else - { //i>=2 - constraintNormalAng = frameAworld.getColumn(i % 3); - posError = angleDiff[i % 3]; - fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, - constraintNormalLin, pivotAworld, pivotBworld, - posError, - infoGlobal, - -m_maxAppliedImpulse, m_maxAppliedImpulse, true); - } - } -} - -void btMultiBodySliderConstraint::debugDraw(class btIDebugDraw* drawer) -{ - btTransform tr; - tr.setIdentity(); - - if (m_rigidBodyA) - { - btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA; - tr.setOrigin(pivot); - drawer->drawTransform(tr, 0.1); - } - if (m_bodyA) - { - btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); - tr.setOrigin(pivotAworld); - drawer->drawTransform(tr, 0.1); - } - if (m_rigidBodyB) - { - // that ideally should draw the same frame - btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB; - tr.setOrigin(pivot); - drawer->drawTransform(tr, 0.1); - } - if (m_bodyB) - { - btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); - tr.setOrigin(pivotBworld); - drawer->drawTransform(tr, 0.1); - } -} diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h deleted file mode 100644 index b192b6f8f3..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h +++ /dev/null @@ -1,102 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///This file was written by Erwin Coumans - -#ifndef BT_MULTIBODY_SLIDER_CONSTRAINT_H -#define BT_MULTIBODY_SLIDER_CONSTRAINT_H - -#include "btMultiBodyConstraint.h" - -class btMultiBodySliderConstraint : public btMultiBodyConstraint -{ -protected: - btRigidBody* m_rigidBodyA; - btRigidBody* m_rigidBodyB; - btVector3 m_pivotInA; - btVector3 m_pivotInB; - btMatrix3x3 m_frameInA; - btMatrix3x3 m_frameInB; - btVector3 m_jointAxis; - -public: - btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis); - btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis); - - virtual ~btMultiBodySliderConstraint(); - - virtual void finalizeMultiDof(); - - virtual int getIslandIdA() const; - virtual int getIslandIdB() const; - - virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal); - - const btVector3& getPivotInA() const - { - return m_pivotInA; - } - - void setPivotInA(const btVector3& pivotInA) - { - m_pivotInA = pivotInA; - } - - const btVector3& getPivotInB() const - { - return m_pivotInB; - } - - virtual void setPivotInB(const btVector3& pivotInB) - { - m_pivotInB = pivotInB; - } - - const btMatrix3x3& getFrameInA() const - { - return m_frameInA; - } - - void setFrameInA(const btMatrix3x3& frameInA) - { - m_frameInA = frameInA; - } - - const btMatrix3x3& getFrameInB() const - { - return m_frameInB; - } - - virtual void setFrameInB(const btMatrix3x3& frameInB) - { - m_frameInB = frameInB; - } - - const btVector3& getJointAxis() const - { - return m_jointAxis; - } - - void setJointAxis(const btVector3& jointAxis) - { - m_jointAxis = jointAxis; - } - - virtual void debugDraw(class btIDebugDraw* drawer); -}; - -#endif //BT_MULTIBODY_SLIDER_CONSTRAINT_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h deleted file mode 100644 index deed3e2a12..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h +++ /dev/null @@ -1,90 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MULTIBODY_SOLVER_CONSTRAINT_H -#define BT_MULTIBODY_SOLVER_CONSTRAINT_H - -#include "LinearMath/btVector3.h" -#include "LinearMath/btAlignedObjectArray.h" - -class btMultiBody; -class btMultiBodyConstraint; -#include "BulletDynamics/ConstraintSolver/btSolverBody.h" -#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 -{ - 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 { - void* m_originalContactPoint; - btScalar m_unusedPadding4; - }; - - int m_overrideNumSolverIterations; - int m_frictionIndex; - - int m_solverBodyIdA; - btMultiBody* m_multiBodyA; - int m_linkA; - - int m_solverBodyIdB; - btMultiBody* m_multiBodyB; - int m_linkB; - - //for writing back applied impulses - btMultiBodyConstraint* m_orgConstraint; - int m_orgDofIndex; - - enum btSolverConstraintType - { - BT_SOLVER_CONTACT_1D = 0, - BT_SOLVER_FRICTION_1D - }; -}; - -typedef btAlignedObjectArray<btMultiBodySolverConstraint> btMultiBodyConstraintArray; - -#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp deleted file mode 100644 index 00a7ef3579..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp +++ /dev/null @@ -1,180 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2018 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///This file was written by Erwin Coumans - -#include "btMultiBodySphericalJointMotor.h" -#include "btMultiBody.h" -#include "btMultiBodyLinkCollider.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "LinearMath/btTransformUtil.h" -#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" - -btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse) - : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true, MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR), - m_desiredVelocity(0, 0, 0), - m_desiredPosition(0,0,0,1), - m_use_multi_dof_params(false), - m_kd(1., 1., 1.), - m_kp(0.2, 0.2, 0.2), - m_erp(1), - m_rhsClamp(SIMD_INFINITY), - m_maxAppliedImpulseMultiDof(maxMotorImpulse, maxMotorImpulse, maxMotorImpulse), - m_damping(1.0, 1.0, 1.0) -{ - - 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]; - - double kd = m_use_multi_dof_params ? m_kd[row % 3] : m_kd[0]; - btScalar velocityError = (desiredVelocity - currentVelocity) * kd; - - 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); - double kp = m_use_multi_dof_params ? m_kp[row % 3] : m_kp[0]; - posError = kp*angleDiff[row % 3]; - double max_applied_impulse = m_use_multi_dof_params ? m_maxAppliedImpulseMultiDof[row % 3] : m_maxAppliedImpulse; - fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, - btVector3(0,0,0), dummy, dummy, - posError, - infoGlobal, - -max_applied_impulse, max_applied_impulse, true, - 1.0, false, 0, 0, - m_damping[row % 3]); - constraintRow.m_orgConstraint = this; - constraintRow.m_orgDofIndex = row; - break; - } - default: - { - btAssert(0); - } - }; - } - } -} diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h deleted file mode 100644 index bdeccc2e24..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h +++ /dev/null @@ -1,118 +0,0 @@ -/* -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; - bool m_use_multi_dof_params; - btVector3 m_kd; - btVector3 m_kp; - btScalar m_erp; - btScalar m_rhsClamp; //maximum error - btVector3 m_maxAppliedImpulseMultiDof; - btVector3 m_damping; - -public: - btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse); - - 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.0) - { - m_desiredVelocity = velTarget; - m_kd = btVector3(kd, kd, kd); - m_use_multi_dof_params = false; - } - - virtual void setVelocityTargetMultiDof(const btVector3& velTarget, const btVector3& kd = btVector3(1.0, 1.0, 1.0)) - { - m_desiredVelocity = velTarget; - m_kd = kd; - m_use_multi_dof_params = true; - } - - virtual void setPositionTarget(const btQuaternion& posTarget, btScalar kp =1.f) - { - m_desiredPosition = posTarget; - m_kp = btVector3(kp, kp, kp); - m_use_multi_dof_params = false; - } - - virtual void setPositionTargetMultiDof(const btQuaternion& posTarget, const btVector3& kp = btVector3(1.f, 1.f, 1.f)) - { - m_desiredPosition = posTarget; - m_kp = kp; - m_use_multi_dof_params = true; - } - - virtual void setErp(btScalar erp) - { - m_erp = erp; - } - virtual btScalar getErp() const - { - return m_erp; - } - virtual void setRhsClamp(btScalar rhsClamp) - { - m_rhsClamp = rhsClamp; - } - - btScalar getMaxAppliedImpulseMultiDof(int i) const - { - return m_maxAppliedImpulseMultiDof[i]; - } - - void setMaxAppliedImpulseMultiDof(const btVector3& maxImp) - { - m_maxAppliedImpulseMultiDof = maxImp; - m_use_multi_dof_params = true; - } - - btScalar getDamping(int i) const - { - return m_damping[i]; - } - - void setDamping(const btVector3& damping) - { - m_damping = damping; - } - - virtual void debugDraw(class btIDebugDraw* drawer) - { - //todo(erwincoumans) - } -}; - -#endif //BT_MULTIBODY_SPHERICAL_JOINT_MOTOR_H |